v2 WORKING ROTATIONS

2018-11-10 23:23:03 +01:00
parent b41db01a55
commit cbe7b00b3a
7 changed files with 110 additions and 44 deletions
@@ -127,36 +127,41 @@ public class Foot : Component
        Y += translateY;
        Z += translateZ;

-        Y = Y * (invert ? -1 : 1);
-        X = X * (invert ? -1 : -1);
-        Z = Z * (invert ? -1 : -1);
-
        /*RotateX();
        RotateY();
        RotateZ();*/
-
-        rotationY = rotationY * (invert ? 1 : 1);
-
+        
        // Z and X is working. Y has problems when combined with others.
-        Vector3 v = new Vector3((float)Z, (float)Y, (float)X);
-        v = Quaternion.Euler((float)rotationX, (float)rotationY, (float)rotationZ) * v;
-        Z = v.x;
+        Vector3 v = new Vector3((float)X, (float)Y, (float)Z);
+        v = Quaternion.Euler((float)0, (float)rotationY, (float)0) * v;
+        v = Quaternion.Euler((float)rotationX, (float)0, (float)0) * v;
+        v = Quaternion.Euler((float)0, (float)0, (float)rotationZ) * v;
+        X = v.x;
        Y = v.y;
-        X = v.z;
-
-        Y = Y * (invert ? -1 : 1);
-        X = X * (invert ? -1 : -1);
-        Z = Z * (invert ? -1 : -1);
-
+        Z = v.z;
+        
        X -= translateX;
        Y -= translateY;
        Z -= translateZ;

+        // translate to my stupid formula coordinates.
+        double tx = X;
+        double ty = Y;
+        double tz = Z;
+        X = tz * (invert ? -1 : 1);
+        Y = ty * -1;
+        Z = tx * -1;
+
+        // run IK
        O1 = (90 - FO1());
        O2 = T1();
        O3 = 180 - T3();

-        Y = Y * -1;
-        pos = new Vector3((float)Z, (float)Y, (float)X);
+        // translate to my stupid formula coordinates.
+        X = tx;
+        Y = ty;
+        Z = tz;
+
+        pos = new Vector3((float)X * (invert ? -1 : 1), (float)Y, (float)Z * (invert ? -1 : 1));
    }
 }