v1

2018-unity-sim/Assets/Foot.cs

@@ -0,0 +1,162 @@
+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);
+    }
+}

2018-unity-sim/Assets/Foot.cs.meta

@@ -0,0 +1,13 @@
+fileFormatVersion: 2
+guid: c9255406468231049ab79ec93b48f97e
+timeCreated: 1541795571
+licenseType: Free
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

2018-unity-sim/Assets/Leg.cs

@@ -0,0 +1,74 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class Leg : MonoBehaviour {
+    public Transform Hip1;
+    public Transform Hip2;
+    public Transform Knie;
+
+    public double a1 = 0;
+    public double a2 = 0;
+    public double a3 = 0;
+
+    public Vector3 point = new Vector3(10, 10, 0);
+    public Quaternion rotation = new Quaternion(0, 0, 0, 0);
+
+    public bool invert = false;
+
+    public double translateX = 5;
+    public double translateY = 0;
+    public double translateZ = 0;
+
+    public GameObject pointShouldBe;
+    public Vector3 pos = new Vector3(0, 0, 0);
+
+    private Foot foot = new Foot();
+
+    // Use this for initialization
+    void Start () {
+		
+	}
+
+    // Update is called once per frame
+    void Update()
+    {
+        foot.X = point.x;
+        foot.Y = point.y * -1;
+        //foot.Z = point.z * (invert ? -1 : 1);
+        foot.Z = point.z * (invert ? 1 : -1);
+
+        foot.rotationX = rotation.x;
+        foot.rotationY = rotation.y * (invert ? -1 : 1);
+        foot.rotationZ = rotation.z * (invert ? -1 : 1);
+
+        foot.translateX = translateX;
+        foot.translateY = translateY;
+        foot.translateZ = translateZ;
+
+        foot.invert = invert;
+
+        foot.setleg();
+
+        if (!invert)
+        {
+            a1 = foot.O1 - 90;
+            a2 = 90 - foot.O2;
+            a3 = foot.O3 - 90;
+        }
+        else
+        {
+            a1 = (foot.O1 - 90)*-1;
+            a2 = 90 - foot.O2;
+            a3 = foot.O3 - 90;
+        }
+
+        Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
+        Hip2.transform.localRotation = Quaternion.Euler((float)a2, 0, 0);
+        Knie.transform.localRotation = Quaternion.Euler((float)a3, 0, 0);
+
+        pos = foot.pos;
+        pos.x = pos.x * (invert ? 1 : -1);
+        pointShouldBe.transform.localPosition = pos;
+    }
+}

2018-unity-sim/Assets/Leg.cs.meta

@@ -0,0 +1,13 @@
+fileFormatVersion: 2
+guid: 3c4e50b7048aa1f4aaee65a71c7626c3
+timeCreated: 1541794260
+licenseType: Free
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

2018-unity-sim/Assets/Leg.prefab.meta

@@ -0,0 +1,10 @@
+fileFormatVersion: 2
+guid: c2064a01ffb68a84fa55bd109ccf70a4
+timeCreated: 1541791156
+licenseType: Free
+NativeFormatImporter:
+  externalObjects: {}
+  mainObjectFileID: 100100000
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

2018-unity-sim/Assets/Main.unity.meta

@@ -0,0 +1,9 @@
+fileFormatVersion: 2
+guid: 5f4a5f0170e50d546b983a411916f66d
+timeCreated: 1541789909
+licenseType: Free
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

2018-unity-sim/Assets/RotationControl.cs

@@ -0,0 +1,62 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class RotationControl : MonoBehaviour
+{
+    public Quaternion rotation = new Quaternion(0, 0, 0, 0);
+    public List<GameObject> rotation2 = new List<GameObject>();
+    public GameObject body;
+
+    public double min = -15;
+    public double max = 15;
+    public float speed = 0.3f;
+    public float speed2 = 0.3f;
+    public float speed3 = 0.3f;
+    bool dir = false;
+    bool dir2 = false;
+    bool dir3 = false;
+
+    // Use this for initialization
+    void Start ()
+    {
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        /*if (!dir2 && rotation.x < max)
+            rotation.x += speed2;
+        else if (!dir2)
+            dir2 = !dir2;
+        else if (dir2 && rotation.x > min)
+            rotation.x -= speed2;
+        else if (dir2)
+            dir2 = !dir2;*/
+
+        /*if (!dir3 && rotation.y < max)
+            rotation.y += speed3;
+        else if (!dir3)
+            dir3 = !dir3;
+        else if (dir3 && rotation.y > min)
+            rotation.y -= speed3;
+        else if (dir3)
+            dir3 = !dir3;*/
+
+        /*if (!dir && rotation.z < max)
+            rotation.z += speed;
+        else if (!dir)
+            dir = !dir;
+        else if (dir && rotation.z > min)
+            rotation.z -= speed;
+        else if (dir)
+            dir = !dir;*/
+
+        body.transform.eulerAngles = new Vector3(rotation.x*-1, rotation.y, rotation.z);
+
+        for (int index = 0; index < rotation2.Count; ++index)
+        {
+            rotation2[index].GetComponent<Leg>().rotation = rotation;
+        }
+    }
+}

2018-unity-sim/Assets/RotationControl.cs.meta

@@ -0,0 +1,13 @@
+fileFormatVersion: 2
+guid: 30ea8dc0510444c42ae4b4db1f5ee2bd
+timeCreated: 1541847613
+licenseType: Free
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: