Contact Point and direction?

[JoeJ]

Well i don't know if the omega is a vector...

Yes, it's a vector. Imagine a rotating spaceship and the vector is visible on that object.
The spaceship rotates around this vector (so the vector itself seem to stay still).
The faster the object rotates, the longer the vector is.

... of euler angles rotX,rotY,rotZ or what, maybe i should do a quaternion?

Yes, euler angles problem means:
You build 3 rotations from 3 'euler angles' and combine them.
But rotX * rotY * rotZ is not the same as rotZ * rotX * rotY
No matter which order you choose, it's wrong (except if two rotations are identity or other special cases)


'Using quaternion' should mean:
angular velocity already contains a SINGLE axis and a SINGLE angle of rotation.
Converting this to quat is the most usual way to deal with that.
So use it, but recognize that it would also be possible (and easy) to build a rotation matrix from axis and angle.

What matters is that axis angle rotation is well defined, while euler angles always require an additional order convention information,
and you don't have that infomation.
You hate and tend to avoid euler angles

i mean how to calculate the vector from omega the vector at a specific time at some point from center

You mean predicting orientation at next timestep?, if i understand your code right...
Pseudocode:

//predict next orientation
dgVector omega = joint->GetBody0()->GetOmega();
float angle = omega.Length();
vector axis = omega / angle;
quat q; q.FromAxisAndAngle (axis, angle);
matrix temp; q.ToMatrix(temp);
matrix predicted = bodyMatrix * temp; // future orientation

//predict future position of some point
predicted .posit += linearVelocity * timestep; // add linear velocity to get future body position
vector currentPoint (...contact position in worldspace?)
vector temp = bodyMatrix.Untransform(currentPoint); // move to current local space
vector predictedPoint = predicted.Transform(temp): // move to future world space

EDIT: added some comments

[SilentAssassin21]

Code: Select all

i mean how to calculate the vector from omega the vector at a specific time at some point from center

http://en.wikipedia.org/wiki/File:Angular_velocity.svg I mean calculating a v vector that is on the picture perpendicular to r and it's origin is located where r meets the circle...

Code: Select all

... of euler angles rotX,rotY,rotZ or what, maybe i should do a quaternion?

Yeah i mean i know that i need a quaternion but I don't know how to construct a quaternion from euler angles, i have to google about it, this math stuff and physics is something i don't usually deal with so it's not my base knowledge

[JoeJ]

I mean calculating a v vector that is on the picture perpendicular to r and it's origin is located where r meets the circle...

Can we rephrase that: The tip of r is the current position of the contact and you want to get the future position?
If so, i understood right and gave the solution in the above post.

.. that i need a quaternion but I don't know how to construct a quaternion from euler angles

There are no euler angles! You tried to convert the omega to 3 angles, which is wrong in that context (small rotation, less error and vice versa).
Newton does not give you euler angles, it gives the red vector from the linked picture.
You can construct a quaternion to rotate any point in worldspace like i showed in the pseudocode.
All you need is to look at newtion quaternion class for the correct syntax, all functions i wrote should be there with similar names.

Dealing with 3D rotations is not so easy if you're not used to it. You'll need some time.

[SilentAssassin21]

yeah im used to 3d rotations i just don't know much from the API docs about these functions , i thought those are euler angles lol, yeah i rewrite the code so now i built a quaternion, it works also i think even better, I hope that solution works for you too, now i can focus on what was my main objective, after I try adding the joint parent to the feet

[JoeJ]

meanwhile i tried your code, unluckily i get asserts and don't can say anything on it's affect at the moment.
At least i fixed the stuff we talked about and maybe some other issue regarding transformations (see Rotate instead Transform?)
I hope it's correct so far - i'm not used to newton math lib and would need to do visualizations to proof...

Code: Select all

float surfaceHeight = 0;
      NewtonBody* body = (NewtonBody*) (mass1==0 ? body0 : body1);

      dVector omega; NewtonBodyGetOmega (body, (float*)&omega);
      dVector velocity; NewtonBodyGetVelocity (body, (float*)&velocity);
      dMatrix bodyMatrix; NewtonBodyGetMatrix (body, (float*)&bodyMatrix);
      dVector bodyCenter = bodyMatrix.m_posit;

      float angle = sqrt(omega%omega + 0.000000000001);
      dVector axis = omega.Scale (1.0 / angle);
      dMatrix rot( dQuaternion (axis, angle*timestep), dVector(0,0,0,1));


      bodyMatrix.m_posit = dVector(0,0,0,1); // ignore position, handle it seperately
      dMatrix rotatedBodyMatrix = bodyMatrix * rot;

      for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact;
         contact = NewtonContactJointGetNextContact (contactJoint, contact))
      {
         dVector contactPosition;
         dVector contactNormal;
         NewtonMaterial *mat = NewtonContactGetMaterial(contactJoint);

         NewtonMaterialGetContactPositionAndNormal (mat, body, &contactPosition[0], &contactNormal[0]);

         dVector translationVector = contactPosition - bodyCenter;
         translationVector = bodyMatrix.UnrotateVector (translationVector); // now it's in current local space
         dVector contactPositionLater = rotatedBodyMatrix.RotateVector(translationVector) + velocity.Scale(timestep) + bodyCenter; // now it's in future world space
         dVector contactSpeedVector = (contactPositionLater - contactPosition).Scale(1.f/timestep);

         if (contactPositionLater.m_y < 0.f)
         {
            float actualSpeed = surfaceHeight -contactPositionLater.m_y /timestep - contactSpeedVector.m_y;
            if (actualSpeed > 0) NewtonMaterialSetContactNormalAcceleration(mat,actualSpeed);
         }   

      }


[JoeJ]

... and finally i understand what you really wanted to know.
Sorry, that took a while

Code: Select all

// retrieves body's velocity at specific point in global space
inline sVec3 GetPointVelocity (sVec3 &lvel, sVec3 &avel, sVec3 &com, sVec3 &point)
{
   return lvel + avel.Cross (point - com);
}


lvel = linear velocity
avel = angular volocity = omega
com = center og mass

[SilentAssassin21]

Code: Select all

// retrieves body's velocity at specific point in global space
inline sVec3 GetPointVelocity (sVec3 &lvel, sVec3 &avel, sVec3 &com, sVec3 &point)
{
   return lvel + avel.Cross (point - com);
}


ah yeah, i was wondering if simple cross product works on this or I also have to perform some scaling on this... and with that assert of mine ... here is the code so far, i have to go to work now, when i come home i will apply the equation above so i don't have to do matrix manipulations :

Code: Select all

void CNewtonResource::GenericContactProcess (const NewtonJoint* contact, dFloat timestep, int threadIndex)
{
   dgContact* joint=(dgContact*)contact;
   dgVector omega = joint->GetBody0()->GetOmega();
   dgVector velocity1 = joint->GetBody0()->GetVelocity();
   dgVector bodyCenter = joint->GetBody0()->GetPosition();
   dgMatrix bodyMatrix = joint->GetBody0()->GetMatrix();

   dgVector axis=omega;
   double length = std::sqrt(axis.m_x*axis.m_x + axis.m_y*axis.m_y + axis.m_z*axis.m_z);
   axis = axis.Scale(1.f/length);
   dgMatrix rotQ(dgQuaternion(axis,length*timestep),dgVector(0,0,0,1));
   dgMatrix rotatedBodyMatrix = bodyMatrix * rotQ;


   for (void* contactJoint = NewtonContactJointGetFirstContact(contact);contactJoint!=NULL;contactJoint = NewtonContactJointGetNextContact(contact,contactJoint))
   {
      dgVector contactPosition;
      dgVector contactNormal;
      NewtonMaterial *mat = NewtonContactGetMaterial(contactJoint);

      NewtonMaterialGetContactPositionAndNormal(mat,&contactPosition[0],&contactNormal[0]);

      dgVector translationVector = contactPosition - bodyCenter;
      dgVector contactPositionLater = rotatedBodyMatrix.TransformVector(translationVector) + velocity1.Scale(timestep);
      dgVector contactSpeedVector = (contactPositionLater - contactPosition).Scale(1.f/timestep);

      if (contactPositionLater.m_y < 0.f)
      {
         float actualSpeed = 0.1 -contactPositionLater.m_y /timestep - contactSpeedVector.m_y;
         if (actualSpeed > 0)
            NewtonMaterialSetContactNormalAcceleration(mat,actualSpeed);
      }   

   }
   
}

[JoeJ]

I got it working now, but it is like i initially thaught, it makes the simulation worse not better
To provoke the problem i need to move 70% of weight to one foot.
Without the 'fix' thoe foot sinks in (3-4cm), recorves (gets pushed upwards) and then the cycle starts again.
With the 'fix' the foot sinks in much less (1cm only for very short time), but the jitter is much worse - too worse.

That does not mean it's useless - i've needed months to get the powering right, and this is similar,
but it definitely needs more work and tuning.
However - what ever we do, it's a work around, most probably causing other problems we can't see initially.

Never think you can do something better than newton - you're terribly wrong, trust me
It's better to wait if Julio can improve that, because he can do it the right way.


One other assumption i have: Because your powering is wrong, you accidently made a fixed joint with no degree of freedom.
This is unstable, and for a number of 20 bodies it should be very unstable.
So maybe because of this the penetration problem is much worse for you than for me.
(there's absolutely no problem while balancing symetrically on 2 feet - ragdoll sands still with constant 5mm penetration forever)

Code: Select all

for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact;
         contact = NewtonContactJointGetNextContact (contactJoint, contact))
      {
         dVector contactPosition;
         dVector contactNormal;
         NewtonMaterial *mat = NewtonContactGetMaterial (contact); // bugfix previously caused asserts

         NewtonMaterialGetContactPositionAndNormal (mat, body, &contactPosition[0], &contactNormal[0]);

         dVector contactVelocity = velocity + omega * (contactPosition - bodyCenter);
         dVector contactPositionLater = contactPosition + velocity.Scale(timestep);

         if (contactPositionLater.m_y < surfaceHeight)
         {
            //float actualSpeed = (surfaceHeight - contactPositionLater.m_y) / timestep - contactVelocity.m_y;
            //if (actualSpeed > 0) NewtonMaterialSetContactNormalAcceleration (mat, actualSpeed);
            float penetration = dVector (0,0,surfaceHeight - contactPositionLater.m_y) % contactNormal; // maybe better?
            if (penetration > 0) NewtonMaterialSetContactNormalAcceleration (mat, penetration / timestep);
         }   
      }