// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxis(const Real3D& dir)
{
// Normalize the target direction
// If it is degenerate, returns the identity matrix
Real3D target = dir;
REAL len = target.mod();
if (len == 0) return Matrix3D(1);
target /= len;
if (!target.isfinite()) return Matrix3D(1);
// Find a non-degenerate vector orthogonal to the target
Real3D v1 = Real3D(1,0,0) ^ target;
REAL l1 = v1.sqrmod();
Real3D v2 = Real3D(0,1,0) ^ target;
REAL l2 = v2.sqrmod();
Real3D v3 = Real3D(0,0,1) ^ target;
REAL l3 = v3.sqrmod();
// Choose the largest among v1,v2,v3
Real3D v;
if (l1 >= l2 && l1 >= l3) v = v1.vers();
else if (l2 >= l1 && l2 >= l3) v = v2.vers();
else /* if (l3 >= l1 && l3 >= l2) */ v = v3.vers();
// Build the rotation matrix
Real3D w = (target^v).vers();
Matrix3D rot;
rot.setCols(v, w, target);
// Debug Check
assert(fabs(rot.det()-1) < Math::TOLERANCE);
return rot;
}
/// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxisForDXF(const Real3D& dir)
{
// Normalize the target direction
// If it is degenerate, returns the identity matrix
Real3D target = dir;
REAL len = target.mod();
if (len == 0) return Matrix3D(1);
target /= len;
if (!target.isfinite()) return Matrix3D(1);
// Choice of xAxis: see AutoCAD documentation.
Real3D v;
if( fabs(target[0])*64 < 1 && fabs(target[1])*64 < 1 )
v = Real3D(0,1,0) ^ target;
else
v = Real3D(0,0,1) ^ target;
v = v.vers();
Real3D w = (target^v).vers();
Matrix3D rot;
rot.setCols(v, w, target);
// Debug Check
assert(fabs(rot.det()-1) < Math::TOLERANCE);
return rot;
}