• Create Account

We're offering banner ads on our site from just \$5!

# BaCaRoZzo

Member Since 27 Feb 2012
Offline Last Active Mar 23 2012 02:26 AM

### Local to Global frame, translation and rotation

20 March 2012 - 06:05 PM

Hi all,

I've read a lot of stuff (also several thread here) but I'm a bit confused on the best choice (in term of simplicity and efficiency) to update the position of my simulated elements.
I've rigid shapes expressed in term of a local frame and a corresponding transformation matrix 4x4 which translates and rotates bodies from local frame to global one. I've also a constant translation velocity and a constant angular velocity (two vectors in m/s and rad/s) which are updated at certain intervals.

Now, if I want to update the position of a body I just need to update its rotation matrix and its translation vector in the 4x4 matrix. The translation vector can be easily updated:

`translation +=  vel * simulationStep`

The rotation matrix can be update independently (since in the simulation I've no constraint which related translation and rotation). Now, the first choice I've found is pretty easy: since I can concatenate two rotations by multiplying their matrices, I calculate the rotation matrix for the current angular velocity, in term of roll, pitch and yaw (my library has a function for this purpose) and then multiply the matrices. That is (warning: sort of pseudo-code!):

```vector3 rot;  //set to the scalar product of the angular velocity vector and the simulationStep
Matrix3 matrix;
matrix.setRpy(rot.z, rot.y, rot.x);  //create the matrix with the roll pitch and yam desired
matrix * rotationMatrix; //rotationMatrix is the current 3x3 rotation matrix in the 4x4 transformation
```

It is easy and it works (or at last it seems so). Using quaternion, i.e. converting the 3x3 matrix to a quaternion and work with it would be better of could I encounter a performance bottleneck in the conversion? My libraries relies on the 4x4 matrices thus I would end up converting two times (mat--> quat--> mat).

Baca.

### Collision Response and points of contact

27 February 2012 - 06:57 AM

Hi all,

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!
//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);