public Triple Rotate(Triple origin, Triple Axis, float angle)
{
Triple z = Axis.Normalise();
Triple x = z.GeneratePerpendicular().Normalise();
Triple y = z.Cross(x).Normalise();
Triple v = this - origin;
float vx = x.Dot(v);
float vy = y.Dot(v);
float vz = z.Dot(v);
float sin = (float)Math.Sin(angle);
float cos = (float)Math.Cos(angle);
float vx_ = cos * vx - sin * vy;
float vy_ = sin * vx + cos * vy;
return(origin + x * vx_ + y * vy_ + z * vz);
}
