const Quaternion QuaternionManipulator::qSlerp(const Quaternion& q0, const Quaternion& q1, float k) { float angle = acos(q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t); float k0 = sin((1-k)*angle) / sin(angle); float k1 = sin(k*angle) / sin(angle); return qAdd(qMultiply(q0,k0), qMultiply(q1,k1)); }
const Quaternion QuaternionManipulator::qLerp(const Quaternion& q0, const Quaternion& q1, float k) { float cos_angle = q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t; float k0 = 1.0f - k; float k1 = (cos_angle > 0) ? k : -k; return qAdd(qMultiply(q0,k0), qMultiply(q1,k1)); }
Quaternions Quaternions::qSlerp(const Quaternions& q0, const Quaternions& q1, float k) { float angle = acos(q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t); float k0 = sin((1 - k)*angle) / sin(angle); float k1 = sin(k*angle) / sin(angle); Quaternions qi = qAdd(qMultiply(q0, k0), qMultiply(q1, k1)); return qNormalize(qi); }
Quaternions Quaternions::qLerp(const Quaternions& q0, const Quaternions& q1, float k) { float cos_angle = q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t; float k0 = 1.0f - k; float k1 = (cos_angle > 0) ? k : -k; Quaternions qi = qAdd(qMultiply(q0, k0), qMultiply(q1, k1)); return qNormalize(qi); }
/*----------------------------------------------------------------------* * Add/Remove * *----------------------------------------------------------------------*/ int setAdd(SET set,void *data) { /* An ordered set uses the avl tree routines. */ if (SET_Ord(set)) return avlInsert(set->telts,data,data); /* A non-ordered set uses the disjoint flag to check if it needs to * exclude double elements */ if (SET_IsSET(set) && setContains(set,data)) return -1; /* Else, or if BAG, add the element to the queue */ return - !qAdd(set->qelts,data); }