public void rotate(double angle, double x, double y, double z) {
if (angle == 0.0f) return;
Matrix4x4 m = new Matrix4x4(1); // The "1" says to not load the identity.
double c, s, ic;
if (angle == 90.0 || angle == -270.0) {
s = 1.0;
c = 0.0;
} else if (angle == -90.0 || angle == 270.0) {
s = -1.0;
c = 0.0;
} else if (angle == 180.0 || angle == -180.0) {
s = 0.0;
c = -1.0;
} else {
double a = angle * Mathematics.M_PI / 180.0;
c = Math.cos(a);
s = Math.sin(a);
}
boolean quick = false;
if (x == 0.0) {
if (y == 0.0) {
if (z != 0.0) {
// Rotate around the Z axis.
m.set_to_identity();
m.mat4[0] = c;
m.mat4[5] = c;
if (z < 0.0) {
m.mat4[4] = s;
m.mat4[1] = -s;
} else {
m.mat4[4] = -s;
m.mat4[1] = s;
}
m.setType(Mathematics.mat_type_General);
quick = true;
}
} else if (z == 0.0) {
// Rotate around the Y axis.
m.set_to_identity();
m.mat4[0] = c;
m.mat4[10] = c;
if (y < 0.0) {
m.mat4[8] = -s;
m.mat4[2] = s;
} else {
m.mat4[8] = s;
m.mat4[2] = -s;
}
m.setType(Mathematics.mat_type_General);
quick = true;
}
} else if (y == 0.0 && z == 0.0) {
// Rotate around the X axis.
m.set_to_identity();
m.mat4[5] = c;
m.mat4[10] = c;
if (x < 0.0) {
m.mat4[9] = s;
m.mat4[6] = -s;
} else {
m.mat4[9] = -s;
m.mat4[6] = s;
}
m.setType(Mathematics.mat_type_General);
quick = true;
}
if (!quick) {
double len = x * x + y * y + z * z;
if (!Mathematics.fuzzyIsNull(len - 1.0) && Mathematics.fuzzyIsNull(len)) {
len = Math.sqrt(len);
x /= len;
y /= len;
z /= len;
}
ic = 1.0 - c;
m.mat4[0] = x * x * ic + c;
m.mat4[4] = x * y * ic - z * s;
m.mat4[8] = x * z * ic + y * s;
m.mat4[12] = 0.0;
m.mat4[1] = y * x * ic + z * s;
m.mat4[5] = y * y * ic + c;
m.mat4[9] = y * z * ic - x * s;
m.mat4[13] = 0.0;
m.mat4[2] = x * z * ic - y * s;
m.mat4[6] = y * z * ic + x * s;
m.mat4[10] = z * z * ic + c;
m.mat4[14] = 0.0;
m.mat4[3] = 0.0;
m.mat4[7] = 0.0;
m.mat4[11] = 0.0;
m.mat4[15] = 1.0;
}
int flags = flagBits;
// cpp style
// *this *= m;
// java...
this.set_array(this.multiply(m).get_array());
if (flags != Mathematics.mat_type_Identity) flagBits = flags | Mathematics.mat_type_Rotation;
else flagBits = Mathematics.mat_type_Rotation;
}
