public Matrix4X4 Rotate(float angle, Vector3 axis)
{
float x = axis.Elements[0], y = axis.Elements[1], z = axis.Elements[2];
float len = Math.Sqrt(x*x + y*y + z*z);
if (len != 1) {
len = 1 / len;
x *= len;
y *= len;
z *= len;
}
float s = Math.Sin(angle);
float c = Math.Cos(angle);
float t = 1-c;
// Cache the matrix values (makes for huge speed increases!)
float a00 = Elements[0], a01 = Elements[1], a02 = Elements[2], a03 = Elements[3];
float a10 = Elements[4], a11 = Elements[5], a12 = Elements[6], a13 = Elements[7];
float a20 = Elements[8], a21 = Elements[9], a22 = Elements[10], a23 = Elements[11];
// Construct the elements of the rotation matrix
float b00 = x*x*t + c, b01 = y*x*t + z*s, b02 = z*x*t - y*s;
float b10 = x*y*t - z*s, b11 = y*y*t + c, b12 = z*y*t + x*s;
float b20 = x*z*t + y*s, b21 = y*z*t - x*s, b22 = z*z*t + c;
// Perform rotation-specific matrix multiplication
Elements[0] = a00*b00 + a10*b01 + a20*b02;
Elements[1] = a01 * b00 + a11 * b01 + a21 * b02;
Elements[2] = a02 * b00 + a12 * b01 + a22 * b02;
Elements[3] = a03 * b00 + a13 * b01 + a23 * b02;
Elements[4] = a00 * b10 + a10 * b11 + a20 * b12;
Elements[5] = a01 * b10 + a11 * b11 + a21 * b12;
Elements[6] = a02 * b10 + a12 * b11 + a22 * b12;
Elements[7] = a03 * b10 + a13 * b11 + a23 * b12;
Elements[8] = a00 * b20 + a10 * b21 + a20 * b22;
Elements[9] = a01 * b20 + a11 * b21 + a21 * b22;
Elements[10] = a02 * b20 + a12 * b21 + a22 * b22;
Elements[11] = a03 * b20 + a13 * b21 + a23 * b22;
return this;
}
