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