Hexapod_Simulation_Unity/Assets/Walk.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Walk : MonoBehaviour
{
    public List<GameObject> legs = new List<GameObject>();
    private List<Vector3> legsInitial = new List<Vector3>();

    // Use this for initialization
    void Start()
    {
        for (int index = 0; index < legs.Count; ++index)
        {
            legsInitial.Add(legs[index].GetComponent<Leg>().point);
            step2.Add(0);
            step6.Add(false);
            //step3.Add(0);
            //step4.Add(new Vector3(0, 0, 0));
            step5.Add(new Vector3());

            freeze.Add(false);
            freeze_y.Add(false);
        }
    }

    struct step
    {
        public Vector3 pos;
        public float rotation;
        public float height;
        public bool inAir;
        public bool inRest;

        public step(Vector3 p, float r, float h, bool i, bool ir)
        {
            pos = p;
            rotation = r;
            height = 0;
            inAir = i;
            inRest = ir;
        }
    }

    // 1, 3, 5 -= 3

    int offset(int i, int offset, int max)
    {
        return  i - offset < 0 ? (max - (offset - i)) : i - offset;
    }

    step[] s = {
        new step(new Vector3(-1, 0, 0), -1, 0, false, false),

        new step(new Vector3(0, -2, 0), 0, 0, false, false),

        new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest

        new step(new Vector3(1, 0, 0), 1, 0, false, true),  // down middle 
        new step(new Vector3(1, 0, 0), 1, 0, false, false),
        
        new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest

        new step(new Vector3(0, 2, 0), 0, 0, false, false),
        new step(new Vector3(-1, 0, 0), -1, 0, true, false),
    };

    public int st = 5;
    public int speed2 = 30;
    int speed = 10;

    public float walkDirection = 0;
    public float centerRotation = 0;

    int size = 8;

    public float walkspeed = 1;

    private List<float> step2 = new List<float>();
    //private List<float> step3 = new List<float>();
    //private List<Vector3> step4 = new List<Vector3>();
    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)
                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;

                if (true)
                {
                    // Hex walk
                    if (index == 1 || index == 3 || index == 5)
                    {
                        t = offset(st, 4, size);
                    }
                }
                else
                {
                    // One leg at a time walk
                    switch (index)
                    {
                        case 0:
                            t = offset(st, 0, size);
                            break;
                        case 1:
                            t = offset(st, 4, size);
                            break;
                        case 2:
                            t = offset(st, 2, size);
                            break;
                        case 3:
                            t = offset(st, 3, size);
                            break;
                        case 4:
                            t = offset(st, 1, size);
                            break;
                        case 5:
                            t = offset(st, 5, size);
                            break;
                    }
                }

                if (!freeze[index])
                {
                    if (s[t].inAir == false)
                    {
                        step2[index] = (centerRotation * s[t].rotation) / speed2;

                        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;
                    }
                    else
                    {
                        Vector3 t2 = (legsInitial[index] + s[t].pos) - legs[index].GetComponent<Leg>().point;
                        step5[index] = t2 / speed2;

                        step6[index] = false;
                        freeze_y[index] = false;
                    }
                }
                else
                {
                    step5[index] = new Vector3(0, 0, 0);
                    step6[index] = false;
                }
            }

            speed = speed2;
        }
        else
        {
            speed--;
            makestep();
        }
    }

    void makestep()
    {
        for (int index = 0; index < legs.Count; ++index)
        {
            legs[index].GetComponent<Leg>().point += step5[index];
            if (step6[index])
            {
                //legs[index].GetComponent<Leg>().point += step5[index];

                // rotate x,y around rotation.
                legs[index].GetComponent<Leg>().setWorldPos(
                    Quaternion.Euler(0, step2[index], 0) * legs[index].GetComponent<Leg>().getWorldPos()
                    );
            }
        }
    }
}