FittingInfo.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  */
00017 package com.generalrobotix.ui.view.tdview;
00018 import com.sun.j3d.utils.picking.*;
00019 import com.sun.j3d.utils.geometry.Primitive;
00020 import com.sun.j3d.utils.geometry.Cone;
00021 import com.sun.j3d.utils.geometry.Cylinder;
00022 import com.sun.j3d.utils.geometry.Sphere;
00023 
00024 import javax.media.j3d.*;
00025 import javax.vecmath.*;
00026 
00027 // Connect(Fitting) 関連のデータ
00028 
00029 public class FittingInfo {
00031     Point3f p3fIntersect;
00033     Vector3f v3fNormal;
00035     Point3f p3fIntersectVW;
00037     Vector3f v3fNormalVW;
00039     Point3d[] p3dPrimitivePoints;
00040 
00042     TransformGroup tgFittingTarget;
00043     TransformGroup tgFittingTargetBefore;
00044     // ユーザへの視覚的表示のための部品群
00045     public final static int iNumberOfSphere = 4;
00047     BranchGroup bgAddTop = new BranchGroup();
00049     TransformGroup[] sphTrans = new TransformGroup [iNumberOfSphere];
00050     TransformGroup tgArrowTranslate = new TransformGroup();
00051     TransformGroup tgArrowRotate = new TransformGroup();
00053     Cone cone;
00055     Cylinder cylinder;
00056 
00064     public FittingInfo(
00065         Color3f c3fArrow,
00066         Color3f c3fSphere,
00067         boolean bInvertArrow
00068     ) {
00069         // 球の追加
00070         Sphere[] sph = new Sphere [iNumberOfSphere];
00071         bgAddTop.setCapability(BranchGroup.ALLOW_DETACH);
00072         tgArrowTranslate.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
00073         tgArrowTranslate.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
00074         tgArrowTranslate.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
00075         tgArrowTranslate.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
00076         tgArrowTranslate.setCapability(
00077             TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ
00078         );
00079         tgArrowRotate.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
00080         tgArrowRotate.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
00081         float fScaleFactor = 0.1f;
00082         Appearance spherelook = new Appearance();
00083         Color3f black = new Color3f(0.0f, 0.0f, 0.0f);
00084         Color3f white = new Color3f(1.0f, 1.0f, 1.0f);
00085         spherelook.setMaterial(
00086             new Material(c3fSphere, black, c3fSphere, white, 50.0f)
00087         );
00088 
00089         // 球用の設定をすます
00090         for (int i = 0 ;i < iNumberOfSphere ; i++) {
00091             sph[i] = new Sphere(0.25f * fScaleFactor, spherelook);
00092             sph[i].setPickable(false);
00093             sphTrans[i] = new TransformGroup ();
00094             sphTrans[i].setCapability (TransformGroup.ALLOW_TRANSFORM_READ);
00095             sphTrans[i].setCapability (TransformGroup.ALLOW_TRANSFORM_WRITE);
00096 
00097             // Add sphere, transform
00098             bgAddTop.addChild (sphTrans[i]);
00099             sphTrans[i].addChild (sph[i]);
00100         }
00101 
00102         // 矢印の追加
00103         Appearance arrowlook = new Appearance();
00104         arrowlook.setMaterial(
00105             new Material(c3fArrow, black, c3fArrow, white, 50.0f)
00106         );
00107 
00108         cone =
00109             new Cone(
00110                 0.5f * fScaleFactor,
00111                 1.0f * fScaleFactor,
00112                 Cone.GENERATE_NORMALS,
00113                 arrowlook
00114             );
00115         cone.setCapability(Primitive.ENABLE_GEOMETRY_PICKING);
00116         for (int i = Cone.BODY; i <= Cone.CAP; i ++) {
00117             PickTool.setCapabilities(cone.getShape(i), PickTool.INTERSECT_FULL);
00118         }
00119         cone.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
00120         cone.setCapability(Primitive.ALLOW_PICKABLE_READ);
00121         cone.setCapability(Primitive.ALLOW_PICKABLE_WRITE);
00122         cylinder =
00123             new Cylinder(0.2f * fScaleFactor, 2.0f * fScaleFactor, arrowlook);
00124         cylinder.setCapability(Primitive.ENABLE_GEOMETRY_PICKING);
00125         cylinder.setCapability(Primitive.ALLOW_PICKABLE_READ);
00126         cylinder.setCapability(Primitive.ALLOW_PICKABLE_WRITE);
00127         for(int i = Cylinder.BODY; i <= Cylinder.BOTTOM; i ++) {
00128             PickTool.setCapabilities(
00129                 cylinder.getShape(i),
00130                 PickTool.INTERSECT_FULL
00131             );
00132         }
00133         cylinder.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
00134         // 二つの部品をくっつけるために三角錐のほうを移動しておく
00135         TransformGroup tgCone = new TransformGroup();
00136         Transform3D t3dCone = new Transform3D();
00137         TransformGroup tgCylinder = new TransformGroup();
00138         Transform3D t3dCylinder = new Transform3D();
00139         t3dCone.setTranslation(new Vector3f(0.0f,1.5f * fScaleFactor,0.0f));
00140         tgCone.setTransform(t3dCone);
00141         tgCone.addChild(cone);
00142         if (bInvertArrow) {
00143             t3dCylinder.setTranslation(
00144                 new Vector3f(0.0f, 2.0f * fScaleFactor, 0.0f)
00145             );
00146             t3dCylinder.setRotation(
00147                 new AxisAngle4f(new Vector3f(1.0f, 0.0f, 0.0f), (float)Math.PI)
00148             );
00149         } else {
00150             t3dCylinder.setTranslation(
00151                 new Vector3f(0.0f, fScaleFactor, 0.0f)
00152             );
00153         }
00154         tgCylinder.addChild(tgCone);
00155         tgCylinder.setTransform(t3dCylinder);
00156         tgCylinder.addChild(cylinder);
00157         tgArrowRotate.addChild(tgCylinder);
00158         tgArrowTranslate.addChild(tgArrowRotate);
00159         bgAddTop.addChild(tgArrowTranslate);
00160     }
00161 
00166     public void addForDisplay() {
00167         if(tgFittingTarget == null) {
00168             System.err.println("物体を表示する対象の TG が設定されていません");
00169         }
00170 
00171         // world 座標における法線を割り出す
00172         v3fNormalVW = new Vector3f();
00173         if (p3dPrimitivePoints.length >= 3) {
00174             Point3d pt1 = new Point3d();
00175             Point3d pt2 = new Point3d();
00176             pt1.sub(p3dPrimitivePoints[1],p3dPrimitivePoints[0]);
00177             pt2.sub(p3dPrimitivePoints[2],p3dPrimitivePoints[1]);
00178             Vector3f vecOne = new Vector3f(pt1);
00179             Vector3f vecTwo = new Vector3f(pt2);
00180             // 外積を currVec ベクトルに入れる
00181             v3fNormalVW.cross(vecOne,vecTwo);
00182             double dScale =
00183                 Math.sqrt(
00184                     v3fNormalVW.x * v3fNormalVW.x +
00185                     v3fNormalVW.y * v3fNormalVW.y +
00186                     v3fNormalVW.z * v3fNormalVW.z
00187                 );
00188             v3fNormalVW.x /= dScale;
00189             v3fNormalVW.y /= dScale;
00190             v3fNormalVW.z /= dScale;
00191         }
00192 
00193         // LocalToVworld する
00194         Transform3D t3dSph = new Transform3D();
00195         Transform3D t3dLocalToVworld = new Transform3D();
00196         Transform3D t3dCurrent = new Transform3D();
00197         this.tgFittingTarget.getLocalToVworld(t3dLocalToVworld);
00198         this.tgFittingTarget.getTransform(t3dCurrent);
00199 
00200         t3dLocalToVworld.mul(t3dCurrent);
00201         t3dLocalToVworld.invert();
00202 
00203         int iCoorsNum = p3dPrimitivePoints.length;
00204         // report を返す中での直上の TG 座標へ変換する
00205         for (int iCount = 0; iCount < iNumberOfSphere; iCount++) {
00206             if(iCoorsNum <= iCount) {
00207                 t3dSph.set(new Vector3f(p3dPrimitivePoints[iCoorsNum - 1]));
00208             } else {
00209                 t3dLocalToVworld.transform(this.p3dPrimitivePoints[iCount]);
00210                 t3dSph.set(new Vector3f(this.p3dPrimitivePoints[iCount]));
00211             }
00212 
00213             sphTrans[iCount].setTransform(t3dSph);
00214         }
00215 
00216         // Vector と intersect を report を返す中での直上の TG 座標へ変換する
00217         v3fNormal = new Vector3f(this.v3fNormalVW);
00218         t3dLocalToVworld.transform(this.v3fNormal);
00219         p3fIntersect = new Point3f(this.p3fIntersectVW);
00220         t3dLocalToVworld.transform(this.p3fIntersect);
00221 
00222         Transform3D t3dTranslate = new Transform3D();
00223         Transform3D t3dRotate = new Transform3D();
00224         t3dTranslate.set(new Vector3f(p3fIntersect));
00225         tgArrowTranslate.setTransform(t3dTranslate);
00226 
00227         // 二つのベクトルの外積を軸にして回転する
00228         Vector3f v3fLine = new Vector3f(0.0f,1.0f,0.0f);
00229         Vector3f v3fLineOther = new Vector3f(0.0f,0.0f,1.0f);
00230         Vector3f v3fCross = new Vector3f();
00231 
00232         if (v3fNormal.y != -1.0f) {
00233             v3fCross.cross(v3fLine,v3fNormal);
00234         } else {
00235             v3fCross.cross(v3fLineOther,v3fNormal);
00236         }
00237         t3dRotate.set(new AxisAngle4f(v3fCross,v3fLine.angle(v3fNormal)));
00238         tgArrowRotate.setTransform(t3dRotate);
00239 
00240         if (bgAddTop.isLive()) {
00241             // すでに bgAddTop がどこかの TG にくっついていたらシーングラフ
00242             // から切り離す
00243             for(int i= 0; i < tgFittingTargetBefore.numChildren(); i ++) {
00244                 if (bgAddTop == tgFittingTargetBefore.getChild(i)) {
00245                     tgFittingTargetBefore.removeChild(i);
00246                 }
00247             }
00248         }
00249         bgAddTop.detach();
00250         tgFittingTarget.addChild(bgAddTop);
00251         tgFittingTargetBefore = tgFittingTarget;
00252     }
00253 
00258     public void removeForDisplay() {
00259         for (int i = 0; i < tgFittingTargetBefore.numChildren(); i ++) {
00260             if(bgAddTop == tgFittingTargetBefore.getChild(i)) {
00261                 tgFittingTargetBefore.removeChild(i);
00262             }
00263         }
00264     }
00265 
00271     public void setPickable(boolean bPick) {
00272         this.cone.setPickable(bPick);
00273         this.cylinder.setPickable(bPick);
00274     }
00275 
00281     public void setIntersectPoint(Point3f p3fIntersect) {
00282         this.p3fIntersectVW = new Point3f(p3fIntersect);
00283     }
00284 
00290     public Point3f getIntersectPoint() {
00291         return this.p3fIntersectVW;
00292     }
00293 
00299     public Vector3f getNormalVector() {
00300         return this.v3fNormalVW;
00301     }
00302 
00308     public void setPrimitiveCoordinates(Point3d[] ptCoordinates) {
00309         this.p3dPrimitivePoints = ptCoordinates;
00310     }
00311 
00317     public void setTransformGroup(TransformGroup tgFittingTarget) {
00318         this.tgFittingTarget = tgFittingTarget;
00319     }
00320 
00326     public TransformGroup getTransformGroup() {
00327         return this.tgFittingTarget;
00328     }
00329 
00335     public TransformGroup getArrowTransformGroup() {
00336         return this.tgArrowTranslate;
00337     }
00338 }


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:53