void quatMult(Quaternion *q1, Quaternion *q2, Quaternion *result) {
Quaternion a, b;
if(q1 == NULL || q2 == NULL || result == NULL) { return; }
quatCopy(&a, q1); // Make copies so result can be operand
quatCopy(&b, q2);
result->w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
result->x = a.w*b.x + b.w*a.x + a.y*b.z - a.z*b.y;
result->y = a.w*b.y + b.w*a.y - a.x*b.z + a.z*b.x;
result->z = a.w*b.z + b.w*a.z + a.x*b.y - a.y*b.x;
}
//*****************************************************************************
//
// * The distance between two quaternion
//============================================================================
double SJCQuaterniond::
distance(SJCQuaterniond& quat )
//============================================================================
{
SJCQuaterniond quatCopy( quat );
double dotProd =
w_ * quat.w() + x_ * quat.x() + y_ * quat.y() + z_ * quat.z();
// If the antipode is closer, use it instead.
if( dotProd < 0 ) {
quatCopy.set(quatCopy.w() * -1, quatCopy.x() * -1,
quatCopy.y() * -1, quatCopy.z() * -1);
}
// Represent this Quaternion as an offset from the given Quaternion.
quatCopy.inverse();
quatCopy *= *this;
double angle = SJCACos(quatCopy.w()) * 2;
if( angle > M_PI ) {
angle -= M_2PI;
}
return( fabs(angle) );
}
// qtarget = qcurr*qdisp
// qcurr'*qtarget = qdisp
void slewProcess(Quaternion *input, Quaternion *output) {
Quaternion conjugate, displacement;
bams32_t input_angle, limited_angle;
float sin_input, sin_limited, scale;
if(!is_ready || !is_running || max_angular_displacement == 0) {
quatCopy(output, input);
return;
}
// Calculate displacement
quatConj(&prev_ref, &conjugate);
quatMult(&conjugate, input, &displacement);
// Calculate displacement magnitude
input_angle = bams16ToBams32(bams16Acos(displacement.w)*2);
if(input_angle == 0) { // Check for no displacement case
quatCopy(output, input);
return;
}
sin_input = bams32Sin(input_angle/2);
// Apply displacement limits
if(input_angle > max_angular_displacement) {
limited_angle = max_angular_displacement;
} else if(input_angle < -max_angular_displacement) {
limited_angle = -max_angular_displacement;
} else {
limited_angle = input_angle;
}
sin_limited = bams32SinFine(limited_angle/2);
scale = sin_limited/sin_input;
displacement.w = bams32CosFine(limited_angle/2);
displacement.x = displacement.x*scale;
displacement.y = displacement.y*scale;
displacement.z = displacement.z*scale;
// Apply limited displacement
quatMult(&prev_ref, &displacement, output);
quatNormalize(output);
quatCopy(&prev_ref, output);
}
