Split leg and body logic into FootClass and HexapodClass
Imported from 'hexapod 3.rar'.
This commit is contained in:
mangavalk committed
1 parent
c977b3acab
commit
00bf3187fe
3 files changed
+499
-455
No files matched your search
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user