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