Hexapod_Simulation_Unity/Assets/Foot.cs

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

public class Foot : Component
{
    double radians(double angle)
    {
        return (Math.PI / 180) * angle;
    }

    double degrees(double radians)
    {
        double degrees = (180 / Math.PI) * radians;
        return (degrees);
    }

    //  Inverse Kinemetics Region
    double l3, s;

    double l1=10, l2=15;

    public double X, Y, Z;
    public double rotationX=0, rotationY=0, rotationZ=0;
    public double O1, O2, O3;

    public Vector3 transformation = new Vector3(0, 0, 0);
    public Vector3 transformation2 = new Vector3(0, 0, 0);

    static float knie_offset = 5.5f;

    void RL3()
    {
        if ((X == 0) && (Z == 0))
            l3 = 0;
        else
            l3 = Math.Sqrt(((X * X) + (Z * Z)));

        if (X < 0)
            l3 = l3 * -1;

        //todo
        l3 -= knie_offset;
    }

    double FO1()
    {
        if ((X != 0) & (Z != 0))
        {
            double a = (Z / X);
            return degrees(Math.Atan((a)));
        }
        else
            return degrees(Math.Atan(0.0)); // Math Error Protection.
    }

    double T1()
    {
        RL3();

        double x2 = X * X;
        double y2 = Y * Y;
        double z2 = Z * Z;

        double t = Math.Sqrt(x2 + z2);
        t -= knie_offset;
        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;

        if ((Y == 0) && (l3 == 0))
            t1 = 0; // Math Error Protection.
        else
            t1 = degrees(Math.Atan2(l3, Y));

        double a = (l1 * l1) - (l2 * l2) + s2;
        double b = 2 * l1 * s;
        double t2;
        if (b == 0)
            t2 = -180;// todo: figure out normal value;
        else
            t2 = degrees(Math.Acos(a / b));

        double ret = 0;

        //ret = t1 + Math.Abs(t2);
        ret = t1 + t2;

        return ret;
    }

    double T3()
    {
        double t = (l1 * l1) + (l2 * l2) - (s * s);
        double t2 = (2 * l1 * l2);
        double result = degrees(Math.Acos(t / t2));
        return result;
    }

    void RotateX()
    {
        double nY = Y * Math.Cos(radians(rotationX)) - Z * Math.Sin(radians(rotationX));
        double nZ = Y * Math.Sin(radians(rotationX)) + Z * Math.Cos(radians(rotationX));
        Y = nY;
        Z = nZ;
    }

    void RotateY()
    {
        double nZ = Z * Math.Cos(radians(rotationY)) - X * Math.Sin(radians(rotationY));
        double nX = Z * Math.Sin(radians(rotationY)) + X * Math.Cos(radians(rotationY));
        Z = nZ;
        X = nX;
    }

    void RotateZ()
    {
        double nX = X * Math.Cos(radians(rotationZ)) - Y * Math.Sin(radians(rotationZ));
        double nY = X * Math.Sin(radians(rotationZ)) + Y * Math.Cos(radians(rotationZ));
        X = nX;
        Y = nY;
    }

    public double translateX = 0;
    public double translateY = 0;
    public double translateZ = 0;

    public bool invert = false;

    public Vector3 pos = new Vector3(0, 0, 0);

    public void setleg()
    {
        X += translateX;
        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;
        v = Quaternion.Euler((float)rotationX, (float)0, (float)0) * v;
        v = Quaternion.Euler((float)0, (float)0, (float)rotationZ) * v;
        X = v.x;
        Y = v.y;
        Z = v.z;

        X -= transformation2.x;
        Y -= transformation2.y;
        Z -= transformation2.z;

        X -= translateX;
        Y -= translateY;
        Z -= translateZ;

        // translate to my stupid formula coordinates.
        double tx = X;
        double ty = Y;
        double tz = Z;
        X = tz * (invert ? -1 : 1);
        Y = ty * -1;
        Z = tx * -1;

        // run IK
        // add adjustment from knie

        O1 = (90 - FO1());
        // remove adjustment from knie

        O2 = T1();
        O3 = 180 - T3();

        // translate to my stupid formula coordinates.
        X = tx;
        Y = ty;
        Z = tz;

        pos = new Vector3((float)X * (invert ? -1 : 1), (float)Y, (float)Z * (invert ? -1 : 1));
    }
}