add rotate-around-point to frame

EnvyKong · Dec 30, 2016 · 76bac6f · 76bac6f
1 parent dcc7dd2
commit 76bac6f
Showing 1 changed file with 11 additions and 0 deletions.
diff --git a/math/Frame3f.cs b/math/Frame3f.cs
@@ -115,6 +115,17 @@ public Frame3f Rotated(float fAngle, int nAxis)
             return this.Rotated(new Quaternionf(GetAxis(nAxis), fAngle));
         }
 
+        public void RotateAround(Vector3f point, Quaternionf q)
+        {
+            Vector3f dv = q * (origin - point);
+            rotation = q * rotation;
+            origin = point + dv;
+        }
+        public Frame3f RotatedAround(Vector3f point, Quaternionf q)
+        {
+            Vector3f dv = q * (this.origin - point);
+            return new Frame3f(point + dv, q * this.rotation);
+        }
 
         public void AlignAxis(int nAxis, Vector3f vTo)
         {