Quaternions

[Jesus]

I am trying to represent by means of a cube, an IMU (inertial measurement unit) and I am using the Euler angles, obtaining very low results. Could I work directly with quaternions ?. In the information of some page on GLScene, it indicates that there are auxiliary functions to work with rotation matrices and quaternions, but I have not seen anything.

Thank you for your attention, greetings.

[Achim Wrobel]

Hello,

I did the same work some time ago which worked very well finally.
What do you mean with "low results"?
I converted the quaternions to Euler Angles to be used with GLScene. If you dont want to create code for it, there are many proper codes found on the internet.

[Jerome.D (BeanzMaster)]

Hi for using Quaternion take a look in GLVectorGeometry unit you'l find this :

//------------------------------------------------------------------------------
// Quaternion functions
//------------------------------------------------------------------------------

type
   TEulerOrder = (eulXYZ, eulXZY, eulYXZ, eulYZX, eulZXY, eulZYX);

// Creates a quaternion from the given values
function QuaternionMake(const Imag: array of Single; Real : Single) : TQuaternion;
// Returns the conjugate of a quaternion
function QuaternionConjugate(const Q : TQuaternion) : TQuaternion;
// Returns the magnitude of the quaternion
function QuaternionMagnitude(const Q : TQuaternion) : Single;
// Normalizes the given quaternion
procedure NormalizeQuaternion(var Q : TQuaternion);

// Constructs a unit quaternion from two points on unit sphere
function QuaternionFromPoints(const V1, V2: TAffineVector): TQuaternion;
// Converts a unit quaternion into two points on a unit sphere
procedure QuaternionToPoints(const Q: TQuaternion; var ArcFrom, ArcTo: TAffineVector);
// Constructs a unit quaternion from a rotation matrix
function QuaternionFromMatrix(const mat : TMatrix) : TQuaternion;
{ Constructs a rotation matrix from (possibly non-unit) quaternion.
   Assumes matrix is used to multiply column vector on the left: 
   vnew = mat vold.
   Works correctly for right-handed coordinate system and right-handed rotations. }
function QuaternionToMatrix(quat : TQuaternion) : TMatrix;
{ Constructs an affine rotation matrix from (possibly non-unit) quaternion. }
function QuaternionToAffineMatrix(quat : TQuaternion) : TAffineMatrix;
// Constructs quaternion from angle (in deg) and axis
function QuaternionFromAngleAxis(const angle  : Single; const axis : TAffineVector) : TQuaternion;
// Constructs quaternion from Euler angles
function QuaternionFromRollPitchYaw(const r, p, y : Single) : TQuaternion;
// Constructs quaternion from Euler angles in arbitrary order (angles in degrees)
function QuaternionFromEuler(const x, y, z: Single; eulerOrder : TEulerOrder) : TQuaternion;

{ Returns quaternion product qL * qR.
   Note: order is important!
   To combine rotations, use the product QuaternionMuliply(qSecond, qFirst),
   which gives the effect of rotating by qFirst then qSecond. }
function QuaternionMultiply(const qL, qR : TQuaternion): TQuaternion;

{ Spherical linear interpolation of unit quaternions with spins.
   QStart, QEnd - start and end unit quaternions 
   t            - interpolation parameter (0 to 1) 
   Spin         - number of extra spin rotations to involve  }
function QuaternionSlerp(const QStart, QEnd: TQuaternion; Spin: Integer; t: Single): TQuaternion; overload;
function QuaternionSlerp(const source, dest: TQuaternion; const t : Single) : TQuaternion; overload;

[Jesus]

Thank you very much to both of you for answering. Excuse my English level and my delay in answering for work reasons. The use of Euler angles causes problems in the representation of objects, it is always recommended to use quaternions or rotation matrices (For example: attitude can only vary between +/- 90 degrees). Another problem that I have found is that the Euler angles are referenced to the local axes in such a way that after rotating the object when doing (0, 0, 0) it does not return to the initial position. Finally, the conversion of quaternions to Euler angles gives very unstable and erratic results.
Here I have a link to a video where you can see to the left a Java application that transforms the quaternions to rotation matrices and make the turn (perfect) and to the right my application in GLScene where you can see how the movements do not match the original.

Link of video

I would like to be able to rotate the object without using the Euler angles, using for example the rotation matrices. Is it possible?

Thanks again for your help and greetings.

[Achim Wrobel]

There are no problems in converting quaternions to Euler angles, if done correctly.
The problem of gimbal lock does not occur, if the IMU calculates everything in quaternions. There is no problem in converting quaternions into their actual Euler representation. Doing rotations with Euler is more critical due to gimbal locks.
I used a IMU and got a 100% correct representation with GLScene at 100 Hz, very fast AND smooth motions without any visual delay.
You are right, you have to return an object to its initial rotation (0,0,0) before "adding" another rotation. Then everyting stays correct.

[Jesus]

Thank you very much for answering Achim Wrobel. The IMU generates the data in quaternions and as you can see in the video, the Java application works correctly. The conversion that I am making, is the standard:

function GetEuler(qt: Quaternion): Angles;  //  OK   //
var
  Ang: Angles;
  qw2, qx2, qy2, qz2, test: Double;
  h, a, b: Double;
begin
        qw2 := Power(qt[0],2);
        qx2 := Power(qt[1],2);
        qy2 := Power(qt[2],2);
        qz2 := Power(qt[3],2);
        test := qt[1]qt[2]+qt[3]qt[0];

        if (test > 0.499)
        then begin
               Ang[1] := (360/PI)ArcTan2(qt[1],qt[0]);
               Ang[2] := 90;
               Ang[0] := 0;
               Result := Ang;
               Exit;
             end;

        if (test < -0.499)
        then begin
               Ang[1] := (-1)(360/PI)ArcTan2(qt[1],qt[0]);
               Ang[2] := -90;
               Ang[0] := 0;
               Result := Ang;
               Exit;
             end;

        h := ArcTan2(2qt[0]qt[2]-2qt[1]qt[3],1-2qy2-2qz2);
        a := ArcSin(2qt[1]qt[2]+2qt[3]qt[0]);
        b := ArcTan2(2qt[1]qt[0]-2qt[2]qt[3],1-2qx2-2qz2);

        // heading = atan2(2qyqw-2qxqz , 1 - 2qy2 - 2qz2)
        // attitude = asin(2qxqy + 2qzqw)
        // bank = atan2(2qxqw-2qyqz , 1 - 2qx2 - 2qz2)

        Ang[1] := h180/PI;
        Ang[2] := a180/PI;
        Ang[0] := b180/PI;
        Result := Ang;
end;

The results obtained (bad) can be seen in the previous video.
Another question is how to zero the object, since it is not enough to set the angles to zero. My idea is to disable the object, put the initial orientation, put the angles of Euler and enable the object, is it correct ?.

Greetings and thanks again.

[Achim Wrobel]

I did it this way:
...
GLCube1.ResetRotations;
GLCube1.TurnAngle := data.yaw;
GLCube1.PitchAngle := data.pitch;
GLCube1.RollAngle := -data.roll;
...

[Jerome.D (BeanzMaster)]

Hi Jesus I must check. In waiting you can check yourself your function's result here : https://www.andre-gaschler.com/rotationconverter/

In waiting i've attached a sample on Quaternion rotation try and tell me

it's for Lazarus

EDIT : Oups i've made one error i have invert Roll and Pitch

Ok Attachement updated

[Jesus]

Thank you all for your contributions. But I can not go from quaternions to Euler angles. From Euler to quaternions I have no problem, but conversely I do not get it. I have made a small application in Delphi to test it, go from Euler -> quaternions -> Euler. If someone has time and wants to look at it, I would appreciate it.

A greeting and thanks to all.

[Jerome.D (BeanzMaster)]

Impossible to test need more code :

TQuatAxesAng; ?????
EulerToQuat(Roll, Pitch, Yaw); ?????
VQuaternion: Quaternion; ?????
VQuatAxesAng: TQuatAxesAng; ?????
QuatToEuler.....
etc.......

Put the code of your Quaternion unit please

[Jerome.D (BeanzMaster)]

Sorry it's present an error while i converted to lazarus

[Jerome.D (BeanzMaster)]

Ok i checked you can't get rights result your Quaternion functions are wrong see the sceen and compare results with . At the begining EulerToQuat is already false
https://www.andre-gaschler.com/rotationconverter/

[Jerome.D (BeanzMaster)]

Ok i 've not time now, but i need also check glscene's quarternion something is wrong to
i must compare with my vector math lib

[Jesus]

Thanks again for the time spent on the problem. There is some inconsistency in the simulation performed, I work in degrees and not in radians. I attach an image with the results obtained when working in degrees and with ZYX conversion of Euler angles. I have reviewed the equations a thousand times and I do not see anything wrong, (it will surely be a small failure, difficult to see).
I think the solution is to use the quaternions as: [Ang Vx Vy Vz] and not transform to Euler angles.

Regards, and thank you very much.

[Jerome.D (BeanzMaster)]

Hi Jesus SF is come back, i can answer :)

I had a little time and i tracked, found and corrected the bug in glscene you said at the beginning of this topic about Euler to Quaternion correction.

Concerning your code, i took a look more deeply. Your EulToQuat and QuatToEul functions don't use the same Euler Order. EulToQuat is ZXY and QuatToEul is YZX so.
I've made more research on web.
The algorithm you've choose like is describe in many web reference is for standar euler used in major "aeronotic indistutry" YZX " for 3D or computer (nb : in real life is YXZ)
See :
https://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
http://ressources.univ-lemans.fr/AccesLibre/UM/Pedago/physique/02/meca/angleeuler.html
http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
https://www.learnopencv.com/rotation-matrix-to-euler-angles/
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/derivation/index.htm
* http://www.euclideanspace.com/maths/geometry/rotations/for/index.htm

for more informations
About QuatToEul if you want have good result you must derive the equation to extract result according Euler Order.

Now you'll can use directly GLScene functions from GLVectorGeometry Unit.

First in GLVectorGeometry unit
search and replace :

// QuaternionFromEuler
//
function QuaternionFromEuler(const x, y, z: Single; eulerOrder: TEulerOrder): TQuaternion;
// input angles in degrees
var
   gimbalLock: Boolean;
   quat1, quat2: TQuaternion;

   function EulerToQuat(const X, Y, Z: Single; eulerOrder: TEulerOrder) : TQuaternion;
   const
      cOrder : array [Low(TEulerOrder)..High(TEulerOrder)] of array [1..3] of Byte =
         ( (1, 2, 3), (1, 3, 2), (2, 1, 3),     // eulXYZ, eulXZY, eulYXZ,
           (3, 1, 2), (2, 3, 1), (3, 2, 1) );   // eulYZX, eulZXY, eulZYX
   var
      q : array [1..3] of TQuaternion;
   begin
      q[cOrder[eulerOrder][1]]:=QuaternionFromAngleAxis(X, XVector);
      q[cOrder[eulerOrder][2]]:=QuaternionFromAngleAxis(Y, YVector);
      q[cOrder[eulerOrder][3]]:=QuaternionFromAngleAxis(Z, ZVector);
      Result:=QuaternionMultiply(q[2], q[3]);
      Result:=QuaternionMultiply(q[1], Result);
   end;

const
   SMALL_ANGLE = 0.001;
begin
   NormalizeDegAngle(x);
   NormalizeDegAngle(y);
   NormalizeDegAngle(z);
   case EulerOrder of
      eulXYZ, eulZYX: GimbalLock := Abs(Abs(y) - 90.0) <= EPSILON2; // cos(Y) = 0;
      eulYXZ, eulZXY: GimbalLock := Abs(Abs(x) - 90.0) <= EPSILON2; // cos(X) = 0;
      eulXZY, eulYZX: GimbalLock := Abs(Abs(z) - 90.0) <= EPSILON2; // cos(Z) = 0;
   else
      Assert(False);
      gimbalLock:=False;
   end;
   if gimbalLock then begin
      case EulerOrder of
        eulXYZ, eulZYX: quat1 := EulerToQuat(x, y - SMALL_ANGLE, z, EulerOrder);
        eulYXZ, eulZXY: quat1 := EulerToQuat(x - SMALL_ANGLE, y, z, EulerOrder);
        eulXZY, eulYZX: quat1 := EulerToQuat(x, y, z - SMALL_ANGLE, EulerOrder);
      end;
      case EulerOrder of
        eulXYZ, eulZYX: quat2 := EulerToQuat(x, y + SMALL_ANGLE, z, EulerOrder);
        eulYXZ, eulZXY: quat2 := EulerToQuat(x + SMALL_ANGLE, y, z, EulerOrder);
        eulXZY, eulYZX: quat2 := EulerToQuat(x, y, z + SMALL_ANGLE, EulerOrder);
      end;
      Result := QuaternionSlerp(quat1, quat2, 0.5);
   end else begin
      Result := EulerToQuat(x, y, z, EulerOrder);
   end;
end;

by :

function QuaternionFromEuler(const x, y, z: Single; eulerOrder: TEulerOrder): TQuaternion;
const
  cOrder : array [Low(TEulerOrder)..High(TEulerOrder)] of array [1..3] of Byte =
    ( (1, 2, 3), (1, 3, 2), (2, 1, 3),     // eulXYZ, eulXZY, eulYXZ
      (2, 3, 1), (3, 1, 2), (3, 2, 1) );   // eulYZX, eulZXY, eulZYX
var
  q : array [1..3] of TQuaternion;

begin
  q[1]:=QuaternionFromAngleAxis(X, XVector);
  q[2]:=QuaternionFromAngleAxis(Y, YVector);
  q[3]:=QuaternionFromAngleAxis(Z, ZVector);

  Result:=QuaternionMultiply(q[cOrder[eulerOrder][1]], q[cOrder[eulerOrder][2]]);
  Result:=QuaternionMultiply(Result, q[cOrder[eulerOrder][3]]);
end;  

now in Interface add

procedure QuaternionToEuler(const Q: TQuaternion; eulerOrder: TEulerOrder; Out Roll, Pitch, Yaw : Double);
procedure QuaternionToAngleAxis(const Q: TQuaternion; out angle  : Single; out axis : TAffineVector);

and in implementation add

// Get euler angle from rotation matrix
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
// https://www.learnopencv.com/rotation-matrix-to-euler-angles/
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/derivation/index.htm
// http://www.euclideanspace.com/maths/geometry/rotations/for/index.htm

// @TODO : Take care of singularity at pole SUD  <-0.999999
procedure QuaternionToEuler(const Q: TQuaternion; eulerOrder: TEulerOrder; Out Roll, Pitch, Yaw : Double);
var
  rm : TMatrix;
  m00,m01,m02,m10,m11,m12,m20,m21,m22 : Double;
begin
  rm := QuaternionToMatrix(Q);
  // Rotation X
  m00 := rm.V[0].X;
  m01 := Clamp(rm.V[0].Y,-1,1);
  m02 := Clamp(rm.V[0].Z,-1,1);
  // Rotation Y
  m10 := Clamp(rm.V[1].X,-1,1);
  m11 := rm.V[1].Y;
  m12 := Clamp(rm.V[1].Z,-1,1);
  // Rotation Z
  m20 := Clamp(rm.V[2].X,-1,1);
  m21 := Clamp(rm.V[2].Y,-1,1);
  m22 := rm.V[2].Z;

  // Remember :
  // ROLL = X ; YAW = Y ; PITCH = Z
  case EulerOrder of
    eulXYZ :
      begin
        Yaw := ArcSin(m20);
        if Abs(m20) < 0.99999 then
        begin
          Roll := ArcTan2(-m21,m22);
          Pitch := ArcTan2(-m10,m00)
          //Yaw := -cPIDiv2; //DegToRad(-90);
        end
        else
        // if abs(m20)> 0.99999
        begin
          Roll := ArcTan2(m12,m11);
          Pitch := 0;
        end;
      end;
    eulXZY :
      begin
        Pitch := ArcSin(m10);
        if Abs(m10) < 0.99999 then
        begin
           Roll := ArcTan2(m12,m11);
           Yaw := ArcTan2(m20,m00);
        end
        else
        begin
          Roll := ArcTan2(-m21,m22);
          Yaw := 0;
        end;
      end;
    eulYXZ :
      begin
        Roll := ArcSin(-m21);
        if Abs(m21)< 0.99999 then
        begin
          Yaw :=ArcTan2(m20,m22);
          Pitch := ArcTan2(m01,m11);
        end
        else
        begin
          Yaw := ArcTan2(-m02,m00);
          Pitch := 0;
        end;
      end;
    eulYZX :
      begin
        Pitch := ArcSin(m01);
        if Abs(m01) < 0.99999 then
        begin
          Roll := ArcTan2(-m21,m11);
          Yaw := ArcTan2(-m02,m00);
        end
        else
        begin
          Roll := 0;
          Yaw := ArcTan2(m20,m22);
        end;
      end;
    eulZXY :
      begin
        Roll := ArcSin(m12);
        if Abs(m21)< 0.99999 then
        begin
          Yaw := ArcTan2(-m02,m22);
          Pitch := ArcTan2(-m10,m11);
        end
        else
        begin
          Yaw := 0;
          Pitch := ArcTan2(m01,m00);
        end;
      end;
    eulZYX :
      begin
        Yaw := ArcSin(m02);
        if Abs(m02) < 0.99999 then
        begin
          Roll := ArcTan2(m12,m22);
          Pitch := ArcTan2(m01,m00);
        end
        else
        begin
          Roll := 0;
          Pitch :=  ArcTan2(-m10,m11);
        end;
      end;
  end;
  Roll := RadToDeg(Roll);
  Pitch := RadToDeg(Pitch);
  Yaw := RadToDeg(Yaw);
end;

I've made many many test and draws some 3x3 grids with numbers, letters and some others strange glyphs on paper to approximatively understand how to swap data to get good result. I'm not "a real math man" so some (often) time, i must find others ways for solving problems. I cannot explain how it work.

Now you can directly using GLScene's functions, try and tell me

Remember with Quaternion EULER ORDER is really important
and keep in your mind this :

X = Roll = Bank = Tilt
Y = Yaw = Heading = Azimuth
Z = Pitch = Attitude = Elevation

[Jerome.D (BeanzMaster)]

Oups i forget :

procedure QuaternionToAngleAxis(const Q: TQuaternion; out angle  : Single; out axis : TAffineVector);
var
  qw2: Double;
  den: Double;
begin
  qw2 := Q.RealPart*Q.RealPart;
  Angle := RadToDeg(2*ArcCos(Q.RealPart));
  den := Sqrt(1-qw2);
  if den <> 0 then
  begin
    Axis.x := Q.ImagPart.X/den;
    Axis.y := Q.ImagPart.Y/den;
    Axis.z := Q.ImagPart.Z/den;
  end
  else
  begin
    Axis.x := 0;
    Axis.y := 0;
    Axis.z := 0;
  end;
end;     

[Jesus]

Wow, great job. I thought the problem could be in the XYZ order of the angles, but I thought that by deriving from the quaternions, the order would not matter. I will implement the indicated functions and test the results and post them in the forum. One question, why do not they include these functions in GLScene ?.

A greeting.

[Jerome.D (BeanzMaster)]

Thanks

One question, why do not they include these functions in GLScene ?.

It will do. Pavel should do update for Delphi soon. And for Lazarus i'm actually preparing a big update so i'll upload all to svn at one time.

I'm wiating for your tests results. Juste one thing, QuaternionToEuler can have bug with singularity at pole sud (<-1.0) I don't take in charge this.

[Jesus]

I can not find the "clamp" function, is it new? or in which unit it is defined.

Greetings.

[Jerome.D (BeanzMaster)]

It s clampsingle or double under delphi iirc.

Function clamp(a,b,c:double):double;
Begin
  Result := a;
    If a<b then result:=b
    Else  If a>c then result:=c;
End;

[Jesus]

I have tried the new functions and they work perfectly. Thank you very much Jerome.D for your help, it has been decisive to be able to continue advancing in my project.
Anyway I want to comment on the use of Euler angles, in most cases they advise against using them, the reason for avoiding them is that they present singularities (division by zero) and one of the angles is limited to + - 90º, and this is a very important limitation.
Following with my initial question: could not work with the quaternions or with the rotation matrices directly on the object and not pass it to Euler angles?

Thanks again to all and especially to Jerome.D

P.D .: I will post a video with the advances when I finish configuring the program.

[Jerome.D (BeanzMaster)]

I Have some questions :
What the type of data you retreive from your IMU, exactly ? and what the order Pitch/Yaw/Roll ?
Your project is for doing what at final ?

It seems you can escape to the use of quaternion :

Unit quaternions, also known as versors, provide a convenient mathematical notation for representing orientations and rotations of objects in three dimensions. Compared to Euler angles they are simpler to compose and avoid the problem of gimbal lock. Compared to rotation matrices they are more compact, more numerically stable, and more efficient. Quaternions have applications in computer graphics, computer vision, robotics, navigation, molecular dynamics, flight dynamics,[1] orbital mechanics of satellites[2] and crystallographic texture analysis.[3]
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

Under GLScene you can use matrix of course. have you tried Achim Wrobel solution ? :

I did it this way:
...
GLCube1.ResetRotations;
GLCube1.TurnAngle := data.yaw;
GLCube1.PitchAngle := data.pitch;
GLCube1.RollAngle := -data.roll;
...

All vector/matrix/quaternions :) functions that can help you are in GLVectorGeometry unit
Take a look also to this topic :
https://sourceforge.net/p/glscene/discussion/93606/thread/6488283b/

maybe that will give you clues

[Jerome.D (BeanzMaster)]

Just share with you a very interesting and more technicals in pratice article :
http://www.starlino.com/imu_guide.html

[robinot raynald]

bjr quelqu un arait il le GLScene_Quaternions_TestConversion. en delphi pas en lazarus merci