+++ /dev/null
-/*******************************************************************************\r
- * Copyright (c) 2007- VTT Technical Research Centre of Finland.\r
- * All rights reserved. This program and the accompanying materials\r
- * are made available under the terms of the Eclipse Public License v1.0\r
- * which accompanies this distribution, and is available at\r
- * http://www.eclipse.org/legal/epl-v10.html\r
- *\r
- * Contributors:\r
- * VTT Technical Research Centre of Finland - initial API and implementation\r
- *******************************************************************************/\r
-package org.simantics.proconf.g3d.base;\r
-\r
-import java.util.Collection;\r
-import java.util.HashMap;\r
-\r
-import javax.vecmath.AxisAngle4d;\r
-import javax.vecmath.Point3d;\r
-import javax.vecmath.Quat4d;\r
-import javax.vecmath.Tuple3d;\r
-import javax.vecmath.Vector3d;\r
-\r
-import org.simantics.db.Graph;\r
-import org.simantics.db.Resource;\r
-import org.simantics.layer0.utils.EntityFactory;\r
-import org.simantics.layer0.utils.IEntity;\r
-import org.simantics.proconf.g3d.Resources;\r
-import org.simantics.proconf.g3d.stubs.Orientation;\r
-import org.simantics.proconf.g3d.stubs.Position;\r
-\r
-public class TransformationTools {\r
- \r
- private static boolean DEBUG = false;\r
- \r
- private Resource childRelation;\r
- private Resource parentRelation;\r
- \r
- public TransformationTools(Resource childRelation, Resource parentRelation) {\r
- this.childRelation = childRelation;\r
- this.parentRelation = parentRelation;\r
- }\r
- \r
- public IEntity getParent(IEntity node) {\r
- return node.getAtMostOneRelatedObject(parentRelation);\r
- }\r
- \r
- public Point3d getLocalFromWorld(IEntity node, Point3d worldCoord) {\r
- IEntity parent = getParent(node);\r
- if (parent == null) {// this is a rootnode ( has no transformation) \r
- return worldCoord;\r
- }\r
-\r
- Point3d local = getLocalFromWorld2(parent,worldCoord);\r
- return local;\r
- }\r
- \r
- \r
- private Point3d getLocalFromWorld2(IEntity node, Point3d worldCoord) {\r
- IEntity parent = getParent(node);\r
- if (parent == null) {// this is a root node ( has no transformation) \r
- return worldCoord;\r
- }\r
-\r
- Point3d local = getLocalFromWorld2(parent,worldCoord);\r
- if (node.hasStatement(Resources.g3dResource.HasLocalPosition))\r
- local.sub(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));\r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- q.inverse();\r
- MathTools.rotate(q, local, local);\r
- }\r
- return local;\r
- }\r
- \r
- public Point3d getWorldFromLocal(IEntity node,Point3d localCoord) {\r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return localCoord;\r
- return getWorldFromLocal2(parent, localCoord);\r
- }\r
- \r
- private Point3d getWorldFromLocal2(IEntity node,Point3d localCoord) {\r
- \r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- MathTools.rotate(q, localCoord, localCoord);\r
- }\r
- if (node.hasStatement(Resources.g3dResource.HasLocalPosition))\r
- localCoord.add(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));\r
- \r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return localCoord;\r
- \r
- return getWorldFromLocal2(parent,localCoord);\r
- }\r
- \r
- \r
- public AxisAngle4d getLocalFromWorld(IEntity node, AxisAngle4d worldRot) {\r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return worldRot;\r
-\r
- AxisAngle4d local = getLocalFromWorld2(parent,worldRot);\r
-\r
- return local;\r
- }\r
- \r
- private AxisAngle4d getLocalFromWorld2(IEntity node, AxisAngle4d worldRot) {\r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return worldRot;\r
- AxisAngle4d local = getLocalFromWorld2(parent,worldRot);\r
- \r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- q.inverse();\r
- Quat4d q2 = new Quat4d();\r
- q2.set(local);\r
- q.mul(q2);\r
- local.set(q);\r
- }\r
-\r
- return local;\r
- }\r
- \r
- public AxisAngle4d getWorldFromLocal(IEntity node,AxisAngle4d localRot) {\r
- IEntity parent = getParent(node);\r
- if (parent == null)\r
- return localRot;\r
-\r
- return getWorldFromLocal2(parent,localRot);\r
- }\r
- \r
- private AxisAngle4d getWorldFromLocal2(IEntity node,AxisAngle4d localRot) {\r
- \r
- //System.out.print("wtl " + node.getResource() + " " + localCoord);\r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- Quat4d q2 = new Quat4d();\r
- q2.set(localRot);\r
- q.mul(q2);\r
- localRot.set(q);\r
- }\r
- \r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return localRot;\r
- \r
- //System.out.println(" " + localCoord);\r
- return getWorldFromLocal2(parent,localRot);\r
- }\r
- \r
- public Point3d getLocalFromWorldR(IEntity node, Point3d worldCoord) {\r
- \r
- Point3d local = getLocalFromWorldR(null,worldCoord);\r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- q.inverse();\r
- MathTools.rotate(q, local, local);\r
- }\r
- \r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return worldCoord;\r
- \r
- return local;\r
- }\r
- \r
- public Point3d getWorldFromLocalR(IEntity node,Point3d localCoord) {\r
- \r
- if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {\r
- Quat4d q = new Quat4d();\r
- q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));\r
- MathTools.rotate(q, localCoord, localCoord);\r
- } \r
- \r
- IEntity parent = getParent(node);\r
- if (parent == null) // this is a rootnode ( has no transformation)\r
- return localCoord;\r
- \r
- return getWorldFromLocalR(parent,localCoord);\r
- }\r
- \r
- /**\r
- * Updates transformations of all children of given node\r
- * @param node\r
- */\r
- public void propagateTransformChange(IEntity node) {\r
- Collection<IEntity> children = node.getRelatedObjects(childRelation);\r
- IEntity wp = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- IEntity wr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- Quat4d rot = new Quat4d();\r
- if (wr != null)\r
- rot.set(G3DTools.getOrientation(wr));\r
- else\r
- rot.w = 1.0;\r
- Point3d pos = new Point3d();\r
- if (wp != null)\r
- pos = G3DTools.getPoint(wp);\r
- if (DEBUG) {\r
- if (wr != null)\r
- System.out.println("propagate transform " + node.getResource() + " " + pos + " " + G3DTools.getOrientation(wr));\r
- else\r
- System.out.println("propagate transform " + node.getResource() + " " + pos);\r
- }\r
- for (IEntity n : children) {\r
- IEntity lPos = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);\r
- IEntity lRot = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);\r
- if (DEBUG) System.out.print(n);\r
- if (lRot != null) {\r
- AxisAngle4d la = G3DTools.getOrientation(lRot);\r
- AxisAngle4d wa = getWorldFromLocal(n, la);\r
- IEntity wo = n.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- G3DTools.setOrientation(wo,wa);\r
- storeProperty(wo.getResource(), wa);\r
- }\r
- if (lPos != null) {\r
- Point3d lp = G3DTools.getPoint(lPos);\r
- if (DEBUG) System.out.println(lp);\r
- MathTools.rotate(rot, lp, lp);\r
- lp.add(pos);\r
- IEntity nwp = n.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- G3DTools.setTuple3(nwp, lp);\r
- if (DEBUG) System.out.print(" " + lp);\r
- storeProperty(nwp.getResource(), lp);\r
- }\r
- if (DEBUG) System.out.println();\r
- propagateTransformChange(n);\r
- \r
- }\r
- }\r
- \r
- /**\r
- * Updates transformation of one child node without changing its local transformation.\r
- * @param parent\r
- * @param node\r
- */\r
- public void propagateLocalTransformChange(IEntity parent, IEntity node) {\r
- //Collection<IEntity> children = parent.getRelatedObjects(childRelation);\r
- IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- Quat4d parentWRot = new Quat4d();\r
- if (parentWR != null) {\r
- parentWRot.set(G3DTools.getOrientation(parentWR));\r
- \r
- } else\r
- parentWRot.w = 1.0;\r
- Point3d parentWPos = new Point3d();\r
- if (parentWP != null) {\r
- parentWPos = G3DTools.getPoint(parentWP);\r
- }\r
- if (DEBUG) {\r
- if (parentWR != null)\r
- System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));\r
- else\r
- System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);\r
- }\r
- //for (IEntity n : children) {\r
- // if (!n.equals(node))\r
- // continue;\r
- IEntity lPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);\r
- IEntity lRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);\r
- if (DEBUG) System.out.print(node);\r
- if (lRot != null) {\r
- AxisAngle4d aa = G3DTools.getOrientation(lRot);\r
- storeProperty(lRot.getResource(), aa);\r
- AxisAngle4d la = getWorldFromLocal(node, aa);\r
- IEntity wo = node.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- G3DTools.setOrientation(wo,la);\r
- storeProperty(wo.getResource(), la);\r
- }\r
- if (lPos != null) {\r
- Point3d lp = G3DTools.getPoint(lPos);\r
- storeProperty(lPos.getResource(), lp);\r
- MathTools.rotate(parentWRot, lp, lp);\r
- lp.add(parentWPos);\r
- IEntity nwp = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- G3DTools.setTuple3(nwp, lp);\r
- storeProperty(nwp.getResource(), lp);\r
- }\r
- if (DEBUG) System.out.println();\r
- propagateTransformChange(node);\r
- \r
- }\r
-// }\r
- \r
- public static void resetTransformation(IEntity shape) {\r
- Graph graph = shape.getGraph();\r
- if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition) == null) {\r
-\r
- // LocalPosition p = LocalPosition.createDefault(graph);\r
- Position p = Position.createDefault(graph);\r
- shape.addStatement(Resources.g3dResource.HasLocalPosition, p);\r
- // WorldPosition p2 = WorldPosition.createDefault(graph);\r
- Position p2 = Position.createDefault(graph);\r
- shape.addStatement(Resources.g3dResource.HasWorldPosition, p2);\r
- p.setX(new double[] { 0.0 });\r
- p.setY(new double[] { 0.0 });\r
- p.setZ(new double[] { 0.0 });\r
-\r
- p2.setX(new double[] { 0.0 });\r
- p2.setY(new double[] { 0.0 });\r
- p2.setZ(new double[] { 0.0 });\r
-\r
- } else {\r
- G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition), 0.0, 0.0, 0.0);\r
- G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition), 0.0, 0.0, 0.0);\r
- }\r
- if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation) == null) {\r
-\r
- // LocalOrientation r = LocalOrientationFactory.create(graph);\r
- Orientation r = Orientation.createDefault(graph);\r
- shape.addStatement(Resources.g3dResource.HasLocalOrientation, r);\r
- // WorldOrientation r2 = WorldOrientationFactory.create(graph);\r
- Orientation r2 = Orientation.createDefault(graph);\r
- shape.addStatement(Resources.g3dResource.HasWorldOrientation, r2);\r
- r.setAngle(new double[] { 0.0 });\r
- r.setX(new double[] { 1.0 });\r
- r.setY(new double[] { 0.0 });\r
- r.setZ(new double[] { 0.0 });\r
- r2.setAngle(new double[] { 0.0 });\r
- r2.setX(new double[] { 1.0 });\r
- r2.setY(new double[] { 0.0 });\r
- r2.setZ(new double[] { 0.0 });\r
-\r
- } else {\r
- G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation),\r
- new AxisAngle4d(0.0, 1.0, 0.0, 0.0));\r
- G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation),\r
- new AxisAngle4d(0.0, 1.0, 0.0, 0.0));\r
- }\r
- }\r
- \r
- /**\r
- * Updates transformation of one child node without changing its world transformation.\r
- * @param parent\r
- * @param node\r
- */\r
- public void propagateWorldTransformChange(IEntity parent, IEntity node) {\r
- //Collection<IEntity> children = parent.getRelatedObjects(childRelation);\r
- IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- Quat4d parentWQuat = new Quat4d();\r
- if (parentWR != null) {\r
- parentWQuat.set(G3DTools.getOrientation(parentWR));\r
- }\r
- else\r
- parentWQuat.w = 1.0;\r
- Point3d parentWPos = new Point3d();\r
- if (parentWP != null) {\r
- parentWPos = G3DTools.getPoint(parentWP);\r
- }\r
- if (DEBUG){\r
- if (parentWR != null) \r
- System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));\r
- else\r
- System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);\r
- }\r
- //for (IEntity n : children) {\r
- // if (!n.equals(node))\r
- // continue;\r
- IEntity wPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- IEntity wRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- \r
- if (DEBUG) System.out.print(node);\r
- if (wRot != null) {\r
- AxisAngle4d aa = G3DTools.getOrientation(wRot);\r
- storeProperty(wRot.getResource(), aa);\r
- AxisAngle4d la = getLocalFromWorld(node, aa);\r
- IEntity lRot = node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation);\r
- G3DTools.setOrientation(lRot,la);\r
- storeProperty(lRot.getResource(), la);\r
- }\r
- if (wPos != null) {\r
- Point3d lp = G3DTools.getPoint(wPos);\r
- storeProperty(wPos.getResource(), lp);\r
- lp.sub(parentWPos);\r
- parentWQuat.inverse(); \r
- MathTools.rotate(parentWQuat, lp, lp);\r
- IEntity lPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);\r
- G3DTools.setTuple3(lPos, lp);\r
- storeProperty(lPos.getResource(), lp);\r
- }\r
- if (DEBUG) System.out.println();\r
- propagateTransformChange(node);\r
- \r
- // }\r
- }\r
- \r
- public boolean transformationUpdate(Graph graph, Resource resource) {\r
- \r
- //resources.startTransaction("transformationUpdate");\r
- IEntity entity = EntityFactory.create(graph,resource);\r
- return transformationUpdate(entity);\r
- }\r
- \r
- public boolean transformationUpdate(IEntity node) {\r
- if (DEBUG) System.out.println("Node transformation update " + node.getResource());\r
- IEntity worldPos = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);\r
- IEntity localPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);\r
- IEntity worldOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);\r
- IEntity localOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);\r
- \r
- Tuple3d worldP =G3DTools.getPoint(worldPos);\r
- Tuple3d localP = G3DTools.getPoint(localPos);\r
-\r
- AxisAngle4d worldR = null;\r
- AxisAngle4d localR = null;\r
- if (worldOr != null) {\r
- worldR = G3DTools.getOrientation(worldOr);\r
- localR = G3DTools.getOrientation(localOr);\r
- }\r
- \r
- boolean changed = false;\r
- if (localP != null && worldP != null) {\r
- Tuple3d cachedWorldP = (Tuple3d)getProperty(worldPos.getResource());\r
- Tuple3d cachedLocalP = (Tuple3d)getProperty(localPos.getResource());\r
- boolean changedLocalP = false;\r
- boolean changedWorldP = false;\r
- if (cachedLocalP == null)\r
- changedLocalP = true;\r
- else if (changed(cachedLocalP,localP))\r
- changedLocalP = true;\r
-\r
- if (cachedWorldP == null) {\r
- changedWorldP = true;\r
- } else if (changed(cachedWorldP,worldP)){\r
- changedWorldP = true;\r
- }\r
- if (changedLocalP) {\r
- storeProperty(localPos.getResource(), localP);\r
- Tuple3d p = getWorldFromLocal(node, new Point3d(localP));\r
- storeProperty(worldPos.getResource(), p);\r
- G3DTools.setTuple3(worldPos, p);\r
- if (DEBUG) System.out.println("Node changed local: wp " + worldP + " lp " + p + " old " + cachedLocalP);\r
- changed = true;\r
- } else if (changedWorldP) {\r
- storeProperty(worldPos.getResource(), worldP);\r
- Tuple3d p = getLocalFromWorld(node, new Point3d(worldP));\r
- G3DTools.setTuple3(localPos, p);\r
- storeProperty(localPos.getResource(), p);\r
- if (DEBUG) System.out.println("Node changed world: wp " + worldP + " lp " + p + " old " + cachedWorldP);\r
- changed = true;\r
- }\r
- }\r
-\r
- if (localR != null || worldR != null) {\r
- AxisAngle4d cachedWorldR = (AxisAngle4d)getProperty(worldOr.getResource());\r
- AxisAngle4d cachedLocalR = (AxisAngle4d)getProperty(localOr.getResource());\r
- boolean changedLocalR = false;\r
- boolean changedWorldR = false;\r
-\r
- if (cachedLocalR == null)\r
- changedLocalR = true;\r
- else if (changed(cachedLocalR,localR))\r
- changedLocalR = true;\r
-\r
- if (cachedWorldR == null) {\r
- changedWorldR = true;\r
- } else if (changed(cachedWorldR,worldR)){\r
- changedWorldR = true;\r
- }\r
-\r
- if (changedLocalR) {\r
- storeProperty(localOr.getResource(), localR);\r
- AxisAngle4d p = getWorldFromLocal(node, new AxisAngle4d(localR));\r
- G3DTools.setOrientation(worldOr, p);\r
- storeProperty(worldOr.getResource(), p);\r
- if (DEBUG) System.out.println("Node changed localR: wr " + p + " lr " + localR + " old " + cachedLocalR);\r
- changed = true;\r
- } else if (changedWorldR) {\r
- storeProperty(worldOr.getResource(), worldR);\r
- AxisAngle4d p = getLocalFromWorld(node, new AxisAngle4d(worldR));\r
- G3DTools.setOrientation(localOr, p);\r
- storeProperty(localOr.getResource(), p);\r
- if (DEBUG) System.out.println("Node changed worldR: wr " + worldR + " lr " + p + " old " + cachedWorldR);\r
- changed = true;\r
- } \r
- }\r
-\r
- if (changed)\r
- propagateTransformChange(node);\r
- else if (DEBUG)\r
- System.out.println("No Node transformation change detected " + node.getResource());\r
- return changed;\r
- }\r
- \r
- public static boolean changed(Tuple3d v1, Tuple3d v2) {\r
- Vector3d t = new Vector3d(v1);\r
- t.sub(v2);\r
- return t.lengthSquared() > 0.00001;\r
- }\r
- \r
- public static boolean changed (double d1 , double d2) {\r
- return (Math.abs(d1 - d2) > 0.00001 );\r
- \r
- }\r
- \r
- public static boolean changed(AxisAngle4d aa1, AxisAngle4d aa2) {\r
- if (Math.abs(aa1.angle - aa2.angle) > 0.00001)\r
- return true;\r
- Vector3d t1 = new Vector3d(aa1.x-aa2.x,aa1.y-aa2.y,aa1.z-aa2.z);\r
- if (t1.lengthSquared() > 0.00001) {\r
- if (Math.abs(aa1.angle) < 0.0001)\r
- return false;\r
- return true;\r
- }\r
- return false;\r
- }\r
-\r
- \r
- private HashMap<Object, Object> properties = new HashMap<Object, Object>();\r
- \r
- public Object getProperty(Object key) {\r
- return properties.get(key);\r
- }\r
- \r
- public void storeProperty(Object key, Object value) {\r
- properties.put(key, value);\r
- }\r
- \r
- \r
- \r
- \r
-}\r