The AcGeMatrix3d::rotation method returns a new matrix with the specified rotation angle, but how can we extract the angle from an existing matrix? The following code does this.
#define TWOPI 6.28318530718
void getAngleAndAxis(
double& theta,
AcGeVector3d& axis,
const AcGeMatrix3d& m)
{
// Contract: The matrix `m' must NOT contain any scaling,
// shearing or mirroring effects (i.e., m.det() must be 1.0).
// See AcGeScale3d::removeScale( m ) to meet this contract
// requirement. If axis is passed in such that it is non-zero,
// then this method will attempt to orient the returned
// axis in the same direction as the passed in axis,
// i.e., the returned axis will be in the same half-space
// as the original axis. (Note, the passed in axis will
// be overwritten).
//
// Note that if the matrix has a mirror component,
// ( ie the determinant = -1 ), then its not possible to
// construct a rotation that performs the transformation.
// If you need convincing, then imagine how difficult it
// would be rotate your left shoe, to be identical to your
// right shoe ( its mirror image ).
double trace, s;
int i, j, k;
double quat[4];
assert(fabs(m.det() -1.0) <= AcGeContext::gTol.equalVector());
trace = m(0, 0) + m(1, 1) + m(2, 2);
if (trace > 0.0) {
s = sqrt(trace + 1.0);
quat[0] = s * 0.5;
s = 0.5 / s;
quat[1] = s * (m(2, 1) - m(1, 2));
quat[2] = s * (m(0, 2) - m(2, 0));
quat[3] = s * (m(1, 0) - m(0, 1));
} else {
i = 0;
if (m(1, 1) > m(0, 0))
i = 1;
if (m(2, 2) > m(i, i))
i = 2;
j = (i + 1)%3;
k = (i + 2)%3;
s = sqrt(m(i, i) - (m(j, j) + m(k, k)) + 1.0);
quat[i+1] = s * 0.5;
s = 0.5 / s;
quat[0] = s * (m(k, j) - m(j, k));
quat[j+1] = s * (m(j, i) + m(i, j));
quat[k+1] = s * (m(k, i) + m(i, k));
}
theta = 2.0 * acos(quat[0]);
AcGeVector3d tempAxis;
if (fabs(theta) >= AcGeContext::gTol.equalVector()) {
double factor = 1.0 / sin(theta * 0.5);
tempAxis.x = quat[1] * factor;
tempAxis.y = quat[2] * factor;
tempAxis.z = quat[3] * factor;
// Flip direction and adjust angle if need be.
//
if (tempAxis.dotProduct(axis) < 0) {
tempAxis *= -1;
theta = TWOPI - theta;
}
axis = tempAxis;
} else {
theta = 0.0;
}
}
