Refactoring joints

[Julio Jerez]

Joe can you tell me the sizes mass of the boxes so that I can recreate the problem.
or better yet copy teh script code that regerate the bug so that I paste in one of teh demos.?

[JoeJ]

Tried this in the joints demo, but here the body falls just through. Is there something i have to do to enable collisions?

In my own app, after changing kinetic mass from 10000 to 10 the simple box (mass 1) sinks completely through as well, but i can see collision forces acting, just too weak.

Code: Select all

#if 1
   {
      dVector location (0.0f, 6.0f, 0.0f);
      dVector size (1.5f, 2.0f, 2.0f, 0.0f);

      NewtonWorld* const world = scene->GetNewton();
      int materialID =  NewtonMaterialGetDefaultGroupID (world);
      NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
         DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

      dFloat mass = 1.0f;
      dMatrix matrix (dGetIdentityMatrix());
      matrix.m_posit = location;
      matrix.m_posit.m_w = 1.0f;
      NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

      geometry->Release();
      NewtonDestroyCollision(collision);
   }
   {
      dVector location (0.0f, 3.0f, 0.0f);
      dVector size (1.5f, 2.0f, 2.0f, 0.0f);

      NewtonWorld* const world = scene->GetNewton();
      int materialID =  NewtonMaterialGetDefaultGroupID (world);
      NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
         DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

      dFloat mass = 1.0f;
      dMatrix matrix (dGetIdentityMatrix());
      matrix.m_posit = location;
      matrix.m_posit.m_w = 1.0f;
      //NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

            DemoMesh* const mesh = geometry;
         DemoEntity* const entity = new DemoEntity(matrix, NULL);
         scene->Append (entity);
         if (mesh) {
            entity->SetMesh(mesh, dGetIdentityMatrix());
         }
         //NewtonBody* body = CreateSimpleBody (scene->GetNewton(), entity, mass, matrix, collision, materialID);

         NewtonBody* const rigidBody = NewtonCreateKinematicBody (world, collision, &matrix[0][0]);

         NewtonBodySetMassProperties (rigidBody, mass, collision);
         NewtonBodySetUserData (rigidBody, entity);
         NewtonBodySetMaterialGroupID (rigidBody, materialID);
         NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
         NewtonBodySetTransformCallback (rigidBody, DemoEntity::TransformCallback);
         NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);

      geometry->Release();
      NewtonDestroyCollision(collision);
   }
#endif


[JoeJ]

I see you have a kinematic bodies demo try it - bad things happening

[Julio Jerez]

ah thanks, I pasted on the kinematic demo.

The kinematioc demo is broken for a diffrent reason. That is teh I commnetd out few think when we move form wxwidget to imgui.
I need to complet the refactroing of the demo. Thdi does no mean is right it still coild be wrong and because if is commnetd out I did no see the bug earlly.

I now paste you demo and it is quiet clear that there si a bug that nee to be fixed.
if you sync and you runn the demos it will paly and show it.
so I guss fixing thsi take priority.

unfortunally I has to go to work today, so I can only dedicate small times, I have to leave is abpout an hour.
in any case I will work on it tomorrow or when I have time when I come back.

[Julio Jerez]

ok I spend a lithe time and I see why they pass through each other, you forget to call

NewtonBodySetCollidable (rigidBody, true);

on kinematic bodies, they are all created as non collidable by default, I am not sure if that was a change, the end application has to explicitly set then collidable, whi made the like static.

Code: Select all

dgKinematicBody::dgKinematicBody()
   :dgBody()
{
   m_collidable = false;
   m_type = m_kinematicBody;
   m_rtti |= m_kinematicBodyRTTI;
}


I edit the test, can you sync and check it out
maybe you can add the test that actually made it fail.

[JoeJ]

Ah yes, i assumed i forgot that.
So adding this and setting top body mass to 100 reproduces the issue:

Code: Select all

#if 1
   {
      dVector location (0.0f, 6.0f, 0.0f);
      dVector size (1.5f, 2.0f, 2.0f, 0.0f);

      NewtonWorld* const world = scene->GetNewton();
      int materialID =  NewtonMaterialGetDefaultGroupID (world);
      NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
         DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

      dFloat mass = 100.0f;
      dMatrix matrix (dGetIdentityMatrix());
      matrix.m_posit = location;
      matrix.m_posit.m_w = 1.0f;
      NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

      geometry->Release();
      NewtonDestroyCollision(collision);
   }
   {
      dVector location (0.0f, 3.0f, 0.0f);
      dVector size (1.5f, 2.0f, 2.0f, 0.0f);

      NewtonWorld* const world = scene->GetNewton();
      int materialID =  NewtonMaterialGetDefaultGroupID (world);
      NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
         DemoMesh* const geometry = new DemoMesh("primitive", collision, "smilli.tga", "smilli.tga", "smilli.tga");

      dFloat mass = 1.0f;
      dMatrix matrix (dGetIdentityMatrix());
      matrix.m_posit = location;
      matrix.m_posit.m_w = 1.0f;
      //NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);

            DemoMesh* const mesh = geometry;
         DemoEntity* const entity = new DemoEntity(matrix, NULL);
         scene->Append (entity);
         if (mesh) {
            entity->SetMesh(mesh, dGetIdentityMatrix());
         }
         //NewtonBody* body = CreateSimpleBody (scene->GetNewton(), entity, mass, matrix, collision, materialID);

         NewtonBody* const rigidBody = NewtonCreateKinematicBody (world, collision, &matrix[0][0]);

         NewtonBodySetMassProperties (rigidBody, mass, collision);
         NewtonBodySetUserData (rigidBody, entity);
         NewtonBodySetMaterialGroupID (rigidBody, materialID);
         NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
         NewtonBodySetTransformCallback (rigidBody, DemoEntity::TransformCallback);
         NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);

         NewtonBodySetCollidable (rigidBody, true);

      geometry->Release();
      NewtonDestroyCollision(collision);
   }
#endif


[Julio Jerez]

Ok I set teh mass to 100, and this is what I get.

badImage.png (254.57 KiB) Viewed 6879 times

I slide the top box to the side a little so that we can see better what is going on, we see that the top box does sink into the lower box but I believe this is normal.
is possible that I made changes to the way the contact penalty is calculated.
in my next post I will post a picture of the equation so that we agree what is going on and we work with the equation.

are you saying this was different before?

[JoeJ]

are you saying thsi was different before?

Yes, before there was no sinking and all worked fine, but my previous version was few months old.

[Julio Jerez]

is possible I change some stuff on the contact, but let us see if we indenstand whet is happing
I wrote on a piece of paper the derivation of teh equation of constrained motion for two objects resting on each other.

forces.png (5.21 MiB) Viewed 6879 times

to make simple I assume two sphere so that the contact goes over the center of mass.
and all the inertial terms are zero. it is also a one dimentional vector so we do no have to write vecto in 3d since x and z is zero
The picture is too high resolution, you may wnat to download it an watch in paint or some other program.

The important equation are eq 1 and 10 which I will write here.

1) M * a = Fe + Jt * l
10) J * inv(M) * Jt * l = - J * inv(M) * Fet - k0 * vr - k1 * he

here J is the jacobian: J = [1 -1]
mass is a 2 x 2 matrix
M = m0 0
0 m1
fe is the weight
F = [w w]

k0 is the velocity penealty and k1 is the penetration penalty

Let is solve three different problems;
1) two dynamics bodies of equal mass,
2) one dynamics body and the other a static body.
3) one dynamics body and the other a kinematic body.

basically this is simple substitution the body parameters on equation 10 to fine l and substituting l on equation 1 to find the acceleration for each body.
Basically this is all that the newton engine does, figure out a general equation, determine what can be use as a feature and provide and interface to teh user to set those parameters.

are you following this? is so we will solve thsi problem in three different post,
but first I need to know if this is clear.

we can actually replace the box in the demo for tow sphere and step over the code comparing teh result and see if they agree with the theory. this is what I do in many other problems.

edit:
if you sync, I change the demo to two spheres so that It reflects better the equations on the picture.

[JoeJ]

Sounds you wanna give me a math lesson
Ok, having weak math background my main issue is notation but also matrix usage.
But the problem is simple enough i could solve it for just 2 bodies, so might be able to learn a bit about that (but tomorrow, it's late...).

k0 is the velocity penealty and k1 is the penetration penalty

This is a bit unclear to me. So those are constants that define how aggresive collision forces will be. Do the come from a #define, from solver or from user parameters?

[Julio Jerez]

This is a bit unclear to me. So those are constants that define how aggressive collision forces will be. Do the come from a #define, from solver or from user parameters?

not complete they, this values should in fact be zero, for any thsi in whi the number of contacts form a nor ill condition matrix, or for case whe you nee to enforce some conation on teh contact, like the case of teh kinematic body collision. for now let us just assume they are zero, since for dynamics boies penetration and relative contact velocity is zero, so teh has no effect then we will see how the influence teh kinematic body.

I will quickly resolve case 1: two spheres of one of top of each other with non infinite mass (dynamic bodies) also we make equal mass for simplicity.
in this case with copy equations 1 and 10

Code: Select all

1) M * a = Fe + Jt * l
10) J * inv(M) * Jt * l = - J * inv(M) * Fet - k0 * vr - k1 * he
 

here
Fe = [ w w]
j = [1 -1]
M = m 0
0 m

equation 10 becomes

Code: Select all

[1 - 1] * [1/m   0] * [1 -1] * l = -[1 -1] * [1/m 0]  * [w w] - k0 * 0 - k1 * 0
          [0   1/m]                          [0 1/m]

(1/m + 1/m) * l = [w/m - w/m] = 0
l = 0

the magnitude of the reaction force is zero. so equation 1 becomes

M * a = Fe + J * 0

and each body fall with the acceleration of gravity

a0 = (w + 1 * (0))  (1/m)  =  w * (1/m) = -g
a1 = (w - 1 * (0)) * (1/m) =  w * (1/m) = -g

which that is what we expect for such simple problem.


next we will make the body on the bottom be a static body.

[Julio Jerez]

ok now let us solve for a dynamic body falling on top of a static body

again we use eq 1 and 10, but here the conditions are
mass of the bottom body is infinite so it invers mas is zero, thi s

Fe = [ w w]
j = [1 -1]
invM = 1/m 0
0 0

equation 10 becomes

Code: Select all

[1 - 1] * [1/m   0] * [1 -1] * l = -[1 -1] * [1/m 0]  * [w w] - k0 * 0 - k1 * 0
          [0     0]                          [0  0]

(1/m) * l = [w/m - 0] = -w/m0
l = w

the magnitude of the reaction force is the weight of the top body. so equation 1 becomes

a0 = (w + 1 * (-w))  ( 1/ m) = 0 * (1/m) = 0
a1 = (w - 1 * (-w)) * (0)  = 2 * w * 0 = 0

so both bodies remind a rest, which is what we expect for a body resting of top of a static body


next we will do the interesting part which is the kinematic body,

I now we state the definition for a kinematic body.

a Kinematic body in newton is a body that in equation 10 uses the mass on the left side of the equation, but ignored it on the right size of the equation.

This is, the left size of the equation determine the dynamics of the body and the right part of teh equation determine the geometry.
Therefore Kinematic bodies is a static bodies that behaves like a dynamics body.

We immediately see that if a kinematic body has an infinite mass, that makes it a static body, so these are uninteresting bodies because we already have static bodies.

Kinematic bodies are only interesting when they have non infinite mass and there are set to be collidable.

so with that definition mind we will solve the next example and you will see where the mistake is and we will also see that the is not bug either. maybe I change some default value but that the beauty of newton, every thong is govern but some analytical equation, and hack and heuristic are limited to the possible minimum to solve numerical limitations.
Sometime these excesses refresh my mind because is has being so long that I forget some thong how was set up.

Ok I will let you read those two first examples and let me know if you are with me, before posting the third example. because for the this example we will make use of teh K0 and k1 and you will see how the participate on the solution.

[JoeJ]

I wrote a little 1D simulator, later i will extend this to handle multiple bodies, i'm interested in how to calculate exact constraint forces which shouldn't be that hard...

So i put in your equations and it spits out correct numbers, but there are too much zeroes yet to show me the bigger picture.

I'm sure it's wrong to sum up both bodies for w and m like i do here, but this mistake would only show if external acceleration differs and so i'll keep it for now.

Code: Select all

if (bodies[i1].mass != 0)
      {
         float extAcc0 = gravity;
         float extAcc1 = gravity;
         float w = bodies[i0].mass * extAcc0 + bodies[i1].mass * extAcc1;
         float m = bodies[i0].mass + bodies[i1].mass;

         a0 = (w + 1 * (0)) * (1/m);// = w * (1/m) = -g;
         a1 = (w - 1 * (0)) * (1/m);// = w * (1/m) = -g;

         base_debug::logA->PrintV ("Dyn-Dyn  a0 %f  a1 %f", a0, a1);
      }


[Julio Jerez]

if the bodies has different masses or they are subjected to different forces the two equations provide for everything. we will see that in example three.

an we use teh definition of a newton Kinematic body
"Kinematic body in newton is a body that in equation 10 uses the mass on the left side of the equation, but ignored it on the right size of the equation."

so we copy equation 1 and 10 again

Code: Select all

1) M * a = Fe + Jt * l
10) J * inv(M) * Jt * l = - J * inv(M) * Fet - k0 * vr - k1 * he
 

in the example the dynamic body has mass 100 and the kinematic body has mass 1
we plug those numbers in equation 10 and we get.

here
Fe = [ w w]
j = [1 -1]
M = m 0
0 m

equation 10 becomes

Code: Select all

[1 - 1] * [1/100   0] * [1 -1] * l = -[1 -1] * [1/100 0]  * [w w] - k0 * 0 - k1 * 0
          [0   1/1]                          [0 0]

(1/100 + 1/1) * l = [w/100 - 0] = w/m0
l = 0.01 * w

teh reaction force is 1% of the weight, we can see that thsi will not be enough to stop the top body for falling

and each body fall with the acceleration of gravity

a0 = (w - 1 * (w * 0.01))  (1/100)  =  -0.99 * g
a1 = (w + 1 * (0)) * (0) =  0

you can see teh teh top body move with almist the force of gravity


as teh body kee gong down, the secudn teh penetation become larget and k0 * he staip to be a signifcna part, but teh penetaion he has to be so large because the detomination of the equation is so affected by teh kenimatoc body at all.

if we inceare the mass of teh kenamtiuon body by comparable to the top body, the
if yo uplug the value you will see lamd be 0.5 * w

and if we go teh oeth way making is 100 time that o fteh top body the the calcultion is reverse and the top nody will has an acceltion of 0.1 * g

which requires a very small penatration fo k1 * he to calcel it.

this make sence, because we defined a kinematoc body as a static body with mass, but of teh body is stastic that mean it sodul be heavy, bu the examnpel shpow the kenematoi boy to has mass 1 an dteh dynamics body to have mass 100, is like a like a one gram feather trying to stop and a kilo a falling rock of a kilogram

if yo urun the demo, and set the kynemation body mass to 100, yo uwill see a much bether result,
I suggect that you set iot to a somewha large value liek 400 or somethong similar.

can you try that a let me know if is works, please ?

Basically Kinematic bodies are object that can be use to move around and whe the hit other objects that dissiplate momemtum by increasin the denomatination of teh equation us eit mass to reduce teh value of the reation force. the the resulve the penetation with he penalty.

[JoeJ]

Ok, first please look at my implementation:

Code: Select all

      if (bodies[i1].mass < 0) // using negative mass to identify kinetic body
      {
         float extAcc = gravity;
         float w0 = bodies[i0].mass * extAcc;
         //float w1 = 0;
         float m0 = bodies[i0].mass;
         float m1 = -bodies[i1].mass;

         a0 = (w0 - 1 * (w0 * m0/m1)) * (1/m1);//  =  -0.99 * g;    but i get 0.0099 * g
         a1 = (w0 + 1 * (0)) * (0);// =  0

         base_debug::logA->PrintV ("Dyn-Kin  a0 %f  a1 %f", a0, a1);
      }
   
      bodies[i0].acc = a0;
      bodies[i1].acc = a1;


Either i did it wrong, or you calculated -0.99 wrong.

But now i'm even more confused, because if this is all contact force you use, every body would sink at least with very low acceleration (intended or not).

But if i reverse the mass ratio in my sandbox demo the upper body stops and penetration seems zero.
Even if i place 10 boxes on top, they all stop. Is this because they fall a sleep?

If i change platform mass in my app from 10000 to 1000000, the ragdoll (mass of 75) still sinks in at the same speed. If i set low platform mass, the sinking becomes faster but i can't get rid of it. Maybe because the ragdoll can't sleep? (But then, why didn't it sink before?)

Edit:

which requires a very small penatration fo k1 * he to calcel it.

Ah, so the penetration penalty is missing from my implementation, which explains why the boxes stop. But for the ragdoll it does not work.