I've been working on a dune buggy simulator for a uni game programming assignment. I can simulate single bodies pretty well and have a sphere rolling around a terrain with friction. I plan to use 4 spheres as wheels and I'm trying to connect these to a chassis. I got told by the subject tutor that most physics engines used constraints to connect rigid bodies but he doesn't know much about it and that led me to these forums. I've read Kenny's paper and have tried to build a lagrange multiplier constraint system in matlab to test it.
I seem to be having some troubles trying to model a ball and socket joint. At the moment my update loop looks like:
Code: Select all
for i=dt:dt:1
% calculate lambda for constraint forces
b = -J * (Minv * Fext);
A = J * Minv * J';
lambda = A\b
% update postion
u = u + Minv * (J'*lambda + Fext) * dt;
s = s + dt * S * u;
S = getS(s);
% recalculate jacobian
JiAng = -skewMatrix(quatToMatrix(s(4:7))*riAnc);
JjAng = skewMatrix(quatToMatrix(s(11:14))*rjAnc);
J = [JiLin JiAng JjLin JjAng];
plot(s(1), s(2), 'o');
plot(s(8), s(9), '+');
end
This plot is with a directly down force being applied on the left body with the two bodies being connected by a ball and socket joint. This looks incorrect to me as the positions are seperating when they started as far apart as the joint should allow.
I'm pretty sure I've set all the matrices up properly and have printed them out and check them a few times, so I was wondering if there any obvious errors in my update loop. Any help would be great! If I need to post and more code/information just let me know and I can throw it up.
Cheers,
Mark.