v3 working transformations and working knie offset :D

Assets/Foot.cs

@@ -25,6 +25,10 @@ public class Foot : Component
     public double rotationX=0, rotationY=0, rotationZ=0;
     public double O1, O2, O3;
 
+    public Vector3 transformation = new Vector3(0, 0, 0);
+
+    static float temp = 2;
+
     void RL3()
     {
         if ((X == 0) && (Z == 0))
@@ -34,6 +38,9 @@ public class Foot : Component
 
         if (X < 0)
             l3 = l3 * -1;
+
+        //todo
+        l3 -= temp;
     }
 
     double FO1()
@@ -55,7 +62,12 @@ public class Foot : Component
         double y2 = Y * Y;
         double z2 = Z * Z;
 
-        s = Math.Sqrt(x2 + y2 + z2);
+        double t = Math.Sqrt(x2 + z2);
+        t -= temp;
+        t = t * t;
+        //todo: X,Z remove distance temp.
+        s = Math.Sqrt(y2 + t);
+        //s = Math.Sqrt(x2 + y2 + z2);
 
         double t1 = 0;
         double s2 = s * s;
@@ -127,10 +139,14 @@ public class Foot : Component
         Y += translateY;
         Z += translateZ;
 
+        X -= transformation.x;
+        Y -= transformation.y;
+        Z -= transformation.z;
+
         /*RotateX();
         RotateY();
         RotateZ();*/
-        
+
         // the order of these rotation transformations is important!!!
         Vector3 v = new Vector3((float)X, (float)Y, (float)Z);
         v = Quaternion.Euler((float)0, (float)rotationY, (float)0) * v;
@@ -139,7 +155,7 @@ public class Foot : Component
         X = v.x;
         Y = v.y;
         Z = v.z;
-        
+
         X -= translateX;
         Y -= translateY;
         Z -= translateZ;
@@ -153,7 +169,11 @@ public class Foot : Component
         Z = tx * -1;
 
         // run IK
+        // add adjustment from knie
+
         O1 = (90 - FO1());
+        // remove adjustment from knie
+
         O2 = T1();
         O3 = 180 - T3();