v9 walk3 wip

2018-unity-sim/Assets/Leg.cs

@@ -99,6 +99,11 @@ public class Leg : MonoBehaviour {
         pointShouldBe.transform.localPosition = foot.pos;
     }
 
+    public Vector3 getLocalPos()
+    {
+        return point;
+    }
+
     public Vector3 getWorldPos()
     {
         return point + new Vector3((float)translateX, (float)translateY, (float)translateZ);

2018-unity-sim/Assets/Walk3.cs

@@ -18,6 +18,9 @@ public class Walk3 : MonoBehaviour
             //step3.Add(0);
             //step4.Add(new Vector3(0, 0, 0));
             step5.Add(new Vector3());
+
+            freeze.Add(false);
+            freeze_y.Add(false);
         }
     }
 
@@ -27,13 +30,15 @@ public class Walk3 : MonoBehaviour
         public float rotation;
         public float height;
         public bool inAir;
+        public bool inRest;
 
-        public step(Vector3 p, float r, float h, bool i)
+        public step(Vector3 p, float r, float h, bool i, bool ir)
         {
             pos = p;
             rotation = r;
             height = 0;
             inAir = i;
+            inRest = ir;
         }
     }
 
@@ -45,19 +50,19 @@ public class Walk3 : MonoBehaviour
     }
 
     step[] s = {
-        new step(new Vector3(-1, 0, 0), -1, 0, false),
+        new step(new Vector3(-1, 0, 0), -1, 0, false, false),
 
-        new step(new Vector3(0, -2, 0), 0, -2, false),
+        new step(new Vector3(0, -2, 0), 0, 0, false, false),
 
-        new step(new Vector3(0, 0, 0), 0, 0, false), // rest
+        new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest
 
-        new step(new Vector3(1, 0, 0), 1, 0, false),
+        new step(new Vector3(1, 0, 0), 1, 0, false, true),  // down middle 
-        new step(new Vector3(1, 0, 0), 1, 0, false),    
+        new step(new Vector3(1, 0, 0), 1, 0, false, false),
         
-        new step(new Vector3(0, 0, 0), 0, 0, false), // rest
+        new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest
 
-        new step(new Vector3(0, 2, 0), 0, 2, false),
+        new step(new Vector3(0, 2, 0), 0, 0, false, false),
-        new step(new Vector3(-1, 0, 0), -1, 0, true),
+        new step(new Vector3(-1, 0, 0), -1, 0, true, false),
     };
 
     public int st = 5;
@@ -77,16 +82,57 @@ public class Walk3 : MonoBehaviour
     private List<Vector3> step5 = new List<Vector3>();
     private List<bool> step6 = new List<bool>();
 
+    private List<bool> freeze = new List<bool>();
+    private List<bool> freeze_y = new List<bool>();
+
     // Update is called once per frame
     void FixedUpdate()
     {
         if (speed <= 0)
         {
-            if (st >= size - 1)
+            if (st >= size-1)
                 st = 0;
             else
+            {
                 st++;
 
+                if (centerRotation == 0 && walkspeed == 0)
+                {
+                    for (int index = 0; index < legs.Count; index++)
+                    {
+                        int t = st;
+                        if (index == 1 || index == 3 || index == 5)
+                        {
+                            t = offset(st, 4, size);
+                        }
+
+                        if (s[t].inRest)
+                        {
+                            Vector3 lp = legs[index].GetComponent<Leg>().getLocalPos();// + s[t].pos;
+                            //lp.y -= 2;
+                            if ((Mathf.Abs(lp.y - (legsInitial[index].y-2)) < 0.0001f) 
+                                && (Mathf.Abs(lp.z - legsInitial[index].z) < 0.0001f) 
+                                && (Mathf.Abs(lp.x - legsInitial[index].x) < 1.0001f))
+                            {
+                                freeze[index] = true;
+                                freeze_y[index] = true;
+                            }
+                        }
+                        /*else
+                        {
+                            freeze[index] = false;
+                        }*/
+                    }
+                }
+                else
+                {
+                    for (int index = 0; index < legs.Count; index++)
+                    {
+                        freeze[index] = false;
+                    }
+                }
+            }
+
             for (int index = 0; index < legs.Count; ++index)
             {
                 int t = st;
@@ -95,6 +141,8 @@ public class Walk3 : MonoBehaviour
                     t = offset(st, 4, size);
                 }
 
+                if (!freeze[index])
+                {
                     if (s[t].inAir == false)
                     {
                         step2[index] = (centerRotation * s[t].rotation) / speed2;
@@ -102,6 +150,10 @@ public class Walk3 : MonoBehaviour
                         Vector3 t2 = s[t].pos;
                         t2.x *= walkspeed;
                         // rotate x,y around rotation.
+                        if (freeze_y[index])
+                        {
+                            t2.y = 0;
+                        }
                         step5[index] = (Quaternion.Euler(0, walkDirection, 0) * t2) / speed2;
 
                         step6[index] = true;
@@ -112,6 +164,13 @@ public class Walk3 : MonoBehaviour
                         step5[index] = t2 / speed2;
 
                         step6[index] = false;
+                        freeze_y[index] = false;
+                    }
+                }
+                else
+                {
+                    step5[index] = new Vector3(0, 0, 0);
+                    step6[index] = false;
                 }
             }