Forcing body to position/orientation

[Julio Jerez]

I think you can replicate this way:
Change your grab forde so it acts at body center of mass, not at pick hitpoint.
That's the real difference between our pick stuff!
I hope this helps...

Yes this reproduce it very eassy, thanks

[Julio Jerez]

Ok I found the bug, it is eassy to solve, but I want to think about a sound solution before I dod it. here is part of the bug.
this is the fucntion that calculates the capsule plane shape intersection for core 200

Code: Select all

dgInt32 dgCollisionCapsule::CalculatePlaneIntersection(const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
  dgInt32 count = 0;
  if (dgAbsf(normal.m_x) > dgFloat32(0.999f))
  {
    dgFloat32 x =(normal.m_x > dgFloat32(0.0f)) ? dgFloat32(1.0f) : dgFloat32(-1.0f);
    contactsOut[count] = dgVector(x, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
    count = 1;

  }
  else
  {
    dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
    dgFloat32 cosAng = normal.m_y * magInv;
    dgFloat32 sinAng = normal.m_z * magInv;
    _ASSERTE(dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
    dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
    dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,  origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
    dgPlane plane(normal1, -(normal1 % origin1));
    dgVector maxDir((normal1.m_x > dgFloat32(0.0f)) ? m_silhuette[3].m_x : -m_silhuette[3].m_x, (normal1.m_y > dgFloat32(0.0f)) ?  m_silhuette[3].m_y : -m_silhuette[3].m_y, dgFloat32(0.0f), dgFloat32(0.0f));

    dgFloat32 test0 = plane.Evalue(maxDir);
    dgFloat32 test1 = plane.Evalue(maxDir.Scale(dgFloat32(-1.0f)));
    if ((test0 * test1) > dgFloat32(0.0f))
    {
      test0 = plane.m_w + plane.m_x * m_height[0];
      if (dgAbsf(test0) < m_radius)
      {
        contactsOut[count] = normal1.Scale(-test0);
        contactsOut[count].m_x += m_height[0];
        count = 1;
      }
      else
      {
        test0 = plane.m_w - plane.m_x * m_height[0];
        if (dgAbsf(test0) < m_radius)
        {
          contactsOut[count] = normal1.Scale(-test0);
          contactsOut[count].m_x -= m_height[0];
          count = 1;
        }
      }
    }
    else
    {
      dgVector dp(m_silhuette[1] - m_silhuette[0]);
      dgFloat32 den = normal1 % dp;
      if (dgAbsf(den) > dgFloat32(0.0f))
      {
        test0 = -plane.Evalue(m_silhuette[0]) / den;
        if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
        {
          contactsOut[count] = m_silhuette[0] + dp.Scale(test0);
          count++;
        }
      }

      if (count < 2)
      {
        test0 = plane.m_w - plane.m_x * m_height[0];
        if (dgAbsf(test0) < m_radius)
        {
          dgFloat32 r = -m_height[0];
          dgFloat32 d = plane.m_w + r * plane.m_x;

          dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
          dgFloat32 b = dgFloat32(2.0f) * plane.m_x * d;
          dgFloat32 c = d * d - m_radius * m_radius * plane.m_y * plane.m_y;
          dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
          if (desc > dgFloat32(0.0f))
          {
            _ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
            desc = dgSqrt (desc);
            a = -dgFloat32(0.5f) * b / a;
            dgFloat32 x0 = a + desc;
            dgFloat32 x1 = a - desc;
            if (x0 > dgFloat32(0.0f))
            {
              x0 = x1;
            }
            if (x0 < 0.0f)
            {
              _ASSERTE(x0 <= dgFloat32 (0.0f));
              _ASSERTE(dgAbsf (plane.m_y) > dgFloat32 (0.0f));
              dgFloat32 y = -(plane.m_x * x0 + d) / plane.m_y;
              contactsOut[count] = dgVector(x0 + r, y, dgFloat32(0.0f),
                  dgFloat32(0.0f));
              count++;
            }
          }
        }
      }

      if (count < 2)
      {
        dgVector dp(m_silhuette[3] - m_silhuette[2]);
        den = normal1 % dp;
        if (dgAbsf(den) > dgFloat32(0.0f))
        {
          test0 = -plane.Evalue(m_silhuette[2]) / den;
          if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
          {
            contactsOut[count] = m_silhuette[2] + dp.Scale(test0);
            count++;
          }
        }
      }

      if (count < 2)
      {
        test0 = plane.m_w + plane.m_x * m_height[0];
        if (dgAbsf(test0) < m_radius)
        {
          dgFloat32 r = m_height[0];
          dgFloat32 d = plane.m_w + r * plane.m_x;

          dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
          dgFloat32 b = dgFloat32(2.0f) * plane.m_x * d;
          dgFloat32 c = d * d - m_radius * m_radius * plane.m_y * plane.m_y;
          dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
          if (desc > dgFloat32(0.0f))
          {
            _ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
            desc = dgSqrt (desc);
            a = -dgFloat32(0.5f) * b / a;
            dgFloat32 x0 = a + desc;
            dgFloat32 x1 = a - desc;
            if (x0 < dgFloat32(0.0f))
            {
              x0 = x1;
            }
            if (x0 > 0.0f)
            {
              _ASSERTE(x0 >= dgFloat32 (0.0f));
              _ASSERTE(dgAbsf (plane.m_y) > dgFloat32 (0.0f));
              dgFloat32 y = -(plane.m_x * x0 + d) / plane.m_y;
              contactsOut[count] = dgVector(x0 + r, y, dgFloat32(0.0f),
                  dgFloat32(0.0f));
              count++;
            }
          }
        }
      }
    }

    for (dgInt32 i = 0; i < count; i++)
    {
      dgFloat32 y = contactsOut[i].m_y;
      dgFloat32 z = contactsOut[i].m_z;
      contactsOut[i].m_y = y * cosAng - z * sinAng;
      contactsOut[i].m_z = z * cosAng + y * sinAng;
    }
  }
  return count;
}

this is the same function for core 300

Code: Select all

dgInt32 dgCollisionCapsule::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
   dgInt32 count = 0;
   if (dgAbsf (normal.m_x) > dgFloat32 (0.25f)) {
      contactsOut[0] = SupportVertex (normal);
      count = 1;
   } else {
      dgVector p0 (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
      dgVector dir0 (p0 - origin);
      dgFloat32 dist0 = dir0 % normal;
      if ((dist0 * dist0) < (m_radius * m_radius)) {
         contactsOut[count] = p0 - normal.Scale (dist0);
         count ++;
      }

      dgVector p1 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
      dgVector dir1 (p1 - origin);
      dgFloat32 dist1 = dir1 % normal;
      if ((dist1 * dist1) < (m_radius * m_radius)) {
         contactsOut[count] = p1 - normal.Scale (dist1);
         count ++;
      }
   }
   return count;
}

the second function will only be correct if the collision happen on the surface of the capsule;
what happen is that some time the plane a slightly tilted an only one point get generated.
the shape till even more, and next time get worse an worse.
in my test I do not notice because the forces we not persistence long enough to expose the bug. but now I see the reason.
this will also happen to other similar shapes, so I need to fix then all.

I am not sure if this is the cause of all other bug, but This definitely is a big one.
the good news is that it is no hard to solve.

[JoeJ]

Have you ever thought about using only one contact for capsules?
For me that worked in both cases dynamic capsule vs. other dynamic capsule and capsule vs. plane,
so it should als work for capsule vs. polyhedron.
In case capsule - capsule i used to calculate two points, and interpolated them depending on penetration depth and mass props.
I was surprised it worked as stable as two contacts, even for stacks of capsules, or a 'jenga'-tower of them.

[PJani]

JoeJ Thank you, thank you! This post will be very helpful I was abroad now for two days and i didn't have time to check anything on forums

[Julio Jerez]

Ok JoeJ I think I fixed the bug with the capsules now.

you say this also happen with cylinders and spheres, but i could not make it happens in the same box by dragging those shape from you export scene.
can you sync and test again, while I debug the other popping bug .

[JoeJ]

Yes it's fixed now.
Tested (unscaled) capsule, cylinder, box, polyhedron.
All well, at least for the force based interaction model.


With the joint it is still easy to drag anything into static box or sphere.
But i would not call that a bug, i saw it is there since i've implemented the joint.
It's a reason why i don't recommend joints for game interaction and prefer force.

Lowering the joint friction helps, but i loose the joint's advantage too.
At a value, where the collisions keep pretty correct, i can not lift up the ragdoll in a much more responsive way than with the force method alone.

Maybe you can work at that somewhere in the future - there's always room for improving robustnes in physics
I'd recommend that you impelment grab joint in your demo too, but not for public eyes.
Seems a nice tool for hardcore testing.

Thanks!

[Julio Jerez]

With the joint it is still easy to drag anything into static box or sphere.
But i would not call that a bug, i saw it is there since i've implemented the joint.
It's a reason why i don't recommend joints for game interaction and prefer force.

Lowering the joint friction helps, but i loose the joint's advantage too.
At a value, where the collisions keep pretty correct, i can not lift up the ragdoll in a much more responsive way than with the force method alone.

let us do this, I am working on the other penetration bug that bird provided, I already know what is causing it but the solution will take more than a day to get going.
This high energy bug has taking me a long time to figure out and I funally know what it is.
I fact it is not a bug it is a limitation of a descrete dynamics system where teh prosses is:
-calculate the the of a body (positionm and velocity)
-calculate contact
-calculaet contact and joint constraint derivatives
-solve for the internal reaction forces
-and integrate each body.

the problem is that this first order system do no take into account thet the integration can moev a body int a illigal position
where contacts can be genertae ill contition const joint.
this not too frequent but if you have a large stack of bodies that are bombarded by fast moving bodies the bug can happen quite frequently.
The solution for that is the continue collision solver.

so I will add three mode of continue collision: decrete collsion, adaptive continue collision, rigiruse continue collsion.
adaptive continue collision will be the default and I will work on tha this weekend.


Then after that I will copme back to this penetration bug when joint are acting on te hbody.
It is my beleive that if teh joint has proper fruiction limits, it should never penetrate another surface. At least I want to know why this is not so.
for know at least it is not worse than it was in core 200, and early core 300.

[JoeJ]

... sounds like a plan

[PJani]

Ok I am back to my project from some other android project.

I am working on joint which behaves like bottom two pictures, i cant remember how is this thing being called in English.



The main problem here is i need to make both bodies rotate like they were connected with this joint.

When i add new angular row(NewtonUserJointAddAngularRow) i can define only one pin. I am not sure if this pin is local to both bodies or being global to both?

[Julio Jerez]

you cna not make a singel joint that will generate tha motion, because it is no a simple joint bewee tow bodies, it is conetion three body, leftc shaft to teh center T small object and the cente T to teh right shaft

bascially it is quite eassy if you do that way too, make the center T body and conte to teh lkeft a righ shaft with simple hinges.

[JoeJ]

Ah... the 'Lego' Joint

That is close to the quaternion twist from my example.
I forgot to clear something out:

Code: Select all


sQuat q0, q1;
         BodyGetRotation (body0, q0); q0 *= localRot0; // same as 'cs0' in text above, but in quat form
         BodyGetRotation (body1, q1); q1 *= localRot1;

         // factor rotation about x axis: sQuat qt = q0.Inversed() * q1; float halfTwistAngle = atan (qt[0] / qt[3]);
         float twistAngle = 2.0 * atan (
            ( ( ( (q0[3] * q1[0]) + (-q0[0] * q1[3]) ) + (-q0[1] * q1[2]) ) - (-q0[2] * q1[1]) ) /
            ( ( ( (q0[3] * q1[3]) - (-q0[0] * q1[0]) ) - (-q0[1] * q1[1]) ) - (-q0[2] * q1[2]) ) );

// WARNING: i use quaternion form (x,y,z, w) - Julio uses (w, x,y,z) - BodyGetRotation does the conversation for me

sVec3 twistAxis = sVec3(dir0+dir1).Unit(); // use average axis



What i forgot is to explain what dir0+dir1 is.
Those are the axis build from the bodies matrices, the same as the 'dotted' axis lines as in your black and white image.
That would be in my code:

Vector3 dir0 = q0.XAxis();
Vector3 dir1 = q1.XAxis();

or matrix3x3 temp; temp.FromQuat(q0); dir0 = temp[0];
... I'm sure you know what i mean.

So the joint you may want is my Cone-Twist joint, but without the cone limit and with zero twist limit.
Note that you need the Cone to keep it stable, but you can use a very large angle, eg. 170 Degrees, which is more than possible in reality.

There's a difference between my Twist and your Real Joint:
Real Joint: When the two axis are at 90 degrees, rotating one around its axis rotates the entire other thing around the same axis.
Usually this is unwanted, I hated that happening in my Lego cars - Julio's idea should reproduce that correctly.
Twist Joint: When the two axis are at 90 degrees, rotating one around its axis rotates the other thing around its own axis the same number of degrees.
Maybe that bahaviour is ok (or better) for your use case.

[Julio Jerez]

Wow JoeJ that is a good idea. That did not occur to me.
bascially a ball an socked with that twist rotation row shooul do the job, and I believe a lot simple than the three bodies and more stable numarically.

I beleive that at this point JoeJ has mastered the art of making joints usin teh newton thheuque better than I do I am kind of rusty

Marc Indeed think JoeJ solution is much better.

[PJani]

@JoeJ: I am back to limited ball and socket joint and I tried to use your example code but i get some weird results(the "child" body when it hits the limit just rotates to some degree and continues with rotation 0_0).
Here is my code currently is a big mess because i am trying to get this thing to work.

Code: Select all

   dMatrix matrix0;
   dMatrix matrix1;
   // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
   CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

   const dVector& p0 = matrix0.m_posit;
   const dVector& p1 = matrix1.m_posit;

   // Restrict the movement on the pivot point along all tree orthonormal direction
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
   NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);

   dVector localPin(0,1,0);
   dVector rotationalPin(0,1,0);
   dVector rotationalPinLeft(0,0,1);
   dVector rotationalPinFront(1,0,0);

   dVector pin0 = matrix0.RotateVector(localPin);
   dVector pin1 = matrix1.RotateVector(localPin);

   dFloat angle = angleBetweenVectors(pin0, pin1);
   
   if(angle >= m_coneAngle)
   {
      dVector swing_pin = normalize(pin0 * pin1);

      dFloat fixangle = angle - m_coneAngle;
      
      NewtonUserJointAddAngularRow (m_joint, fixangle, &swing_pin[0]);
      NewtonUserJointSetRowMinimumFriction (m_joint, 0.0); // important: allow the arm to move inside the cone
   }

   dVector gRotPin0 = matrix0.RotateVector(rotationalPin);
   dVector gRotPin1 = matrix1.RotateVector(rotationalPin);
   
   dVector gRotPinLeft1 = matrix0.RotateVector(rotationalPinLeft);

   dQuaternion q0(matrix0);
   dQuaternion q1(matrix1);

   dQuaternion q01 = q0.Inverse() * q1;

   dVector axis = gRotPin0;//(gRotPin0 + gRotPin1); axis = axis.Scale(1/dSqrt(axis % axis));

   float twistAngle = 2.0 * atan(q01.m_q1 / q01.m_q0);
   //dVector vv0 = p0;
   //dVector vv1 = ???;

   //dFloat twistAngle = signedAngleBetweenVectors(vv0,vv1,rotationalPin);
   //dFloat twistAngle = atan2(p_i.m_z , p_i.m_x);
   //std::cout << twistAngle*(180.0 / 3.14) << " " <<  rot  << std::endl;
   if (m_minTwistAngle == m_maxTwistAngle) // this joint does NOT allow twisting
   {
      NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&axis);
   }
   else if (twistAngle > m_maxTwistAngle)
   {
      NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&axis);
      NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
   }
   else if (twistAngle < m_minTwistAngle)
   {
      NewtonUserJointAddAngularRow (m_joint, twistAngle - m_minTwistAngle, (float*)&axis);
      NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
   }


@Julio what is actually that angular row friction, how is defined?

[JoeJ]

Your Code looks good, but there's the difference that you use y axis (rotationalPin(0,1,0)) where i use x axis so it's hard to compare our code.
Try following changes:

dQuaternion q01 = q1 * q0.Inverse(); // because Julios rotation order is swapped against my math lib
...
float twistAngle = 2.0 * atan(q01.m_q2 / q01.m_q0); // because you use y, not x

Does the cone limit alone work, and you have only a problem with the twist section?

[PJani]

Jep cone limit works ok. Twist is making the problem. I tried as you said but it doesn't work right.

Here is video...Twist Limit is set for (-30,+30) deg