/**
* Rotate about an arbitrary axis in space, defined by 3-vector c
*
* <p>The procedure is as follows: 1. Make appropriate rotations to make the line coincide with
* one of the axes (x-axis). 2. Rotate the object about x-axis by required angle 3. Apply the
* inverse of step 2 4. Apply the inverse of step 1
*/
public static BasicMatrix axisRotationMatrix(double theta, double cX, double cY, double cZ) {
// Rotate along the y-axis so that the xy-plane
// gets aligned with the specified axis.
double tau1 = Math.atan2(cX, cZ);
BasicMatrix alpha =
new BasicMatrix(
new double[][] {
{Math.cos(tau1), 0, Math.sin(tau1), 0},
{0, 1, 0, 0},
{-Math.sin(tau1), 0, Math.cos(tau1), 0},
{0, 0, 0, 1}
});
// cZ should is now positioned at the origin.
// Rotate along the z-axis until the x-axis coincides
// with the specified x-axis.
double tau2 = Math.atan2(cY, Math.sqrt(cX * cX + cZ * cZ));
BasicMatrix beta =
new BasicMatrix(
new double[][] {
{Math.cos(tau2), -Math.sin(tau2), 0, 0},
{Math.sin(tau2), Math.cos(tau2), 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
});
// Both cZ and CY should now be positioned at the origin.
// Note that the second coordinate of the arctangent function
// equals the length of the projection of the axis on the zy-plane.
// Rotate along the x-axis
BasicMatrix gamma =
new BasicMatrix(
new double[][] {
{1, 0, 0, 0},
{0, Math.cos(theta), -Math.sin(theta), 0},
{0, Math.sin(theta), Math.cos(theta), 0},
{0, 0, 0, 1}
});
// Inverse forms of rotation matrices alpha and beta
BasicMatrix betaInverse =
new BasicMatrix(
new double[][] {
{Math.cos(tau2), Math.sin(tau2), 0, 0},
{-Math.sin(tau2), Math.cos(tau2), 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
});
BasicMatrix alphaInverse =
new BasicMatrix(
new double[][] {
{Math.cos(tau1), 0, -Math.sin(tau1), 0},
{0, 1, 0, 0},
{Math.sin(tau1), 0, Math.cos(tau1), 0},
{0, 0, 0, 1}
});
return alphaInverse.multiply(betaInverse).multiply(gamma).multiply(beta).multiply(alpha);
}
public Basic3DMatrix rotateOverAxis(
double theta, double cX, double cY, double cZ, double mX, double mY, double mZ) {
BasicMatrix A = this.translate(new double[] {mX, mY, mZ}).homogenous();
BasicMatrix B = axisRotationMatrix(theta, cX, cY, cZ);
BasicMatrix C = B.multiply(A);
BasicMatrix D = C.crop(M, N).translate(new double[] {-mX, -mY, -mZ});
return new Basic3DMatrix(D);
}
public Basic3DMatrix rotateOverZ(double theta) {
BasicMatrix A = this.homogenous();
BasicMatrix B = zRotationMatrix(theta);
BasicMatrix C = B.multiply(A);
return new Basic3DMatrix(C.crop(M, N));
}
public Basic3DMatrix rotateOverAxis(double theta, double cX, double cY, double cZ) {
BasicMatrix A = this.homogenous();
BasicMatrix B = axisRotationMatrix(theta, cX, cY, cZ);
BasicMatrix C = B.multiply(A);
return new Basic3DMatrix(C.crop(M, N));
}
public Basic3DMatrix(BasicMatrix matrix) {
super(matrix.getData());
if (rows() != 3) {
throw new RuntimeException("Illegal Matrix Dimensions.");
}
}
