CalcInertiaUtil.java
Go to the documentation of this file.
1 package com.generalrobotix.ui.util;
2 
3 import javax.vecmath.Matrix3d;
4 import javax.vecmath.Vector3d;
5 
6 public class CalcInertiaUtil {
7 
8  public static Vector3d calcScale(Matrix3d I, double m){
9  Matrix3d R = new Matrix3d();
10  Matrix3d II = new Matrix3d();
11  if (diagonalize(I,R,II)){
12  double sum = II.m00+II.m11+II.m22;
13  Vector3d sv = new Vector3d(
14  m*Math.sqrt(sum/II.m00),
15  m*Math.sqrt(sum/II.m11),
16  m*Math.sqrt(sum/II.m22));
17  return sv;
18  }else{
19  System.out.println("diagonalization failed"); //$NON-NLS-1$
20  }
21  return new Vector3d(0,0,0);
22  }
23 
31  private static boolean diagonalize(Matrix3d a, Matrix3d U, Matrix3d W){
32  int i=0,j=0,l,m,p,q,count;
33  double max,theta;
34  Matrix3d oldU = new Matrix3d();
35  Matrix3d newW = new Matrix3d();
36  W.set(a);
37 
38  //計算結果としてだされた直行行列を格納するための配列を単位行列に初期化しておく。
39  for(p=0;p<3;p++) {
40  for(q=0;q<3;q++) {
41  if(p==q){
42  U.setElement(p, q, 1.0);
43  }else{
44  U.setElement(p, q, 0.0);
45  }
46  }
47  }
48 
49  for(count=0;count<=10000;count++) {
50 
51  //配列olduは新たな対角化計算を行う前にかけてきた直行行列を保持する。
52  for(p=0;p<3;p++) {
53  for(q=0;q<3;q++) {
54  oldU.setElement(p, q, U.getElement(p, q));
55  }
56  }
57  //非対角要素の中から絶対値の最大のものを見つける
58  max=0.0;
59  for(p=0;p<3;p++) {
60  for(q=0;q<3;q++) {
61  if(max<Math.abs(W.getElement(p, q)) && p!=q) {
62  max=Math.abs(W.getElement(p, q));
63  //その最大のものの成分の行と列にあたる数を記憶しておく。
64  i=p;
65  j=q;
66  }
67  }
68  }
69  /*先ほど選んだ最大のものが指定の値より小さければ対角化終了*/
70  if(max < 1.0e-10) {
71  break;
72  }
73  /*条件によってシータの値を決める*/
74  if(W.getElement(i,i)==W.getElement(j,j)){
75  theta=Math.PI/4.0;
76  }else{
77  theta=Math.atan(-2*W.getElement(i,j)/(W.getElement(i,i)-W.getElement(j,j)))/2.0;
78  }
79 
80  //ここでこのときに実対称行列にかける個々の直行行列uが決まるが 特にここでの計算の意味はない。(する必要はない。)*/
81  double sth = Math.sin(theta);
82  double cth = Math.cos(theta);
83 
84  /*ここでいままで実対称行列にかけてきた直行行列を配列Uに入れる。*/
85  for(p=0;p<3;p++) {
86  U.setElement(p,i,oldU.getElement(p,i)*cth-oldU.getElement(p,j)*sth);
87  U.setElement(p,j,oldU.getElement(p,i)*sth+oldU.getElement(p,j)*cth);
88  }
89 
90  //対角化計算によってでた新たな実対称行列の成分を配列newaに入れる。
91  newW.setElement(i,i,W.getElement(i,i)*cth*cth
92  +W.getElement(j,j)*sth*sth-2.0*W.getElement(i,j)*sth*cth);
93  newW.setElement(j, j, W.getElement(i,i)*sth*sth
94  +W.getElement(j,j)*cth*cth+2.0*W.getElement(i,j)*sth*cth);
95  newW.setElement(i,j,0.0);
96  newW.setElement(j,i,0.0);
97  for(l=0;l<3;l++) {
98  if(l!=i && l!=j) {
99  newW.setElement(i,l,W.getElement(i,l)*cth-W.getElement(j,l)*sth);
100  newW.setElement(l,i,newW.getElement(i,l));
101  newW.setElement(j,l,W.getElement(i,l)*sth+W.getElement(j,l)*cth);
102  newW.setElement(l,j,newW.getElement(j,l));
103  }
104  }
105  for(l=0;l<3;l++) {
106  for(m=0;m<3;m++) {
107  if(l!=i && l!=j && m!=i && m!=j) newW.setElement(l, m, W.getElement(l,m));
108  }
109  }
110 
111  //次の対角化計算を行う行列の成分を配列aへ上書きする。
112  W.set(newW);
113 
114  }
115  if(count==10000) {
116  System.out.println("対角化するためにはまだ作業を繰り返す必要があります"); //$NON-NLS-1$
117  return false;
118  }else{
119  return true;
120  }
121  }
122 
123 }
static boolean diagonalize(Matrix3d a, Matrix3d U, Matrix3d W)
diagonalize symmetric matrix
png_uint_32 i
Definition: png.h:2735
static Vector3d calcScale(Matrix3d I, double m)
static int max(int a, int b)


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