It's a 2 link chain "connected" with a revolute joint (no actual constraint on the parts), where 1 link is immobile, and the other swings freely under gravity. Collisions are off. Mass and inertia values are 1 to simplify calculations whilst I'm debugging.
I've attached a modified RagdollDemo.cpp file which demonstrates the problem.
During a timestep, I do the following:
- Calculate linear (v) and angular velocity (w) from joint velocity (qd).
- Use these to determine joint accelerations (qdd)
- Use the joint acceleration to calculate linear (a) and angular acceleration (alpha).
- v = v + a*timeStep, w = w + alpha*timeStep, qd = qd + qdd * step
- Set the body to have these velocities
- Step the simulation
The problem I have is that it will drift, in no time at all the bodies have separated.
To try and get a better grasp of what's going on, I did velocity integration myself, and setting rotations and positions, pretty much taking bullet out of the equation. This also left me explicit control over the substepping. I find the drift reduces with more substeps. The demo initially has 20 substeps on the featherstone algorithm (in clientMoveAndDisplay()).
In the demo you can toggle Bullet on (b) or use my integration (n) with key presses to see the difference in drift that I'm talking about (it also resets to the original position).
How can I increase the accuracy in the case of Bullet? I have tried substepping, but I am either not using it correctly, or there aren't enough substeps to reduce the drift, because I can't see an improvement. It's bad enough with a single link, it's going to be a mess within just a few seconds of simulation with a full character.
Any ideas?
Tom