Quaternion& Quaternion::operator*= (const Quaternion& rhs) { m_valid = m_valid && rhs.m_valid; m_age = m_age + rhs.m_age; checkNormalization(); CoordType old_w = m_w; m_w = m_w * rhs.m_w - Dot(m_vec, rhs.m_vec); m_vec = old_w * rhs.m_vec + rhs.m_w * m_vec - Cross(m_vec, rhs.m_vec); return *this; }
template<> RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion& q) { Vector<3> vec; vec.setValid(); m_valid = m_valid && q.isValid(); m_age += q.age(); // rotate both sides by q for(int vec_num = 0; vec_num < 3; ++vec_num) { for(int elem_num = 0; elem_num < 3; ++elem_num) vec[elem_num] = m_elem[vec_num][elem_num]; vec.rotate(q); for(int elem_num = 0; elem_num < 3; ++elem_num) m_elem[vec_num][elem_num] = vec[elem_num]; } checkNormalization(); return *this; }