四元数几个操作

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