AxisAngle4d.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * General Robotix Inc.
00008  * National Institute of Advanced Industrial Science and Technology (AIST) 
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;   //mag=2sin(th)
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 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:15