# Matrix 2 quaternion not stable

## Details

• Type: Task
• Status: Open
• Priority: Trivial
• Resolution: Unresolved
• Affects Version/s: 1.4.0
• Fix Version/s:
• Component/s:
• Labels:
None
• Environment:

Operating System: All
Platform: All

• Issuezilla Id:
564
• Status Whiteboard:
Hide

owner-needed

Show
owner-needed

## Description

The conversion from matrix to quaternion and back is not stable. A small test
program appended shows that. Raising the eps2 limit for the trace from 1e-30 to
1e-3 for instance seems to solve the problem.

In cases when the trace is very small, computed quaternions seems to have not
normalized values (all near 0 for instance) and normalization creates a complete
new quaternion which of course results in a different matrix.

**************************************

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;

/**

• @author Volker Martens, martens@rob.uni-luebeck.de
*
*/
public class QuaternionenTest
{

public static void getQuaternion(double[] quat, double[] a)
{
double trace = a[0] + a[5] + a[10] + 1;
////////////////////////////
if (trace >= 0.001)
////////////////////////////

{ double s = 0.5 / Math.sqrt(trace); quat[0] = 0.25 / s; // qw = 0.5 * sqrt(trace); quat[1] = (a[6] - a[9]) * s; // qx quat[2] = (a[8] - a[2]) * s; // qy quat[3] = (a[1] - a[4]) * s; // qz }

else
{
if ((a[0] > a[5]) && (a[0] > a[10]))

{ double s = 2*Math.sqrt(1+a[0]-a[5]-a[10]); quat[1] = 0.25 * s; quat[2] = (a[4] + a[1]) / s; quat[3] = (a[8] + a[2]) / s; quat[0] = (a[6] - a[9]) / s; }

else if (a[5] > a[10])

{ double s = 2*Math.sqrt(1+a[5]-a[0]-a[10]); quat[1] = (a[4] + a[1]) / s; quat[2] = 0.25 * s; quat[3] = (a[9] + a[6]) / s; quat[0] = (a[8] - a[2]) / s; }

else

{ double s = 2*Math.sqrt(1+a[10]-a[5]-a[0]); quat[1] = (a[8] + a[2]) / s; quat[2] = (a[6] + a[9]) / s; quat[3] = 0.25 * s; quat[0] = (a[1] - a[4]) / s; }

}

// normalize quaternion
double normalizedQuat = Math.sqrt(quat[0]*quat[0] + quat[1]*quat[1] +
quat[2]*quat[2] + quat[3]*quat[3]);
for (int i=0; i<4; i++)

{ quat[i] = quat[i] / normalizedQuat; }

}

public static Matrix3d YawPitchRoll(double yaw, double pitch, double roll)

{ // yaw around z Matrix3d Rz = new Matrix3d(); Rz.rotZ(Math.toRadians(yaw)); // pitch around y Matrix3d Ry = new Matrix3d(); Ry.rotY(Math.toRadians(pitch)); // roll around x Matrix3d Rx = new Matrix3d(); Rx.rotX(Math.toRadians(roll)); // Pnew = Rz*Ry*Rx*Pold Rz.mul(Ry); Rz.mul(Rx); return Rz; }

public static void QuaternionenTest3d()
{
Quat4d quat = new Quat4d();
Matrix3d mat2 = new Matrix3d();
double[] ypr = new double[3];

double[] q = new double[4];
double[] m = new double[16];
for (int i=0; i<16; i++)
m[i] = 0;
m[15] = 1;
Quat4d q2 = new Quat4d();

for (int a=-181; a<181; a++)
{
ypr[0] = a;
for (int b=-181; b<181; b++)
{
ypr[1] = b;
for (int c=-91; c<91; c++)
{
ypr[2] = c;//*0.01;
Matrix3d mat = CoordSystem.YawPitchRoll(ypr[0], ypr[1], ypr[2]);

quat.set(mat);
quat.normalize();
mat2.setIdentity();
mat2.set(quat);
double diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.001)

{ System.out.println("Java: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); // System.out.println(mat); // System.out.println(quat); // System.out.println(mat2); }

// in our method, matrix is columnwise!!!
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
m[k*4+j] = mat.getElement(j, k);
getQuaternion(q,m);
mat2.setIdentity();
q2.w = q[0];
q2.x = q[1];
q2.y = q[2];
q2.z = q[3];
mat2.set(q2);
diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.001)

{ System.out.println("with other eps: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); // System.out.println(mat); // System.out.println(q2); // System.out.println(mat2); }

}
}
}
}

public static void QuaternionenTest4d()
{
Quat4d quat = new Quat4d();
Matrix4d mat2 = new Matrix4d();
double[] ypr = new double[3];

double[] q = new double[4];
double[] m = new double[16];
for (int i=0; i<16; i++)
m[i] = 0;
m[15] = 1;
Quat4d q2 = new Quat4d();

for (int a=-181; a<181; a++)
{
ypr[0] = a;//*0.01;
// System.out.println(ypr[0]" "+ypr[1]" "+ypr[2]);
for (int b=-181; b<181; b++)
{
ypr[1] = b;//*0.01;
for (int c=-91; c<91; c++)
{
ypr[2] = c;//*0.01;
Matrix3d mat = CoordSystem.YawPitchRoll(ypr[0], ypr[1], ypr[2]);
Matrix4d m4d = new Matrix4d(mat,new Vector3d(),1);
quat.set(m4d);
quat.normalize();
mat2.setIdentity();
mat2.set(quat);
double diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.01)

{ System.out.println("Java: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); // System.out.println(mat); // System.out.println(quat); // System.out.println(mat2); }

// in our method, matrix is columnwise!!!
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
m[k*4+j] = mat.getElement(j, k);
getQuaternion(q,m);
mat2.setIdentity();
q2.w = q[0];
q2.x = q[1];
q2.y = q[2];
q2.z = q[3];
mat2.set(q2);
diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.01)

{ System.out.println("with other eps: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); // System.out.println(mat); // System.out.println(q2); // System.out.println(mat2); }

}
}
}
}

public static void traceChange()
{
Quat4d quat = new Quat4d();
Matrix3d mat2 = new Matrix3d();
double[] ypr = new double[3];

double[] q = new double[4];
double[] m = new double[16];
for (int i=0; i<16; i++)
m[i] = 0;
m[15] = 1;
Quat4d q2 = new Quat4d();

Matrix3d mat = CoordSystem.YawPitchRoll(180.0,0,45);
// System.out.println(mat);
// mat = CoordSystem.YawPitchRoll(0.0,180,45);
// System.out.println(mat);

ypr[0] = 180;
ypr[1] = 0;
ypr[2] = 45;
for (int c=0; c<101; c++)
{
ypr[0] = 180+(c-50)*0.1;
mat = CoordSystem.YawPitchRoll(ypr[0], ypr[1], ypr[2]);
// System.out.println(mat);
quat.set(mat);
quat.normalize();
mat2.setIdentity();
mat2.set(quat);
double diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.001)

{ System.out.println("Java: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); System.out.println(mat); System.out.println(quat); System.out.println(mat2); }

// in our method, matrix is columnwise!!!
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
m[k*4+j] = mat.getElement(j, k);
getQuaternion(q,m);
mat2.setIdentity();
q2.w = q[0];
q2.x = q[1];
q2.y = q[2];
q2.z = q[3];
mat2.set(q2);
diff = 0;
for (int j=0; j<3; j++)
for (int k=0; k<3; k++)
diff += Math.abs(mat2.getElement(j, k)-mat.getElement(j, k));
if (diff > 0.001)

{ System.out.println("with other eps: "+ypr[0]+" "+ypr[1]+" "+ypr[2]+" => "+diff); System.out.println(mat); System.out.println(q2); System.out.println(mat2); }

}
}

public static void main(String[] args)

{ QuaternionenTest.QuaternionenTest3d(); // QuaternionenTest.QuaternionenTest4d(); QuaternionenTest.traceChange(); }

}

## Activity

Hide
kcr added a comment -

Set to P5 owner-needed.

Show
kcr added a comment - Set to P5 owner-needed.

## People

• Assignee:
java3d-issues
Reporter:
rasenderroland
• Votes:
0 Vote for this issue
Watchers:
0 Start watching this issue

## Dates

• Created:
Updated: