[Helix][perf] Rewrite rotatePoint

diff --git a/ShaheenGandhi.pde b/ShaheenGandhi.pde
index ff6ce2edcf82d4ce840eb105eaff462a5bda97a2..a11d9caffbcbf5b4e81710610dbd7d1a56b449cf 100644 (file)
--- a/ShaheenGandhi.pde
+++ b/ShaheenGandhi.pde
@@ -40,14 +40,14 @@ class HelixPattern extends SCPattern {
       return getPointAt(getTValue(pt));
     }
 
-    PVector rotatePoint(final PVector pt, final float rads) {
-      Vec3D axisVec3D = new Vec3D(vector.x, vector.y, vector.z);
-      Vec3D originVec3D = new Vec3D(origin.x, origin.y, origin.z);
-      Matrix4x4 mat = new Matrix4x4().identity()
-        .rotateAroundAxis(axisVec3D, rads);
-      Vec3D ptVec3D = new Vec3D(pt.x, pt.y, pt.z).sub(originVec3D);
-      Vec3D rotatedPt = mat.applyTo(ptVec3D).add(originVec3D);
-      return new PVector(rotatedPt.x, rotatedPt.y, rotatedPt.z);
+    PVector rotatePoint(final PVector p, final float t) {
+      final PVector o = origin;
+      final PVector v = vector;
+      
+      float x = (o.x*(v.y*v.y + v.z*v.z) - v.x*(o.y*v.y + o.z*v.z - v.x*p.x - v.y*p.y - v.z*p.z))*(1 - cos(t)) + p.x*cos(t) + (-o.z*v.y + o.y*v.z - v.z*p.y + v.y*p.z)*sin(t);
+      float y = (o.y*(v.x*v.x + v.z*v.z) - v.y*(o.x*v.x + o.z*v.z - v.x*p.x - v.y*p.y - v.z*p.z))*(1 - cos(t)) + p.y*cos(t) + (o.z*v.x - o.x*v.z + v.z*p.x - v.x*p.z)*sin(t);
+      float z = (o.z*(v.x*v.x + v.y*v.y) - v.z*(o.x*v.x + o.y*v.y - v.x*p.x - v.y*p.y - v.z*p.z))*(1 - cos(t)) + p.z*cos(t) + (-o.y*v.x + o.x*v.y - v.y*p.x + v.x*p.y)*sin(t);
+      return new PVector(x, y, z);
     }
   }