SceneGraphModifier.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 
00019 import java.util.*;
00020 
00021 import javax.vecmath.*;
00022 import javax.media.j3d.*;
00023 
00024 import com.generalrobotix.ui.item.GrxLinkItem;
00025 import com.generalrobotix.ui.item.GrxModelItem;
00026 import com.sun.j3d.utils.picking.PickTool;
00027 
00028 public class SceneGraphModifier {
00029     //--------------------------------------------------------------------
00030     // 定数
00031     public static final int CREATE_BOUNDS = 0;
00032     public static final int RESIZE_BOUNDS = 1;
00033     private static final int SHADING_MODE  = 2;
00034 
00035     public static final int SHADING    = 0;
00036     public static final int WIRE_FRAME = 1;
00037 
00038     //private static String USERDATA_SWITCH = "this is bounding box switch.";
00039 
00040     //--------------------------------------------------------------------
00041     // インスタンス変数
00042     private static SceneGraphModifier this_;
00043     private float[] lower_   = new float[3];          // 頂点のMax値
00044     private float[] upper_   = new float[3];          // 頂点のMin値
00045     public boolean init_;
00046     private int polygonMode_;
00047     private int cullFace_;
00048     public int mode_;
00049 
00050     //--------------------------------------------------------------------
00051     // コンストラクタ(Singleton pattern)
00052     private SceneGraphModifier() {}
00053 
00054     //--------------------------------------------------------------------
00055     // スタティックメソッド
00056     public static SceneGraphModifier getInstance() {
00057         if (this_ == null) {
00058             this_ = new SceneGraphModifier();
00059         }
00060         return this_;
00061     }
00062 
00068     public static Hashtable<String, Object> getHashtableFromTG(TransformGroup tg) {
00069         if (tg == null) 
00070                         return null; 
00071 
00072         Object userData = tg.getUserData();
00073         if (userData instanceof Hashtable) 
00074             return (Hashtable<String, Object>)userData;
00075 
00076         return null;
00077     }
00078 
00084     public static GrxLinkItem getLinkFromTG(TransformGroup tg){
00085         Hashtable<String, Object> htable = getHashtableFromTG(tg);
00086         if (htable != null){
00087                 GrxLinkItem link = (GrxLinkItem)htable.get("linkInfo");
00088                 return link;
00089         }
00090         return null;
00091     }
00092 
00098     public static GrxModelItem getModelFromTG(TransformGroup tg){
00099         Hashtable<String, Object> htable = getHashtableFromTG(tg);
00100         if (htable != null){
00101                 GrxModelItem model = (GrxModelItem)htable.get("object");
00102                 return model;
00103         }
00104         return null;
00105     }
00106     //--------------------------------------------------------------------
00107     // 公開メソッド
00108 /*
00109     public void changeShadingMode(SimulationElementNode node, int mode) {
00110         mode_ = SHADING_MODE;
00111 
00112         switch (mode) {
00113         case SHADING:
00114             polygonMode_ = PolygonAttributes.POLYGON_FILL;
00115             cullFace_ = PolygonAttributes.CULL_BACK;
00116             break;
00117         case WIRE_FRAME:
00118             polygonMode_ = PolygonAttributes.POLYGON_LINE;
00119             cullFace_ = PolygonAttributes.CULL_BACK;
00120             break;
00121         }
00122 
00123         // ノードのRootのTransformGroupを取得
00124         TransformGroup tg = node.getTransformGroupRoot();
00125 
00126         for (int i = 0; i < tg.numChildren(); i++) {
00127             try {
00128                 _calcUpperLower(tg.getChild(i), new Transform3D());
00129             } catch (CapabilityNotSetException ex) {
00130                 ex.printStackTrace();
00131             }
00132         }
00133     }
00134 */
00135     //--------------------------------------------------------------------
00136     // プライベートメソッド
00137     private void _setSegmentCapabilities(TransformGroup tg) {
00138         tg.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
00139         tg.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
00140         tg.setCapability(TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ);
00141     }
00142 
00148     public  void _calcUpperLower(Node node, Transform3D t3dParent) {
00149         if (init_) {
00150             for (int i = 0; i < 3; i  ++) {
00151                 upper_[i] = 0.0f;
00152                 lower_[i] = 0.0f;
00153             }
00154         }
00155         if (node instanceof Group) {
00156                 // added for GrxUI
00157             if (node instanceof BranchGroup) {
00158                 BranchGroup bg = (BranchGroup)node;
00159                 for (int i = 0; i < bg.numChildren(); i++) {
00160                     _calcUpperLower(bg.getChild(i), t3dParent);
00161                 }
00162             } else 
00163             // added for GrxUI  
00164             if (node instanceof TransformGroup) {
00165                 Transform3D t3dLocal = new Transform3D();
00166                 TransformGroup tg = (TransformGroup)node;
00167                 if (mode_ == CREATE_BOUNDS) {
00168                     _setSegmentCapabilities(tg);
00169                 }
00170                 tg.getTransform(t3dLocal);
00171                 Transform3D t3dWorld = new Transform3D(t3dParent);
00172                 t3dWorld.mul(t3dLocal);
00173                 for (int i = 0; i < tg.numChildren(); i++) {
00174                     _calcUpperLower(tg.getChild(i), t3dWorld);
00175                 }
00176             }else{
00177                 boolean flag = true;
00178                 if ((node instanceof Switch)) {
00179                     //Object obj = node.getUserData();
00180                     //if(!(obj instanceof String && USERDATA_SWITCH.equals(obj)))
00181                     //{
00182                         flag = false;
00183                     //}
00184                 }
00185                 if(flag){
00186                     Group group = (Group)node;
00187                     if (mode_ == CREATE_BOUNDS) {
00188                         group.setCapability(Group.ALLOW_CHILDREN_READ);
00189                     }
00190                     for (int i = 0; i < group.numChildren(); i++) {
00191                         _calcUpperLower(group.getChild(i), t3dParent);
00192                     }
00193                 }
00194             }
00195         } else if (node instanceof Link) {
00196             Link lk = (Link) node;
00197             if (mode_ == CREATE_BOUNDS) {
00198                 lk.setCapability(Link.ALLOW_SHARED_GROUP_READ);
00199             }
00200             SharedGroup sg = lk.getSharedGroup();
00201             if (mode_ == CREATE_BOUNDS) {
00202                 sg.setCapability(SharedGroup.ALLOW_CHILDREN_READ);
00203             }
00204             Group group = (Group) sg;
00205             for (int i = 0; i < group.numChildren(); i++) {
00206                 _calcUpperLower(group.getChild(i), t3dParent);
00207             }
00208         } else if (node instanceof Shape3D) {
00209             Shape3D shape = (Shape3D) node;
00210             Appearance appearance = shape.getAppearance();
00211             
00212             if (mode_ == CREATE_BOUNDS) {
00213                 shape.setCapability(Node.ENABLE_PICK_REPORTING);
00214                 shape.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
00215                 PickTool.setCapabilities(shape, PickTool.INTERSECT_FULL);
00216 
00217                 // for shading change
00218                 shape.setCapability(Shape3D.ALLOW_APPEARANCE_READ);
00219                 shape.setCapability(Shape3D.ALLOW_APPEARANCE_WRITE);
00220 
00221                 if (appearance != null){
00222                         appearance.setCapability(
00223                                         Appearance.ALLOW_POLYGON_ATTRIBUTES_READ
00224                         );
00225                         appearance.setCapability(
00226                                         Appearance.ALLOW_POLYGON_ATTRIBUTES_WRITE
00227                         );
00228                 }
00229             }
00230 
00231             if (mode_ == SHADING_MODE) {
00232                 //PolygonAttributes attributes = appearance.getPolygonAttributes();
00233                 PolygonAttributes attributes = new PolygonAttributes();
00234                 if (attributes == null) {
00235                     System.out.println("can't get PolygonAttributes");
00236                     return;
00237                 }
00238                 attributes.setPolygonMode(polygonMode_);
00239                 attributes.setCullFace(cullFace_);
00240                 //System.out.println("koko" + attributes + appearance);
00241                 if(appearance!=null){
00242                     appearance.setPolygonAttributes(attributes);
00243                 }
00244                 return;
00245             }
00246 
00247             // 三角形の頂点を求める
00248             Geometry geometry = shape.getGeometry();
00249 
00250             if (mode_ == CREATE_BOUNDS) {
00251                 geometry.setCapability(GeometryArray.ALLOW_COORDINATE_READ);
00252                 geometry.setCapability(GeometryArray.ALLOW_COUNT_READ);
00253             }
00254 
00255             if (geometry instanceof GeometryArray) {        // GeometryArray
00256                 GeometryArray ga = (GeometryArray)geometry;
00257                 for (int i = 0; i < ga.getVertexCount(); i++) {
00258                     float[] point = new float[3];                  // 頂点
00259                     ga.getCoordinate(i, point);               // 頂点座標取得
00260                     Point3f point3f = new Point3f(point);
00261                     t3dParent.transform(point3f);
00262                     point[0] = point3f.x;
00263                     point[1] = point3f.y;
00264                     point[2] = point3f.z;
00265                     _updateUpperLower(point);
00266                }
00267             }
00268         }
00269     }
00270 
00271     private void _updateUpperLower(float[] point) {
00272         if (init_) {
00273             for (int i = 0; i < 3; i++) {
00274                 upper_[i] = point[i];
00275                 lower_[i] = point[i];
00276             }
00277             init_ = false;
00278         } else {
00279             for (int i = 0; i < 3; i++) {
00280                 if (point[i] > upper_[i]) upper_[i] = point[i];
00281                 else if (point[i] < lower_[i]) lower_[i] = point[i];
00282             }
00283         }
00284     }
00285 
00290     public Point3f[] _makePoints() {
00291         return _makePoints(upper_, lower_);
00292     }
00293 
00294     public static Point3f[] _makePoints(float x, float y, float z){
00295         float[] upper = new float[]{x/2,y/2,z/2};
00296         float[] lower = new float[]{-x/2,-y/2,-z/2};
00297         return _makePoints(upper, lower);
00298     }
00299     
00306     public static Point3f[] _makePoints(float[] upper, float[] lower) {
00307         Point3f[] points = new Point3f[24];
00308         points[0]  = new Point3f(upper[0], upper[1], upper[2]); // A
00309         points[1]  = new Point3f(lower[0], upper[1], upper[2]); // B
00310         points[2]  = new Point3f(lower[0], lower[1], upper[2]); // C
00311         points[3]  = new Point3f(upper[0], lower[1], upper[2]); // D
00312         points[4]  = new Point3f(upper[0], lower[1], lower[2]); // H
00313         points[5]  = new Point3f(lower[0], lower[1], lower[2]); // G
00314         points[6]  = new Point3f(lower[0], upper[1], lower[2]); // F
00315         points[7]  = new Point3f(upper[0], upper[1], lower[2]); // E
00316         points[8]  = new Point3f(upper[0], upper[1], upper[2]); // A
00317         points[9]  = new Point3f(upper[0], lower[1], upper[2]); // D
00318         points[10] = new Point3f(upper[0], lower[1], lower[2]); // H
00319         points[11] = new Point3f(upper[0], upper[1], lower[2]); // E
00320         points[12] = new Point3f(lower[0], upper[1], lower[2]); // F
00321         points[13] = new Point3f(lower[0], lower[1], lower[2]); // G
00322         points[14] = new Point3f(lower[0], lower[1], upper[2]); // C
00323         points[15] = new Point3f(lower[0], upper[1], upper[2]); // B
00324         points[16] = new Point3f(upper[0], lower[1], upper[2]); // C
00325         points[17] = new Point3f(lower[0], lower[1], upper[2]); // D
00326         points[18] = new Point3f(lower[0], lower[1], lower[2]); // H
00327         points[19] = new Point3f(upper[0], lower[1], lower[2]); // G
00328         points[20] = new Point3f(upper[0], upper[1], lower[2]); // A
00329         points[21] = new Point3f(lower[0], upper[1], lower[2]); // B
00330         points[22] = new Point3f(lower[0], upper[1], upper[2]); // F
00331         points[23] = new Point3f(upper[0], upper[1], upper[2]); // E
00332         return points;
00333     }
00334 
00335     private static Vector3f[] _makeNormals() {
00336         Vector3f[] normal = new Vector3f[24];
00337         normal[0] = new Vector3f(0.0f, 0.0f, 1.0f);
00338         normal[1] = normal[0];
00339         normal[2] = normal[0];
00340         normal[3] = normal[0];
00341         normal[4] = new Vector3f(0.0f, 0.0f, -1.0f);
00342         normal[5] = normal[4];
00343         normal[6] = normal[4];
00344         normal[7] = normal[4];
00345         normal[8] = new Vector3f(1.0f, 0.0f, 0.0f);
00346         normal[9] = normal[8];
00347         normal[10] = normal[8];
00348         normal[11] = normal[8];
00349         normal[12] = new Vector3f(-1.0f, 0.0f, 0.0f);
00350         normal[13] = normal[12];
00351         normal[14] = normal[12];
00352         normal[15] = normal[12];
00353         normal[16] = new Vector3f(0.0f, -1.0f, 0.0f);
00354         normal[17] = normal[16];
00355         normal[18] = normal[16];
00356         normal[19] = normal[16];
00357         normal[20] = new Vector3f(0.0f, 1.0f, 0.0f);
00358         normal[21] = normal[20];
00359         normal[22] = normal[20];
00360         normal[23] = normal[20];
00361         return normal;
00362     }
00363 
00364     public static Shape3D _makeCube(Color3f color, Point3f[] points){
00365         QuadArray quads = new QuadArray(
00366                 points.length,
00367                 QuadArray.COLOR_3 | QuadArray.COORDINATES | QuadArray.NORMALS
00368             );
00369             quads.setCapability(QuadArray.ALLOW_COORDINATE_READ);
00370             quads.setCapability(QuadArray.ALLOW_COORDINATE_WRITE);
00371             quads.setCoordinates(0, points);
00372             Vector3f[] normals = _makeNormals();
00373             Color3f[] colors = new Color3f[points.length];
00374             for (int i = 0; i < points.length; i++) {
00375                 colors[i] = color;
00376             }
00377             quads.setNormals(0, normals);
00378             quads.setColors(0, colors);
00379             PolygonAttributes attr = new PolygonAttributes();
00380             attr.setPolygonMode(PolygonAttributes.POLYGON_LINE);
00381             attr.setCullFace(PolygonAttributes.CULL_NONE);
00382             Appearance appearance = new Appearance();
00383             appearance.setPolygonAttributes(attr);
00384             Shape3D shape = new Shape3D(quads, appearance);
00385             shape.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
00386             shape.setCapability(Shape3D.ALLOW_GEOMETRY_WRITE);
00387             shape.setCapability(Node.ENABLE_PICK_REPORTING);
00388             PickTool.setCapabilities(shape, PickTool.INTERSECT_FULL);
00389             return shape;
00390     }
00391 
00392     public Shape3D _makeBoundingBox(Color3f color) {
00393         Point3f[] points = _makePoints();
00394         return _makeCube(color, points);
00395     }
00396 
00402     public Point3f[] makeAxisPoints(Vector3d jointAxis){
00403         Point3f[] points = new Point3f[2];
00404         
00405         float upperlen = (float)(
00406                         upper_[0]*jointAxis.x+
00407                         upper_[1]*jointAxis.y+
00408                         upper_[2]*jointAxis.z);
00409         float lowerlen = (float)(
00410                         lower_[0]*jointAxis.x+
00411                         lower_[1]*jointAxis.y+
00412                         lower_[2]*jointAxis.z);
00413         float offset = 0.05f;
00414         if(upperlen < lowerlen)
00415                 offset = -0.05f;
00416         
00417         points[0]  = new Point3f(
00418             (upperlen + offset) * (float)jointAxis.x,
00419             (upperlen + offset) * (float)jointAxis.y,
00420             (upperlen + offset) * (float)jointAxis.z
00421         ); // A
00422 
00423         points[1]  = new Point3f(
00424             (lowerlen - offset) * (float)jointAxis.x,
00425             (lowerlen - offset) * (float)jointAxis.y,
00426             (lowerlen - offset) * (float)jointAxis.z
00427         ); // B
00428         return points;
00429     }
00433     public Shape3D _makeAxisLine(Vector3d jointAxis) {
00434         Point3f[] points = makeAxisPoints(jointAxis);
00435         
00436         LineArray lines = new LineArray(
00437             points.length,
00438             LineArray.COLOR_3 | LineArray.COORDINATES | LineArray.NORMALS
00439         );
00440         lines.setCapability(LineArray.ALLOW_COORDINATE_WRITE);
00441         lines.setCoordinates(0, points);                // 座標
00442         Color3f[] colors = new Color3f[points.length];
00443         Vector3f[] normals = new Vector3f[points.length];
00444         Color3f color = new Color3f(1.0f, 0.0f, 1.0f);
00445         Vector3f normal = new Vector3f(0.0f, 0.0f, 1.0f);
00446         for (int i = 0; i < points.length; i++) {
00447             colors[i] = color;
00448             normals[i] = normal;
00449         }
00450         lines.setNormals(0, normals);  // 法線
00451         lines.setColors(0, colors);    // 色
00452         lines.setCapability(GeometryArray.ALLOW_COUNT_READ);
00453         lines.setCapability(GeometryArray.ALLOW_FORMAT_READ);
00454         lines.setCapability(GeometryArray.ALLOW_COORDINATE_READ);
00455         LineAttributes lineAttr = new LineAttributes();
00456         lineAttr.setLineWidth(4.0f);
00457         Appearance appearance = new Appearance();
00458         appearance.setLineAttributes(lineAttr);
00459         Shape3D shape = new Shape3D(lines, appearance);
00460         shape.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
00461         shape.setCapability(Shape3D.ALLOW_GEOMETRY_WRITE);
00462         //shape.setCapability(Node.ENABLE_PICK_REPORTING);
00463         return shape;
00464     }
00465 
00469     public static Switch _makeSwitchNode(Shape3D shape) {
00470         Switch switchNode = _makeSwitchNode();
00471         switchNode.addChild(shape);
00472         return switchNode;
00473     }
00474 
00475     public static Switch _makeSwitchNode() {
00476         Switch switchNode = new Switch();
00477         switchNode.setCapability(Switch.ALLOW_CHILDREN_EXTEND);
00478         switchNode.setCapability(Switch.ALLOW_CHILDREN_READ);
00479         switchNode.setCapability(Switch.ALLOW_CHILDREN_WRITE);
00480         switchNode.setCapability(Switch.ALLOW_SWITCH_READ);
00481         switchNode.setCapability(Switch.ALLOW_SWITCH_WRITE);
00482         //switchNode.setUserData(USERDATA_SWITCH);
00483         //switchNode.setWhichChild(Switch.CHILD_ALL);
00484         return switchNode;
00485     }
00486 }


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