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

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;
    }
  }
}