[ODE] Developing a Physics Engine!

[Yashar Moradbakhti]

I am (with a friend) developing a physics engine as a university project 
and have currently realized that my mathematical approaches do not work 
entirely. I wonder if anyone could give some pointers on where I am 
going wrong and where to redirect my focus.

First of all, the engine does not handle articulated objects. There are 
only unliteral constraints involved.

My coordinates are [x (COM position), q (quaternion, orientation), P 
(linear momentum) and L (angular momentum)].  The state derivative is 
given by [v (COM velocity), qdot(0.5*w*q), F (all forces on body), T(all 
torques on body)].
Currently I support only forward euler and rk4 to step in time and 
evolve the differential equations. Which _sometimes_ do impose problems 
during the simulation. I believe this has to do with that these methods 
do not guarantee the correct and legal states when bodies are in 
contact. For example I have situations where a body is in contact at a 
point with relative normal distance, velocity and acceleration which are 
satisfying the nonpenetration constraints. However after a step (say 
dt=0.05, ie big step) with FE or RK4 the bodies will still 
interpenetrate even though correct forces and impulses have been 
applied. This is not so strange since it is most possible that bodies 
will change velocities during a big step. Consequently my algorithm will 
step "back in time" with halfing the step size (bisection) but at worst 
this will halt the simulation since this will be done _a lot_.  However 
I don't want to lower my step which will slow everything down but I 
really do not want my simulations to halt either.  I am now trying to 
figure out if there is a way to include a backward euler scheme which 
guarantees a correct position, velocity and acceleration at the end of 
the step. But to solve  nonlinear equations does not seem feasible and I 
really do not now where to start and how to set these equations up. 
Would like to have some pointers on what papers to read and if it is a 
good idea at all to implement such a scheme.

Also for further notice I am basing the calculations of resting forces 
and collsion impulses with multiple contacts  on Baraffs LCP work. 
Somewhat like ODEs approach (I think). 
I have the following collision routines. One broadphase for culling away 
objects which cannot collide and one narrowphase for convex polyhedra 
based on a algorithm by Lin, based on voronoi regions. And a third for 
concave polyhedra.

The real bottlenecks seem to be the use of integration scheme with the 
lousy bisection method I am using; rk4 doesn't really make it. I need a 
first order scheme such as FE but stabillity breaks with it. The current 
collision solutions really do not let me use anything else than 
bisection since they do not provide any information after the collision. 
They could but that would not be feasible either...

Any help and pointers would help me a lot and I would really appreciate it!

Thanks,
Yashar