1. 向量v绕一轴旋转一角度（用四元数表示q）
v’ = q*v*q’
2.四元数分解到另一轴上
/**
Assume that the current quaternion represents the relative orientation between two coordinate frames A and B.
This method decomposes the current relative rotation into a twist of frame B around the axis v passed in as a
parameter, and another more arbitrary rotation.
AqB = AqT * TqB, where T is a frame that is obtained by rotating frame B around the axis v by the angle
that is returned by this function.
In the T coordinate frame, v is the same as in B, and AqT is a rotation that aligns v from A to that
from T.
It is assumed that vB is a unit vector!! This method returns TqB, which represents a twist about
the axis vB.
*/
Quaternion Quaternion::decomposeRotation(const Vector3d vB) const{
//we need to compute v in A's coordinates
Vector3d vA = this->rotate(vB);
vA.toUnit();
double temp = 0;
//compute the rotation that alligns the vector v in the two coordinate frames (A and T)
Vector3d rotAxis = vA.crossProductWith(vB);
rotAxis.toUnit();
double rotAngle = -safeACOS(vA.dotProductWith(vB));
Quaternion TqA = Quaternion::getRotationQuaternion(rotAngle, rotAxis*(-1));
return TqA * (*this);
}