void quatRotate(Quaternion *q, Quaternion *v, Quaternion *result) {
if(q == NULL || v == NULL || result == NULL) { return; }
Quaternion temp;
quatConj(q, &temp);
quatMult(q, v, result);
quatMult(result, &temp, result);
}
// 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);
}
F2D* quatRot(F2D* vec, F2D* rQuat)
{
F2D *ret;
int nr, i, j, k, rows, cols;
F2D *tv, *vQuat, *temp, *temp1;
F2D *retVec;
nr = vec->height;
tv = fSetArray(nr, 1, 0);
vQuat = fHorzcat(tv, vec);
temp = quatMul(rQuat, vQuat);
temp1 = quatConj(rQuat);
retVec = quatMul(temp, temp1);
rows = retVec->height;
cols = retVec->width;
ret = fSetArray(rows, 3, 0);
for(i=0; i<rows; i++)
{
k = 0;
for(j=1; j<4; j++)
{
subsref(ret,i,k) = subsref(retVec,i,j);
k++;
}
}
fFreeHandle(tv);
fFreeHandle(vQuat);
fFreeHandle(temp);
fFreeHandle(temp1);
fFreeHandle(retVec);
return ret;
}
