AxisAngle4d.java
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
10 
11 package com.generalrobotix.ui.util;
12 
13 import javax.vecmath.AxisAngle4f;
14 import javax.vecmath.Matrix3d;
15 import javax.vecmath.Vector3d;
16 
17 @SuppressWarnings("serial")
18 public class AxisAngle4d extends javax.vecmath.AxisAngle4d {
19 
20  private static double EPS = 1.0e-6;
21 
22  public AxisAngle4d(AxisAngle4d a1){
23  super(a1);
24  }
25 
26  public AxisAngle4d(AxisAngle4f a1){
27  super(a1);
28  }
29 
30  public AxisAngle4d(double x, double y, double z, double angle){
31  super(x, y, z, angle);
32  }
33 
34  public AxisAngle4d(Vector3d axis, double angle){
35  super(axis, angle);
36  }
37 
38  public AxisAngle4d(double[] ds) {
39  super(ds);
40  }
41 
42  public AxisAngle4d() {
43  super();
44  }
45 
46  public void setMatrix(Matrix3d m1)
47  {
48  x = m1.m21 - m1.m12;
49  y = m1.m02 - m1.m20;
50  z = m1.m10 - m1.m01;
51 
52  double mag = x*x + y*y + z*z; //mag=2sin(th)
53  double cos = 0.5*(m1.m00 + m1.m11 + m1.m22 - 1.0);
54 
55  if (mag > EPS ) {
56  mag = Math.sqrt(mag);
57  double sin = 0.5*mag;
58  angle = Math.atan2(sin, cos);
59  double invMag = 1.0/mag;
60  x = x*invMag;
61  y = y*invMag;
62  z = z*invMag;
63  } else {
64  if( Math.abs(cos-1.0)<EPS ){
65  x = 0.0f;
66  y = 1.0f;
67  z = 0.0f;
68  angle = 0.0f;
69  }else{
70  double x0 = m1.m00+1;
71  double y0 = m1.m11+1;
72  double z0 = m1.m22+1;
73  if( x0 < 0 && x0 > -EPS )
74  x0 = 0;
75  if( y0 < 0 && y0 > -EPS )
76  y0 = 0;
77  if( z0 < 0 && z0 > -EPS )
78  z0 = 0;
79  x = Math.sqrt(x0*0.5);
80  y = Math.sqrt(y0*0.5);
81  z = Math.sqrt(z0*0.5);
82  angle = Math.PI;
83 
84  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}};
85  Matrix3d m2 = new Matrix3d();
86  int j=0;
87  double min=0.0;
88  for(int i=0; i<8; i++){
89  m2.set(new AxisAngle4d(sign[i][0]*x, sign[i][1]*y, sign[i][2]*z, angle));
90  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) +
91  (m1.m10-m2.m10)*(m1.m10-m2.m10) + (m1.m11-m2.m11)*(m1.m11-m2.m11) + (m1.m12-m2.m12)*(m1.m12-m2.m12) +
92  (m1.m20-m2.m20)*(m1.m20-m2.m20) + (m1.m21-m2.m21)*(m1.m21-m2.m21) + (m1.m22-m2.m22)*(m1.m22-m2.m22) ;
93  if(i==0 || err < min){
94  j = i;
95  min = err;
96  }
97  }
98  x *= sign[j][0];
99  y *= sign[j][1];
100  z *= sign[j][2];
101  }
102  }
103  }
104 }
105 
* x
Definition: IceUtils.h:98
static int min(int a, int b)
#define EPS
png_uint_32 i
Definition: png.h:2735
AxisAngle4d(double x, double y, double z, double angle)
* y
Definition: IceUtils.h:97
AxisAngle4d(Vector3d axis, double angle)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:02