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; }