Split leg and body logic into FootClass and HexapodClass

Imported from 'hexapod 3.rar'.

2012-simulation/processing-3d/hexapod/FootClass.pde

@@ -0,0 +1,228 @@
+class Foot
+{
+  float TranslateX, TranslateY, TranslateZ;
+  boolean leftLegorRight=false;
+
+  void SetFootCoordinates(PVector P)
+  {
+    Xi = P.x;
+    Yi = P.y;
+    Zi = P.z;
+  }
+
+  void Translate(int x, int y, int z)
+  {
+    TranslateX = x;  
+    TranslateY = y;
+    TranslateZ = z;
+  }
+
+  void Translate(PVector i)
+  {
+    TranslateX = i.x;  
+    TranslateY = i.y;
+    TranslateZ = i.z;
+  }
+
+  void DrawFoot()
+  {
+    setleg(leftLegorRight);
+
+    pushMatrix();
+
+    translate(TranslateX, TranslateY, TranslateZ);
+
+    stroke(!error?255:0, !error?0:255, 0);
+    noFill();
+    beginShape();
+
+    Vertex(0, 0, 0);
+    Vertex(0, 0, 0);
+    if (leftLegorRight)
+      Vertex(-1*X2, -1*Y2, Z2);
+    else
+      Vertex(X2, -1*Y2, Z2);
+
+    endShape(); 
+
+    beginShape();
+
+    if (leftLegorRight)
+    {
+      Vertex(-1*X2, -1*Y2, Z2);
+      Vertex(-1*X2, -1*Y2, Z2);
+      Vertex(-1*X, Y, -1*Z);
+    } else
+    {
+      Vertex(X2, -1*Y2, Z2);
+      Vertex(X2, -1*Y2, Z2);
+      Vertex(X, Y, -1*Z);
+    }
+
+    endShape();       
+
+    popMatrix();
+  }
+
+  //  Inverse Kinemetics Region
+  double l3, s, O1, O2, O3, X2, Y2, Z2;
+  double l1=10, l2=15;
+  double Xi, Yi, Zi;
+  double X, Y, Z;
+  boolean error=false;
+
+  void RotateX()
+  {
+    Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
+    Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
+  }
+
+  void RotateY()
+  {
+    Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
+    X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
+  }
+
+  void RotateZ()
+  {
+    X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
+    Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
+  }
+
+  void RL3 ()
+  {
+    double x = X;
+    double z = Z;
+
+    if ((x == 0) && (z == 0)) 
+      l3 = 0;
+    else l3 = sqrt((float)(x*x + z*z));
+
+    if (x < 0)
+      l3 = l3 * -1;
+  }
+
+  float O1 ()
+  {
+    RL3();
+
+    double x = X;
+    double z = Z;
+    double r;
+
+    if ((x != 0) & (z != 0)) {
+      double a = (z / x);
+      return atan((float)(a));
+    } else
+      return atan(0.0); // Math Error Protection.
+  }
+
+  double T1 ()
+  {
+    double x = X;
+    double y = Y;
+    double z = Z;
+    double x2 = x * x;
+    double y2 = y * y;
+    double z2 = z * z;
+    double l32 = l3 * l3;
+
+    s = sqrt((float)(x2 + y2 + z2));
+
+    double t1 = 0;
+
+    double s2 = s * s;
+
+    if ((y == 0) && (l3 == 0))
+      t1 = 0; // Math Error Protection.
+    else
+      t1 = (atan2((float)(l3), (float)(y)));
+
+    double a = (l1 * l1) - (l2*l2) + s2;
+    double b = 2 * l1 * s;
+    double t2 = acos((float)(a / b));
+
+    double ret = 0;
+
+    ret = t1 + abs((float)(t2));
+
+    return ret;
+  }
+
+  double T3 ()
+  {
+    return (acos((float)(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2))));
+  }
+
+  double PI = 3.14159265;
+
+  void setleg (boolean Side)
+  {
+    X = Xi;
+    Y = Yi;
+    Z = Zi;
+
+    //  Gaits
+    if (leftLegorRight)
+    {  //  Right
+      X = (-1*X) + TranslateX;
+      Z = (-1*Z) + TranslateZ;
+    } else
+    {
+      X = Xi + TranslateX;
+      Z = (-1*Z) + TranslateZ;
+    }
+
+    RotateY();
+    RotateX();
+    RotateZ();
+
+    //  Remove gaits
+    if (leftLegorRight)
+    {  //  Right
+      X = -1 * (X - TranslateX);
+      Z = -1 * (Z - TranslateZ);
+    } else
+    {
+      X -= TranslateX;
+      Z = -1 * (Z - TranslateZ);
+    }
+
+    double a = (90 - degrees((float)(O1())));
+    double b = (180 - degrees((float)(T1())));
+    double c = degrees((float)(T3()));
+
+    if ((a >= 15) & (a <= 165)
+      & (b >= 15) & (b <= 165)
+      & (c >= 15) & (c <= 165))
+    {
+      error = false;
+
+      O1 = a;
+      O2 = b;
+      O3 = c;
+
+      double d = sin((float)(b*(PI/180)));
+      if ( d != 0 )
+        X2 = d * l1;
+      else
+        X2 = 0;
+
+      d = cos((float)(b*(PI/180)));
+      if ( d != 0 )
+        Y2 = d * l1;
+      else
+        Y2 = 0;
+
+      d = sin((float)((a-90)*(PI/180)));
+      if ( d != 0 )
+        Z2 = d * abs((float)(X2));
+      else
+        Z2 = 0;
+    } else
+    {
+      error = true;
+    }
+  }
+}
+