v2 WORKING ROTATIONS

2018-unity-sim/Assets/Foot.cs

@@ -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);
+        Vector3 v = new Vector3((float)X, (float)Y, (float)Z);
-        v = Quaternion.Euler((float)rotationX, (float)rotationY, (float)rotationZ) * v;
+        v = Quaternion.Euler((float)0, (float)rotationY, (float)0) * v;
-        Z = v.x;
+        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;
+        Z = v.z;
-
+        
-        Y = Y * (invert ? -1 : 1);
-        X = X * (invert ? -1 : -1);
-        Z = Z * (invert ? -1 : -1);
-
         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;
+        // translate to my stupid formula coordinates.
-        pos = new Vector3((float)Z, (float)Y, (float)X);
+        X = tx;
+        Y = ty;
+        Z = tz;
+
+        pos = new Vector3((float)X * (invert ? -1 : 1), (float)Y, (float)Z * (invert ? -1 : 1));
     }
 }

2018-unity-sim/Assets/Leg.cs

@@ -34,13 +34,12 @@ public class Leg : MonoBehaviour {
     void Update()
     {
         foot.X = point.x;
-        foot.Y = point.y * -1;
+        foot.Y = point.y;
-        //foot.Z = point.z * (invert ? -1 : 1);
+        foot.Z = point.z;
-        foot.Z = point.z * (invert ? 1 : -1);
 
         foot.rotationX = rotation.x;
-        foot.rotationY = rotation.y * (invert ? -1 : 1);
+        foot.rotationY = rotation.y;
-        foot.rotationZ = rotation.z * (invert ? -1 : 1);
+        foot.rotationZ = rotation.z;
 
         foot.translateX = translateX;
         foot.translateY = translateY;
@@ -67,8 +66,6 @@ public class Leg : MonoBehaviour {
         Hip2.transform.localRotation = Quaternion.Euler((float)a2, 0, 0);
         Knie.transform.localRotation = Quaternion.Euler((float)a3, 0, 0);
 
-        pos = foot.pos;
+        pointShouldBe.transform.localPosition = foot.pos;
-        pos.x = pos.x * (invert ? 1 : -1);
-        pointShouldBe.transform.localPosition = pos;
     }
 }

2018-unity-sim/Assets/RotationControl.cs

@@ -25,34 +25,34 @@ public class RotationControl : MonoBehaviour
     // Update is called once per frame
     void Update()
     {
-        /*if (!dir2 && rotation.x < max)
+        if (!dir2 && rotation.x < max)
             rotation.x += speed2;
         else if (!dir2)
             dir2 = !dir2;
         else if (dir2 && rotation.x > min)
             rotation.x -= speed2;
         else if (dir2)
-            dir2 = !dir2;*/
+            dir2 = !dir2;
 
-        /*if (!dir3 && rotation.y < max)
+        if (!dir3 && rotation.y < max)
             rotation.y += speed3;
         else if (!dir3)
             dir3 = !dir3;
         else if (dir3 && rotation.y > min)
             rotation.y -= speed3;
         else if (dir3)
-            dir3 = !dir3;*/
+            dir3 = !dir3;
 
-        /*if (!dir && rotation.z < max)
+        if (!dir && rotation.z < max)
             rotation.z += speed;
         else if (!dir)
             dir = !dir;
         else if (dir && rotation.z > min)
             rotation.z -= speed;
         else if (dir)
-            dir = !dir;*/
+            dir = !dir;
 
-        body.transform.eulerAngles = new Vector3(rotation.x*-1, rotation.y, rotation.z);
+        body.transform.eulerAngles = new Vector3(rotation.x*-1, rotation.y*-1, rotation.z*-1);
 
         for (int index = 0; index < rotation2.Count; ++index)
         {