Hinge Joint Jittering

[FSA]

Hi. Im sorry that I ask once angain...
My problem is when i walked into a door(square), wich is connected with a hinge joint to the world, the door starts jittering. I alreade have set NewtonBodySetContinuousCollisionMode to Player and door, but no diffrent occours. How can realise that the door dont move, when it is on Limit?
Tank you.

[JoeJ]

Let me say something about your previous post first:
I've tried the kinematic joint for interaction model too, but can't recommend it for that purpose.
It has some unnatural behaviour: Try to constrain a body to a very distant position (x, y, z - all must differ).
The body will not reach its target in a straight path. It'll move in a strange curvy path.
Maybe that's because the kinematic joint solves all world axis offsets independent of each other.
And maybe this bevaviour induces jitter to objects in contact.

A much better method to do the interaction model ist to simply calculate forces and torques yourself, see http://newtondynamics.com/forum/viewtopic.php?f=9&t=7193

To combine that with the character controller:
target position = character position + some offset vector where the Hand is.
target orientation = character orientation + some offset rotation how the object should be oriented.

When you get that working, you'll recognize that interacting with objects now feels totally stable and fine.
Now you could also use target pos & orn to push a body against the door to see if the door still jitters.
I guess not. I don't have a player controller, target pos & orn are linked to free floating camera.
I use it that way to put stress on much more complex objects, like powered ragdolls with a lot of custom hinge joints.
And that way it's pretty impossible to make anything jitter.

If that's the same for you, you could ignore the problems with the player controller, and check again when the new one is finished.
Or you could make / modify the controller yourself. I guess it's impossible tom make a out of the box player, that satisfies everyones needs.

[Julio Jerez]

It has some unnatural behaviour: Try to constrain a body to a very distant position (x, y, z - all must differ).
The body will not reach its target in a straight path. It'll move in a strange curvy path.

if this is the case that is a bug, it sould reac the path in a perfect or near perfect staigh line.
I thonk I knoa what it will act the way you descrcive.
Th erteason is that the teh joint may be usin a local axis the have a fix orienation. that will produce three independe force each one tru to react the tareg as fast as it can.
howeven because thes force can no be used in full, (they get clipped by the joint friction) the effect is that the accelration these force apply on teh body are no a linear combination of diatnce to the target,
therfore the body will follow a curve path.

The solution is to align the local body axis of the joint with the error distance in each submit constrains update.
The will generate three forces, however only one force will be clipped since the other two will be perpediculate to the trajectory, and will be there to inforce the stability.
the force along the distance to the target may be clipped to teh max friction value, but the resulting accelration will still be a linear combination of the trajectory.

I will make that modifcation, but I am quite occupied now, do you think you copudl try that theory?.

[JoeJ]

Argh, it happened again: Iv'e wrote alot, got logged off meanwhile, clicked 'submit' and all text is lost...
NEVER FORGET TO COPY YOUR TEXT BEFORE SUBMIT

I've had the same idea to fix it as you. But i see a major problem: if we build the constraint space along the error vector,
we need something like gram schmidt to construct the other 2 axis.
That will fix the 'curvy path' problem, but the jitter will get MUCH worse because the 'randomly moving' axis.

I will make that modifcation, but I am quite occupied now, do you think you copudl try that theory?

Ok, i'm still busy porting my j2me games to iphone and could need a break from that boring work
I'll try out how good it works, let you know and post code...
I guess at the end we may find another need for your planned '3-hinge-super-orientationl-joint', to make it perfect.

letter123: Please test if the problem remains if you dis/enable the angular constraint:
SetPickMode (0) -> constrain orientation, using gram schmidt
SetPickMode (1) -> leave orientation as is, with some damping

Maybe it turns out that the curvy path is no problem in practice, but the gram schmidt is the root of all evil...

[Julio Jerez]

I've had the same idea to fix it as you. But i see a major problem: if we build the constraint space along the error vector,
we need something like gram schmidt to construct the other 2 axis.
That will fix the 'curvy path' problem, but the jitter will get MUCH worse because the 'randomly moving' axis.

not, it will not, because the error along the perpendicular axis is is so small that is amost zero.
In thoery the twe perpendicual axis are not really nessesary, but is practice they provided stability.
say example the body is a position A, and target is B. along the way there is a force field, (like a wind)
the to wide force are the to make sure the body fallow the shortest path from A to B.
the side forces are not zero, but the displacement perpedicular to teh path for the path is in theory is zero, of cource in practive it is not.

This has actualy worked very well in the pass.

[FSA]

I have test it to add force and torque to a body. I have no problem with Force. But how can i calculate angluar velocity from an angle? I only have the angle, how far the body has to rotate.

Code: Select all


Vector3 Obj = MatrixGetPosition(m);
Vector3 f = m_pCamera->GetCameraDir();
Vector3 pos = m_pCamera->GetCameraPos();
pos += f * 7.5f;
Obdi = pos - Obj;
g_vForce = (Obdi*15.0f - vel ) * (mass / fTimePassed);


So i calculate the liniear force to add. Obj is the PickedObject position. vel is the velocity of the Body(NewtonBodyGetVelocity(body, vel))
But how can i calculate the torque. So I calculate the KinematicJoint rotation:

Code: Select all

Matrix mJointMatrix;
mJointMatrix = mTmpMatrix * MatrixRotationY(CamHandler->GetCameraAngleX() - CameraOffsetX);

CustomKinematicControllerSetTargetMatrix(PickController, mJointMatrix);
CustomKinematicControllerSetTargetPosit (PickController, JointPos);


mTmpMatrix is the Matrix from the body when it get picked. Then the matrix doesn't change anymore. CameraOffsetX is the CameraAngleX when the body get picked. It also doesn't change anymore.
Thank you.

[JoeJ]

I'm still too dump to compile the Joint Library, so i needed to replicate the Kinmenatic Joint, which took some time...
After that, the change is only a few lines:

Code: Select all

void KCJoint::SubmitConstraints (dFloat timestep, int threadIndex)
   {
      dVector v;
      dVector w;
      dVector cg;
      dMatrix matrix0;
      dFloat invTimestep;
      // 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]);


      invTimestep = 1.0f / timestep;
      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 = GetIdentityMatrix(); // matrix0;(?!?)
      else matrixCS = dgGrammSchmidt(relPosit);

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


      //////////////////
      //
      //////////////////

      if (m_pickMode) {
         dFloat mag;
         dQuaternion rotation;

         NewtonBodyGetRotation (m_body0, &rotation.m_q0);
         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, timestep) - w);
         mag = relOmega % relOmega;
         if (mag > 1.0e-6f) {
            dFloat relAlpha;
            dFloat relSpeed;
            dVector pin (relOmega.Scale (1.0f / mag));
            dMatrix basis (dgGrammSchmidt (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);
      }
   }


Now the curvy path is gone and behaviour looks good.
I'll do some testing to see if my doupts against gramm schmidt are not reasonable, as you say.
I'll also plug it into my interaction model and compare with my method.

One annoying thing:
Previously i've used body matrix in case of a too small errror to use Gramm Schmidt.
My debug visualisation showed, that it's axis ar not normalizid, they have a length of approx 3.0, when the body is at rest. (Commented code)
I'll update to actual code version and check again tomorrow...

[Julio Jerez]

Now the curvy path is gone and behaviour looks good.
I'll do some testing to see if my doupts against gramm schmidt are not reasonable, as you say.
I'll also plug it into my interaction model and compare with my method.

One annoying thing:
Previously i've used body matrix in case of a too small errror to use Gramm Schmidt.
My debug visualisation showed, that it's axis ar not normalizid, they have a length of approx 3.0, when the body is at rest. (Commented code)
I'll update to actual code version and check again tomorrow...

the two methods are not exclusive, if you have a system where the errors are too great and they are very umpredictable, them applying force is probably better.
for systems when you need more precise control and the move are more gentle, like objects placement for a level editor, the joint is what you want.
I actually use both methods. for picking in the demos I use forces. but I will be use the joint for exact object placement in the editor.

also when the error is small, then you can simple use any othonormal matrix. here you apply the compspt that if errors are small, any the linarization of any mathematical correction is commutative.

[JoeJ]

But how can i calculate the torque

I've posted my entire code a while ago, forgot about that:
http://newtondynamics.com/forum/viewtopic.php?f=9&t=6706
It uses quaternions to bulid a rotation between current und target orientations to solve what you ask for.

if you have a system whe the error are too great, an dthey are very umpredictable, them applying force is probably better.
for systems when you need more precise control an dteh move are more gently, like objects placement for a level editor the joint is what you want.

After some testing i can confirm that. I've no jitter problems with the improved, but also not with the original kinematic joint in my test bed.
The improved version is surely better, but i can't 'feel' a real difference between them.
For a in-game interaction model i still recommend the force method, mostly because there's no unwanted swinging forth and back motion.
Using both methods together also works well.

when the error is small, then you can simple use any othonormal matrix

I've done that. You can just update your code with the one from my last post, but please look if the threshold i've chosen is good - i'm always unsure about that
The annoying scaled body matrix thingy did not happen again after syncing to svn.

[Julio Jerez]

if you have a system whe the error are too great, an dthey are very umpredictable, them applying force is probably better.
for systems when you need more precise control an dteh move are more gently, like objects placement for a level editor the joint is what you want.

After some testing i can confirm that. I've no jitter problems with the improved, but also not with the original kinematic joint in my test bed.
The improved version is surely better, but i can't 'feel' a real difference between them.
For a in-game interaction model i still recommend the force method, mostly because there's no unwanted swinging forth and back motion.

do not lose track of what is the objective to have a joint like that.
I will provide two examples.

Say you are editing a scene in a lever editor. let us say it is a book shelve with objects that can be stacked, or touching each other.
in a case like that the force method will not know what is the relation between the objects, it will must certainly apply too much or too little force
and in all cases it will most certanly make big unpredictable disarray of the objects that were already in organized.
say in a game you want to place a new Jar on a table that already has many objects on the table top. Imaging when at the force method will do there. The joint will work like a charm.

the other example is say you are placing object in a game, but you want these object to be glue to their location, more like a sticky force the keep the object in place but if too much force it will break.
The force method will never work, while the joint would do the job.

Like I said for single object like gravity Gun in game like half life, the fore is better, for precise control the Joint should be better.

One very good reason to use the force method, is that it is much faster.

[JoeJ]

a case like that the force method will not know what is the relation between the objects

Yes, for example i can lift up the entire ragdoll with the joint method with ease.
The Force method alone only knows the mass of the single picked body and so it feels sluggish to drag the corpse around.
That's really a good reason to combine both methods.
With a little tweaking and maybe predefined interaction constants for various objects, we can always get a perfect feel.

However, for me it's not really important. If i can gat the living ragdoll working, i'll attach objects to it using the same constraint / force couple i already use for the joints itself. So i'll need neither a interaction model nor a player controller at the end... hopefully.

single object like gravity Gun in game like half life

They use 'gravity guns' to hide their jitter. With Newton we can use something that feels like 'real hands'
Valve should really get in contact with you

[Stucuk]

Argh, it happened again: Iv'e wrote alot, got logged off meanwhile, clicked 'submit' and all text is lost...
NEVER FORGET TO COPY YOUR TEXT BEFORE SUBMIT

Use Chrome, While its not infallible, most of the time when you hit the "Back" button the text you have written in boxes is still there.

[FSA]

OK now i have test it with the code from JoeJ:

Code: Select all

dMatrix mat;
NewtonBodyGetMatrix(body, &mat[0][0]);
dQuaternion dQ;
Vector3 Omg;
NewtonBodyGetRotation(body, &dQ.m_q0);
NewtonBodyGetOmega(body, Omg);
Matrix Tmpm = MatrixRotationY(m_pCamera->GetCameraAngleX());
dMatrix Tmpmd(   dVector(Tmpm.m11, Tmpm.m21, Tmpm.m31, Tmpm.m41),
                  dVector(Tmpm.m12, Tmpm.m22, Tmpm.m32, Tmpm.m42),
                  dVector(Tmpm.m13, Tmpm.m23, Tmpm.m33, Tmpm.m43),
                  dVector(Tmpm.m14, Tmpm.m24, Tmpm.m34, Tmpm.m44) );
dQuaternion dTar(Tmpmd);
Vector3 angVel = PhysicUtils::AngularVelocityAToB(dQ, dTar) * 1.0f/fTimePassed*0.1f;
Angl = PhysicUtils::AngularVelocityToTorque(angVel, Omg, mat, fTimePassed, mass, a, b, c);


AngularVelocityAToB:

Code: Select all

Vector3   PhysicUtils::AngularVelocityAToB(const dQuaternion &qActual, const dQuaternion &qTarget)
{
const float matchTolerance = 0.01f;
const float faultTolerance = 0.05f;

   dQuaternion q;
   {
      dQuaternion qBuf;
      if (qActual.DotProduct(qTarget) < 0.0f)
      {
         qBuf.m_q0 = qActual.m_q0;
         qBuf.m_q1 = qActual.m_q1;
         qBuf.m_q2 = qActual.m_q2;
         qBuf.m_q3 = -qActual.m_q3;
      }
      else
      {
         qBuf.m_q0 = -qActual.m_q0;
         qBuf.m_q1 = -qActual.m_q1;
         qBuf.m_q2 = -qActual.m_q2;
         qBuf.m_q3 = qActual.m_q3;
      }
      q = qTarget * qBuf;
   }

   dVector vOmegaDir(q.m_q0, q.m_q1, q.m_q2, q.m_q3);/*q.GetXYZ_EulerAngles();*/
   if (Vector3LengthSq(Vector3(vOmegaDir.m_x, vOmegaDir.m_y, vOmegaDir.m_z)) < (matchTolerance * matchTolerance))
        return Vector3 (0, 0, 0);

    float fLength = Vector3Length( Vector3(vOmegaDir.m_x, vOmegaDir.m_y, vOmegaDir.m_z) );
   if (q.m_q3 < -1) q.m_q3 = -1;
    if (q.m_q3 >  1) q.m_q3 =  1;

   Vector3 angVel( (Vector3(vOmegaDir.m_x, vOmegaDir.m_y, vOmegaDir.m_z) / fLength) * (2.0 * acosf(q.m_q3)) );
    if (fLength   < faultTolerance)
      angVel *= (fLength - matchTolerance) / (faultTolerance - matchTolerance);

    return angVel;
}


AngularVelocityToTorque:

Code: Select all

Vector3 PhysicUtils::AngularVelocityToTorque(const Vector3 &vTargetAng,
                                    const Vector3 &vCurrentAng,
                                    const dMatrix &BodyMatrix,
                                    const float &fTimestep,
                                    const float &fMass,
                                    const float &Ixx,
                                    const float &Iyy,
                                    const float &Izz)
{
   dVector torque = (vTargetAng - vCurrentAng) / fTimestep;
    torque = BodyMatrix.UnrotateVector (torque);
    torque[0] *= Ixx;
    torque[1] *= Iyy;
    torque[2] *= Izz;
    torque = BodyMatrix.RotateVector (torque);
    return Vector3(torque.m_x, torque.m_y, torque.m_z);
}


It doesnt work. My Body rotate in all directions and jitter. "Angl" is the Torque I add. I would be glad if you can find the mistake. I have search about 1 Day and cant find it
Tank you very much!

[JoeJ]

There are some major differences from the math lib i use in my code and the newton math lib.
1. matrix & quaternion multiplication is reversed: mine: c = a * b; newton: c = b * a. - I'm not sure if such a case exists here.
2. and more important: newton quaternion is (w,x,y,z), mine is (x,y,z,w).


So this is wrong:
{
qBuf.m_q0 = qActual.m_q0;
qBuf.m_q1 = qActual.m_q1;
qBuf.m_q2 = qActual.m_q2;
qBuf.m_q3 = -qActual.m_q3;
}
... and should be:
{
qBuf.m_q1 = qActual.m_q1;
qBuf.m_q2 = qActual.m_q2;
qBuf.m_q3 = qActual.m_q3;
qBuf.m_q0 = -qActual.m_q0;
}
... same for else condition for sure.

This leads to more errors, because the following code casts the directional xyz part of the quaternion to a vector.
This is simply a quaternion to axis and angle conversation. AngVelFromAToB is not really a goos name for that function.
I'll translate from my version to newton math lib...

Code: Select all

dVector AngVelFromAToB (const dQuat &qA, const dQuat &qB)
{

   const float matchTolerance = 0.01f;
   const float faultTolerance = 0.05f;

   dQuat q = QuatFromAToB (qA, qB);
   dVector omegaDir (q.m_q1, q.m_q2, q.m_q3); // quat xyz = axis of rotation, which we'll normalize
   float sql = omegaDir .dot (omegaDir ); // newton uses % for dot product ?
   if (sql   < (matchTolerance * matchTolerance))
      return dVector (0, 0, 0);

   float length = sqrt (sql);
float quatW = dQuat.m_q0;
   if (quatW  < -1) quatW  = -1;
   if (quatW  >  1) quatW  =  1;
   dVector angVel = (omegaDir / length) * (2.0 * acos(quatW )); // 2.0 * acos(quatW ) = angle of rotation
   if (length   < faultTolerance) angVel *= (length - matchTolerance) / (faultTolerance - matchTolerance);
   return angVel;

}


With a little luck it will work after those fixes.

[FSA]

Thank you. That works, bu when I holding my Object a little bit longer it starts to jitter. Left down in the Corner you see the Angular Velocity/Toque that is applied to the Body. When the Body starts to Jitter, the values are higer than 3000!
I've upload a video:
https://www.dropbox.com/s/5be77kjrcdghb ... Jitter.avi

My Code:
(It's not finish yet. It has some crazy things )
Function1

Code: Select all

Vector3   PhysicUtils::AngularVelocityAToB(const dQuaternion &qActual, const dQuaternion &qTarget)
{
   const float matchTolerance = 0.01f;
    const float faultTolerance = 0.05f;

   dQuaternion q;
   {
      dQuaternion qBuf;
      if (qActual.DotProduct(qTarget) < 0.0f)
      {
         qBuf.m_q1 = qActual.m_q1;
         qBuf.m_q2 = qActual.m_q2;
         qBuf.m_q3 = qActual.m_q3;
         qBuf.m_q0 = -qActual.m_q0;
      }
      else
      {
         qBuf.m_q1 = -qActual.m_q1;
         qBuf.m_q2 = -qActual.m_q2;
         qBuf.m_q3 = -qActual.m_q3;
         qBuf.m_q0 = qActual.m_q0;
      }
      q = qTarget * qBuf;
   }

   dVector vOmegaDir(q.m_q1, q.m_q2, q.m_q3);
   float sq1 = vOmegaDir %  vOmegaDir;

   if (sq1 < (matchTolerance * matchTolerance))
        return Vector3 (0, 0, 0);

    float fLength = sqrt(sq1);
   if (q.m_q0 < -1) q.m_q0 = -1;
    if (q.m_q0 >  1) q.m_q0 =  1;

   Vector3 angVel = (Vector3(vOmegaDir.m_x, vOmegaDir.m_y, vOmegaDir.m_z) / fLength ) * (2.0f * acos(q.m_q0));

    if (fLength < faultTolerance)
      angVel = angVel * (fLength - matchTolerance) / (faultTolerance - matchTolerance);

    return Vector3(angVel.x, angVel.y, angVel.z);
}


Function2

Code: Select all

Vector3 PhysicUtils::AngularVelocityToTorque(const Vector3 &vTargetAng,
                                    const Vector3 &vCurrentAng,
                                    const dMatrix &BodyMatrix,
                                    const float &fTimestep,
                                    const float &fMass,
                                    const float &Ixx,
                                    const float &Iyy,
                                    const float &Izz)
{
   dVector torque = (vTargetAng - vCurrentAng) / fTimestep;
    torque = BodyMatrix.UnrotateVector (torque);
    torque[0] *= Ixx;
    torque[1] *= Iyy;
    torque[2] *= Izz;
    torque = BodyMatrix.RotateVector (torque);
    return Vector3(torque.m_x, torque.m_y, torque.m_z);
}


Picking:

Code: Select all

NewtonBody*   body = MapHandler->GetPhysicObject(0);
      if(bo)
      {
         bo = false;
         CamOff = m_pCamera->GetCameraAngleX();
         NewtonBodyGetMatrix(body, gmat);
      }

      //******************************
      // Translation
      Vector3 vel;
      NewtonBodySetForceAndTorqueCallback(body, callb);
      NewtonBodySetContinuousCollisionMode(body, 1);
      NewtonBodyGetVelocity(body, vel);
      Matrix m;
      float mass,a,b,c;
      NewtonBodyGetMatrix(body, m);
      NewtonBodyGetMassMatrix(body, &mass, &a, &b, &c);

      Vector3 Obj = MatrixGetPosition(m);
      Vector3 f = m_pCamera->GetCameraDir();
      Vector3 pos = m_pCamera->GetCameraPos();
      pos += f * 7.5f;

      Obdi = pos - Obj;
      g_vForce = (Obdi*15.0f - vel ) * (mass / fTimePassed);

      // ****************************
      // Rotation
      dMatrix mat;
      NewtonBodyGetMatrix(body, &mat[0][0]);
      dQuaternion dQ;
      Vector3 Omg;
      NewtonBodyGetRotation(body, &dQ.m_q0);
      NewtonBodyGetOmega(body, Omg);
      Matrix Tmpm = gmat * MatrixRotationY(-(m_pCamera->GetCameraAngleX() - CamOff));
      dMatrix Tmpmd(   dVector(Tmpm.m11, Tmpm.m21, Tmpm.m31, Tmpm.m41),
                  dVector(Tmpm.m12, Tmpm.m22, Tmpm.m32, Tmpm.m42),
                  dVector(Tmpm.m13, Tmpm.m23, Tmpm.m33, Tmpm.m43),
                  dVector(Tmpm.m14, Tmpm.m24, Tmpm.m34, Tmpm.m44) );

      dQuaternion dTar(Tmpmd);
      Vector3 angVel = PhysicUtils::AngularVelocityAToB(dQ, dTar) * 1.0f/fTimePassed*0.05f;
      Angl += PhysicUtils::AngularVelocityToTorque(angVel, Omg, mat, fTimePassed, mass, a, b, c);


Callback:

Code: Select all

void callb (const NewtonBody* body, dFloat timestep, int threadIndex)
{
   float mass;
   float Ixx;
   float Iyy;
   float Izz;

   NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
   Vector3 force (0.0f, mass * -10.0f, 0.0f);
   NewtonBodyAddTorque(body, Angl);
   NewtonBodyAddForce (body, g_vForce);
}


Thank you very much for you time!