Ok I believe I found a flaw in the sytem.
not really a problem that can not be solve, but it requires a refactoring.
Basically the inverse dynamic solver is writing to solve constraints in global space.
This is fine as long as the targers are independent of the body positions.
For example the six axis demo is fixed to the world, so local and global targets are identical. When the root is a floating body this is not the case.
For a floating root the moment any part of the configuration is used to calculate a target, you get into a mathematical regression.
This is equivalent to taking a child bone of an articulated body and use its rotation to calculate the parent rotation.
Imagine the parent is a zero yaw angle and the child at one degree from the parent.
After one step if you say the parent is now at the child rotation in global space this means that the child moves to two degrees and so on. This is not good.
One solution is to reformulated the solver so that it works in local space.
Basically what this means is that the mass matrix is transform by the inverse
of the root matrix, and the root is not allowed to be constrained.
It is eassy to say but it takes some work to get it done, plus the demos has to be rewriting.
I will commit what I have as work in progress so that you guys can see what I mean,
It will look like a bug, but it is a wip.
I did not envision this recursion problem, but it is like they say, the devil is in the detail
I would not find out if we did not have the parts that we have already.