1 /*******************************************************************************
\r
2 * Copyright (c) 2007- VTT Technical Research Centre of Finland.
\r
3 * All rights reserved. This program and the accompanying materials
\r
4 * are made available under the terms of the Eclipse Public License v1.0
\r
5 * which accompanies this distribution, and is available at
\r
6 * http://www.eclipse.org/legal/epl-v10.html
\r
9 * VTT Technical Research Centre of Finland - initial API and implementation
\r
10 *******************************************************************************/
\r
11 package org.simantics.proconf.g3d.base;
\r
13 import java.util.Collection;
\r
14 import java.util.HashMap;
\r
16 import javax.vecmath.AxisAngle4d;
\r
17 import javax.vecmath.Point3d;
\r
18 import javax.vecmath.Quat4d;
\r
19 import javax.vecmath.Tuple3d;
\r
20 import javax.vecmath.Vector3d;
\r
22 import org.simantics.db.Graph;
\r
23 import org.simantics.db.Resource;
\r
24 import org.simantics.layer0.utils.EntityFactory;
\r
25 import org.simantics.layer0.utils.IEntity;
\r
26 import org.simantics.proconf.g3d.Resources;
\r
27 import org.simantics.proconf.g3d.stubs.Orientation;
\r
28 import org.simantics.proconf.g3d.stubs.Position;
\r
30 public class TransformationTools {
\r
32 private static boolean DEBUG = false;
\r
34 private Resource childRelation;
\r
35 private Resource parentRelation;
\r
37 public TransformationTools(Resource childRelation, Resource parentRelation) {
\r
38 this.childRelation = childRelation;
\r
39 this.parentRelation = parentRelation;
\r
42 public IEntity getParent(IEntity node) {
\r
43 return node.getAtMostOneRelatedObject(parentRelation);
\r
46 public Point3d getLocalFromWorld(IEntity node, Point3d worldCoord) {
\r
47 IEntity parent = getParent(node);
\r
48 if (parent == null) {// this is a rootnode ( has no transformation)
\r
52 Point3d local = getLocalFromWorld2(parent,worldCoord);
\r
57 private Point3d getLocalFromWorld2(IEntity node, Point3d worldCoord) {
\r
58 IEntity parent = getParent(node);
\r
59 if (parent == null) {// this is a root node ( has no transformation)
\r
63 Point3d local = getLocalFromWorld2(parent,worldCoord);
\r
64 if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
\r
65 local.sub(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
\r
66 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
67 Quat4d q = new Quat4d();
\r
68 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
70 MathTools.rotate(q, local, local);
\r
75 public Point3d getWorldFromLocal(IEntity node,Point3d localCoord) {
\r
76 IEntity parent = getParent(node);
\r
77 if (parent == null) // this is a rootnode ( has no transformation)
\r
79 return getWorldFromLocal2(parent, localCoord);
\r
82 private Point3d getWorldFromLocal2(IEntity node,Point3d localCoord) {
\r
84 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
85 Quat4d q = new Quat4d();
\r
86 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
87 MathTools.rotate(q, localCoord, localCoord);
\r
89 if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
\r
90 localCoord.add(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
\r
92 IEntity parent = getParent(node);
\r
93 if (parent == null) // this is a rootnode ( has no transformation)
\r
96 return getWorldFromLocal2(parent,localCoord);
\r
100 public AxisAngle4d getLocalFromWorld(IEntity node, AxisAngle4d worldRot) {
\r
101 IEntity parent = getParent(node);
\r
102 if (parent == null) // this is a rootnode ( has no transformation)
\r
105 AxisAngle4d local = getLocalFromWorld2(parent,worldRot);
\r
110 private AxisAngle4d getLocalFromWorld2(IEntity node, AxisAngle4d worldRot) {
\r
111 IEntity parent = getParent(node);
\r
112 if (parent == null) // this is a rootnode ( has no transformation)
\r
114 AxisAngle4d local = getLocalFromWorld2(parent,worldRot);
\r
116 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
117 Quat4d q = new Quat4d();
\r
118 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
120 Quat4d q2 = new Quat4d();
\r
129 public AxisAngle4d getWorldFromLocal(IEntity node,AxisAngle4d localRot) {
\r
130 IEntity parent = getParent(node);
\r
131 if (parent == null)
\r
134 return getWorldFromLocal2(parent,localRot);
\r
137 private AxisAngle4d getWorldFromLocal2(IEntity node,AxisAngle4d localRot) {
\r
139 //System.out.print("wtl " + node.getResource() + " " + localCoord);
\r
140 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
141 Quat4d q = new Quat4d();
\r
142 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
143 Quat4d q2 = new Quat4d();
\r
149 IEntity parent = getParent(node);
\r
150 if (parent == null) // this is a rootnode ( has no transformation)
\r
153 //System.out.println(" " + localCoord);
\r
154 return getWorldFromLocal2(parent,localRot);
\r
157 public Point3d getLocalFromWorldR(IEntity node, Point3d worldCoord) {
\r
159 Point3d local = getLocalFromWorldR(null,worldCoord);
\r
160 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
161 Quat4d q = new Quat4d();
\r
162 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
164 MathTools.rotate(q, local, local);
\r
167 IEntity parent = getParent(node);
\r
168 if (parent == null) // this is a rootnode ( has no transformation)
\r
174 public Point3d getWorldFromLocalR(IEntity node,Point3d localCoord) {
\r
176 if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
\r
177 Quat4d q = new Quat4d();
\r
178 q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
\r
179 MathTools.rotate(q, localCoord, localCoord);
\r
182 IEntity parent = getParent(node);
\r
183 if (parent == null) // this is a rootnode ( has no transformation)
\r
186 return getWorldFromLocalR(parent,localCoord);
\r
190 * Updates transformations of all children of given node
\r
193 public void propagateTransformChange(IEntity node) {
\r
194 Collection<IEntity> children = node.getRelatedObjects(childRelation);
\r
195 IEntity wp = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
196 IEntity wr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
197 Quat4d rot = new Quat4d();
\r
199 rot.set(G3DTools.getOrientation(wr));
\r
202 Point3d pos = new Point3d();
\r
204 pos = G3DTools.getPoint(wp);
\r
207 System.out.println("propagate transform " + node.getResource() + " " + pos + " " + G3DTools.getOrientation(wr));
\r
209 System.out.println("propagate transform " + node.getResource() + " " + pos);
\r
211 for (IEntity n : children) {
\r
212 IEntity lPos = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
213 IEntity lRot = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
214 if (DEBUG) System.out.print(n);
\r
215 if (lRot != null) {
\r
216 AxisAngle4d la = G3DTools.getOrientation(lRot);
\r
217 AxisAngle4d wa = getWorldFromLocal(n, la);
\r
218 IEntity wo = n.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
219 G3DTools.setOrientation(wo,wa);
\r
220 storeProperty(wo.getResource(), wa);
\r
222 if (lPos != null) {
\r
223 Point3d lp = G3DTools.getPoint(lPos);
\r
224 if (DEBUG) System.out.println(lp);
\r
225 MathTools.rotate(rot, lp, lp);
\r
227 IEntity nwp = n.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
228 G3DTools.setTuple3(nwp, lp);
\r
229 if (DEBUG) System.out.print(" " + lp);
\r
230 storeProperty(nwp.getResource(), lp);
\r
232 if (DEBUG) System.out.println();
\r
233 propagateTransformChange(n);
\r
239 * Updates transformation of one child node without changing its local transformation.
\r
243 public void propagateLocalTransformChange(IEntity parent, IEntity node) {
\r
244 //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
\r
245 IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
246 IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
247 Quat4d parentWRot = new Quat4d();
\r
248 if (parentWR != null) {
\r
249 parentWRot.set(G3DTools.getOrientation(parentWR));
\r
252 parentWRot.w = 1.0;
\r
253 Point3d parentWPos = new Point3d();
\r
254 if (parentWP != null) {
\r
255 parentWPos = G3DTools.getPoint(parentWP);
\r
258 if (parentWR != null)
\r
259 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
\r
261 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
\r
263 //for (IEntity n : children) {
\r
264 // if (!n.equals(node))
\r
266 IEntity lPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
267 IEntity lRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
268 if (DEBUG) System.out.print(node);
\r
269 if (lRot != null) {
\r
270 AxisAngle4d aa = G3DTools.getOrientation(lRot);
\r
271 storeProperty(lRot.getResource(), aa);
\r
272 AxisAngle4d la = getWorldFromLocal(node, aa);
\r
273 IEntity wo = node.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
274 G3DTools.setOrientation(wo,la);
\r
275 storeProperty(wo.getResource(), la);
\r
277 if (lPos != null) {
\r
278 Point3d lp = G3DTools.getPoint(lPos);
\r
279 storeProperty(lPos.getResource(), lp);
\r
280 MathTools.rotate(parentWRot, lp, lp);
\r
281 lp.add(parentWPos);
\r
282 IEntity nwp = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
283 G3DTools.setTuple3(nwp, lp);
\r
284 storeProperty(nwp.getResource(), lp);
\r
286 if (DEBUG) System.out.println();
\r
287 propagateTransformChange(node);
\r
292 public static void resetTransformation(IEntity shape) {
\r
293 Graph graph = shape.getGraph();
\r
294 if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition) == null) {
\r
296 // LocalPosition p = LocalPosition.createDefault(graph);
\r
297 Position p = Position.createDefault(graph);
\r
298 shape.addStatement(Resources.g3dResource.HasLocalPosition, p);
\r
299 // WorldPosition p2 = WorldPosition.createDefault(graph);
\r
300 Position p2 = Position.createDefault(graph);
\r
301 shape.addStatement(Resources.g3dResource.HasWorldPosition, p2);
\r
302 p.setX(new double[] { 0.0 });
\r
303 p.setY(new double[] { 0.0 });
\r
304 p.setZ(new double[] { 0.0 });
\r
306 p2.setX(new double[] { 0.0 });
\r
307 p2.setY(new double[] { 0.0 });
\r
308 p2.setZ(new double[] { 0.0 });
\r
311 G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition), 0.0, 0.0, 0.0);
\r
312 G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition), 0.0, 0.0, 0.0);
\r
314 if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation) == null) {
\r
316 // LocalOrientation r = LocalOrientationFactory.create(graph);
\r
317 Orientation r = Orientation.createDefault(graph);
\r
318 shape.addStatement(Resources.g3dResource.HasLocalOrientation, r);
\r
319 // WorldOrientation r2 = WorldOrientationFactory.create(graph);
\r
320 Orientation r2 = Orientation.createDefault(graph);
\r
321 shape.addStatement(Resources.g3dResource.HasWorldOrientation, r2);
\r
322 r.setAngle(new double[] { 0.0 });
\r
323 r.setX(new double[] { 1.0 });
\r
324 r.setY(new double[] { 0.0 });
\r
325 r.setZ(new double[] { 0.0 });
\r
326 r2.setAngle(new double[] { 0.0 });
\r
327 r2.setX(new double[] { 1.0 });
\r
328 r2.setY(new double[] { 0.0 });
\r
329 r2.setZ(new double[] { 0.0 });
\r
332 G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation),
\r
333 new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
\r
334 G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation),
\r
335 new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
\r
340 * Updates transformation of one child node without changing its world transformation.
\r
344 public void propagateWorldTransformChange(IEntity parent, IEntity node) {
\r
345 //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
\r
346 IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
347 IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
348 Quat4d parentWQuat = new Quat4d();
\r
349 if (parentWR != null) {
\r
350 parentWQuat.set(G3DTools.getOrientation(parentWR));
\r
353 parentWQuat.w = 1.0;
\r
354 Point3d parentWPos = new Point3d();
\r
355 if (parentWP != null) {
\r
356 parentWPos = G3DTools.getPoint(parentWP);
\r
359 if (parentWR != null)
\r
360 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
\r
362 System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
\r
364 //for (IEntity n : children) {
\r
365 // if (!n.equals(node))
\r
367 IEntity wPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
368 IEntity wRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
370 if (DEBUG) System.out.print(node);
\r
371 if (wRot != null) {
\r
372 AxisAngle4d aa = G3DTools.getOrientation(wRot);
\r
373 storeProperty(wRot.getResource(), aa);
\r
374 AxisAngle4d la = getLocalFromWorld(node, aa);
\r
375 IEntity lRot = node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
376 G3DTools.setOrientation(lRot,la);
\r
377 storeProperty(lRot.getResource(), la);
\r
379 if (wPos != null) {
\r
380 Point3d lp = G3DTools.getPoint(wPos);
\r
381 storeProperty(wPos.getResource(), lp);
\r
382 lp.sub(parentWPos);
\r
383 parentWQuat.inverse();
\r
384 MathTools.rotate(parentWQuat, lp, lp);
\r
385 IEntity lPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
386 G3DTools.setTuple3(lPos, lp);
\r
387 storeProperty(lPos.getResource(), lp);
\r
389 if (DEBUG) System.out.println();
\r
390 propagateTransformChange(node);
\r
395 public boolean transformationUpdate(Graph graph, Resource resource) {
\r
397 //resources.startTransaction("transformationUpdate");
\r
398 IEntity entity = EntityFactory.create(graph,resource);
\r
399 return transformationUpdate(entity);
\r
402 public boolean transformationUpdate(IEntity node) {
\r
403 if (DEBUG) System.out.println("Node transformation update " + node.getResource());
\r
404 IEntity worldPos = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
\r
405 IEntity localPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
\r
406 IEntity worldOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
\r
407 IEntity localOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
\r
409 Tuple3d worldP =G3DTools.getPoint(worldPos);
\r
410 Tuple3d localP = G3DTools.getPoint(localPos);
\r
412 AxisAngle4d worldR = null;
\r
413 AxisAngle4d localR = null;
\r
414 if (worldOr != null) {
\r
415 worldR = G3DTools.getOrientation(worldOr);
\r
416 localR = G3DTools.getOrientation(localOr);
\r
419 boolean changed = false;
\r
420 if (localP != null && worldP != null) {
\r
421 Tuple3d cachedWorldP = (Tuple3d)getProperty(worldPos.getResource());
\r
422 Tuple3d cachedLocalP = (Tuple3d)getProperty(localPos.getResource());
\r
423 boolean changedLocalP = false;
\r
424 boolean changedWorldP = false;
\r
425 if (cachedLocalP == null)
\r
426 changedLocalP = true;
\r
427 else if (changed(cachedLocalP,localP))
\r
428 changedLocalP = true;
\r
430 if (cachedWorldP == null) {
\r
431 changedWorldP = true;
\r
432 } else if (changed(cachedWorldP,worldP)){
\r
433 changedWorldP = true;
\r
435 if (changedLocalP) {
\r
436 storeProperty(localPos.getResource(), localP);
\r
437 Tuple3d p = getWorldFromLocal(node, new Point3d(localP));
\r
438 storeProperty(worldPos.getResource(), p);
\r
439 G3DTools.setTuple3(worldPos, p);
\r
440 if (DEBUG) System.out.println("Node changed local: wp " + worldP + " lp " + p + " old " + cachedLocalP);
\r
442 } else if (changedWorldP) {
\r
443 storeProperty(worldPos.getResource(), worldP);
\r
444 Tuple3d p = getLocalFromWorld(node, new Point3d(worldP));
\r
445 G3DTools.setTuple3(localPos, p);
\r
446 storeProperty(localPos.getResource(), p);
\r
447 if (DEBUG) System.out.println("Node changed world: wp " + worldP + " lp " + p + " old " + cachedWorldP);
\r
452 if (localR != null || worldR != null) {
\r
453 AxisAngle4d cachedWorldR = (AxisAngle4d)getProperty(worldOr.getResource());
\r
454 AxisAngle4d cachedLocalR = (AxisAngle4d)getProperty(localOr.getResource());
\r
455 boolean changedLocalR = false;
\r
456 boolean changedWorldR = false;
\r
458 if (cachedLocalR == null)
\r
459 changedLocalR = true;
\r
460 else if (changed(cachedLocalR,localR))
\r
461 changedLocalR = true;
\r
463 if (cachedWorldR == null) {
\r
464 changedWorldR = true;
\r
465 } else if (changed(cachedWorldR,worldR)){
\r
466 changedWorldR = true;
\r
469 if (changedLocalR) {
\r
470 storeProperty(localOr.getResource(), localR);
\r
471 AxisAngle4d p = getWorldFromLocal(node, new AxisAngle4d(localR));
\r
472 G3DTools.setOrientation(worldOr, p);
\r
473 storeProperty(worldOr.getResource(), p);
\r
474 if (DEBUG) System.out.println("Node changed localR: wr " + p + " lr " + localR + " old " + cachedLocalR);
\r
476 } else if (changedWorldR) {
\r
477 storeProperty(worldOr.getResource(), worldR);
\r
478 AxisAngle4d p = getLocalFromWorld(node, new AxisAngle4d(worldR));
\r
479 G3DTools.setOrientation(localOr, p);
\r
480 storeProperty(localOr.getResource(), p);
\r
481 if (DEBUG) System.out.println("Node changed worldR: wr " + worldR + " lr " + p + " old " + cachedWorldR);
\r
487 propagateTransformChange(node);
\r
489 System.out.println("No Node transformation change detected " + node.getResource());
\r
493 public static boolean changed(Tuple3d v1, Tuple3d v2) {
\r
494 Vector3d t = new Vector3d(v1);
\r
496 return t.lengthSquared() > 0.00001;
\r
499 public static boolean changed (double d1 , double d2) {
\r
500 return (Math.abs(d1 - d2) > 0.00001 );
\r
504 public static boolean changed(AxisAngle4d aa1, AxisAngle4d aa2) {
\r
505 if (Math.abs(aa1.angle - aa2.angle) > 0.00001)
\r
507 Vector3d t1 = new Vector3d(aa1.x-aa2.x,aa1.y-aa2.y,aa1.z-aa2.z);
\r
508 if (t1.lengthSquared() > 0.00001) {
\r
509 if (Math.abs(aa1.angle) < 0.0001)
\r
517 private HashMap<Object, Object> properties = new HashMap<Object, Object>();
\r
519 public Object getProperty(Object key) {
\r
520 return properties.get(key);
\r
523 public void storeProperty(Object key, Object value) {
\r
524 properties.put(key, value);
\r