Gimbal Lock and Euler angles

[misho]

So did it work with any of the 2*6 possible combinations at this code?
If it does not, i suggest making a transposed copy of ent->m_curRotation and try again.

Hi,

No, it did not . One thing that I forgot to mention is that Simulator coordinate system is LEFT handed, while Newton is RIGHT handed. I have incorporated the adjustment from one to another in my conversion routines from XYZ to Lat,Lon, Alt and vice versa. I use these routines to convert from Newton to Sim just before I display them in the Sim engine. While the positioning of the objects seems to be working, I've noticed that, when I actually apply Newton body rotations to the debug spheres, they all swap their positions: Y becomes Z, Z becomes X and X becomes Z.

This happens in this piece of code:

Code: Select all

dVector rightWing =   center + bodyMatrix.m_right.Scale(approxShipSize);
dVector nose =      center + bodyMatrix.m_front.Scale(approxShipSize);
dVector up =      center + bodyMatrix.m_up.Scale(approxShipSize);

If I accordingly adjust (shuffle around) the right, up and nose vectors, as such:

Code: Select all

   dVector rightWing = center + bodyMatrix.m_front.Scale(approxShipSize);
   dVector nose =      center + bodyMatrix.m_up.Scale(approxShipSize);
   dVector up =      center + bodyMatrix.m_right.Scale(approxShipSize);

...this gives me correct positioning. This leads me to believe that something else is off with Right to Left coordinate system adjustment. Bearing in mind that this needs to be done only for a visual renderer, where should I be making this adjustment?

[JoeJ]

Simulator coordinate system is LEFT handed, while Newton is RIGHT handed. I have incorporated the adjustment from one to another in my conversion routines from XYZ to Lat,Lon, Alt and vice versa. I use these routines to convert from Newton to Sim just before I display them in the Sim engine.

Initially I'd do all adjustements with a utility function to reduce confusion and focus all the mess in one spot, e.g.:

ConvertNewtonToRenderer (dMatrix &matrixECEF, dVector &eulersECEF, &something_else_related,
const NewtonBody *body)
{
dMatrix &s = NewtonGetMatrix(body);
matrixECEF.right = s.front;
matrixECEF.front = s.up;
matrixECEF.up = s.right;

if (UserInterface.CheckBox("DEBUG_InvertRight")) d.right *= -1.0f; // one way to change handness
// ...and other options you may wanna tweak at runtime

// calculate eulers and other stuff (maybe before changing handness with options to change angle signs afterwards)...
}

For the debug rendering you would use matrixECEF after calling that function instead bodyMatrix.

Setting up debug user interface to try stuff at runtime should save a lot of time.
That's the only real advice i can give

The shuffling you mention indicates a rotation.
Assuming zero rotation that's unexpected.
(Reminds me on http://newtondynamics.com/forum/viewtopic.php?f=9&t=8934)
Maybe you should check the setup code and fix something there to avoid it.

For the handness change i don't know any guides when to do it - another trial and error case.

[misho]

Thanks - I'm at it!

I'm also trying to figure out why this code works:

Code: Select all

   ent = g_sceneManager->GetEntity(1);
   ent->m_curPosition = rightWing;

And this code does not work:

Code: Select all

   ent = g_sceneManager->GetEntity(1);
   dMatrix posMatrix1(zeroRotation, rightWing);
   NewtonBodySetMatrix (ent->nBody, &posMatrix1[0][0]);

The first snippet is from the code I posted before in this thread. ent is a wrapper class for Newton body. I'm trying to set the position directly on the Newton body, not through entity. What am I doing wrong?

Thanks,
Misho

[JoeJ]

Not sure, but probably you can't change the matrix anywhere and you have to do it inside force torque callback or before NewtonUpdate. The entity class may handle this internally.

To match the euler angles you don't need to use a real body at all,
you can create your own matrix instead, so you don't have to deal with torques or NewtonBodeSetMatrix just to do testing:

dMatrix mP = dPitchMatrix(debugAngle0);
dMatrix mY = dYawMatrix(debugAngle1);
dMatrix mR = dRollMatrix(debugAngle2);

dMatrix myPYR = Identity
if (debugBoolGlobal) // do rotations in global space...
{
myPYR = myPYR * mP;
myPYR = myPYR * mY;
myPYR = myPYR * mR;
}
else // ...or in local space (maybe vice versa, depends on mathlib multiplication convention)
{
myPYR = mP * myPYR;
myPYR = mY * myPYR;
myPYR = mR * myPYR;
}


dMatrix matrixECEF = ConvertNewtonToRenderer (myPYR);
DebugRender (matrixECEF);
RenderModel (debugAngle0, debugAngle1, debugAngle2);

float proofAngles[3] = MatrixToEuler (myPYR, orderPYR); // should match debugAngle0-3


EDIT:
Using this approach you can try it the other way around:
Create a matrix from euler angles, match things, and later care for the inverse way.
Advantage is you don't initially need the MatrixToEuler function.

[misho]

Hmmmm I think the point of the debugging spheres (visuals) was to see what the Newton body was doing under the application of torques on local axis. I don't think I want to introduce another layer of uncertainty . I think I have the newton body (and debug visuals) do what I expect them to do... I just need to get those Euler angles to behave.

What I did notice is that, if I introduce a bank (roll) rotation, I get a set of Euler angles that look something like this:

heading= 0.0
pitch = 0.0
bank = 30.415563

Then, if I introduce a pitch in the current rotation, I get

heading= -1.276788
pitch = 28.4124
bank = 30.42486

I would expect that heading and bank readouts would not change...
The more pitch I introduce, the more drastically heading and bank angles change. These seem to be Euler angles on the world principal axes, not the angles on body's principal rotated axes. In fact, the Sim is expecting a set of angles as they go through a rotation axis by axis, so if I introduced a bank of 30 degrees, then a pitch of 15 degrees, then a heading of 45 degrees, my readout would eventually say

heading= 45
pitch = 15
bank = 30

And none of the other 2 angles would change as I am changing one. Does that make sense? How do I get this kind of behaviour?

[JoeJ]

Hmmmm I think the point of the debugging spheres (visuals) was to see what the Newton body was doing under the application of torques on local axis. I don't think I want to introduce another layer of uncertainty .

Actually, a physics simulator is a layer of uncertainty in a case all you want is matrix to euler angles coversation.
Consider the idea at a point when you feel you're really stuck...

These seem to be Euler angles on the world principal axes, not the angles on body's principal rotated axes.

I'm afraid (but not sure) this indicates that the MatrixToEuler function has this inrinsic / extrinsic mismatch, the difference being to rotate in world or local space.
You can try to invert the matrix before getting the euler angles, but i don't this this fixes it.
We would need a different euler angle function in this case...

And none of the other 2 angles would change as I am changing one. Does that make sense? How do I get this kind of behaviour?

Because euler angles are three rotations, it's common to get unexpected angle changes by following rotations - picking the right order may fix it.
But if you're pretty sure no order will help, this indicates again you're already stuck because MatrixToEuler operates in wrong space.

I suggest you make the posit (0,0,0,1), invert with Inverse or Transpose (should have the same effect for a pure rotation), and try the euler conversion again. Maybe there is luck.

If not i see no other way than to make a RendererEulers to matrix function like suggested above.
It's guaranteed you can produce a matrix that matches the renderer this way.
You then can post the code and ask "i have this euler to matrix function, can anybody do the inverse?".

[misho]

I'm beginning to suspect that I am not after the Euler angles (in classical sense) after all. The solution might be as simple as keeping a track on rotations localized to body's principal axis...

So, if I rotate by 30 degrees in Z, then 15 degrees in X, the Euler angles returned aren't in relation to principal world axis... they are just that, the angles of rotation, in succession, about the body's principal axis.

Geez... I need to step away from this and go and chop some wood or something

Misho

[misho]

Hi JoeJ,

Alright, chopping wood helped!

After a bit more research, I think I need exactly what you were suspecting: an extrinsic to intrinsic rotation.

Wiki covers the conversion between the two in matrix notation, but I'd prefer a more elegant quaternion solution. Any idea how to accomplish this?

Also, another quick q: in a default, unrotated matrix in Newton, what axis are m_front, m_up and m_right?

thanks,
Misho

[JoeJ]

Any idea how to accomplish this?

Nope. I would do the conversation from the matrix to euler directly, but after too much time i'd end up with very inefficient code taking more time to optimize (in the best case).
Try anything you find, hard to find any resources on this topic...

what axis are m_front, m_up and m_right?

Unrotated matrix = Identity matrix =
(1,0,0), x axis m_front
(0,1,0), y axis m_up
(0,0,1) z axis m_right

The directional names Newton gives them are arbitary - you can choose any other convention you like,
personally i prefer x = left, y = up, z = front.
It's totally up to you and there are no consequences on Newtons side.

[misho]

Nope. I would do the conversation from the matrix to euler directly, but after too much time i'd end up with very inefficient code taking more time to optimize (in the best case).
Try anything you find, hard to find any resources on this topic...

Okay. I will reach out to Julio, plus, I posted a question on Stackexchange

Unrotated matrix = Identity matrix =
(1,0,0), x axis m_front
(0,1,0), y axis m_up
(0,0,1) z axis m_right

The directional names Newton gives them are arbitary - you can choose any other convention you like,
personally i prefer x = left, y = up, z = front.
It's totally up to you and there are no consequences on Newtons side.

Perfect - that's what I suspected. My debug objects have been placed correctly, but as soon as I applied rotation, they all switched places. This was why - my sim axis are X:right, Y:front and Z:up. I adjusted this and now they are all in correct places.

Thanks!

[JoeJ]

I have a solution

Just tried the transposing matrix assumption i've had and it works, you just need to negate the resulting angles as well

Code: Select all

sVec3 eulers (0.2, 0.5, 1.1);

      sMat3 rX = sMat3::rotationX (eulers[0]);
      sMat3 rY = sMat3::rotationY (eulers[1]);
      sMat3 rZ = sMat3::rotationZ (eulers[2]);

      sMat3 matrix1; matrix1.Identity();
      matrix1 = matrix1 * rX;
      matrix1 = matrix1 * rY;
      matrix1 = matrix1 * rZ;

      sMat3 matrix2; matrix2.Identity();
      matrix2 = rX * matrix2;
      matrix2 = rY * matrix2;
      matrix2 = rZ * matrix2;

      sVec3 proof1 = matrix1.ToEuler (0x012);
      sVec3 proof2 = matrix2.ToEuler (0x012);

      SystemTools::Log ("proof1 = (%f, %f, %f)\n", proof1[0], proof1[1], proof1[2]); // (0.534252, 0.036083, 1.161104)
      SystemTools::Log ("proof2 = (%f, %f, %f)\n", proof2[0], proof2[1], proof2[2]); // (0.200000, 0.500000, 1.100000)

      matrix1 = matrix1.Transposed();
      proof1 = matrix1.ToEuler (0x012);
      proof1 *= -1.0f;

      SystemTools::Log ("transposing\n");
      SystemTools::Log ("proof1 = (%f, %f, %f)\n", proof1[0], proof1[1], proof1[2]); // (0.200000, 0.500000, 1.100000)   


You see i get back to the initial angles.
My toEuler function is the same as in Newton, but returns only one set of angles, here's code:

Code: Select all

__forceinline sVec3 ToEuler (const int order = 0x012)
   {
      //int order = 0x012;
      int a0 = (order>>8)&3;
      int a1 = (order>>4)&3;
      int a2 = (order>>0)&3;

      sVec3 euler;
      // Assuming the angles are in radians.
      if ((*this)[a0][a2] > (1.0f-FP_EPSILON))
      { // singularity at north pole
         euler[a0] = -atan2((*this)[a2][a1], (*this)[a1][a1]);
         euler[a1] = -3.1415926535897932384626433832795f/2.0f;
         euler[a2] = 0;
         return euler;
      }
      if ((*this)[a0][a2] < -(1.0f-FP_EPSILON))
      { // singularity at south pole
         euler[a0] = -atan2((*this)[a2][a1], (*this)[a1][a1]);
         euler[a1] = 3.1415926535897932384626433832795f/2.0f;
         euler[a2] = 0;
         return euler;
      }
      euler[a0] =   -atan2(-(*this)[a1][a2], (*this)[a2][a2]);
      euler[a1] =   -asin ( (*this)[a0][a2]);
      euler[a2] =   -atan2(-(*this)[a0][a1], (*this)[a0][a0]);
      return euler;
   }


Matrix transpose is just swapping rows with columns, Newton has it too.

[misho]

Hi, thanks for giving it a try!

I immediately put this in to see if it works - but sadly, no.

I tried all axis combinations. Here's my code - let me know if I missed a step or something:

Code: Select all

TBEntity *ent;
ent = g_sceneManager->GetEntity(index);
dVector dvEuler1, dvEuler2;
dMatrix bMatrix, tMatrix;
NewtonBodyGetMatrix(ent->nBody, &bMatrix[0][0]);
tMatrix = bMatrix.Transpose();
tMatrix.GetEulerAngles(dvEuler1, dvEuler2, PYR);
Attitude->Pitch =   dvEuler1.m_x;
Attitude->Bank =    dvEuler1.m_y;
Attitude->Heading = dvEuler1.m_z;

EDIT: I think I did miss a few steps... It is not as simple as just transposing rotation matrix... I'll pick this up in the morning, it's 3AM here

Misho

[JoeJ]

You forgot to negate resulting angles
dvEuler1 = dvEuler1.Scale(-1)

[JoeJ]

I just did the same with newton math lib to verify, euler0 gives the expected results too:

Code: Select all

sVec3 eulers (0.2, 0.5, 1.1);

      sMat3 rX = sMat3::rotationX (eulers[0]);
      sMat3 rY = sMat3::rotationY (eulers[1]);
      sMat3 rZ = sMat3::rotationZ (eulers[2]);

      sMat3 matrix1; matrix1.Identity();
      matrix1 = matrix1 * rX;
      matrix1 = matrix1 * rY;
      matrix1 = matrix1 * rZ;
      
      sMat3 matrix2; matrix2.Identity();
      matrix2 = rX * matrix2;
      matrix2 = rY * matrix2;
      matrix2 = rZ * matrix2;

      dMatrix matrixN1 = dGetIdentityMatrix();
      for (int i=0; i<3; i++) for (int j=0; j<3; j++) matrixN1[i][j] = matrix1[i][j];
      dMatrix matrixN2 = dGetIdentityMatrix();
      for (int i=0; i<3; i++) for (int j=0; j<3; j++) matrixN2[i][j] = matrix2[i][j];

      sVec3 proof1 = matrix1.ToEuler (m_pitchYawRoll);
      sVec3 proof2 = matrix2.ToEuler (m_pitchYawRoll);
      SystemTools::Log ("proof1 = (%f, %f, %f)\n", proof1[0], proof1[1], proof1[2]); // (0.534252, 0.036083, 1.161104)
      SystemTools::Log ("proof2 = (%f, %f, %f)\n", proof2[0], proof2[1], proof2[2]); // (0.200000, 0.500000, 1.100000)
      dVector euler0, euler1;
      matrixN1.GetEulerAngles (euler0, euler1, m_pitchYawRoll);
      SystemTools::Log ("proof1 Newton euler0 = (%f, %f, %f)\n", euler0[0], euler0[1], euler0[2]);
      SystemTools::Log ("proof1 Newton euler1 = (%f, %f, %f)\n", euler1[0], euler1[1], euler1[2]);
      matrixN2.GetEulerAngles (euler0, euler1, m_pitchYawRoll);
      SystemTools::Log ("proof2 Newton euler0 = (%f, %f, %f)\n", euler0[0], euler0[1], euler0[2]);
      SystemTools::Log ("proof2 Newton euler1 = (%f, %f, %f)\n", euler1[0], euler1[1], euler1[2]);

      SystemTools::Log ("transposing\n");

      matrix1 = matrix1.Transposed();
      proof1 = matrix1.ToEuler (m_pitchYawRoll);
      proof1 *= -1.0f;
      SystemTools::Log ("proof1 = (%f, %f, %f)\n", proof1[0], proof1[1], proof1[2]); // (0.200000, 0.500000, 1.100000)

      matrixN1 = matrixN1.Transpose();
      matrixN1.GetEulerAngles (euler0, euler1, m_pitchYawRoll);
      euler0 = euler0.Scale(-1.0f);
      euler1 = euler1.Scale(-1.0f);
      SystemTools::Log ("proof0 Newton euler0 = (%f, %f, %f)\n", euler0[0], euler0[1], euler0[2]);
      SystemTools::Log ("proof0 Newton euler1 = (%f, %f, %f)\n", euler1[0], euler1[1], euler1[2]);


output:

Code: Select all


proof1 = (0.534252, 0.036083, 1.161104)
proof2 = (0.200000, 0.500000, 1.100000)
proof1 Newton euler0 = (0.534252, 0.036083, 1.161104)
proof1 Newton euler1 = (-2.607340, 3.105509, -1.980489)
proof2 Newton euler0 = (0.200000, 0.500000, 1.100000)
proof2 Newton euler1 = (-2.941593, 2.641592, -2.041593)
transposing
proof1 = (0.200000, 0.500000, 1.100000)
proof0 Newton euler0 = (0.200000, 0.500000, 1.100000)
proof0 Newton euler1 = (-2.941593, 2.641592, -2.041593)


[misho]

You forgot to negate resulting angles
dvEuler1 = dvEuler1.Scale(-1)

Yeah, I noticed and fixed that later... still bupkis.

I looked into it a bit closer and implemented this: (it closely follows your approach)

Code: Select all

   TBEntity *ent;
   ent = g_sceneManager->GetEntity(index);
   dVector   dvEuler1, dvEuler2;
   dMatrix bMatrix;
   NewtonBodyGetMatrix(ent->nBody, &bMatrix[0][0]);
   bMatrix.GetEulerAngles(dvEuler1, dvEuler2, PYR);

   dMatrix rX = dMatrix(dvEuler1.m_x, 0.0, 0.0, ent->m_curPosition);
   dMatrix rY = dMatrix(0.0, dvEuler1.m_y, 0.0, ent->m_curPosition);
   dMatrix rZ = dMatrix(0.0, 0.0, dvEuler1.m_z, ent->m_curPosition);

   dMatrix matrix1; matrix1 = dGetIdentityMatrix();
   matrix1 = matrix1 * rX;
   matrix1 = matrix1 * rY;
   matrix1 = matrix1 * rZ;
   matrix1 = matrix1.Transpose();

   matrix1.GetEulerAngles(dvEuler1, dvEuler2, PYR);
   dvEuler1.Scale(-1.0f);

   Attitude->Pitch =   dvEuler1.m_x;
   Attitude->Bank =    dvEuler1.m_y;
   Attitude->Heading = dvEuler1.m_z;

Did I assemble rX, rY, rZ correctly?