// 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;
}
// note that for theta=0 the force is set to 0, while the energy as function of theta has a cusp
bool _computeForceRaw(Real3D& force12,
Real3D& force32,
const Real3D& dist12,
const Real3D& dist32) const {
const real SMALL_EPSILON = 1.0E-9;
real dist12_sqr = dist12 * dist12;
real dist32_sqr = dist32 * dist32;
real dist12_magn = sqrt(dist12_sqr);
real dist32_magn = sqrt(dist32_sqr);
real a11, a12, a22;
// theta is the angle between r_{12} and r_{32}
real cos_theta = dist12 * dist32 / (dist12_magn * dist32_magn);
if(cos_theta < -1.0) cos_theta = -1.0;
if(cos_theta > 1.0) cos_theta = 1.0;
real sin_theta = sqrt(1.0 - cos_theta*cos_theta);
a11 = cos_theta / dist12_sqr;
a12 = -1.0 / (dist12_magn * dist32_magn);
a22 = cos_theta / dist32_sqr;
if (sin_theta > SMALL_EPSILON) { //sin_theta is always positive
force12 = (Kcos_theta0-Ksin_theta0*cos_theta/sin_theta)*(a11 * dist12 + a12 * dist32);
force32 = (Kcos_theta0-Ksin_theta0*cos_theta/sin_theta)*(a22 * dist32 + a12 * dist12);}
else {
force12 = Real3D(0.0,0.0,0.0);
force32 = Real3D(0.0,0.0,0.0);
}
return true;
}
