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;

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

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

    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;

        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;

        Y = Y * (invert ? -1 : 1);
        X = X * (invert ? -1 : -1);
        Z = Z * (invert ? -1 : -1);

        /*RotateX();
        RotateY();
        RotateZ();*/

        rotationY = rotationY * (invert ? 1 : 1);

        // Z and X is working. Y has problems when combined with others.
        Vector3 v = new Vector3((float)Z, (float)Y, (float)X);
        v = Quaternion.Euler((float)rotationX, (float)rotationY, (float)rotationZ) * v;
        Z = v.x;
        Y = v.y;
        X = v.z;

        Y = Y * (invert ? -1 : 1);
        X = X * (invert ? -1 : -1);
        Z = Z * (invert ? -1 : -1);

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

        O1 = (90 - FO1());
        O2 = T1();
        O3 = 180 - T3();

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