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