From c977b3acab77cdc55e2cc4aaa0ae094105ff2925 Mon Sep 17 00:00:00 2001
From: MangaValk <patrickvalkmanga@hotmail.com>
Date: Mon, 6 Oct 2014 22:39:00 +0200
Subject: [PATCH] Clean up main sketch

Imported from 'hexapod 2.rar'.
---
 .../processing-3d/hexapod/hexapod.pde         | 112 +++++-------------
 1 file changed, 32 insertions(+), 80 deletions(-)

diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde
index 13db192..c993b00 100644
--- a/2012-simulation/processing-3d/hexapod/hexapod.pde
+++ b/2012-simulation/processing-3d/hexapod/hexapod.pde
@@ -42,7 +42,7 @@ void draw()
 
   translate(width/2, height/2, 0);
 
-  switch(/*i5*/2)
+  switch(i5)
   {
   case 0:
     rotateX(-1*(PI/10)); 
@@ -61,9 +61,9 @@ void draw()
 
   float rotationRange = 8;
 
-  //rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) );
+  rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) );
   rotationY = ( i4%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i4%(rotationRange*2) : rotationRange+(-1*(i4%(rotationRange*2))) );
-  //rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) );
+  rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) );
 
   if (c2.delay(25))
   {
@@ -145,9 +145,7 @@ void draw()
   {
     animationSubStep++;
   }
-  
-  animationSubStep = 0;  //  REMOVE
-  
+
   foot1.SetFootCoordinates(Animation(2));
   foot2.SetFootCoordinates(Animation(3));
   foot3.SetFootCoordinates(Animation(4));//
@@ -348,86 +346,20 @@ class Foot
 
   void RotateX()
   {
-    if (leftLegorRight)
-    {
-      double yt = Y + TranslateY;
-      double zt = Z + TranslateZ;
-
-      double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX));
-      double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX));
-
-      Y = NY - TranslateY;
-      Z = NZ - TranslateZ;
-    } else
-    {
-      double yt = Yi + TranslateY;
-      double zt = Zi + TranslateZ;
-
-      double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX));
-      double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX));
-
-      Y = NY - TranslateY;
-      Z = NZ - TranslateZ;
-    }
+    Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
+    Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
   }
 
   void RotateY()
   {
-    if (leftLegorRight)
-    {  //  Right
-      /*double xt = (-1*X) + TranslateX;
-      double zt = -1*(Z + TranslateZ);
-
-      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
-      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));
-
-      X = -1 * (NX - TranslateX);
-      Z = -1*(NZ + TranslateZ);*/
-      double xt = (-1*X) + TranslateX;
-      double zt = (-1*Z) + TranslateZ;
-
-      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
-      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));
-
-      X = -1 * (NX - TranslateX);
-      Z = -1 * (NZ - TranslateZ);
-    } else
-    {
-      double xt = X + TranslateX;
-      double zt = Z + TranslateZ;
-
-      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
-      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));
-
-      X = NX - TranslateX;
-      Z = NZ - TranslateZ;
-    }
+    Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
+    X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
   }
 
   void RotateZ()
   {
-    if (leftLegorRight)
-    {
-      double xt = (-1*X) + TranslateX;
-      double yt = Y + TranslateY;
-
-      double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ));
-      double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ));
-
-      X = -1 * (NX - TranslateX);
-      Y = NY - TranslateY;
-    } else
-    {
-      double xt = X + TranslateX;
-      double yt = Y + TranslateY;
-
-      double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ));
-      double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ));
-
-      X = NX - TranslateX;
-      Y = NY - TranslateY;
-      Z = Zi;
-    }
+    X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
+    Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
   }
 
   void RL3 ()
@@ -503,9 +435,29 @@ class Foot
     Y = Yi;
     Z = Zi;
 
+    if (leftLegorRight)
+    {  //  Right
+      X = (-1*X) + TranslateX;
+      Z = (-1*Z) + TranslateZ;
+    } else
+    {
+      X = Xi + TranslateX;
+      Z = (-1*Z) + TranslateZ;
+    }
+
     RotateY();
-    //RotateX();
-    //RotateZ();
+    RotateX();
+    RotateZ();
+
+    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())));