void assign(Quaternion<T>& quat, const Radian& roll, const Radian& pitch, const Radian& yaw) { double angle = roll.v() * 0.5; double sr = std::sin(angle); double cr = std::cos(angle); angle = pitch.v() * 0.5; double sp = std::sin(angle); double cp = std::cos(angle); angle = yaw.v() * 0.5; double sy = std::sin(angle); double cy = std::cos(angle); double cpcy = cp * cy; double spcy = sp * cy; double cpsy = cp * sy; double spsy = sp * sy; quat.setW(cr * cpcy + sr * spsy); quat.setX(sr * cpcy - cr * spsy); quat.setY(cr * spcy + sr * cpsy); quat.setZ(cr * cpsy - sr * spcy); normalise(quat); }
void assign(Quaternion<T>& quat, const Vector3f& axis, const Radian& angle) { double sinA = std::sin(angle.v()); double cosA = std::cos(angle.v()); quat.setW(cosA); quat.setX(axis.x() * sinA); quat.setY(axis.y() * sinA); quat.setZ(axis.z() * sinA); }