From 00bf3187feabdf61e67d5f834a000f233a3cfc36 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Sun, 12 Oct 2014 16:17:00 +0200 Subject: [PATCH] Split leg and body logic into FootClass and HexapodClass Imported from 'hexapod 3.rar'. --- .../processing-3d/hexapod/FootClass.pde | 228 ++++++++ .../processing-3d/hexapod/HexapodClass.pde | 230 ++++++++ .../processing-3d/hexapod/hexapod.pde | 496 ++---------------- 3 files changed, 499 insertions(+), 455 deletions(-) create mode 100644 2012-simulation/processing-3d/hexapod/FootClass.pde create mode 100644 2012-simulation/processing-3d/hexapod/HexapodClass.pde diff --git a/2012-simulation/processing-3d/hexapod/FootClass.pde b/2012-simulation/processing-3d/hexapod/FootClass.pde new file mode 100644 index 0000000..5aa9321 --- /dev/null +++ b/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; + } + } +} + diff --git a/2012-simulation/processing-3d/hexapod/HexapodClass.pde b/2012-simulation/processing-3d/hexapod/HexapodClass.pde new file mode 100644 index 0000000..00a90cd --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/HexapodClass.pde @@ -0,0 +1,230 @@ +Foot foot1 = new Foot(); +Foot foot2 = new Foot(); +Foot foot3 = new Foot(); +Foot foot4 = new Foot(); +Foot foot5 = new Foot(); +Foot foot6 = new Foot(); + +// 9.5 +// 14 +// 9.5 + +// 10 +// 10 + +PVector point1 = new PVector(4.75, 0, 10); +PVector point2 = new PVector(7, 0, 0); +PVector point3 = new PVector(4.75, 0, -10); +PVector point4 = new PVector(-4.75, 0, -10); +PVector point5 = new PVector(-7, 0, 0); +PVector point6 = new PVector(-4.75, 0, 10); + +float rotationX=0; +float rotationY=0; +float rotationZ=0; + +int animationStep=0; +int animationSubStep=0; + +boolean robotStanding = false; + +float robotHeigth = 12; + +void drawTestSpot(PVector p, boolean leftLegorRight) +{ + beginShape(); + stroke(0, 0, 255); + if (leftLegorRight) + { + Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); + Vertex((-1*p.x)-1, p.y, (-1*p.z)+1); + Vertex((-1*p.x)+1, p.y, (-1*p.z)+1); + Vertex((-1*p.x)+1, p.y, (-1*p.z)-1); + Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); + } else + { + Vertex(p.x-1, p.y, (-1*p.z)-1); + Vertex(p.x-1, p.y, (-1*p.z)+1); + Vertex(p.x+1, p.y, (-1*p.z)+1); + Vertex(p.x+1, p.y, (-1*p.z)-1); + Vertex(p.x-1, p.y, (-1*p.z)-1); + } + endShape(); +} + +PVector []walk1 = new PVector[6]; + +PVector Animation(int addition) +{ + + PVector P = new PVector(); + + P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); + + return P; +} + +PVector animationSubSteps(PVector from, PVector to) +{ + PVector newStep = new PVector(); + int steps=5; + + if (animationSubStep < steps) + { + float differenceX = (to.x - from.x) / steps; + float differenceY = (to.y - from.y) / steps; + float differenceZ = (to.z - from.z) / steps; + + newStep.x = from.x + ( differenceX * animationSubStep ); + newStep.y = from.y + ( differenceY * animationSubStep ); + newStep.z = from.z + ( differenceZ * animationSubStep ); + } else + { + animationSubStep=0; + animationStep++; + newStep = to; + } + + return newStep; +} + +void Vertex(double x, double y, double z) +{ + vertex((float)x, (float)y, (float)z); +} + +class CustomTimer +{ + int time; + boolean start=true; + public boolean delay(int delay) + { + if (start) + { + time = millis(); + start = false; + } + if (millis() - time >= delay) + { + start = true; + return true; + } else + return false; + } +} + +void DrawFeet() +{ + foot1.Translate(point1); + foot1.DrawFoot(); + + foot2.Translate(point2); + foot2.DrawFoot(); + + foot3.Translate(point3); + foot3.DrawFoot(); + + foot4.Translate(point4); + foot4.leftLegorRight=true; + foot4.DrawFoot(); + + foot5.Translate(point5); + foot5.leftLegorRight=true; + foot5.DrawFoot(); + + foot6.Translate(point6); + foot6.leftLegorRight=true; + foot6.DrawFoot(); +} + +void SetFeetCoordinatesUsingAnimation() +{ + foot1.SetFootCoordinates(Animation(2)); + foot2.SetFootCoordinates(Animation(3)); + foot3.SetFootCoordinates(Animation(4));// + foot4.SetFootCoordinates(Animation(1));// + foot5.SetFootCoordinates(Animation(0)); + foot6.SetFootCoordinates(Animation(5)); +} + +void DrawGround() +{ + noStroke(); // Ground + fill(128); + beginShape(); + Vertex(-60, robotHeigth, -60); + Vertex(60, robotHeigth, -60); + Vertex(60, robotHeigth, 60); + Vertex(-60, robotHeigth, 60); + Vertex(-60, robotHeigth, -60); + endShape(); +} + +void DrawTestSpots() +{ + drawTestSpot(new PVector((Animation(2).x + point1.x), + robotHeigth, + (Animation(2).z - point1.z)), false); + drawTestSpot(new PVector((Animation(3).x + point2.x), + robotHeigth, + (Animation(3).z - point2.z)), false); + drawTestSpot(new PVector((Animation(4).x + point3.x), + robotHeigth, + (Animation(4).z - point3.z)), false); + drawTestSpot(new PVector((Animation(1).x - point4.x), + robotHeigth, + (Animation(1).z - point4.z)), true); + drawTestSpot(new PVector((Animation(0).x - point5.x), + robotHeigth, + (Animation(0).z - point5.z)), true); + drawTestSpot(new PVector((Animation(5).x - point6.x), + robotHeigth, + (Animation(5).z - point6.z)), true); +} + +void DrawBody() +{ + // Rotate body + rotateX(radians(-1*rotationX)); + rotateY(radians(-1*rotationY)); + rotateZ(radians(-1*rotationZ)); + + stroke(100, 100, 100); // Body + fill(0, 0, 255); + beginShape(); + Vertex(point1.x, point1.y, point1.z); + Vertex(point2.x, point2.y, point2.z); + Vertex(point3.x, point3.y, point3.z); + Vertex(point4.x, point4.y, point4.z); + Vertex(point5.x, point5.y, point5.z); + Vertex(point6.x, point6.y, point6.z); + Vertex(point1.x, point1.y, point1.z); + endShape(); + stroke(100, 100, 100); // Body top + fill(0, 255, 0); + beginShape(); + Vertex(point1.x, point1.y, point1.z); + Vertex(point2.x, point2.y, point2.z); + Vertex(point5.x, point5.y, point5.z); + Vertex(point6.x, point6.y, point6.z); + Vertex(point1.x, point1.y, point1.z); + endShape(); + + SetFeetCoordinatesUsingAnimation(); + + DrawFeet(); +} + +PVector []standup = new PVector[6]; + +void RobotStandup() +{ + standup[0] = new PVector(3, 5, 0); + standup[1] = new PVector(12, robotHeigth, -5); + standup[2] = new PVector(12, robotHeigth, -5); + + if (!robotStanding) + { + } +} + diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde index c993b00..c42db6c 100644 --- a/2012-simulation/processing-3d/hexapod/hexapod.pde +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -1,40 +1,28 @@ void setup() { size(600, 600, P3D); background(0); + + walk1[0] = new PVector(12, robotHeigth, -5); + walk1[1] = new PVector(12, robotHeigth, 0); + walk1[2] = new PVector(12, robotHeigth, 5); + walk1[3] = new PVector(12, robotHeigth-2, 5); + walk1[4] = new PVector(12, robotHeigth-2, 0); + walk1[5] = new PVector(12, robotHeigth-2, -5); } -Custom c1 = new Custom(); -Custom c2 = new Custom(); -Custom c3 = new Custom(); -Custom c4 = new Custom(); -Custom c5 = new Custom(); +CustomTimer timerAnimation = new CustomTimer(); +CustomTimer timerXRotation = new CustomTimer(); +CustomTimer timerYRotation = new CustomTimer(); +CustomTimer timerZRotation = new CustomTimer(); +CustomTimer timerScene = new CustomTimer(); -Foot foot1 = new Foot(); -Foot foot2 = new Foot(); -Foot foot3 = new Foot(); -Foot foot4 = new Foot(); -Foot foot5 = new Foot(); -Foot foot6 = new Foot(); +float counterRotation=0; +int counterXRotation=0; +int counterYRotation=0; +int counterZRotation=0; +int counterScene=0; -PVector point1 = new PVector(8, 0, 20); -PVector point2 = new PVector(12, 0, 0); -PVector point3 = new PVector(8, 0, -20); -PVector point4 = new PVector(-8, 0, -20); -PVector point5 = new PVector(-12, 0, 0); -PVector point6 = new PVector(-8, 0, 20); - -float rotationX=0; -float rotationY=0; -float rotationZ=0; - -int animationStep=0; -int animationSubStep=0; - -float i=0; -int i2=0; -int i3=0; -int i4=0; -int i5=0; +float rotationRange = 8; void draw() { @@ -42,458 +30,56 @@ void draw() translate(width/2, height/2, 0); - switch(i5) + switch(counterScene) { case 0: rotateX(-1*(PI/10)); rotateY(radians(-210)); break; case 1: - rotateY(i); + rotateY(counterRotation); break; case 2: rotateX(radians(-90)); rotateY(radians(180)); break; default: - i5=0; + counterScene=0; } - float rotationRange = 8; + rotationX = ( counterXRotation%(rotationRange*4) < (rotationRange*2) ? -rotationRange+counterXRotation%(rotationRange*2) : rotationRange+(-1*(counterXRotation%(rotationRange*2))) ); + rotationY = ( counterYRotation%(rotationRange*4) < (rotationRange*2) ? -rotationRange+counterYRotation%(rotationRange*2) : rotationRange+(-1*(counterYRotation%(rotationRange*2))) ); + rotationZ = ( counterZRotation%(rotationRange*4) < (rotationRange*2) ? -rotationRange+counterZRotation%(rotationRange*2) : rotationRange+(-1*(counterZRotation%(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))) ); - - if (c2.delay(25)) + // Animation + if (timerAnimation.delay(1)) { - i-=0.012f; - i2++; + animationSubStep++; } - if (c3.delay(50)) + if (timerZRotation.delay(25)) { - i3++; + counterRotation-=0.012f; + counterZRotation++; } - if (c4.delay(75)) + if (timerXRotation.delay(50)) { - i4++; + counterXRotation++; } - if (c5.delay(5000)) + if (timerYRotation.delay(75)) { - i5++; + counterYRotation++; + } + if (timerScene.delay(5000)) + { + counterScene++; } scale(6); - noStroke(); // Ground - fill(128); - beginShape(); - Vertex(-60, 15, -60); - Vertex(60, 15, -60); - Vertex(60, 15, 60); - Vertex(-60, 15, 60); - Vertex(-60, 15, -60); - endShape(); + DrawGround(); - drawTestSpot(new PVector((Animation(2).x + point1.x), - 15, - (Animation(2).z - point1.z)), false); - drawTestSpot(new PVector((Animation(3).x + point2.x), - 15, - (Animation(3).z - point2.z)), false); - drawTestSpot(new PVector((Animation(4).x + point3.x), - 15, - (Animation(4).z - point3.z)), false); - drawTestSpot(new PVector((Animation(1).x - point4.x), - 15, - (Animation(1).z - point4.z)), true); - drawTestSpot(new PVector((Animation(0).x - point5.x), - 15, - (Animation(0).z - point5.z)), true); - drawTestSpot(new PVector((Animation(5).x - point6.x), - 15, - (Animation(5).z - point6.z)), true); + DrawTestSpots(); - // Rotate body - rotateX(radians(-1*rotationX)); - rotateY(radians(-1*rotationY)); - rotateZ(radians(-1*rotationZ)); - - stroke(100, 100, 100); // Body - fill(0, 0, 255); - beginShape(); - Vertex(point1.x, point1.y, point1.z); - Vertex(point2.x, point2.y, point2.z); - Vertex(point3.x, point3.y, point3.z); - Vertex(point4.x, point4.y, point4.z); - Vertex(point5.x, point5.y, point5.z); - Vertex(point6.x, point6.y, point6.z); - Vertex(point1.x, point1.y, point1.z); - endShape(); - stroke(100, 100, 100); // Body top - fill(0, 255, 0); - beginShape(); - Vertex(point1.x, point1.y, point1.z); - Vertex(point2.x, point2.y, point2.z); - Vertex(point5.x, point5.y, point5.z); - Vertex(point6.x, point6.y, point6.z); - Vertex(point1.x, point1.y, point1.z); - endShape(); - - // Animation - if (c1.delay(1)) - { - animationSubStep++; - } - - foot1.SetFootCoordinates(Animation(2)); - foot2.SetFootCoordinates(Animation(3)); - foot3.SetFootCoordinates(Animation(4));// - foot4.SetFootCoordinates(Animation(1));// - foot5.SetFootCoordinates(Animation(0)); - foot6.SetFootCoordinates(Animation(5)); - - foot1.Translate(point1); - foot1.DrawFoot(); - - foot2.Translate(point2); - foot2.DrawFoot(); - - foot3.Translate(point3); - foot3.DrawFoot(); - - foot4.Translate(point4); - foot4.leftLegorRight=true; - foot4.DrawFoot(); - - foot5.Translate(point5); - foot5.leftLegorRight=true; - foot5.DrawFoot(); - - foot6.Translate(point6); - foot6.leftLegorRight=true; - foot6.DrawFoot(); -} - -void drawTestSpot(PVector p, boolean leftLegorRight) -{ - beginShape(); - stroke(0, 0, 255); - if (leftLegorRight) - { - Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); - Vertex((-1*p.x)-1, p.y, (-1*p.z)+1); - Vertex((-1*p.x)+1, p.y, (-1*p.z)+1); - Vertex((-1*p.x)+1, p.y, (-1*p.z)-1); - Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); - } else - { - Vertex(p.x-1, p.y, (-1*p.z)-1); - Vertex(p.x-1, p.y, (-1*p.z)+1); - Vertex(p.x+1, p.y, (-1*p.z)+1); - Vertex(p.x+1, p.y, (-1*p.z)-1); - Vertex(p.x-1, p.y, (-1*p.z)-1); - } - endShape(); -} - -PVector []walk1 = new PVector[6]; - -PVector Animation(int addition) -{ - walk1[0] = new PVector(12, 15, -5); - walk1[1] = new PVector(12, 15, 0); - walk1[2] = new PVector(12, 15, 5); - walk1[3] = new PVector(12, 10, 5); - walk1[4] = new PVector(12, 10, 0); - walk1[5] = new PVector(12, 10, -5); - - PVector P = new PVector(); - - P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); - - if (false) - { - P.x=10; - P.y=15; - P.z=0; - } - - return P; -} - -PVector animationSubSteps(PVector from, PVector to) -{ - PVector newStep = new PVector(); - int steps=5; - - if (animationSubStep < steps) - { - float differenceX = (to.x - from.x) / steps; - float differenceY = (to.y - from.y) / steps; - float differenceZ = (to.z - from.z) / steps; - - newStep.x = from.x + ( differenceX * animationSubStep ); - newStep.y = from.y + ( differenceY * animationSubStep ); - newStep.z = from.z + ( differenceZ * animationSubStep ); - } else - { - animationSubStep=0; - animationStep++; - newStep = to; - } - - return newStep; -} - -void Vertex(double x, double y, double z) -{ - vertex((float)x, (float)y, (float)z); -} - -class Custom -{ - int time; - boolean start=true; - public boolean delay(int delay) - { - if (start) - { - time = millis(); - start = false; - } - if (millis() - time >= delay) - { - start = true; - return true; - } else - return false; - } -} - -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; - - if (leftLegorRight) - { // Right - X = (-1*X) + TranslateX; - Z = (-1*Z) + TranslateZ; - } else - { - X = Xi + TranslateX; - Z = (-1*Z) + TranslateZ; - } - - RotateY(); - 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()))); - 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; - } - } + DrawBody(); }