[ODE] Iterative solution

[david@csworkbench.com]

Hi,

Just wanted to present an update on how the Iterative solution is coming
and get some advice from people with more physics/math experience than me.

I've gotten to a decent level of functionality with the algorithm in
place.  Right now, I'm running at 5 iterations and simulating everything
believably.  The iterated algorithm is actually a bit slower (due to the 5
iterations) in the common case where you have lots of small islands of
bodies.... like 20 separate cars.  However, if those 20 cars all get in a
pileup, the iterated solution trucks along at the same speed, while the
old solution plummets to fractional framerates, and eventually segfaults
without an error (on my machine at least.... I think it has something to
do with compiling the operating system and all my packages with -O3
optimizations).

My next task is to optimize the dInternalStep routine with the knowledge
that it will always get 1 joint and either 1 or 2 bodies.  Here's the
question for the math/physics gurus:  Is the LCP solver even necessary if
you know beforehand that there is only one joint (up to 6 constraints, 1
per DOF I believe) in effect?  It seems like this should be the trivial
case for the solver, and that there should be a simpler, faster, and more
direct solution for this case, but I don't know enough about them to know
for sure (anyone have a link to a good paper on them?).

Another thing that's probably slowing it down a good bit is the fact that
collision detection has to be run before every iteration.  I'm trying to
think of ways to either only do collision detection once per set of
iterations, or maybe only call dCollide on pairs that were sent to the
nearCallback on the first iteration on subsequent iterations.

I'll start profiling after I put in the obvious optimizations tomorrow. 
Hopefully, with enough optimization, this algorithm will be the fastest
option in all but the most trivial cases.  If not, it's still possible to
run in normal mode most of the simulation, and only drop into iterated
mode for example when more than a set limit of contacts were generated.

One simulation I ran kind of surprised me.  There's a 20x20x20 block
falling out of the sky, and 25 four-wheeled cars (modified test_buggies)
arranged in a 5x5 grid, all just touching.  If the block hits the ground,
then the cars move forward in a group and run into the block, everything's
fine.  The iterated solution sails along, and the old solution craps as
expected.  However, if the cars move forward and get under the block
before it touches down, even the iterated solution slows down by a factor
of 10 or more (the old solution still craps).  I thought the iterated
solution shouldn't have this problem.

Let me know your thoughts,
David Whittaker