public void Rotate(float angle, Vec3f v)
{
var a = angle;
var c = (float)Math.Cos(a);
var s = (float)Math.Sin(a);
Vec3f axis = v.CreateNormalizedVector();
Vec3f temp = (1 - c) * axis;
Mat4f Rotate = new Mat4f();
Rotate[0][0] = c + temp[0] * axis[0];
Rotate[0][1] = temp[0] * axis[1] + s * axis[2];
Rotate[0][2] = temp[0] * axis[2] - s * axis[1];
Rotate[1][0] = temp[1] * axis[0] - s * axis[2];
Rotate[1][1] = c + temp[1] * axis[1];
Rotate[1][2] = temp[1] * axis[2] + s * axis[0];
Rotate[2][0] = temp[2] * axis[0] + s * axis[1];
Rotate[2][1] = temp[2] * axis[1] - s * axis[0];
Rotate[2][2] = c + temp[2] * axis[2];
Mat4f Result = new Mat4f();
Result[0] = this[0] * Rotate[0][0] + this[1] * Rotate[0][1] + this[2] * Rotate[0][2];
Result[1] = this[0] * Rotate[1][0] + this[1] * Rotate[1][1] + this[2] * Rotate[1][2];
Result[2] = this[0] * Rotate[2][0] + this[1] * Rotate[2][1] + this[2] * Rotate[2][2];
Result[3] = this[3];
for (int i = 0; i < Size; i++)
{
for (int j = 0; j < Size; j++)
{
Result[i, j] = (float)Result[i, j];
}
}
Set(Result);
}
