v4 transformation after rotation added.

2018-unity-sim/Assets/Foot.cs

@@ -26,8 +26,9 @@ public class Foot : Component
     public double O1, O2, O3;
 
     public Vector3 transformation = new Vector3(0, 0, 0);
+    public Vector3 transformation2 = new Vector3(0, 0, 0);
 
-    static float temp = 2;
+    static float knie_offset = 2;
 
     void RL3()
     {
@@ -40,7 +41,7 @@ public class Foot : Component
             l3 = l3 * -1;
 
         //todo
-        l3 -= temp;
+        l3 -= knie_offset;
     }
 
     double FO1()
@@ -63,7 +64,7 @@ public class Foot : Component
         double z2 = Z * Z;
 
         double t = Math.Sqrt(x2 + z2);
-        t -= temp;
+        t -= knie_offset;
         t = t * t;
         //todo: X,Z remove distance temp.
         s = Math.Sqrt(y2 + t);
@@ -156,6 +157,10 @@ public class Foot : Component
         Y = v.y;
         Z = v.z;
 
+        X -= transformation2.x;
+        Y -= transformation2.y;
+        Z -= transformation2.z;
+
         X -= translateX;
         Y -= translateY;
         Z -= translateZ;

2018-unity-sim/Assets/Leg.cs

@@ -15,6 +15,7 @@ public class Leg : MonoBehaviour {
     public Quaternion rotation = new Quaternion(0, 0, 0, 0);
 
     public Vector3 transformation = new Vector3(0, 0, 0);
+    public Vector3 transformation2 = new Vector3(0, 0, 0);
 
     public bool invert = false;
 
@@ -47,8 +48,12 @@ public class Leg : MonoBehaviour {
         foot.translateY = translateY;
         foot.translateZ = translateZ;
 
+        // transform before rotation.
         foot.transformation = transformation;
 
+        // transform after rotation.
+        foot.transformation2 = transformation2;
+
         foot.invert = invert;
 
         foot.setleg();

2018-unity-sim/Assets/RotationControl.cs

@@ -2,22 +2,72 @@
 using System.Collections.Generic;
 using UnityEngine;
 
-public class RotationControl : MonoBehaviour
+public class rotation
 {
-    public Quaternion rotation = new Quaternion(0, 0, 0, 0);
-    public List<GameObject> rotation2 = new List<GameObject>();
-    public GameObject body;
-
     public float min = -15;
     public float max = 15;
     public float speed = 0.3f;
-    public float speed2 = 0.3f;
-    public float speed3 = 0.3f;
     public bool dir = false;
-    public bool dir2 = false;
-    public bool dir3 = false;
+    public bool enabled = true;
 
-    public bool p = false, r = false, y = false;
+    public float i = 0;
+
+    public rotation(float speed, float min, float max)
+    {
+        this.speed = speed;
+        this.min = min;
+        this.max = max;
+    }
+
+    public float update()
+    {
+        if (enabled)
+        {
+            if (!dir && i < max)
+            {
+                i += speed;
+            }
+            else if (!dir)
+            {
+                dir = !dir;
+                i = (float)max;
+            }
+            else if (dir && i > min)
+            {
+                i -= speed;
+            }
+            else if (dir)
+            {
+                dir = !dir;
+                i = (float)min;
+            }
+        }
+
+        return i;
+    }
+}
+
+public class RotationControl : MonoBehaviour
+{
+    public Vector3 transformation = new Vector3(0, 0, 0);
+    public Vector3 transformation2 = new Vector3(0, 0, 0);
+
+    public Quaternion rotation = new Quaternion(0, 0, 0, 0);
+    public List<GameObject> rotation2 = new List<GameObject>();
+    public GameObject body;
+    public GameObject body2;
+
+    public rotation r1 = new rotation(0.1f, -15, 15);
+    public rotation r2 = new rotation(0.2f, -15, 15);
+    public rotation r3 = new rotation(0.3f, -15, 15);
+
+    public rotation r4 = new rotation(0.01f, -3, 3);
+    public rotation r5 = new rotation(0.02f, -3, 3);
+    public rotation r6 = new rotation(0.03f, -3, 3);
+
+    public rotation r7 = new rotation(0.001f, -3, 3);
+    public rotation r8 = new rotation(0.002f, -3, 3);
+    public rotation r9 = new rotation(0.003f, -3, 3);
 
     // Use this for initialization
     void Start ()
@@ -27,65 +77,27 @@ public class RotationControl : MonoBehaviour
     // Update is called once per frame
     void FixedUpdate()
     {
-        if (p)
-        {
-            if (!dir2 && rotation.x < max)
-                rotation.x += speed2;
-            else if (!dir2)
-            {
-                dir2 = !dir2;
-                rotation.x = (float)max;
-            }
-            else if (dir2 && rotation.x > min)
-                rotation.x -= speed2;
-            else if (dir2)
-            {
-                dir2 = !dir2;
-                rotation.x = (float)min;
-            }
-        }
+        rotation.x = r1.update();
+        rotation.y = r2.update();
+        rotation.z = r3.update();
 
-        if (r)
-        {
-            if (!dir3 && rotation.y < max)
-                rotation.y += speed3;
-            else if (!dir3)
-            {
-                dir3 = !dir3;
-                rotation.y = (float)max;
-            }
-            else if (dir3 && rotation.y > min)
-                rotation.y -= speed3;
-            else if (dir3)
-            {
-                dir3 = !dir3;
-                rotation.y = (float)min;
-            }
-        }
+        transformation.x = r4.update();
+        transformation.y = r5.update();
+        transformation.z = r6.update();
 
-        if (y)
-        {
-            if (!dir && rotation.z < max)
-                rotation.z += speed;
-            else if (!dir)
-            {
-                dir = !dir;
-                rotation.z = (float)max;
-            }
-            else if (dir && rotation.z > min)
-                rotation.z -= speed;
-            else if (dir)
-            {
-                dir = !dir;
-                rotation.z = (float)min;
-            }
-        }
+        transformation2.x = r7.update();
+        transformation2.y = r8.update();
+        transformation2.z = r9.update();
 
         body.transform.eulerAngles = new Vector3(rotation.x*-1, rotation.y*-1, rotation.z*-1);
+        body.transform.position = transformation;
+        body2.transform.localPosition = transformation2;
 
         for (int index = 0; index < rotation2.Count; ++index)
         {
             rotation2[index].GetComponent<Leg>().rotation = rotation;
+            rotation2[index].GetComponent<Leg>().transformation = transformation;
+            rotation2[index].GetComponent<Leg>().transformation2 = transformation2;
             rotation2[index].GetComponent<Leg>().FixedUpdate();
         }
     }