1 package org.simantics.proconf.g3d.base;
\r
3 import java.util.Collection;
\r
4 import java.util.HashMap;
\r
6 import javax.vecmath.AxisAngle4d;
\r
7 import javax.vecmath.Point3d;
\r
8 import javax.vecmath.Quat4d;
\r
9 import javax.vecmath.Tuple3d;
\r
10 import javax.vecmath.Vector3d;
\r
12 import org.simantics.db.Graph;
\r
13 import org.simantics.db.Resource;
\r
14 import org.simantics.layer0.utils.EntityFactory;
\r
15 import org.simantics.layer0.utils.IEntity;
\r
16 import org.simantics.proconf.g3d.Resources;
\r
17 import org.simantics.proconf.g3d.stubs.Orientation;
\r
18 import org.simantics.proconf.g3d.stubs.Position;
\r
20 public class TransformationTools {
\r
22 private static boolean DEBUG = false;
\r
24 private Resource childRelation;
\r
25 private Resource parentRelation;
\r
27 public TransformationTools(Resource childRelation, Resource parentRelation) {
\r
28 this.childRelation = childRelation;
\r
29 this.parentRelation = parentRelation;
\r
32 public IEntity getParent(IEntity node) {
\r
33 return node.getAtMostOneRelatedObject(parentRelation);
\r
36 public Point3d getLocalFromWorld(IEntity node, Point3d worldCoord) {
\r
37 IEntity parent = getParent(node);
\r
38 if (parent == null) {// this is a rootnode ( has no transformation)
\r
42 Point3d local = getLocalFromWorld2(parent,worldCoord);
\r
47 private Point3d getLocalFromWorld2(IEntity node, Point3d worldCoord) {
\r
48 IEntity parent = getParent(node);
\r
49 if (parent == null) {// this is a root node ( has no transformation)
\r
53 Point3d local = getLocalFromWorld2(parent,worldCoord);
\r
54 if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
\r
55 local.sub(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
\r
56 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
57 Quat4d q = new Quat4d();
\r
58 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
60 MathTools.rotate(q, local, local);
\r
65 public Point3d getWorldFromLocal(IEntity node,Point3d localCoord) {
\r
66 IEntity parent = getParent(node);
\r
67 if (parent == null) // this is a rootnode ( has no transformation)
\r
69 return getWorldFromLocal2(parent, localCoord);
\r
72 private Point3d getWorldFromLocal2(IEntity node,Point3d localCoord) {
\r
74 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
75 Quat4d q = new Quat4d();
\r
76 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
77 MathTools.rotate(q, localCoord, localCoord);
\r
79 if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
\r
80 localCoord.add(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
\r
82 IEntity parent = getParent(node);
\r
83 if (parent == null) // this is a rootnode ( has no transformation)
\r
86 return getWorldFromLocal2(parent,localCoord);
\r
90 public AxisAngle4d getLocalFromWorld(IEntity node, AxisAngle4d worldRot) {
\r
91 IEntity parent = getParent(node);
\r
92 if (parent == null) // this is a rootnode ( has no transformation)
\r
95 AxisAngle4d local = getLocalFromWorld2(parent,worldRot);
\r
100 private AxisAngle4d getLocalFromWorld2(IEntity node, AxisAngle4d worldRot) {
\r
101 IEntity parent = getParent(node);
\r
102 if (parent == null) // this is a rootnode ( has no transformation)
\r
104 AxisAngle4d local = getLocalFromWorld2(parent,worldRot);
\r
106 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
107 Quat4d q = new Quat4d();
\r
108 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
110 Quat4d q2 = new Quat4d();
\r
119 public AxisAngle4d getWorldFromLocal(IEntity node,AxisAngle4d localRot) {
\r
120 IEntity parent = getParent(node);
\r
121 if (parent == null)
\r
124 return getWorldFromLocal2(parent,localRot);
\r
127 private AxisAngle4d getWorldFromLocal2(IEntity node,AxisAngle4d localRot) {
\r
129 //System.out.print("wtl " + node.getResource() + " " + localCoord);
\r
130 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
131 Quat4d q = new Quat4d();
\r
132 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
133 Quat4d q2 = new Quat4d();
\r
139 IEntity parent = getParent(node);
\r
140 if (parent == null) // this is a rootnode ( has no transformation)
\r
143 //System.out.println(" " + localCoord);
\r
144 return getWorldFromLocal2(parent,localRot);
\r
147 public Point3d getLocalFromWorldR(IEntity node, Point3d worldCoord) {
\r
149 Point3d local = getLocalFromWorldR(null,worldCoord);
\r
150 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
151 Quat4d q = new Quat4d();
\r
152 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
154 MathTools.rotate(q, local, local);
\r
157 IEntity parent = getParent(node);
\r
158 if (parent == null) // this is a rootnode ( has no transformation)
\r
164 public Point3d getWorldFromLocalR(IEntity node,Point3d localCoord) {
\r
166 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
167 Quat4d q = new Quat4d();
\r
168 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
169 MathTools.rotate(q, localCoord, localCoord);
\r
172 IEntity parent = getParent(node);
\r
173 if (parent == null) // this is a rootnode ( has no transformation)
\r
176 return getWorldFromLocalR(parent,localCoord);
\r
180 * Updates transformations of all children of given node
\r
183 public void propagateTransformChange(IEntity node) {
\r
184 Collection<IEntity> children = node.getRelatedObjects(childRelation);
\r
185 IEntity wp = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
186 IEntity wr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
187 Quat4d rot = new Quat4d();
\r
189 rot.set(G3DTools.getOrientation(wr));
\r
192 Point3d pos = new Point3d();
\r
194 pos = G3DTools.getPoint(wp);
\r
197 System.out.println("propagate transform " + node.getResource() + " " + pos + " " + G3DTools.getOrientation(wr));
\r
199 System.out.println("propagate transform " + node.getResource() + " " + pos);
\r
201 for (IEntity n : children) {
\r
202 IEntity lPos = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
203 IEntity lRot = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
204 if (DEBUG) System.out.print(n);
\r
205 if (lRot != null) {
\r
206 AxisAngle4d la = G3DTools.getOrientation(lRot);
\r
207 AxisAngle4d wa = getWorldFromLocal(n, la);
\r
208 IEntity wo = n.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
209 G3DTools.setOrientation(wo,wa);
\r
210 storeProperty(wo.getResource(), wa);
\r
212 if (lPos != null) {
\r
213 Point3d lp = G3DTools.getPoint(lPos);
\r
214 if (DEBUG) System.out.println(lp);
\r
215 MathTools.rotate(rot, lp, lp);
\r
217 IEntity nwp = n.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
218 G3DTools.setTuple3(nwp, lp);
\r
219 if (DEBUG) System.out.print(" " + lp);
\r
220 storeProperty(nwp.getResource(), lp);
\r
222 if (DEBUG) System.out.println();
\r
223 propagateTransformChange(n);
\r
229 * Updates transformation of one child node without changing its local transformation.
\r
233 public void propagateLocalTransformChange(IEntity parent, IEntity node) {
\r
234 //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
\r
235 IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
236 IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
237 Quat4d parentWRot = new Quat4d();
\r
238 if (parentWR != null) {
\r
239 parentWRot.set(G3DTools.getOrientation(parentWR));
\r
242 parentWRot.w = 1.0;
\r
243 Point3d parentWPos = new Point3d();
\r
244 if (parentWP != null) {
\r
245 parentWPos = G3DTools.getPoint(parentWP);
\r
248 if (parentWR != null)
\r
249 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
\r
251 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
\r
253 //for (IEntity n : children) {
\r
254 // if (!n.equals(node))
\r
256 IEntity lPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
257 IEntity lRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
258 if (DEBUG) System.out.print(node);
\r
259 if (lRot != null) {
\r
260 AxisAngle4d aa = G3DTools.getOrientation(lRot);
\r
261 storeProperty(lRot.getResource(), aa);
\r
262 AxisAngle4d la = getWorldFromLocal(node, aa);
\r
263 IEntity wo = node.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
264 G3DTools.setOrientation(wo,la);
\r
265 storeProperty(wo.getResource(), la);
\r
267 if (lPos != null) {
\r
268 Point3d lp = G3DTools.getPoint(lPos);
\r
269 storeProperty(lPos.getResource(), lp);
\r
270 MathTools.rotate(parentWRot, lp, lp);
\r
271 lp.add(parentWPos);
\r
272 IEntity nwp = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
273 G3DTools.setTuple3(nwp, lp);
\r
274 storeProperty(nwp.getResource(), lp);
\r
276 if (DEBUG) System.out.println();
\r
277 propagateTransformChange(node);
\r
282 public static void resetTransformation(IEntity shape) {
\r
283 Graph graph = shape.getGraph();
\r
284 if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition) == null) {
\r
286 // LocalPosition p = LocalPosition.createDefault(graph);
\r
287 Position p = Position.createDefault(graph);
\r
288 shape.addStatement(Resources.g3dResource.HasLocalPosition, p);
\r
289 // WorldPosition p2 = WorldPosition.createDefault(graph);
\r
290 Position p2 = Position.createDefault(graph);
\r
291 shape.addStatement(Resources.g3dResource.HasWorldPosition, p2);
\r
292 p.setX(new double[] { 0.0 });
\r
293 p.setY(new double[] { 0.0 });
\r
294 p.setZ(new double[] { 0.0 });
\r
296 p2.setX(new double[] { 0.0 });
\r
297 p2.setY(new double[] { 0.0 });
\r
298 p2.setZ(new double[] { 0.0 });
\r
301 G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition), 0.0, 0.0, 0.0);
\r
302 G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition), 0.0, 0.0, 0.0);
\r
304 if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation) == null) {
\r
306 // LocalOrientation r = LocalOrientationFactory.create(graph);
\r
307 Orientation r = Orientation.createDefault(graph);
\r
308 shape.addStatement(Resources.g3dResource.HasLocalOrientation, r);
\r
309 // WorldOrientation r2 = WorldOrientationFactory.create(graph);
\r
310 Orientation r2 = Orientation.createDefault(graph);
\r
311 shape.addStatement(Resources.g3dResource.HasWorldOrientation, r2);
\r
312 r.setAngle(new double[] { 0.0 });
\r
313 r.setX(new double[] { 1.0 });
\r
314 r.setY(new double[] { 0.0 });
\r
315 r.setZ(new double[] { 0.0 });
\r
316 r2.setAngle(new double[] { 0.0 });
\r
317 r2.setX(new double[] { 1.0 });
\r
318 r2.setY(new double[] { 0.0 });
\r
319 r2.setZ(new double[] { 0.0 });
\r
322 G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation),
\r
323 new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
\r
324 G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation),
\r
325 new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
\r
330 * Updates transformation of one child node without changing its world transformation.
\r
334 public void propagateWorldTransformChange(IEntity parent, IEntity node) {
\r
335 //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
\r
336 IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
337 IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
338 Quat4d parentWQuat = new Quat4d();
\r
339 if (parentWR != null) {
\r
340 parentWQuat.set(G3DTools.getOrientation(parentWR));
\r
343 parentWQuat.w = 1.0;
\r
344 Point3d parentWPos = new Point3d();
\r
345 if (parentWP != null) {
\r
346 parentWPos = G3DTools.getPoint(parentWP);
\r
349 if (parentWR != null)
\r
350 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
\r
352 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
\r
354 //for (IEntity n : children) {
\r
355 // if (!n.equals(node))
\r
357 IEntity wPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
358 IEntity wRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
360 if (DEBUG) System.out.print(node);
\r
361 if (wRot != null) {
\r
362 AxisAngle4d aa = G3DTools.getOrientation(wRot);
\r
363 storeProperty(wRot.getResource(), aa);
\r
364 AxisAngle4d la = getLocalFromWorld(node, aa);
\r
365 IEntity lRot = node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
366 G3DTools.setOrientation(lRot,la);
\r
367 storeProperty(lRot.getResource(), la);
\r
369 if (wPos != null) {
\r
370 Point3d lp = G3DTools.getPoint(wPos);
\r
371 storeProperty(wPos.getResource(), lp);
\r
372 lp.sub(parentWPos);
\r
373 parentWQuat.inverse();
\r
374 MathTools.rotate(parentWQuat, lp, lp);
\r
375 IEntity lPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
376 G3DTools.setTuple3(lPos, lp);
\r
377 storeProperty(lPos.getResource(), lp);
\r
379 if (DEBUG) System.out.println();
\r
380 propagateTransformChange(node);
\r
385 public boolean transformationUpdate(Graph graph, Resource resource) {
\r
387 //resources.startTransaction("transformationUpdate");
\r
388 IEntity entity = EntityFactory.create(graph,resource);
\r
389 return transformationUpdate(entity);
\r
392 public boolean transformationUpdate(IEntity node) {
\r
393 if (DEBUG) System.out.println("Node transformation update " + node.getResource());
\r
394 IEntity worldPos = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
395 IEntity localPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
396 IEntity worldOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
397 IEntity localOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
399 Tuple3d worldP =G3DTools.getPoint(worldPos);
\r
400 Tuple3d localP = G3DTools.getPoint(localPos);
\r
402 AxisAngle4d worldR = null;
\r
403 AxisAngle4d localR = null;
\r
404 if (worldOr != null) {
\r
405 worldR = G3DTools.getOrientation(worldOr);
\r
406 localR = G3DTools.getOrientation(localOr);
\r
409 boolean changed = false;
\r
410 if (localP != null && worldP != null) {
\r
411 Tuple3d cachedWorldP = (Tuple3d)getProperty(worldPos.getResource());
\r
412 Tuple3d cachedLocalP = (Tuple3d)getProperty(localPos.getResource());
\r
413 boolean changedLocalP = false;
\r
414 boolean changedWorldP = false;
\r
415 if (cachedLocalP == null)
\r
416 changedLocalP = true;
\r
417 else if (changed(cachedLocalP,localP))
\r
418 changedLocalP = true;
\r
420 if (cachedWorldP == null) {
\r
421 changedWorldP = true;
\r
422 } else if (changed(cachedWorldP,worldP)){
\r
423 changedWorldP = true;
\r
425 if (changedLocalP) {
\r
426 storeProperty(localPos.getResource(), localP);
\r
427 Tuple3d p = getWorldFromLocal(node, new Point3d(localP));
\r
428 storeProperty(worldPos.getResource(), p);
\r
429 G3DTools.setTuple3(worldPos, p);
\r
430 if (DEBUG) System.out.println("Node changed local: wp " + worldP + " lp " + p + " old " + cachedLocalP);
\r
432 } else if (changedWorldP) {
\r
433 storeProperty(worldPos.getResource(), worldP);
\r
434 Tuple3d p = getLocalFromWorld(node, new Point3d(worldP));
\r
435 G3DTools.setTuple3(localPos, p);
\r
436 storeProperty(localPos.getResource(), p);
\r
437 if (DEBUG) System.out.println("Node changed world: wp " + worldP + " lp " + p + " old " + cachedWorldP);
\r
442 if (localR != null || worldR != null) {
\r
443 AxisAngle4d cachedWorldR = (AxisAngle4d)getProperty(worldOr.getResource());
\r
444 AxisAngle4d cachedLocalR = (AxisAngle4d)getProperty(localOr.getResource());
\r
445 boolean changedLocalR = false;
\r
446 boolean changedWorldR = false;
\r
448 if (cachedLocalR == null)
\r
449 changedLocalR = true;
\r
450 else if (changed(cachedLocalR,localR))
\r
451 changedLocalR = true;
\r
453 if (cachedWorldR == null) {
\r
454 changedWorldR = true;
\r
455 } else if (changed(cachedWorldR,worldR)){
\r
456 changedWorldR = true;
\r
459 if (changedLocalR) {
\r
460 storeProperty(localOr.getResource(), localR);
\r
461 AxisAngle4d p = getWorldFromLocal(node, new AxisAngle4d(localR));
\r
462 G3DTools.setOrientation(worldOr, p);
\r
463 storeProperty(worldOr.getResource(), p);
\r
464 if (DEBUG) System.out.println("Node changed localR: wr " + p + " lr " + localR + " old " + cachedLocalR);
\r
466 } else if (changedWorldR) {
\r
467 storeProperty(worldOr.getResource(), worldR);
\r
468 AxisAngle4d p = getLocalFromWorld(node, new AxisAngle4d(worldR));
\r
469 G3DTools.setOrientation(localOr, p);
\r
470 storeProperty(localOr.getResource(), p);
\r
471 if (DEBUG) System.out.println("Node changed worldR: wr " + worldR + " lr " + p + " old " + cachedWorldR);
\r
477 propagateTransformChange(node);
\r
479 System.out.println("No Node transformation change detected " + node.getResource());
\r
483 public static boolean changed(Tuple3d v1, Tuple3d v2) {
\r
484 Vector3d t = new Vector3d(v1);
\r
486 return t.lengthSquared() > 0.00001;
\r
489 public static boolean changed (double d1 , double d2) {
\r
490 return (Math.abs(d1 - d2) > 0.00001 );
\r
494 public static boolean changed(AxisAngle4d aa1, AxisAngle4d aa2) {
\r
495 if (Math.abs(aa1.angle - aa2.angle) > 0.00001)
\r
497 Vector3d t1 = new Vector3d(aa1.x-aa2.x,aa1.y-aa2.y,aa1.z-aa2.z);
\r
498 if (t1.lengthSquared() > 0.00001) {
\r
499 if (Math.abs(aa1.angle) < 0.0001)
\r
507 private HashMap<Object, Object> properties = new HashMap<Object, Object>();
\r
509 public Object getProperty(Object key) {
\r
510 return properties.get(key);
\r
513 public void storeProperty(Object key, Object value) {
\r
514 properties.put(key, value);
\r