// RotateToMap returns a rotation map which rotates fromVec to have the // same direction as toVec. // fromVec and toVec should be unit vectors RotationMapR3 RotateToMap(const VectorR3& fromVec, const VectorR3& toVec) { VectorR3 crossVec = fromVec; crossVec *= toVec; double sintheta = crossVec.Norm(); double costheta = fromVec ^ toVec; if (sintheta <= 1.0e-40) { if (costheta > 0.0) { return (RotationMapR3( 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)); } else { GetOrtho(toVec, crossVec); // Get arbitrary orthonormal vector return (VrRotate(costheta, sintheta, crossVec)); } } else { crossVec /= sintheta; // Normalize the vector return (VrRotate(costheta, sintheta, crossVec)); } }
RotationMapR3 operator*(const RotationMapR3& A, const RotationMapR3& B) // Matrix product (composition) { return (RotationMapR3(A.m11 * B.m11 + A.m12 * B.m21 + A.m13 * B.m31, A.m21 * B.m11 + A.m22 * B.m21 + A.m23 * B.m31, A.m31 * B.m11 + A.m32 * B.m21 + A.m33 * B.m31, A.m11 * B.m12 + A.m12 * B.m22 + A.m13 * B.m32, A.m21 * B.m12 + A.m22 * B.m22 + A.m23 * B.m32, A.m31 * B.m12 + A.m32 * B.m22 + A.m33 * B.m32, A.m11 * B.m13 + A.m12 * B.m23 + A.m13 * B.m33, A.m21 * B.m13 + A.m22 * B.m23 + A.m23 * B.m33, A.m31 * B.m13 + A.m32 * B.m23 + A.m33 * B.m33)); }
RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec) { VectorR3 crossVec = fromVec; crossVec *= toVec; double sinetheta = crossVec.Norm(); // Not yet normalized! if ( sinetheta < 1.0e-40 ) { return ( RotationMapR3( 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) ); } crossVec /= sinetheta; // Normalize the vector double scale = 1.0/sqrt(fromVec.NormSq()*toVec.NormSq()); sinetheta *= scale; double cosinetheta = (fromVec^toVec)*scale; return ( VrRotate( cosinetheta, sinetheta, crossVec) ); }