FittingInfo.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  */
17 package com.generalrobotix.ui.view.tdview;
18 import com.sun.j3d.utils.picking.*;
19 import com.sun.j3d.utils.geometry.Primitive;
20 import com.sun.j3d.utils.geometry.Cone;
21 import com.sun.j3d.utils.geometry.Cylinder;
22 import com.sun.j3d.utils.geometry.Sphere;
23 
24 import javax.media.j3d.*;
25 import javax.vecmath.*;
26 
27 // Connect(Fitting) 関連のデータ
28 
29 public class FittingInfo {
31  Point3f p3fIntersect;
33  Vector3f v3fNormal;
35  Point3f p3fIntersectVW;
37  Vector3f v3fNormalVW;
39  Point3d[] p3dPrimitivePoints;
40 
42  TransformGroup tgFittingTarget;
43  TransformGroup tgFittingTargetBefore;
44  // ユーザへの視覚的表示のための部品群
45  public final static int iNumberOfSphere = 4;
47  BranchGroup bgAddTop = new BranchGroup();
49  TransformGroup[] sphTrans = new TransformGroup [iNumberOfSphere];
50  TransformGroup tgArrowTranslate = new TransformGroup();
51  TransformGroup tgArrowRotate = new TransformGroup();
53  Cone cone;
55  Cylinder cylinder;
56 
64  public FittingInfo(
65  Color3f c3fArrow,
66  Color3f c3fSphere,
67  boolean bInvertArrow
68  ) {
69  // 球の追加
70  Sphere[] sph = new Sphere [iNumberOfSphere];
71  bgAddTop.setCapability(BranchGroup.ALLOW_DETACH);
72  tgArrowTranslate.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
73  tgArrowTranslate.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
74  tgArrowTranslate.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
75  tgArrowTranslate.setCapability(TransformGroup.ALLOW_CHILDREN_READ);
76  tgArrowTranslate.setCapability(
77  TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ
78  );
79  tgArrowRotate.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
80  tgArrowRotate.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
81  float fScaleFactor = 0.1f;
82  Appearance spherelook = new Appearance();
83  Color3f black = new Color3f(0.0f, 0.0f, 0.0f);
84  Color3f white = new Color3f(1.0f, 1.0f, 1.0f);
85  spherelook.setMaterial(
86  new Material(c3fSphere, black, c3fSphere, white, 50.0f)
87  );
88 
89  // 球用の設定をすます
90  for (int i = 0 ;i < iNumberOfSphere ; i++) {
91  sph[i] = new Sphere(0.25f * fScaleFactor, spherelook);
92  sph[i].setPickable(false);
93  sphTrans[i] = new TransformGroup ();
94  sphTrans[i].setCapability (TransformGroup.ALLOW_TRANSFORM_READ);
95  sphTrans[i].setCapability (TransformGroup.ALLOW_TRANSFORM_WRITE);
96 
97  // Add sphere, transform
98  bgAddTop.addChild (sphTrans[i]);
99  sphTrans[i].addChild (sph[i]);
100  }
101 
102  // 矢印の追加
103  Appearance arrowlook = new Appearance();
104  arrowlook.setMaterial(
105  new Material(c3fArrow, black, c3fArrow, white, 50.0f)
106  );
107 
108  cone =
109  new Cone(
110  0.5f * fScaleFactor,
111  1.0f * fScaleFactor,
112  Cone.GENERATE_NORMALS,
113  arrowlook
114  );
115  cone.setCapability(Primitive.ENABLE_GEOMETRY_PICKING);
116  for (int i = Cone.BODY; i <= Cone.CAP; i ++) {
117  PickTool.setCapabilities(cone.getShape(i), PickTool.INTERSECT_FULL);
118  }
119  cone.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
120  cone.setCapability(Primitive.ALLOW_PICKABLE_READ);
121  cone.setCapability(Primitive.ALLOW_PICKABLE_WRITE);
122  cylinder =
123  new Cylinder(0.2f * fScaleFactor, 2.0f * fScaleFactor, arrowlook);
124  cylinder.setCapability(Primitive.ENABLE_GEOMETRY_PICKING);
125  cylinder.setCapability(Primitive.ALLOW_PICKABLE_READ);
126  cylinder.setCapability(Primitive.ALLOW_PICKABLE_WRITE);
127  for(int i = Cylinder.BODY; i <= Cylinder.BOTTOM; i ++) {
128  PickTool.setCapabilities(
129  cylinder.getShape(i),
130  PickTool.INTERSECT_FULL
131  );
132  }
133  cylinder.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
134  // 二つの部品をくっつけるために三角錐のほうを移動しておく
135  TransformGroup tgCone = new TransformGroup();
136  Transform3D t3dCone = new Transform3D();
137  TransformGroup tgCylinder = new TransformGroup();
138  Transform3D t3dCylinder = new Transform3D();
139  t3dCone.setTranslation(new Vector3f(0.0f,1.5f * fScaleFactor,0.0f));
140  tgCone.setTransform(t3dCone);
141  tgCone.addChild(cone);
142  if (bInvertArrow) {
143  t3dCylinder.setTranslation(
144  new Vector3f(0.0f, 2.0f * fScaleFactor, 0.0f)
145  );
146  t3dCylinder.setRotation(
147  new AxisAngle4f(new Vector3f(1.0f, 0.0f, 0.0f), (float)Math.PI)
148  );
149  } else {
150  t3dCylinder.setTranslation(
151  new Vector3f(0.0f, fScaleFactor, 0.0f)
152  );
153  }
154  tgCylinder.addChild(tgCone);
155  tgCylinder.setTransform(t3dCylinder);
156  tgCylinder.addChild(cylinder);
157  tgArrowRotate.addChild(tgCylinder);
158  tgArrowTranslate.addChild(tgArrowRotate);
159  bgAddTop.addChild(tgArrowTranslate);
160  }
161 
166  public void addForDisplay() {
167  if(tgFittingTarget == null) {
168  System.err.println("物体を表示する対象の TG が設定されていません");
169  }
170 
171  // world 座標における法線を割り出す
172  v3fNormalVW = new Vector3f();
173  if (p3dPrimitivePoints.length >= 3) {
174  Point3d pt1 = new Point3d();
175  Point3d pt2 = new Point3d();
176  pt1.sub(p3dPrimitivePoints[1],p3dPrimitivePoints[0]);
177  pt2.sub(p3dPrimitivePoints[2],p3dPrimitivePoints[1]);
178  Vector3f vecOne = new Vector3f(pt1);
179  Vector3f vecTwo = new Vector3f(pt2);
180  // 外積を currVec ベクトルに入れる
181  v3fNormalVW.cross(vecOne,vecTwo);
182  double dScale =
183  Math.sqrt(
184  v3fNormalVW.x * v3fNormalVW.x +
185  v3fNormalVW.y * v3fNormalVW.y +
186  v3fNormalVW.z * v3fNormalVW.z
187  );
188  v3fNormalVW.x /= dScale;
189  v3fNormalVW.y /= dScale;
190  v3fNormalVW.z /= dScale;
191  }
192 
193  // LocalToVworld する
194  Transform3D t3dSph = new Transform3D();
195  Transform3D t3dLocalToVworld = new Transform3D();
196  Transform3D t3dCurrent = new Transform3D();
197  this.tgFittingTarget.getLocalToVworld(t3dLocalToVworld);
198  this.tgFittingTarget.getTransform(t3dCurrent);
199 
200  t3dLocalToVworld.mul(t3dCurrent);
201  t3dLocalToVworld.invert();
202 
203  int iCoorsNum = p3dPrimitivePoints.length;
204  // report を返す中での直上の TG 座標へ変換する
205  for (int iCount = 0; iCount < iNumberOfSphere; iCount++) {
206  if(iCoorsNum <= iCount) {
207  t3dSph.set(new Vector3f(p3dPrimitivePoints[iCoorsNum - 1]));
208  } else {
209  t3dLocalToVworld.transform(this.p3dPrimitivePoints[iCount]);
210  t3dSph.set(new Vector3f(this.p3dPrimitivePoints[iCount]));
211  }
212 
213  sphTrans[iCount].setTransform(t3dSph);
214  }
215 
216  // Vector と intersect を report を返す中での直上の TG 座標へ変換する
217  v3fNormal = new Vector3f(this.v3fNormalVW);
218  t3dLocalToVworld.transform(this.v3fNormal);
219  p3fIntersect = new Point3f(this.p3fIntersectVW);
220  t3dLocalToVworld.transform(this.p3fIntersect);
221 
222  Transform3D t3dTranslate = new Transform3D();
223  Transform3D t3dRotate = new Transform3D();
224  t3dTranslate.set(new Vector3f(p3fIntersect));
225  tgArrowTranslate.setTransform(t3dTranslate);
226 
227  // 二つのベクトルの外積を軸にして回転する
228  Vector3f v3fLine = new Vector3f(0.0f,1.0f,0.0f);
229  Vector3f v3fLineOther = new Vector3f(0.0f,0.0f,1.0f);
230  Vector3f v3fCross = new Vector3f();
231 
232  if (v3fNormal.y != -1.0f) {
233  v3fCross.cross(v3fLine,v3fNormal);
234  } else {
235  v3fCross.cross(v3fLineOther,v3fNormal);
236  }
237  t3dRotate.set(new AxisAngle4f(v3fCross,v3fLine.angle(v3fNormal)));
238  tgArrowRotate.setTransform(t3dRotate);
239 
240  if (bgAddTop.isLive()) {
241  // すでに bgAddTop がどこかの TG にくっついていたらシーングラフ
242  // から切り離す
243  for(int i= 0; i < tgFittingTargetBefore.numChildren(); i ++) {
244  if (bgAddTop == tgFittingTargetBefore.getChild(i)) {
245  tgFittingTargetBefore.removeChild(i);
246  }
247  }
248  }
249  bgAddTop.detach();
250  tgFittingTarget.addChild(bgAddTop);
251  tgFittingTargetBefore = tgFittingTarget;
252  }
253 
258  public void removeForDisplay() {
259  for (int i = 0; i < tgFittingTargetBefore.numChildren(); i ++) {
260  if(bgAddTop == tgFittingTargetBefore.getChild(i)) {
261  tgFittingTargetBefore.removeChild(i);
262  }
263  }
264  }
265 
271  public void setPickable(boolean bPick) {
272  this.cone.setPickable(bPick);
273  this.cylinder.setPickable(bPick);
274  }
275 
281  public void setIntersectPoint(Point3f p3fIntersect) {
282  this.p3fIntersectVW = new Point3f(p3fIntersect);
283  }
284 
290  public Point3f getIntersectPoint() {
291  return this.p3fIntersectVW;
292  }
293 
299  public Vector3f getNormalVector() {
300  return this.v3fNormalVW;
301  }
302 
308  public void setPrimitiveCoordinates(Point3d[] ptCoordinates) {
309  this.p3dPrimitivePoints = ptCoordinates;
310  }
311 
317  public void setTransformGroup(TransformGroup tgFittingTarget) {
318  this.tgFittingTarget = tgFittingTarget;
319  }
320 
326  public TransformGroup getTransformGroup() {
327  return this.tgFittingTarget;
328  }
329 
335  public TransformGroup getArrowTransformGroup() {
336  return this.tgArrowTranslate;
337  }
338 }
#define null
our own NULL pointer
Definition: IceTypes.h:57
png_uint_32 i
Definition: png.h:2735
FittingInfo(Color3f c3fArrow, Color3f c3fSphere, boolean bInvertArrow)
void setTransformGroup(TransformGroup tgFittingTarget)
void setIntersectPoint(Point3f p3fIntersect)
void setPrimitiveCoordinates(Point3d[] ptCoordinates)


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