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