# This script generates C code for the partial derivatives of the
# objective function used by the lidar point optimizer. The objective
# function is the sum of squared differences between the target points
# and the transformed source points. This script only computes the
# partial derivatives for one term of the sum. After running this
# script, the generated C code should be copied into the loop that
# computes the sum of squared difference between the points. See
# the file icp-gsl.cpp in the function 'grad' to see how this was
# done.

p0 = q0;
p1 = q1;
p2 = q2;
p3 = q3;

inv = 1.0 / sqrt(p0 * p0 + p1 * p1 + p2 * p2 + p3 * p3);
p0 = p0 * inv;
p1 = p1 * inv;
p2 = p2 * inv;
p3 = p3 * inv;

s0 = s0 - cor0;
s1 = s1 - cor1;
s2 = s2 - cor2;

a = s0;
b = s1;
c = s2;

w = p1 * a + p2 * b + p3 * c;
s0 = 2.0 * (p0 * (a * p0 - (p2 * c - p3 * b)) + w * p1) - a;
s1 = 2.0 * (p0 * (b * p0 - (p3 * a - p1 * c)) + w * p2) - b;
s2 = 2.0 * (p0 * (c * p0 - (p1 * b - p2 * a)) + w * p3) - c;

s0 = s0 + cor0;
s1 = s1 + cor1;
s2 = s2 + cor2;

s0 = s0 + x * m;
s1 = s1 + y * m;
s2 = s2 + z * m;

d2 = ( s0 - t0 )^2 + ( s1 - t1 )^2 + ( s2 - t2 )^2;

0;
0;
print_csrc(diff(d2,x));
0;
0;
print_csrc(diff(d2,y));
0;
0;
print_csrc(diff(d2,z));
0;
0;
print_csrc(diff(d2,q0));
0;
0;
print_csrc(diff(d2,q1));
0;
0;
print_csrc(diff(d2,q2));
0;
0;
print_csrc(diff(d2,q3));
0;
0;
