This is an object implementation of animating a chain of sticks depending on quaternions and physics laws ( rendering using my OpenGL Enigne, display it later if you want), sorry about this big chunk of code.
#include "Headers.h"
extern Chain Anchors;
extern _3DModel *ModelDraw3;
extern Renderer Renderor;
Vector GetCrossProduct(Vector U,Vector V)
{
Vector temp;
temp.x = U.y*V.z-V.y*U.z;
temp.y = -U.x*V.z+V.x*U.z;
temp.z = U.x*V.y-V.x*U.y;
return temp;
}
void Stick::InitializeSettings(float K,float D,float M,Vertex *Pos,Vertex *Normal)
{
DeltaT = 10;
Center.x = Center.y = Center.z = 0;
for(unsigned int i = 0; i<8; i++)
{
LocalCenter.x += Pos[i].x;
LocalCenter.y += Pos[i].y;
LocalCenter.z += Pos[i].z;
Particles[i].Position = Pos[i];
Particles[i].Mass = M;
}
for(unsigned int i = 0; i<6; i++)
Particles[i].Normal = Normal[i];
LocalCenter.x /= 8;
LocalCenter.y /= 8;
LocalCenter.z /= 8;
a.x = (Pos[4].x + Pos[5].x + Pos[6].x + Pos[7].x)/4;
a.y = (Pos[4].y + Pos[5].y + Pos[6].y + Pos[7].y)/4;
a.z = (Pos[4].z + Pos[5].z + Pos[6].z + Pos[7].z)/4;
b.x = (Pos[0].x + Pos[1].x + Pos[2].x + Pos[3].x)/4;
b.y = (Pos[0].y + Pos[1].y + Pos[2].y + Pos[3].y)/4;
b.z = (Pos[0].z + Pos[1].z + Pos[2].z + Pos[3].z)/4;
olda = a;
oldb = b;
SpringCoefficient = K;
DampingCoefficient = D;
CurrentRotation.Clear();
CurrentIinv = Matrix16::GetInverse4x4(GetCurrentI());
}
float Stick::ComputeTotalMass()
{
float M = 0;
for(unsigned int i = 0; i<8; i++)
M += Particles[i].Mass;
return M;
}
void Stick::RenderStick()
{
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glBegin(GL_QUADS);
glColor3f(1,1,1);
glNormal3f(Particles[0].Normal.x,Particles[0].Normal.y,Particles[0].Normal.z);
glVertex3f(Particles[0].Position.x,Particles[0].Position.y,Particles[0].Position.z); glVertex3f(Particles[1].Position.x,Particles[1].Position.y,Particles[1].Position.z);
glVertex3f(Particles[2].Position.x,Particles[2].Position.y,Particles[2].Position.z); glVertex3f(Particles[3].Position.x,Particles[3].Position.y,Particles[3].Position.z);
glNormal3f(Particles[1].Normal.x,Particles[1].Normal.y,Particles[1].Normal.z);
glVertex3f(Particles[2].Position.x,Particles[2].Position.y,Particles[2].Position.z); glVertex3f(Particles[3].Position.x,Particles[3].Position.y,Particles[3].Position.z);
glVertex3f(Particles[7].Position.x,Particles[7].Position.y,Particles[7].Position.z); glVertex3f(Particles[6].Position.x,Particles[6].Position.y,Particles[6].Position.z);
glNormal3f(Particles[2].Normal.x,Particles[2].Normal.y,Particles[2].Normal.z);
glVertex3f(Particles[0].Position.x,Particles[0].Position.y,Particles[0].Position.z); glVertex3f(Particles[1].Position.x,Particles[1].Position.y,Particles[1].Position.z);
glVertex3f(Particles[5].Position.x,Particles[5].Position.y,Particles[5].Position.z); glVertex3f(Particles[4].Position.x,Particles[4].Position.y,Particles[4].Position.z);
glNormal3f(Particles[3].Normal.x,Particles[3].Normal.y,Particles[3].Normal.z);
glVertex3f(Particles[1].Position.x,Particles[1].Position.y,Particles[1].Position.z); glVertex3f(Particles[2].Position.x,Particles[2].Position.y,Particles[2].Position.z);
glVertex3f(Particles[6].Position.x,Particles[6].Position.y,Particles[6].Position.z); glVertex3f(Particles[5].Position.x,Particles[5].Position.y,Particles[5].Position.z);
glNormal3f(Particles[4].Normal.x,Particles[4].Normal.y,Particles[4].Normal.z);
glVertex3f(Particles[0].Position.x,Particles[0].Position.y,Particles[0].Position.z); glVertex3f(Particles[3].Position.x,Particles[3].Position.y,Particles[3].Position.z);
glVertex3f(Particles[7].Position.x,Particles[7].Position.y,Particles[7].Position.z); glVertex3f(Particles[4].Position.x,Particles[4].Position.y,Particles[4].Position.z);
glNormal3f(Particles[5].Normal.x,Particles[5].Normal.y,Particles[5].Normal.z);
glVertex3f(Particles[4].Position.x,Particles[4].Position.y,Particles[4].Position.z); glVertex3f(Particles[5].Position.x,Particles[5].Position.y,Particles[5].Position.z);
glVertex3f(Particles[6].Position.x,Particles[6].Position.y,Particles[6].Position.z); glVertex3f(Particles[7].Position.x,Particles[7].Position.y,Particles[7].Position.z);
glEnd();
glPushMatrix();
glTranslatef(LocalCenter.x,LocalCenter.y,LocalCenter.z);
glScalef(1.5,1.5,1.5);
Renderor.Draw(*ModelDraw3);
glPopMatrix();
}
Vector Stick::ComputeTotalForcesAtA(Vertex bminus1,Vertex oldbminus1)
{
Vector AVelocity;
Vector BVelocity;
DeltaT = 0.1f;
AVelocity.x = (a.x - olda.x)/DeltaT;
AVelocity.y = (a.y - olda.y)/DeltaT;
AVelocity.z = (a.z - olda.z)/DeltaT;
BVelocity.x = (bminus1.x - oldbminus1.x)/DeltaT;
BVelocity.y = (bminus1.y - oldbminus1.y)/DeltaT;
BVelocity.z = (bminus1.z - oldbminus1.z)/DeltaT;
Vector Dt(bminus1.x - a.x,bminus1.y - a.y,bminus1.z - a.z);
Dt.SetOpposite();
Vector DampingForce = Forces.ComputeDampingForce(DampingCoefficient,AVelocity,BVelocity);
Vector GravityForce = Forces.ComputeGravityForce(ComputeTotalMass());
Vector SpringForce = Forces.ComputeSpringForce(SpringCoefficient,Dt);
Vector TotalForces(SpringForce.x+GravityForce.x+DampingForce.x,
SpringForce.y+GravityForce.y+DampingForce.y,
SpringForce.z+GravityForce.z+DampingForce.z);
return TotalForces;
}
Vector Stick::ComputeTotalForcesAtB(Vertex aplus1,Vertex oldaplus1,float DampingCoefficientplus1,float SpringCoefficienttplus1)
{
Vector AVelocity;
Vector BVelocity;
DeltaT = 0.1f;
AVelocity.x = (aplus1.x - oldaplus1.x)/DeltaT;
AVelocity.y = (aplus1.y - oldaplus1.y)/DeltaT;
AVelocity.z = (aplus1.z - oldaplus1.z)/DeltaT;
BVelocity.x = (b.x - oldb.x)/DeltaT;
BVelocity.y = (b.y - oldb.y)/DeltaT;
BVelocity.z = (b.z - oldb.z)/DeltaT;
Vector Dt(aplus1.x - b.x,aplus1.y - b.y,aplus1.z - b.z);
Dt.SetOpposite();
Vector DampingForce = Forces.ComputeDampingForce(DampingCoefficientplus1,AVelocity,BVelocity);
Vector GravityForce = Forces.ComputeGravityForce(ComputeTotalMass());
Vector SpringForce = Forces.ComputeSpringForce(SpringCoefficienttplus1,Dt);
Vector TotalForces(SpringForce.x+GravityForce.x+DampingForce.x,
SpringForce.y+GravityForce.y+DampingForce.y,
SpringForce.z+GravityForce.z+DampingForce.z);
return TotalForces;
}
Vector Force::ComputeDampingForce(float D, Vector Va, Vector Vb)
{
Vector Damper;
Damper.x = -D*(Va.x-Vb.x);
Damper.y = -D*(Va.y-Vb.y);
Damper.z = -D*(Va.z-Vb.z);
return Damper;
}
Vector Force::ComputeGravityForce(float M)
{
float GForce = 9.8f;
Vector Gravity;
Vector Direction(0,-1,0);
Gravity.x = 0.5f*M*GForce*Direction.x;
Gravity.y = 0.5f*M*GForce*Direction.y;
Gravity.z = 0.5f*M*GForce*Direction.z;
return Gravity;
}
void Stick::UpdatePosition(float x,float y, float z)
{
for(unsigned int i = 0; i<8; i++)
{
Particles[i].Position.x += x;
Particles[i].Position.y += y;
Particles[i].Position.z += z;
}
a.x = (Particles[4].Position.x + Particles[5].Position.x + Particles[6].Position.x + Particles[7].Position.x)/4;
a.y = (Particles[4].Position.y + Particles[5].Position.y + Particles[6].Position.y + Particles[7].Position.y)/4;
a.z = (Particles[4].Position.z + Particles[5].Position.z + Particles[6].Position.z + Particles[7].Position.z)/4;
b.x = (Particles[0].Position.x + Particles[1].Position.x + Particles[2].Position.x + Particles[3].Position.x)/4;
b.y = (Particles[0].Position.y + Particles[1].Position.y + Particles[2].Position.y + Particles[3].Position.y)/4;
b.z = (Particles[0].Position.z + Particles[1].Position.z + Particles[2].Position.z + Particles[3].Position.z)/4;
olda = a;
oldb = b;
}
Vector Force::ComputeSpringForce(float K, Vector CurrentDisplacement)
{
Vector SpringForce;
SpringForce.x = -K*CurrentDisplacement.x;
SpringForce.y = -K*CurrentDisplacement.y;
SpringForce.z = -K*CurrentDisplacement.z;
return SpringForce;
}
Vector Stick::GetTotalForce(Vertex aplus1,Vertex oldaplus1,Vertex bminus1,Vertex oldbminus1,float DampingCoefficientplus1,float SpringCoefficienttplus1)
{
Vector F;
F.x = ComputeTotalForcesAtB(aplus1,oldaplus1,DampingCoefficientplus1,SpringCoefficienttplus1).x + ComputeTotalForcesAtA(bminus1,oldbminus1).x;
F.y = ComputeTotalForcesAtB(aplus1,oldaplus1,DampingCoefficientplus1,SpringCoefficienttplus1).y + ComputeTotalForcesAtA(bminus1,oldbminus1).y;
F.z = ComputeTotalForcesAtB(aplus1,oldaplus1,DampingCoefficientplus1,SpringCoefficienttplus1).z + ComputeTotalForcesAtA(bminus1,oldbminus1).z;
return F;
}
Vector Stick::GetTotalTorque(Vertex aplus1,Vertex oldaplus1,Vertex bminus1,Vertex oldbminus1,float DampingCoefficientplus1,float SpringCoefficienttplus1)
{
Vector T,Ta,Tb;
Vector TempA(a.x-Center.x,a.y-Center.y,a.z-Center.z);
Vector TempB(b.x-Center.x,b.y-Center.y,b.z-Center.z);
Ta = GetCrossProduct(TempA,ComputeTotalForcesAtB(aplus1,oldaplus1,DampingCoefficientplus1,SpringCoefficienttplus1));
Tb = GetCrossProduct(TempB,ComputeTotalForcesAtA(bminus1,oldbminus1));
T.x = Ta.x + Tb.x;
T.y = Ta.y + Tb.y;
T.z = Ta.z + Tb.z;
return T;
}
Matrix16 Stick::GetCurrentI()
{
Matrix16 I;
float M = ComputeTotalMass();
I.Ma[0] = M*(Particles[0].Position.y*Particles[0].Position.y + Particles[0].Position.z*Particles[0].Position.z);
I.Ma[5] = M*(Particles[0].Position.x*Particles[0].Position.x + Particles[0].Position.z*Particles[0].Position.z);
I.Ma[10] = M*(Particles[0].Position.x*Particles[0].Position.x + Particles[0].Position.y*Particles[0].Position.y);
return I;
}
void Stick::ApplyAlgorithm(Vertex aplus1,Vertex oldaplus1,Vertex bminus1,Vertex oldbminus1,float DampingCoefficientplus1,float SpringCoefficienttplus1)
{
float M = ComputeTotalMass();
/////////////////////////////////////
Vector Force = GetTotalForce(aplus1,oldaplus1,bminus1,oldbminus1,DampingCoefficientplus1,SpringCoefficienttplus1);
Vector Torque = GetTotalTorque(aplus1,oldaplus1,bminus1,oldbminus1,DampingCoefficientplus1,SpringCoefficienttplus1);
float Dt = 0.1f;
/////////////////////////////////////
Vector PVelocity;
PVelocity.x = Force.x - TotalF.x;
PVelocity.y = Force.y - TotalF.y;
PVelocity.z = Force.z - TotalF.z;
TotalF = Force;
Vector LVelocity;
LVelocity.x = Torque.x - TotalT.x;
LVelocity.y = Torque.y - TotalT.y;
LVelocity.z = Torque.z - TotalT.z;
TotalT = Torque;
Pmomentum.x += PVelocity.x*Dt;Pmomentum.y += PVelocity.y*Dt;Pmomentum.z += PVelocity.z*Dt;
Lmomentum.x += LVelocity.x*Dt;Lmomentum.y += LVelocity.y*Dt;Lmomentum.z += LVelocity.z*Dt;
/////////////////////////////////////
Vector Temp(Lmomentum.x-TotalL.x,Lmomentum.y-TotalL.y,Lmomentum.z-TotalL.z);
Vector AngularVelocity = CurrentIinv*Lmomentum;
Vector V[3];
Vector U[3];
U[0].x = CurrentRotation.Ma[0];U[0].y = CurrentRotation.Ma[4];U[0].z = CurrentRotation.Ma[8];
U[1].x = CurrentRotation.Ma[1];U[1].y = CurrentRotation.Ma[5];U[1].z = CurrentRotation.Ma[9];
U[2].x = CurrentRotation.Ma[2];U[2].y = CurrentRotation.Ma[6];U[2].z = CurrentRotation.Ma[10];
V[0] = GetCrossProduct(AngularVelocity,U[0]);
V[1] = GetCrossProduct(AngularVelocity,U[1]);
V[2] = GetCrossProduct(AngularVelocity,U[2]);
Matrix16 RotationVelocity;
RotationVelocity.Ma[0] = V[0].x;RotationVelocity.Ma[4] = V[0].y;RotationVelocity.Ma[8] = V[0].z;
RotationVelocity.Ma[1] = V[1].x;RotationVelocity.Ma[5] = V[1].y;RotationVelocity.Ma[9] = V[1].z;
RotationVelocity.Ma[2] = V[2].x;RotationVelocity.Ma[6] = V[2].y;RotationVelocity.Ma[10] = V[2].z;
Vector CenterVelocity;
CenterVelocity.x = (1/M)*(Pmomentum.x-TotalP.x);
CenterVelocity.y = (1/M)*(Pmomentum.y-TotalP.y);
CenterVelocity.z = (1/M)*(Pmomentum.z-TotalP.z);
TotalP = Pmomentum;
TotalL = Pmomentum;
/////////////////////////////////////
Matrix16 RotationAcceleration = TotalRotationVelocity-RotationVelocity;
TotalRotationVelocity = RotationVelocity;
for(int j = 0; j<16; j++)
CurrentRotation.Ma[j] += RotationAcceleration.Ma[j];
Vertex CenterAcceleration(CenterVelocity.x - TotalCenterVelocity.x,CenterVelocity.y - TotalCenterVelocity.y,CenterVelocity.z - TotalCenterVelocity.z);
TotalCenterVelocity = CenterVelocity;
Center.x += CenterVelocity.x;
Center.y += CenterVelocity.y;
Center.z += CenterVelocity.z;
////////////////////////////////////
CurrentIinv = CurrentRotation*CurrentIinv*CurrentRotation.GetTranspose(CurrentRotation);
for(unsigned int i = 0; i<8; i++)
{
/////////////////////////////////////
Particles[i].Position.x -= LocalCenter.x;Particles[i].Position.y -= LocalCenter.y;Particles[i].Position.z -= LocalCenter.z;
Particles[i].Position = Particles[i].Position*CurrentRotation;
Particles[i].Position.x += LocalCenter.x;Particles[i].Position.y += LocalCenter.y;Particles[i].Position.z += LocalCenter.z;
Particles[i].Position.x += Center.x;
Particles[i].Position.y += Center.y;
Particles[i].Position.z += Center.z;
}
a.x = (Particles[4].Position.x + Particles[5].Position.x + Particles[6].Position.x + Particles[7].Position.x)/4;
a.y = (Particles[4].Position.y + Particles[5].Position.y + Particles[6].Position.y + Particles[7].Position.y)/4;
a.z = (Particles[4].Position.z + Particles[5].Position.z + Particles[6].Position.z + Particles[7].Position.z)/4;
b.x = (Particles[0].Position.x + Particles[1].Position.x + Particles[2].Position.x + Particles[3].Position.x)/4;
b.y = (Particles[0].Position.y + Particles[1].Position.y + Particles[2].Position.y + Particles[3].Position.y)/4;
b.z = (Particles[0].Position.z + Particles[1].Position.z + Particles[2].Position.z + Particles[3].Position.z)/4;
LocalCenter.x = LocalCenter.y = LocalCenter.z = 0;
for(unsigned int i = 0; i<8; i++)
{
LocalCenter.x += Particles[i].Position.x;
LocalCenter.y += Particles[i].Position.y;
LocalCenter.z += Particles[i].Position.z;
}
LocalCenter.x /= 8;
LocalCenter.y /= 8;
LocalCenter.z /= 8;
}
void Chain::Simulate()
{
for(int i = 0; i<8;i++)
{
Sticks[i].olda = Sticks[i].a;
Sticks[i].oldb = Sticks[i].b;
}
Vertex aplus1,bminus1,oldaplus1,oldbminus1;
float Kplus1,Dplus1;
aplus1 = Sticks[1].a;
oldaplus1 = Sticks[1].olda;
bminus1 = Anchors.Sticks[0].b;
oldbminus1 = Anchors.Sticks[0].oldb;
Kplus1 = Sticks[1].SpringCoefficient;
Dplus1 = Sticks[1].DampingCoefficient;
Sticks[0].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[2].a;
oldaplus1 = Sticks[2].olda;
bminus1 = Sticks[0].b;
oldbminus1 = Sticks[0].oldb;
Kplus1 = Sticks[2].SpringCoefficient;
Dplus1 = Sticks[2].DampingCoefficient;
Sticks[1].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[3].a;
oldaplus1 = Sticks[3].olda;
bminus1 = Sticks[1].b;
oldbminus1 = Sticks[1].oldb;
Kplus1 = Sticks[3].SpringCoefficient;
Dplus1 = Sticks[3].DampingCoefficient;
Sticks[2].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[4].a;
oldaplus1 = Sticks[4].olda;
bminus1 = Sticks[2].b;
oldbminus1 = Sticks[2].oldb;
Kplus1 = Sticks[4].SpringCoefficient;
Dplus1 = Sticks[4].DampingCoefficient;
Sticks[3].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[5].a;
oldaplus1 = Sticks[5].olda;
bminus1 = Sticks[3].b;
oldbminus1 = Sticks[3].oldb;
Kplus1 = Sticks[5].SpringCoefficient;
Dplus1 = Sticks[5].DampingCoefficient;
Sticks[4].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[6].a;
oldaplus1 = Sticks[6].olda;
bminus1 = Sticks[4].b;
oldbminus1 = Sticks[4].oldb;
Kplus1 = Sticks[6].SpringCoefficient;
Dplus1 = Sticks[6].DampingCoefficient;
Sticks[5].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Sticks[7].a;
oldaplus1 = Sticks[7].olda;
bminus1 = Sticks[5].b;
oldbminus1 = Sticks[5].oldb;
Kplus1 = Sticks[7].SpringCoefficient;
Dplus1 = Sticks[7].DampingCoefficient;
Sticks[6].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
aplus1 = Anchors.Sticks[1].a;
oldaplus1 = Anchors.Sticks[1].olda;
bminus1 = Sticks[6].b;
oldbminus1 = Sticks[6].oldb;
Kplus1 = Sticks[7].SpringCoefficient;
Dplus1 = Sticks[7].DampingCoefficient;
Sticks[7].ApplyAlgorithm(aplus1,oldaplus1,bminus1,oldbminus1,Dplus1,Kplus1);
}