Today was a productive day.
Here is the recipe to implement Velocity Verlet in Chipmunk.
1. Define Two new integration functions
Code: Select all
void ChargedBodyUpdateVelocityVerlet(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
body->v = cpvadd(body->v, cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), 0.5e0*dt));
body->w = body->w + body->t*body->i_inv*0.5e0*dt;
body->f=cpvzero;
body->t=0.0e0;
cpArray *bodies = space->bodies;
cpBody* B;
cpVect delta,f;
Sing *aux=(Sing*)body->data;
cpFloat r2;
for(int i=0; i< bodies->num; i++)
{
B=(cpBody*)bodies->arr[i];
Sing *aux2=(Sing*)B->data;
if(!(B==body))
{
// Calculate the forces between the singularities of different bodies
for (int j=0; j<aux->Nsing; j++)
{
for (int k=0; k<aux2->Nsing; k++)
{
delta = cpvsub(aux2->Gpos[k],aux->Gpos[j]);
r2 = cpvlengthsq(delta);
f=cpvmult(cpvnormalize(delta),
-aux->sing[j].value*aux2->sing[k].value/r2);
//Force applied to body A
delta = cpv(aux->sing[k].position.x,aux->sing[k].position.y);
cpBodyApplyForce(body,f,delta);
}
}
}
}
body->v = cpvadd(cpvmult(body->v,damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), 0.5e0*dt));
body->w = body->w*damping + body->t*body->i_inv*0.5e0*dt;
}
void ChargedBodyUpdatePositionVerlet(cpBody *body, cpFloat dt)
{
cpVect dp = cpvmult(cpvadd(body->v, body->v_bias), dt);
dp = cpvadd(dp,cpvmult(cpvmult(body->f,body->m_inv),0.5e0*dt*dt));
body->p = cpvadd(body->p, dp);
cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt
+ 0.5*body->t*body->i_inv*dt*dt);
// Update position of the singularities
aux = (Sing*)body->data;
for (int i=0; i<aux->Nsing; i++)
aux->Gpos[i]=cpvadd(body->p,cpvrotate(cpv(aux->sing[i].position.x,
aux->sing[i].position.y), body->rot));
body->v_bias = cpvzero;
body->w_bias = 0.0f;
}
1b. The definition of the algorithm can be found here:
http://en.wikipedia.org/wiki/Verlet_integration
I checked it and the total energy is pretty well conserved (except in collisions). The order of the algortihm is something like O(dt^3.4778). It should be O(dt^4) but I guess the collisions add errors in the speed. So it must be O(dt^a, N^b), where N is some kind of average number of colliding shapes or the number of total collisions.
Enjoy