Picking up objects

[pHySiQuE]

I'd like to improve our code that lets the player carry objects. Right now I am applying force and torque to control it, and it's not very smooth when collisions occur:


Any ideas how to improve this? Maybe a special joint?

One of my users has a realyl nice game coming out and I would like to try and help them:
http://steamcommunity.com//sharedfiles/ ... =447417347

[JoeJ]

I don't have those jitter problems.
I post my entire picking code, but it is very messy.
Both methods (force / joint) are supported (it also works to use both at the same time).
The advantage of joint method is that it works with connected multi-bodies like a ragdoll with joints.
The force method does not work well for this because it uses mass from only the single picked body, so it becomes too weak for that.
For single bodies i'd recommend force method.

The joint i use is very similar to newton kinematic joint but rotational behaviour is improved (see mdification comment).
You can use newton kinematic joint instead.

Before each Call to NewtonUpdate i call this:

interact->UpdateHandPosition (step);
interact->AccumulateForce (step);

and UpdateCharacterPosition() once per frame.

Code: Select all

class InteractionModel
{
public:

   PhysicsWorld* world;
   Body* pickedBody;
   Body* lastPickedBody;
   float curScale[3];

   float reachRadius;
   float carryRadius;
   sVec3 pickPos; // character center
   sVec3 pickDir; // eye vector

   sVec3 handPos;
   sQuat handOrn;

   sVec3 targetPos;
   sQuat targetOrn;

   KCJoint kineJoint;

   InteractionModel (PhysicsWorld* world = 0)
   {
      this->world = world;
      pickedBody = 0;
      lastPickedBody = 0;
      //defaultForceCallBack = 0;
      reachRadius = 150;
      carryRadius = 10;
      pickPos = sVec3 (0,0,0);
      pickDir = sVec3 (0,0,-1);
   }
   ~InteractionModel ()
   {
      pickedBody = 0;
      lastPickedBody = 0;
      ReleaseBody ();
   }

   void UpdateHandPosition (float dt, int vis=0)
   {
      if (pickedBody)
      {
         float mix = 0.4f;
         handPos = handPos * (1-mix) + targetPos * mix;

         mix = 0.7f;
         handOrn = handOrn.Slerp (targetOrn, mix);
         handOrn.Normalize();

         if (vis)
         {
            sVec3 x = handOrn.Rotate (sVec3(1,0,0));
            sVec3 y = handOrn.Rotate (sVec3(0,1,0));
            sVec3 z = handOrn.Rotate (sVec3(0,0,1));

            RenderVector (1, v3f (handPos[0], handPos[1], handPos[2]), v3f (x[0], x[1], x[2]), 1,0,0);
            RenderVector (1, v3f (handPos[0], handPos[1], handPos[2]), v3f (y[0], y[1], y[2]), 0,1,0);
            RenderVector (1, v3f (handPos[0], handPos[1], handPos[2]), v3f (z[0], z[1], z[2]), 0,0,1);
         }

         if (kineJoint.m_joint)
         {
            kineJoint.m_targetPosit = dVector(handPos[0], handPos[1], handPos[2]);
            kineJoint.m_targetRot = dQuaternion(handOrn[3], handOrn[0], handOrn[1], handOrn[2]);
         }
      }
   }

   void UpdateCharacterPosition (float *m16, int vis=0)
   {
      if (propsPhysics::flags & propsPhysics::HAND_LOCK) return;
      sMat4 *m = (sMat4*) m16;
      pickPos = *m->Translation();
      sVec3 dir = m->Rotate (sVec3 (0,0,-1));

      targetPos = pickPos + dir * carryRadius;
      pickDir = pickPos + dir * reachRadius;
      
      //m->Rotation()->ToQuat (targetOrn);
      sMat3 rot; rot.FromEulerZYX (sVec3 (propsPhysics::f[propsPhysics::handRX], propsPhysics::f[propsPhysics::handRY], propsPhysics::f[propsPhysics::handRZ]));
      rot = *m->Rotation() * rot;
      rot.ToQuat (targetOrn);
if (vis)
{
sVec3 crosshair = pickPos + dir * 2;
glPointSize(3);
RenderPoint (crosshair, 1,1,1);
glPointSize(1);
}
   }

   bool PickBody ()
   {
//
      if (pickedBody) return 0;

      sMat4 matrix;
      sVec3 p0 = pickPos;
      sVec3 p1 = pickDir;

      RayCastInfo rc;
      world->RayCastUnfilterd (p0, p1, rc);
//return 0;      
      if (!rc.hitBody) return 0;
      pickedBody = rc.hitBody;
      Shape *shape = NewtonBodyGetCollision (pickedBody);      
      NewtonCollisionGetScale (shape, &curScale[0], &curScale[1], &curScale[2]);
      propsPhysics::f[propsPhysics::scaleBody] = curScale[0];


      BodyGetMatrix (pickedBody, matrix);
      sVec3 p = *matrix.Translation();

      BodyGetRotation (pickedBody, handOrn);
      handPos = p;


      carryRadius = (sVec3(p - pickPos)).Length();


      // joint

      if (!kineJoint.m_joint)
      {
         kineJoint.m_body0 = pickedBody;
         kineJoint.m_joint = NewtonConstraintCreateUserJoint (world->world, 6, KCJointCallback, 0, pickedBody, 0);
         NewtonJointSetUserData (kineJoint.m_joint, &kineJoint);
         NewtonBodySetAutoSleep (pickedBody, 0);
      }


      return 1;
   }

   void ReleaseBody ()
   {
//return;
      if (pickedBody)
      {
         BodyActivate (pickedBody);
         lastPickedBody = pickedBody;
         pickedBody = 0;
      }

      if (kineJoint.m_joint)
      {
         //NewtonBodySetAutoSleep (pickedBody, 1);
         NewtonJointSetUserData (kineJoint.m_joint, NULL); 
         NewtonJointSetDestructor (kineJoint.m_joint, NULL); 
         NewtonDestroyJoint(world->world, kineJoint.m_joint);
         kineJoint.m_joint = 0;
         kineJoint.m_body0 = 0;
         
      }
   }

   void AccumulateForce (float timestep)
   {
      if (!pickedBody) return;
      BodyData *data = (BodyData*) BodyGetUserData (pickedBody);

      sVec3 targetPos = handPos;
      sQuat targetOrn = handOrn;

      float mass, Ixx, Iyy, Izz; BodyGetMassMatrix (pickedBody, mass, Ixx, Iyy, Izz);
      sMat4 matrix;      BodyGetMatrix(pickedBody, matrix);
      sVec3 curLinvel;   BodyGetVelocity (pickedBody, curLinvel);
      sVec3 com = matrix[3]; // assuming there's no offset
      sQuat orn;         BodyGetRotation (pickedBody, orn);
      sVec3 curAngvel;   BodyGetAngVel (pickedBody, curAngvel);

      sVec3 targetLinvel = (targetPos - com) / timestep; // move distance in one step
      targetLinvel -= curLinvel; // subtract unwanted current velocity
      sVec3 force = 0.3 * targetLinvel * (mass / timestep);
      sVec3 gravity (0.0f, mass * propsPhysics::f[propsPhysics::gravity], 0.0f);
      force -= gravity; // subtract unwanted gravity

      sVec3 targetAngVel = AngVelFromAToB (orn, targetOrn) / timestep;
      sVec3 torque = 0.3 * ConvertAngVelToTorque (sVec3(targetAngVel - curAngvel), matrix, timestep, Ixx, Iyy, Izz);


      if (propsPhysics::flags & propsPhysics::HAND_POWER) data->force += force; // this flag comes from user interface to turn off / on powering method
      if (propsPhysics::flags & propsPhysics::HAND_CONORN) // turn off / on joint method
      {
         if (propsPhysics::flags & propsPhysics::HAND_POWER) data->torque += torque;
         kineJoint.m_pickMode = 1;
      }
      else
      {
         kineJoint.m_pickMode = 0;
      }

      BodyActivate (pickedBody);
   }

   void UpdateWorldPin (Body *body, float timestep)
   {
      if (!pickedBody)
      {
         pickedBody = body;
         world = world;
         handOrn.Identity();
         BodyGetRotation (pickedBody, handOrn);
         BodyGetCenterOfMass (pickedBody, handPos);

         if (!kineJoint.m_joint)
         {
            kineJoint.m_body0 = pickedBody;
            kineJoint.m_joint = NewtonConstraintCreateUserJoint (world->world, 6, KCJointCallback, 0, pickedBody, 0);
            NewtonJointSetUserData (kineJoint.m_joint, &kineJoint);
            NewtonBodySetAutoSleep (pickedBody, 0);
            kineJoint.m_localHandle = dVector(0,0,0);
            kineJoint.m_targetPosit = dVector(handPos[0],handPos[1],handPos[2]);
         }

      }
      
      sVec3 p2; BodyGetCenterOfMass (pickedBody, p2);
      RenderLine (handPos, p2, 1,1,1);

      AccumulateForce (timestep);
   }

   void ScaleBody (float scale, float timestep) // ignore this, i use it to scale a picked body
   {
      Body *body = pickedBody;
      if (!body) body = lastPickedBody;
      if (!body) return;

      world->ScaleBody (body, scale, timestep);
      /*Shape *shape = NewtonBodyGetCollision (body);
      
      curScale[0] = scale;
      curScale[1] = scale;
      curScale[2] = scale;

      NewtonCollisionSetScale (shape, curScale[0], curScale[1], curScale[2]);*/

   }
};


Code: Select all

   void KCJoint::SubmitConstraints (dFloat timestep, int threadIndex)
   {
//return;
#ifdef NOZEROTSROWS
if (timestep<=0) return;
#endif


//if (timestep<0) return;
      // check if this is an impulsive time step
      //dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
      dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;

      if (!(propsPhysics::flags & propsPhysics::HAND_JOINT)) return;
      m_maxLinearFriction = (propsPhysics::f[propsPhysics::handLinMaxF]);

      
      dVector v;
      dVector w;
      dVector cg;
      dMatrix matrix0;
      // calculate the position of the pivot point and the Jacobian direction vectors, in global space.

      NewtonBodyGetOmega (m_body0, &w[0]);
      NewtonBodyGetVelocity (m_body0, &v[0]);
      NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
      NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);


   
      dVector p0 (matrix0.TransformVector (m_localHandle));


      dVector pointVeloc = v + w * matrix0.RotateVector (m_localHandle - cg);
      dVector relPosit (m_targetPosit - p0);
      dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
      dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));

      //////////////////
      //
      //  Modification: Use Gramm Schmidt Projection along error vector instead of body space
      //
      //////////////////

      dMatrix matrixCS;
      if ((relPosit % relPosit) < 0.000001) matrixCS = dGetIdentityMatrix(); // matrix0;(?!?)
      else matrixCS = dGrammSchmidt(relAccel);//relPosit);
      //if ((relVeloc % relVeloc) < 0.000001) matrixCS = GetIdentityMatrix(); // matrix0;(?!?)
      //else matrixCS = dgGrammSchmidt(relVeloc);

/*RenderVector ((sVec3&) matrix0[3], (sVec3&)m_targetPosit, 1,1,1);
RenderVector ((sVec3&) matrixCS[3], (sVec3&)matrixCS[0], 1,0,0); // if boby at rest, matrix0 is NOT normalized (?!?)
RenderVector ((sVec3&) matrixCS[3], (sVec3&)matrixCS[1], 0,1,0);
RenderVector ((sVec3&) matrixCS[3], (sVec3&)matrixCS[2], 0,0,1);*/

      // Restrict the movement on the pivot point along all tree orthonormal direction
      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_front[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrixCS.m_front);
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_up[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrixCS.m_up);
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_right[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrixCS.m_right);
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

//return;
      //////////////////
      //
      //////////////////

      if (m_pickMode)
      {
         dFloat mag;
         dQuaternion rotation;

         NewtonBodyGetRotation (m_body0, &rotation.m_q0);

         //rotation.Scale(-1);
         //dQuaternion m_targetRot = this->m_targetRot;
         //m_targetRot.Scale(-1);


         if (m_targetRot.DotProduct (rotation) < 0.0f) {
            rotation.m_q0 *= -1.0f;
            rotation.m_q1 *= -1.0f;
            rotation.m_q2 *= -1.0f;
            rotation.m_q3 *= -1.0f;
         }

         dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep).Scale(0.3) - w);
         mag = relOmega % relOmega;
         if (mag > 1.0e-6f) {
            dFloat relAlpha;
            dFloat relSpeed;
            dVector pin (relOmega.Scale (1.0f / mag));
            dMatrix basis (dGrammSchmidt (pin));    
            relSpeed = dSqrt (relOmega % relOmega);
            relAlpha = relSpeed * invTimestep;

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
            NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
            NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

         } else {

            dVector relAlpha = w.Scale (-invTimestep);
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
         }

      } else {
         // this is the single handle pick mode, add soem angular friction

         /*dVector relAlpha = w.Scale (-invTimestep);
         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);*/
      }

   }

};


Edit: Forgot some stuff ( let me know if there is more):

Code: Select all

inline sVec3 AngVelFromAToB (const sQuat &qA, const sQuat &qB)
{

   const float matchTolerance = 0.0001f;
   const float faultTolerance = 0.0005f;

   sQuat q = QuatFromAToB (qA, qB);
   sVec3 *omegaDir = (sVec3*)&q;
   float sql = omegaDir->SqL();
   if (sql   < (matchTolerance * matchTolerance))
      return sVec3 (0, 0, 0);

   float length = sqrt (sql);
   if (q[3] < -1) q[3] = -1;
   if (q[3] >  1) q[3] =  1;
   sVec3 angVel = (*omegaDir / length) * (2.0 * acos(q[3]));
   if (length   < faultTolerance) angVel *= (length - matchTolerance) / (faultTolerance - matchTolerance);
   return angVel;

}

inline sQuat QuatFromAToB (const sQuat &qA, const sQuat &qB) // global
{
   sQuat q;
   if (qA.Dot(qB) < 0.0f)
   {
      q[0] = qA[0]; q[1] = qA[1]; q[2] = qA[2];
      q[3] = -qA[3];
   }
   else
   {
      q[0] = -qA[0]; q[1] = -qA[1]; q[2] = -qA[2];
      q[3] = qA[3];
   }
         
   return qB * q; // note: My mathlib has opposite matrix and quat multiplication order than newtons - with newton it should be q * qB, and i use xyzw while newton uses wxyz
}

inline sVec3 ConvertAngVelToTorque (sVec3 &targetAngVel, sMat4 &matrix,
                        float timestep, float Ixx, float Iyy, float Izz)
{
      sVec3 torque = targetAngVel / timestep;
      torque = matrix.Unrotate (torque);
      torque[0] *= Ixx;
      torque[1] *= Iyy;
      torque[2] *= Izz;
      torque = matrix.Rotate (torque);
      return torque;
}


[Julio Jerez]

hey Joe, this insight

//////////////////
//
// Modification: Use Gramm Schmidt Projection along error vector instead of body space
//
//////////////////

Is a very good idea. I will added to the joint. what else did you add?

and pHySiQuE yes joe's answer is the way of doing that, it will make stable on many different cases.
the joint give you a precisely control force, as opposed to the applying direct forces.

[pHySiQuE]

I found the Kinematic controller to handle collisions well, but the joint has a very high amount of "rubber banding" behavior.

[JoeJ]

Is a very good idea.

... it's your own: http://newtondynamics.com/forum/viewtopic.php?f=26&t=7332

I don't made other relevant changes, i just commented out the "angular friction" section because i did not want this on for some of my tests.


Another reminder would be this one: http://newtondynamics.com/forum/viewtopic.php?f=9&t=7541&hilit=+ragdoll+joint


Physique - the rubberband might be what the fix is about.
For sure you need max friction high enough to make it stiff -
but if it should be springy the fix makes swinging natural, without the fix it appeared "axis snapping".

[Julio Jerez]

what is rubber banding?

You can use a reasonable metric for setting the friction; say that you human can have the strength of living serving some number of G forces.
This you can use for setting the fiction.
FrictionForce = MaxG * body Mass
FrictionForcetorque = max(Ixx, Iyy, Izz) * Max (radiosOfBox) * MaxG
Since we know that a 15 G crash is the max sorvability of a Crash you can set G to a value from 20 to 50 20