Below is the operator for my dynamic auto diff cost function. At every iteration additional constraints such as fixing solid element 1
and applying parallel cost between two lines belonging to solid element 1 and solid element 2 to align the line of solid element 2 with
that of solid element 1.
The parameters (xd1, yd1, zd1) often have (0, 0, 1) type of values adn
(rot_x, rot_y, rot_z) often have (0, Pi, 2*Pi) kind of values.
template <typename T>
bool ParallelLines3D_Cost::operator()(const T* const* variables, T* residual) const {
//Rotation of first solid element
const T& rot_x1 = variables[0][ var_indices[0] ];
const T& rot_y1 = variables[0][ var_indices[1] ];
const T& rot_z1 = variables[0][ var_indices[2] ];
//Rotation of second solid element
const T& rot_x2 = variables[0][ var_indices[3] ];
const T& rot_y2 = variables[0][ var_indices[4] ];
const T& rot_z2 = variables[0][ var_indices[5] ];
(linedir_x1, linedir_y1, linedir_z1) => Global line direction values
(linedir_x2, linedir_y2, linedir_z2) => Global line direction values
(xd1, yd1, zd1) => Local Line 1 Dir values relative to solid element 1
(xd2, yd2, zd2) => Local Line 2 Dir values relative to solid element 2
T linedir_x1 = ( ( ( (ace_ptr->xd1) * cos( rot_y1 ) + ( (ace_ptr->yd1) * sin( rot_x1 ) + (ace_ptr->zd1) * cos( rot_x1 ) ) * sin( rot_y1 ) ) * cos( rot_z1 ) )
- ( ( (ace_ptr->yd1) * cos( rot_x1 ) - (ace_ptr->zd1) * sin( rot_x1 ) ) * sin( rot_z1 ) ) );
T linedir_x2 = ( ( ( (ace_ptr->xd2) * cos( rot_y2 ) + ( (ace_ptr->yd2) * sin( rot_x2 ) + (ace_ptr->zd2) * cos( rot_x2 ) ) * sin( rot_y2 ) ) * cos( rot_z2 ) )
- ( ( (ace_ptr->yd2) * cos( rot_x2 ) - (ace_ptr->zd2) * sin( rot_x2 ) ) * sin( rot_z2 ) ) );
T linedir_y1 = ( ( ( (ace_ptr->xd1) * cos( rot_y1 ) + ( (ace_ptr->yd1) * sin( rot_x1 ) + (ace_ptr->zd1) * cos( rot_x1 ) ) * sin( rot_y1 ) ) * sin( rot_z1 ) )
+ ( ( (ace_ptr->yd1) * cos( rot_x1 ) - (ace_ptr->zd1) * sin( rot_x1 ) ) * cos( rot_z1 ) ) );
T linedir_y2 = ( ( ( (ace_ptr->xd2) * cos( rot_y2 ) + ( (ace_ptr->yd2) * sin( rot_x2 ) + (ace_ptr->zd2) * cos( rot_x2 ) ) * sin( rot_y2 ) ) * sin( rot_z2 ) )
+ ( ( (ace_ptr->yd2) * cos( rot_x2 ) - (ace_ptr->zd2) * sin( rot_x2 ) ) * cos( rot_z2 ) ) );
T linedir_z1 = ( ( -1*(ace_ptr->xd1)*sin( rot_y1 ) + ( (ace_ptr->yd1) * sin( rot_x1 ) + (ace_ptr->zd1) * cos( rot_x1 ) ) )*cos( rot_y1 ) );
T linedir_z2 = ( ( -1*(ace_ptr->xd2)*sin( rot_y2 ) + ( (ace_ptr->yd2) * sin( rot_x2 ) + (ace_ptr->zd2) * cos( rot_x2 ) ) )*cos( rot_y2 ) )
//Cross product of lines is zero for parallel lines
residual[0] = linedir_y1*linedir_z2 - linedir_z1*linedir_y2;
residual[1] = linedir_x1*linedir_z2 - linedir_z1*linedir_x2;
residual[2] = linedir_x1*linedir_y2 - linedir_y1*linedir_x2;
return true;