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