CalcInertiaUtil.java
Go to the documentation of this file.
00001 package com.generalrobotix.ui.util;
00002 
00003 import javax.vecmath.Matrix3d;
00004 import javax.vecmath.Vector3d;
00005 
00006 public class CalcInertiaUtil {
00007         
00008         public static Vector3d calcScale(Matrix3d I, double m){
00009                 Matrix3d R = new Matrix3d();
00010                 Matrix3d II = new Matrix3d();
00011                 if (diagonalize(I,R,II)){
00012                         double sum = II.m00+II.m11+II.m22;
00013                         Vector3d sv = new Vector3d(
00014                                         m*Math.sqrt(sum/II.m00),
00015                                         m*Math.sqrt(sum/II.m11),
00016                                         m*Math.sqrt(sum/II.m22));
00017                         return sv;
00018                 }else{
00019                         System.out.println("diagonalization failed"); //$NON-NLS-1$
00020                 }       
00021                 return new Vector3d(0,0,0);
00022         }
00023         
00031     private static boolean diagonalize(Matrix3d a, Matrix3d U, Matrix3d W){
00032         int i=0,j=0,l,m,p,q,count;
00033         double max,theta;
00034         Matrix3d oldU = new Matrix3d();
00035         Matrix3d newW = new Matrix3d();
00036         W.set(a);
00037 
00038         //計算結果としてだされた直行行列を格納するための配列を単位行列に初期化しておく。
00039         for(p=0;p<3;p++) {
00040                 for(q=0;q<3;q++) {
00041                         if(p==q){
00042                                 U.setElement(p, q, 1.0);
00043                         }else{
00044                                 U.setElement(p, q, 0.0);
00045                         }
00046                 }
00047         }
00048 
00049         for(count=0;count<=10000;count++) {
00050 
00051                 //配列olduは新たな対角化計算を行う前にかけてきた直行行列を保持する。
00052                 for(p=0;p<3;p++) {
00053                         for(q=0;q<3;q++) {
00054                                 oldU.setElement(p, q, U.getElement(p, q));
00055                         }
00056                 }
00057                 //非対角要素の中から絶対値の最大のものを見つける
00058                 max=0.0;
00059                 for(p=0;p<3;p++) {
00060                         for(q=0;q<3;q++) {
00061                                 if(max<Math.abs(W.getElement(p, q)) && p!=q) {
00062                                         max=Math.abs(W.getElement(p, q));
00063                                         //その最大のものの成分の行と列にあたる数を記憶しておく。
00064                                         i=p;
00065                                         j=q;
00066                                 }
00067                         }
00068                 }
00069                 /*先ほど選んだ最大のものが指定の値より小さければ対角化終了*/
00070                 if(max < 1.0e-10) {
00071                         break;
00072                 }
00073                 /*条件によってシータの値を決める*/
00074                 if(W.getElement(i,i)==W.getElement(j,j)){
00075                         theta=Math.PI/4.0;
00076                 }else{
00077                         theta=Math.atan(-2*W.getElement(i,j)/(W.getElement(i,i)-W.getElement(j,j)))/2.0;
00078                 }
00079 
00080                 //ここでこのときに実対称行列にかける個々の直行行列uが決まるが 特にここでの計算の意味はない。(する必要はない。)*/
00081                 double sth = Math.sin(theta);
00082                 double cth = Math.cos(theta);
00083 
00084                 /*ここでいままで実対称行列にかけてきた直行行列を配列Uに入れる。*/
00085                 for(p=0;p<3;p++) {
00086                         U.setElement(p,i,oldU.getElement(p,i)*cth-oldU.getElement(p,j)*sth);
00087                         U.setElement(p,j,oldU.getElement(p,i)*sth+oldU.getElement(p,j)*cth);
00088                 }
00089 
00090                 //対角化計算によってでた新たな実対称行列の成分を配列newaに入れる。
00091                 newW.setElement(i,i,W.getElement(i,i)*cth*cth
00092                                 +W.getElement(j,j)*sth*sth-2.0*W.getElement(i,j)*sth*cth);
00093                 newW.setElement(j, j, W.getElement(i,i)*sth*sth
00094                                 +W.getElement(j,j)*cth*cth+2.0*W.getElement(i,j)*sth*cth);
00095                 newW.setElement(i,j,0.0);
00096                 newW.setElement(j,i,0.0);
00097                 for(l=0;l<3;l++) {
00098                         if(l!=i && l!=j) {
00099                                 newW.setElement(i,l,W.getElement(i,l)*cth-W.getElement(j,l)*sth);
00100                                 newW.setElement(l,i,newW.getElement(i,l));
00101                                 newW.setElement(j,l,W.getElement(i,l)*sth+W.getElement(j,l)*cth);
00102                                 newW.setElement(l,j,newW.getElement(j,l));
00103                         }
00104                 }
00105                 for(l=0;l<3;l++) {
00106                         for(m=0;m<3;m++) {
00107                                 if(l!=i && l!=j && m!=i && m!=j) newW.setElement(l, m, W.getElement(l,m));
00108                         }
00109                 }
00110 
00111                 //次の対角化計算を行う行列の成分を配列aへ上書きする。
00112                 W.set(newW);
00113 
00114         }
00115         if(count==10000) {
00116                 System.out.println("対角化するためにはまだ作業を繰り返す必要があります"); //$NON-NLS-1$
00117                 return false;
00118         }else{
00119                 return true;
00120         }
00121     }
00122 
00123 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:52