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