Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 package com.generalrobotix.ui.util;
00012
00013 import javax.vecmath.AxisAngle4f;
00014 import javax.vecmath.Matrix3d;
00015 import javax.vecmath.Vector3d;
00016
00017 @SuppressWarnings("serial")
00018 public class AxisAngle4d extends javax.vecmath.AxisAngle4d {
00019
00020 private static double EPS = 1.0e-6;
00021
00022 public AxisAngle4d(AxisAngle4d a1){
00023 super(a1);
00024 }
00025
00026 public AxisAngle4d(AxisAngle4f a1){
00027 super(a1);
00028 }
00029
00030 public AxisAngle4d(double x, double y, double z, double angle){
00031 super(x, y, z, angle);
00032 }
00033
00034 public AxisAngle4d(Vector3d axis, double angle){
00035 super(axis, angle);
00036 }
00037
00038 public AxisAngle4d(double[] ds) {
00039 super(ds);
00040 }
00041
00042 public AxisAngle4d() {
00043 super();
00044 }
00045
00046 public void setMatrix(Matrix3d m1)
00047 {
00048 x = m1.m21 - m1.m12;
00049 y = m1.m02 - m1.m20;
00050 z = m1.m10 - m1.m01;
00051
00052 double mag = x*x + y*y + z*z;
00053 double cos = 0.5*(m1.m00 + m1.m11 + m1.m22 - 1.0);
00054
00055 if (mag > EPS ) {
00056 mag = Math.sqrt(mag);
00057 double sin = 0.5*mag;
00058 angle = Math.atan2(sin, cos);
00059 double invMag = 1.0/mag;
00060 x = x*invMag;
00061 y = y*invMag;
00062 z = z*invMag;
00063 } else {
00064 if( Math.abs(cos-1.0)<EPS ){
00065 x = 0.0f;
00066 y = 1.0f;
00067 z = 0.0f;
00068 angle = 0.0f;
00069 }else{
00070 double x0 = m1.m00+1;
00071 double y0 = m1.m11+1;
00072 double z0 = m1.m22+1;
00073 if( x0 < 0 && x0 > -EPS )
00074 x0 = 0;
00075 if( y0 < 0 && y0 > -EPS )
00076 y0 = 0;
00077 if( z0 < 0 && z0 > -EPS )
00078 z0 = 0;
00079 x = Math.sqrt(x0*0.5);
00080 y = Math.sqrt(y0*0.5);
00081 z = Math.sqrt(z0*0.5);
00082 angle = Math.PI;
00083
00084 int[][] sign = {{1,1,1},{1,1,-1},{1,-1,1},{1,-1,-1},{-1,1,1},{-1,1,-1},{-1,-1,1},{-1,-1,-1}};
00085 Matrix3d m2 = new Matrix3d();
00086 int j=0;
00087 double min=0.0;
00088 for(int i=0; i<8; i++){
00089 m2.set(new AxisAngle4d(sign[i][0]*x, sign[i][1]*y, sign[i][2]*z, angle));
00090 double err = (m1.m00-m2.m00)*(m1.m00-m2.m00) + (m1.m01-m2.m01)*(m1.m01-m2.m01) + (m1.m02-m2.m02)*(m1.m02-m2.m02) +
00091 (m1.m10-m2.m10)*(m1.m10-m2.m10) + (m1.m11-m2.m11)*(m1.m11-m2.m11) + (m1.m12-m2.m12)*(m1.m12-m2.m12) +
00092 (m1.m20-m2.m20)*(m1.m20-m2.m20) + (m1.m21-m2.m21)*(m1.m21-m2.m21) + (m1.m22-m2.m22)*(m1.m22-m2.m22) ;
00093 if(i==0 || err < min){
00094 j = i;
00095 min = err;
00096 }
00097 }
00098 x *= sign[j][0];
00099 y *= sign[j][1];
00100 z *= sign[j][2];
00101 }
00102 }
00103 }
00104 }
00105