Vector3Extensions: Angle and Rotate

Vector3Extensions.cs

public static class Vector3Extensions {

   public static double Angle(this Vector3 a, Vector3 b) {
      var norm = 1.0f / (Normalize(a) * Normalize(b));
      var dot = Vector3.Dot(a, b) * norm;

      if (dot > 1.0f)
         return 0.0f;
      else if (dot < -1.0f)
         return Math.PI;

      return Math.Acos(dot);
   }

   public static Vector3 Rotate(this Vector3 v, double angle, Vector3 axis) {
      double c = Math.Cos(angle); //cosine a float.
      double s = Math.Sin(angle); 
      
      var k = axis.NormalizeTo(); 

      /* Right Hand, Rodrigues' rotation formula:
      v = v*cos(t) + (kxv)sin(t) + k*(k.v)(1 - cos(t))
      */
      
      var v1 = v.Scale((float)c);

      var v2 = Vector3.Cross(k, v);
      v2 = v2.Scale(s);

      v1 = Vector3.Add(v1, v2);

      v2 = k.Scale(Vector3.Dot(k, v) * (1.0f - c));

      return Vector3.Add(v1, v2); 
   }

   public static Vector3 Scale(this Vector3 vector, double scale) {
      return vector * (float)scale;
   }

   public static Vector3 NormalizeTo(this Vector3 vector) {
      var norm = Normalize(vector);
      
      if(norm < Double.Epsilon) {
         return Vector3.Zero;
      }

      return Scale(vector, 1.0f / norm);
   }

   public static double Normalize(this Vector3 vector) {
      return Math.Sqrt(Vector3.Dot(vector, vector));
   }
   
}