Details

    • Type: Task Task
    • Status: Open
    • Priority: Trivial Trivial
    • Resolution: Unresolved
    • Affects Version/s: 1.4.0
    • Fix Version/s: not determined
    • Component/s: vecmath
    • 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: