Simple Footstep controller for mechscale game

TK_FootStepController.cs

using UnityEngine;
using System;
using System.Collections;

// This moves a node to follow "stepGoal" in a foot step like manner. stepGoal is 
// typically an empty parented to your model that sits on the ground and is offset (left
// or right) from the center of mass. 

// This just moves the foot, a separate IK controller is needed to follow wiht the legs

public class TK_FootStepController : MonoBehaviour {

    protected const double TWO_PI = Math.PI * 2;
    protected const double HALF_PI = Math.PI / 2;

    public float warpDist = 10.0f; // If foot further than this, just warp it to position
    public float stepDist = 4.0f;  // If foot further than this, take a step
    public float stepTime = 2.0f;  // how long a step takes (s)
    public float footLift = 4.0f;  // How high to lift foot
    public Transform stepGoal;  // Where the step lands
    public TK_FootStepController oppositeFoot;

    private Vector3 stepStartPos;
    private Quaternion stepStartRot;
    private bool isStepping;
    private float stepTimeLeft;
    protected bool canStep = true; // Can I step or is the other foot stepping now?
        
    private Color dbgOrigColor;

	// Use this for initialization
	void Start () {
        isStepping = false;

        var endMtl = GetComponent<Renderer>().material;
        dbgOrigColor = endMtl.color;


    }

    public static double EaseInOutCubic(double t, double b, double c, double d)
    {
        if ((t /= d / 2) < 1) return c / 2 * t * t * t + b;
        return c / 2 * ((t -= 2) * t * t + 2) + b;
    }
    float easeFootstep( float tval )
    {
        //return (float)EaseInOutBounce(tval, 0.0, 1.0, 1.0, 0.0, 0.2);
        return (float)EaseInOutCubic(tval, 0.0, 1.0, 1.0 );
    }
	// Update is called once per frame
	void Update () {
        Vector3 currStepOffs = (stepGoal.position - transform.position);
        currStepOffs.y = 0.0f;
        float currStepDist = currStepOffs.magnitude;
        if (currStepDist > warpDist)
        {
            // past the too far distance, just warp the foot to place
            Debug.DrawLine(
                transform.position, stepGoal.position, Color.magenta, 0.3f);

            transform.position = stepGoal.position;
            isStepping = false;
        }
        else if ((!isStepping) && (currStepDist > stepDist) && canStep)
        {
            stepStartPos = transform.position;
            stepStartRot = transform.rotation;

            isStepping = true;
            stepTimeLeft = stepTime;

            // Debug code to change the color of end effector when stepping
            var endMtl = GetComponent<Renderer>().material;
            endMtl.color = Color.cyan;
            
            // Tell opposite foot if present
            if (oppositeFoot != null)
            {
                oppositeFoot.canStep = false;
            }
        }

        if (isStepping)
        {
            float traw =  (stepTime - stepTimeLeft) / stepTime;
            float tval = easeFootstep(traw);
            float upval = easeFootstep( 1.0f - Mathf.Abs(traw * 2.0f - 1.0f));

           // Debug.Log("upval " + upval);

            Vector3 currStep = Vector3.Lerp(stepStartPos, stepGoal.position, tval);
            currStep.y = (upval * footLift) + 0.2f; // A little instant height to not look like sliding
            transform.position = currStep;
            transform.rotation = Quaternion.Lerp(stepStartRot, stepGoal.rotation, tval);

            stepTimeLeft -= Time.deltaTime;
            if (stepTimeLeft < 0.0f)
            {
                // Step finished
                stepTimeLeft = 0.0f;
                isStepping = false;
                transform.position = stepGoal.position;
                transform.rotation = stepGoal.rotation;

                var endMtl = GetComponent<Renderer>().material;
                endMtl.color = dbgOrigColor;
                
                // Tell opposite foot if present
                if (oppositeFoot != null)
                {
                    oppositeFoot.canStep = true;
                }

            }
        }
	}
}