public static Mat4f RotationMatrix(Mat4f m, float angle, Vec3f v)
{
float a = angle;
float c = (float)Math.Cos(a);
float s = (float)Math.Sin(a);
Vec3f axis = v.unit();
Vec3f temp = new Vec3f((1.0f - c) * axis);
Mat4f Rotate = new Mat4f(0.0f);
Rotate.data[0][0] = c + temp[0] * axis[0];
Rotate.data[0][1] = temp[0] * axis[1] + s * axis[2];
Rotate.data[0][2] = temp[0] * axis[2] - s * axis[1];
Rotate.data[1][0] = temp[1] * axis[0] - s * axis[2];
Rotate.data[1][1] = c + temp[1] * axis[1];
Rotate.data[1][2] = temp[1] * axis[2] + s * axis[0];
Rotate.data[2][0] = temp[2] * axis[0] + s * axis[1];
Rotate.data[2][1] = temp[2] * axis[1] - s * axis[0];
Rotate.data[2][2] = c + temp[2] * axis[2];
Mat4f Result = new Mat4f(0.0f);
Result[0] = m[0] * Rotate.data[0][0] + m[1] * Rotate.data[0][1] + m[2] * Rotate.data[0][2];
Result[1] = m[0] * Rotate.data[1][0] + m[1] * Rotate.data[1][1] + m[2] * Rotate.data[1][2];
Result[2] = m[0] * Rotate.data[2][0] + m[1] * Rotate.data[2][1] + m[2] * Rotate.data[2][2];
Result[3] = m[3];
return(Result);
}
public void Rotate(float angle, Vec3f v)
{
float a = angle;
float c = (float)Math.Cos(a);
float s = (float)Math.Sin(a);
Vec3f axis = v.unit();
Vec3f temp = new Vec3f((1.0f - c) * axis);
Mat4f Rotate = new Mat4f(0.0f);
Rotate.data[0][0] = c + temp[0] * axis[0];
Rotate.data[0][1] = temp[0] * axis[1] + s * axis[2];
Rotate.data[0][2] = temp[0] * axis[2] - s * axis[1];
Rotate.data[1][0] = temp[1] * axis[0] - s * axis[2];
Rotate.data[1][1] = c + temp[1] * axis[1];
Rotate.data[1][2] = temp[1] * axis[2] + s * axis[0];
Rotate.data[2][0] = temp[2] * axis[0] + s * axis[1];
Rotate.data[2][1] = temp[2] * axis[1] - s * axis[0];
Rotate.data[2][2] = c + temp[2] * axis[2];
data[0] = data[0] * Rotate.data[0][0] + data[1] * Rotate.data[0][1] + data[2] * Rotate.data[0][2];
data[1] = data[0] * Rotate.data[1][0] + data[1] * Rotate.data[1][1] + data[2] * Rotate.data[1][2];
data[2] = data[0] * Rotate.data[2][0] + data[1] * Rotate.data[2][1] + data[2] * Rotate.data[2][2];
}
