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]; }