I'm working on a prototype simulator in which 3d entities move and collide. Collisions can be totally elastic or totally inelastic. The collision detection is demanded to V-Clip which returns the normal to the collision and the closest points (contact points) and features.
I've searched the web for a collision response algorithm and the impulse-based one seems perfectly suited for my purposes. Here started the madness for me, since I've found different formulas and my geometry skills are not that high. I have tried two of formulas I've found buth seems that neither of them work properly. I've searched for ideas about what I'm missing but ended up writing this post. Thus, sorry for the over-abused argument!
The implementation I've tried are based on this approach (in particular the "Code" section, rearranged for Java) and the wikipedia one.
Applying both I have strange behaviours even in basic simple cases. Consider for instance two cubes side by side. One simulate a wall, the other moves again the second with velocity (5,0,0). In a correct setting I would have a final velocity of (-5,0,0) for the moving object. Instead, I have values like (11,0,0) or (-0.8428571428571425, 0.0, 0.0) like I'm messing up things.
I've noticed that, despite correctly returning the faces of the cubes for such simple collision, V-Clip returns a vertex for each face (not the center of the face) as the collision point. Thus the vector offset is calculate w.r.t. a vertex instead of the center of the face. If I select (manually) as contact points the center of the shapes, the algorithm works correctly returning the right values. Since this behaviour seemed strange, I've tried another configuration: I rotated the first cube by 45° x-axis, z-axis so that the nearest point would be a vertex. In this case V-Clip returned the vertex coordinates (almost) and the corresponding point on the face (not a vertex!) with an (almost) coherent final velocity:
starting velocity: (5.9, 0.0, 0.0)
final velocity: (-5.977962556993951, 0.0, 0.0)
Is that the problem? I mean, is the first one a degenerative cases inherited by V-Clip way of working or I just obtained the right values by chance?? in other words, should I calculate the "correct point" to which apply the response? In the attempt to answer such question(s), I attach the Java code for the second implementation (the one based on wikipedia). I've added comments, so that it should be understandable. Inertia tensor is returned by V-Clip in local frame, so I multiply the rotation matrix for the local frame Inertia tensor and then the result is multiplyed per the transpose of the rotation matrix. That should be the right way. In the final part, no operation is executed on the velocity of the second object as it is supposed be a wall (not moving, mass infinite). "Ptree1" and "Ptree2" are two PolyTree data structures for entity A and entity B, which contains features and transformation matrices from local frame to global frames, inertia tensor, center of mass and so on.
Any help, any clue, anything is really appreciated! Thanks in advance and sorry for the long post (I've tried to be as descriptive as possible).
// INERTIA TENSOR1 = rot I_1_body rotT (Baraff) Matrix3d m1 = new Matrix3d(); Matrix3d rot = new Matrix3d(); ptree1.inertiaTensor(m1); ptree1.getTransform().get(rot); Matrix3d rotT = new Matrix3d(rot); rotT.transpose(); m1.mul(rot, m1); m1.mul(m1, rotT); // // INERTIA TENSOR2 = rot I_2_body rotT (Baraff) Matrix3d m2 = new Matrix3d(); Matrix3d rot2 = new Matrix3d(); ptree2.inertiaTensor(m2); ptree2.getTransform().get(rot2); Matrix3d rotT2 = new Matrix3d(rot2); rotT.transpose(); m2.mul(rot2, m2); m2.mul(m2, rotT2); // // SUPPOSITIONS: // PTREE1 - mass 1, velocity (defined previously) // PTREE2 - mass +inf velocity (0,0,0) // // COR and other data double e = 1; double ma = 1; Vector3d vel2 = new Vector3d(); // motionless double mb = Double.POSITIVE_INFINITY; //Ra --> contact point offset w.r.t. center (center calculated in the local coordinates system and then translated) Vector3d center1 = new Vector3d(); ptree1.centerOfMass(center1); center1.x += ptree1.getTransform().m03; center1.y += ptree1.getTransform().m13; center1.z += ptree1.getTransform().m23; // Vector3d Ra = new Vector3d(drep.getClosestPair().pnt1); // contact point in global coordinates Ra.sub(center1); // final offset for A //Rb --> contact point offset w.r.t. center (center calculated in the local coordinates system and then translated) Vector3d center2 = new Vector3d(); ptree2.centerOfMass(center2); center2.x += ptree2.getTransform().m03; center2.y += ptree2.getTransform().m13; center2.z += ptree2.getTransform().m23; // Vector3d Rb = new Vector3d(drep.getClosestPair().pnt2); // contact point in global coordinates Rb.sub(center2); // final offset for B // // calculate denominator of impulse j // I_a-1 (Ra cross nrm) cross Ra m1.invert(); Vector3d nrm = new Vector3d(); nrm.normalize(drep.getClosestPair().nrml); //normal to the collision ---> returned by V-Clip Vector3d angularVelChangea = new Vector3d(); angularVelChangea.cross(Ra, nrm); m1.transform(angularVelChangea); Vector3d vaLinDueToR = new Vector3d(); vaLinDueToR.cross(angularVelChangea, Ra); // I_b-1 (Rb cross nrm) cross Rb m2.invert(); Vector3d angularVelChangeb = new Vector3d(); angularVelChangeb.cross(Rb, nrm); m2.transform(angularVelChangeb); Vector3d vbLinDueToR = new Vector3d(); vbLinDueToR.cross(angularVelChangeb, Rb); // // final denominator following Wikipedia formulation (the dot product is split since it is distributive) double scalar = 1 / ma + 1 / mb + vaLinDueToR.dot(nrm) + vbLinDueToR.dot(nrm); // // calculating numerator following wikipedia ---> -(1 + e) ((v_b + omega_b cross Rb) - (v_a + omega_a cross Ra)) dot nrm Vector3d vellDiff = new Vector3d(); Vector3d omegaCrossRa = new Vector3d(); omegaCrossRa.cross(rottt, Ra); // rottt is the angular velocity of A in radiants Vector3d velCopy = new Vector3d(vel); // vel is the velocity of A copied to calcuate its sum with the cross product of angular velocity with Ra! velCopy.add(omegaCrossRa); //NOTHING for vel2 ---> it is motionless! vellDiff.sub(vel2, velCopy); double Jmod = -(e + 1) * vellDiff.dot(nrm) / scalar; //here the final j // //calculate inpulse for A and B Vector3d J = new Vector3d(nrm); //set the normal J.scale(Jmod); // product between normal and j // Vector3d ja = new Vector3d(J); Vector3d jb = new Vector3d(J); ja.scale(1 / ma); // product the mass jb.scale(1 / mb); // product the mass // new velocities vel.sub(ja); vel2.add(jb);