Finish Hinge Joint

I fixed some problems over the weekend. Now the simulation can proceed normally except when the random errors are introduced.

To-do list:

1. Fix the remaining problems.
2. Implement Baraff's Linear-time Lagrange solver.
3. Extend this progress to the case of piston and crank with trimesh models.

Some handy translations:

dCROSSMAT(M,a,s,-,+);
-> M.assign(new double[][]{{0, a.z, -a.y},
                           {-a.z, 0, a.x},
                           {a.y, -a.x, 0}});

dMULTIPLY0_331(a,M,b);
-> M.transform(b,a);

dMULTIPLY1_331(a,M,b);
-> M1.transpose(M); M1.transform(b,a);

dQMultiply0(q1,q2,q3);
-> q1.mul(q2,q3);

dQMultiply1(q1,q2,q3);
-> q1.inverse(q2); q1.mul(q3);

dQMultiply2(q1,q2,q3);
-> q1.mulInverse(q2,q3);

dQMultiply3(q1,q2,q3);
-> q1.inverse(q2); q1.mulInverse(q3);