void update (quat& q_cur, quat& q_new, bool interpolate)
{
if (interpolate) {
// DO INTERPOLATION
quat q_diff = q_cur.inverse() * q_new; // q_cur * q_diff = q_new
q_diff.normalize();
const float limit_angle = 20.f;
const float threshold = limit_angle*.5f*ZZ_TO_RAD; // threshold = (limit_angle/2)
const float adjust_angle = 10.0f;
const float adjust_threshold = adjust_angle*.5f*ZZ_TO_RAD;
// we assume that q.w = cos(theta/2).
// CAUTION: theta may be [-pi, pi], but acosf assumes that theta is [0, 2pi]
float theta_d2 = acosf(q_diff.w); // theta >= 0
// if got the big angle(> 180 degree)
if (theta_d2 > threshold) {
// construct vector componet
vec3 v(q_diff.x, q_diff.y, q_diff.z);
if (theta_d2 > ZZ_PI*.5f) { // do opposite direction, if exceeds pi/2
v = -v; // invert direction
}
axis_to_quat(q_diff, v, adjust_threshold);
q_cur = q_cur * q_diff;
q_cur.normalize();
}
else {
q_cur = q_new;
}
}
else {
q_cur = q_new;
}
}
quat rotate_quat( quat const& v, quat const& q )
{
return q * v * q.inverse();
}
vector3 rotate_vector( vector3 const& v, quat const& q )
{
quat p = quat(v.x(), v.y(), v.z(), 0);
quat tp = q * p * q.inverse();
return vector3(tp.qx(), tp.qy(), tp.qz());
}