org.simantics.processeditor/src/org/simantics/processeditor/common/ControlPointTools.java

   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
   7  *\r
   8  * Contributors:\r
   9  *     VTT Technical Research Centre of Finland - initial API and implementation\r
  10  *******************************************************************************/\r
  11 package org.simantics.processeditor.common;\r
  12 \r
  13 import java.util.ArrayList;\r
  14 import java.util.Collection;\r
  15 import java.util.HashMap;\r
  16 import java.util.Stack;\r
  17 \r
  18 import javax.vecmath.AxisAngle4d;\r
  19 import javax.vecmath.Matrix3d;\r
  20 import javax.vecmath.Point3d;\r
  21 import javax.vecmath.Quat4d;\r
  22 import javax.vecmath.Tuple3d;\r
  23 import javax.vecmath.Vector3d;\r
  24 \r
  25 import org.simantics.db.Graph;\r
  26 import org.simantics.db.Resource;\r
  27 import org.simantics.db.Session;\r
  28 import org.simantics.layer0.utils.EntityFactory;\r
  29 import org.simantics.layer0.utils.IEntity;\r
  30 import org.simantics.processeditor.ProcessResource;\r
  31 import org.simantics.processeditor.actions.PositionType;\r
  32 import org.simantics.processeditor.common.PipingTools2.Direction;\r
  33 import org.simantics.processeditor.stubs.PipeControlPoint;\r
  34 import org.simantics.processeditor.stubs.PipeRun;\r
  35 import org.simantics.proconf.g3d.base.G3DTools;\r
  36 import org.simantics.proconf.g3d.base.MathTools;\r
  37 import org.simantics.proconf.g3d.base.TransformationTools;\r
  38 import org.simantics.utils.ui.ErrorLogger;\r
  39 import org.simantics.utils.datastructures.Pair;\r
  40 \r
  41 \r
  42 public class ControlPointTools {\r
  43         \r
  44         private static boolean DEBUG = false;\r
  45         \r
  46         private static TransformationTools tt;\r
  47         \r
  48         public static void initialize() {\r
  49                 tt = new TransformationTools(ProcessResource.plant3Dresource.HasSubPoint,ProcessResource.plant3Dresource.SubPointOf) {\r
  50                         @Override\r
  51                         public IEntity getParent(IEntity node) {\r
  52                                 IEntity parent =  super.getParent(node);\r
  53                                 if (parent == null) {\r
  54                                         parent = node.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.ControlPointOf);\r
  55                                         if (parent != null)\r
  56                                                 parent = parent.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasParent);\r
  57                                         else\r
  58                                                 parent = node.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasParent);\r
  59                                                 \r
  60                                 }\r
  61                                 return parent;\r
  62                         }\r
  63                 };\r
  64         }\r
  65         \r
  66         public static void deinitialize() {\r
  67                 tt = null;\r
  68         }\r
  69         \r
  70         /**\r
  71          * Adds new control point between given control points.\r
  72          * New pcp must be already part of the same piperun as previous CP and nextCP\r
  73          * \r
  74          * SizeChangeControlPoints cannot be inserted with this method, since it does link two different piperuns to each other\r
  75          * \r
  76          * @param newCP\r
  77          * @param previousCP\r
  78          * @param nextCP\r
  79          */\r
  80         public static void insertControlPoint(IEntity newCP, IEntity previousCP, IEntity nextCP) {\r
  81                 // inserting an offsetpoint is error, \r
  82                 assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));\r
  83                 // size change control point cannot be inserted this way, because it ends PipeRun\r
  84                 assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.SizeChangeControlPoint));\r
  85                 \r
  86                 IEntity piperun = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun);\r
  87                 // and just to make sure that control point structure is not corrupted\r
  88                 assert(piperun.equals(nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun)));\r
  89                 assert(piperun.equals(newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun)));\r
  90                 \r
  91                 // insert new BranchControlPoint between straight's control points\r
  92         IEntity previousNext = previousCP.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasNext);\r
  93         IEntity previousPrevious = previousCP.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasPrevious);\r
  94         \r
  95         IEntity offsetCP = null;\r
  96         \r
  97         if (newCP.isInstanceOf(ProcessResource.plant3Dresource.OffsettingPoint)) {\r
  98                 offsetCP = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
  99         }\r
 100         \r
 101                 if (previousNext != null && previousNext.equals(nextCP)) {\r
 102                         assert(!previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));\r
 103                         assert(!nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));\r
 104                         \r
 105                         setStatement(previousCP, ProcessResource.plant3Dresource.HasNext, newCP);\r
 106             setStatement(newCP, ProcessResource.plant3Dresource.HasPrevious, previousCP);\r
 107             \r
 108             if (previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {         \r
 109                 IEntity sccp = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);\r
 110                 setStatement(sccp, ProcessResource.plant3Dresource.HasNext, newCP);\r
 111             }\r
 112             \r
 113             setStatement(newCP, ProcessResource.plant3Dresource.HasNext, nextCP);\r
 114             \r
 115             if (offsetCP == null) {\r
 116                 setStatement(nextCP, ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 117             } else {\r
 118                 setStatement(nextCP, ProcessResource.plant3Dresource.HasPrevious, offsetCP);\r
 119                 setStatement(offsetCP, ProcessResource.plant3Dresource.HasNext, nextCP);\r
 120                 setStatement(offsetCP, ProcessResource.plant3Dresource.HasPrevious, previousCP);\r
 121             }\r
 122             \r
 123             if (nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 124                 IEntity ocp = nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 125                 setStatement(ocp, ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 126             }\r
 127             \r
 128          } else if (previousPrevious != null && previousPrevious.equals(nextCP)) {\r
 129                  // control point were given in reverse order \r
 130                  assert(!nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));\r
 131                          assert(!previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));\r
 132                          \r
 133                          setStatement(newCP, ProcessResource.plant3Dresource.HasNext, previousCP);\r
 134                          if (offsetCP == null) {\r
 135                 setStatement(previousCP, ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 136              } else {\r
 137                 setStatement(previousCP, ProcessResource.plant3Dresource.HasPrevious, offsetCP);\r
 138                 setStatement(offsetCP, ProcessResource.plant3Dresource.HasNext, previousCP);\r
 139                 setStatement(offsetCP, ProcessResource.plant3Dresource.HasPrevious, nextCP);\r
 140              }\r
 141                          \r
 142                  if (previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 143                          IEntity ocp = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 144                          setStatement(ocp, ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 145              }\r
 146                  \r
 147                  setStatement(newCP, ProcessResource.plant3Dresource.HasPrevious, nextCP);\r
 148                  setStatement(nextCP, ProcessResource.plant3Dresource.HasNext, newCP);\r
 149              if (nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 150                  IEntity sccp = nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);\r
 151                  setStatement(sccp, ProcessResource.plant3Dresource.HasNext, newCP);\r
 152              }\r
 153         } else {\r
 154             ErrorLogger.defaultLogError(\r
 155                     "Route pipe : could not find connection between straight pipe's control points", null);\r
 156         }\r
 157 \r
 158         }\r
 159         \r
 160         /**\r
 161          * Adds new control point attaching it to given control point.\r
 162          * If the new control point is SizeChangeControlPoint, it must have its offset point set.\r
 163          * \r
 164          * @param newCP\r
 165          * @param pcp\r
 166          * @param direction\r
 167          */\r
 168         public static void insertControlPoint(IEntity newCP, IEntity pcp, Direction direction) {\r
 169                 assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));\r
 170                 if (direction == Direction.NEXT) {\r
 171                         // if direction is next, user must have given OffsetPoint\r
 172                         assert(!pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));\r
 173                         // basic next/prev links\r
 174                         pcp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 175                         pcp.addStatement(ProcessResource.plant3Dresource.HasNext, newCP);\r
 176                         newCP.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 177                         newCP.addStatement(ProcessResource.plant3Dresource.HasPrevious, pcp);\r
 178                         // and last take care of sizechange / offset points\r
 179                         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 180                                 IEntity sccp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);\r
 181                                 sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 182                                 sccp.addStatement(ProcessResource.plant3Dresource.HasNext, newCP);\r
 183                         }\r
 184                         if (newCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 185                                 IEntity ocp = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 186                                 ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 187                                 ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, pcp);\r
 188                         }\r
 189                 } else {\r
 190                         // if direction is previous, user must have given sizechange\r
 191                         assert(!pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));\r
 192                         // previous direction is more complicated, since if newCP is SizeChangeControlPoint,\r
 193                         // we must link pcp to newCP's OffsetPoint\r
 194                         IEntity nocp = null;\r
 195                         if (newCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 196                                 nocp = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 197                                 nocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 198                                 nocp.addStatement(ProcessResource.plant3Dresource.HasNext, pcp);\r
 199                         }\r
 200                         pcp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 201                         if (nocp == null)\r
 202                                 pcp.addStatement(ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 203                         else\r
 204                                 pcp.addStatement(ProcessResource.plant3Dresource.HasPrevious, nocp);\r
 205                         \r
 206                         newCP.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 207                         newCP.addStatement(ProcessResource.plant3Dresource.HasNext, pcp);\r
 208                         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 209                                 IEntity ocp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 210                                 ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 211                                 if (nocp == null)\r
 212                                         ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, newCP);\r
 213                                 else\r
 214                                         ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, nocp);\r
 215                         }\r
 216                         \r
 217                 }\r
 218         }\r
 219         \r
 220         /**\r
 221          * Returns path legs direction\r
 222          * @param pcp\r
 223          * @param direction\r
 224          * @return\r
 225          */\r
 226         public static Vector3d getPathLegDirection(IEntity pcp,Direction direction) {\r
 227                 IEntity previous = pcp.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasPrevious);\r
 228                 IEntity next = pcp.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasNext);\r
 229                 if (direction == Direction.NEXT) {\r
 230                         if (next != null) {\r
 231                                 if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint))\r
 232                                         pcp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);\r
 233                                 Point3d p1 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 234                                 Point3d p2 = G3DTools.getPoint(next.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 235                                 Vector3d v = new Vector3d();\r
 236                                 v.sub(p2, p1);\r
 237                                 return v;\r
 238                         } else {\r
 239                                 if (pcp.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint))\r
 240                                         throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + pcp.getResource());\r
 241                                 if (previous == null) {\r
 242                                         if (!pcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint))\r
 243                                                 throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + pcp.getResource());\r
 244                                         else\r
 245                                                 return getDirectedControlPointDirection(pcp);\r
 246                                 } else {\r
 247                                         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {\r
 248                                                 Point3d p1 = G3DTools.getPoint(previous.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 249                                                 Point3d p2 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 250                                                 Vector3d v = new Vector3d();\r
 251                                                 v.sub(p2, p1);\r
 252                                                 return v;\r
 253                                         }\r
 254                                         throw new RuntimeException("Missing implementation");\r
 255                                 }\r
 256                         }\r
 257                 } else {\r
 258                         if (previous != null) {\r
 259                                 if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint))\r
 260                                         pcp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);\r
 261                                 Point3d p1 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 262                                 Point3d p2 = G3DTools.getPoint(previous.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 263                                 Vector3d v = new Vector3d();\r
 264                                 v.sub(p2, p1);\r
 265                                 return v;\r
 266                         } else {\r
 267                                 if (pcp.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint))\r
 268                                         throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + pcp.getResource());\r
 269                                 if (next == null) {\r
 270                                         if (!pcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint))\r
 271                                                 throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + pcp.getResource());\r
 272                                         else {\r
 273                                                 Vector3d v = getDirectedControlPointDirection(pcp);\r
 274                                                 v.negate();\r
 275                                                 return v;\r
 276                                         }\r
 277                                 } else {\r
 278                                         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {\r
 279                                                 Point3d p1 = G3DTools.getPoint(next.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 280                                                 Point3d p2 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 281                                                 Vector3d v = new Vector3d();\r
 282                                                 v.sub(p2, p1);\r
 283                                                 return v;\r
 284                                         }\r
 285                                         throw new RuntimeException("Missing implementation");\r
 286                                 }\r
 287                         }\r
 288                 }\r
 289                 \r
 290         }\r
 291         \r
 292         /**\r
 293          * Return positions (world) of an InlineComponents ends\r
 294          * @param pcp\r
 295          * @param p1\r
 296          * @param p2\r
 297          */\r
 298         public static void getInlineControlPointEnds(IEntity pcp, Point3d p1, Point3d p2) {\r
 299                 assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));\r
 300                 \r
 301                 Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 302                 Vector3d dir = ControlPointTools.getPathLegDirection(pcp, Direction.NEXT);\r
 303                 dir.normalize();\r
 304                 double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);\r
 305                 dir.scale(length * 0.5);\r
 306                 p1.set(pos);\r
 307                 p2.set(pos);\r
 308                 p1.add(dir);\r
 309                 p2.sub(dir);\r
 310         }\r
 311         \r
 312         /**\r
 313          * Return positions (world) of an InlineComponents ends\r
 314          * @param pcp\r
 315          * @param p1\r
 316          * @param p2\r
 317          */\r
 318         public static void getInlineControlPointEnds(IEntity pcp, Point3d p1, Point3d p2, Vector3d dir) {\r
 319                 assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));\r
 320                 \r
 321                 Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 322                 dir.set(ControlPointTools.getPathLegDirection(pcp, Direction.NEXT));\r
 323                 double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);\r
 324                 Vector3d d = new Vector3d(dir);\r
 325                 d.scale(length * 0.5);\r
 326                 p1.set(pos);\r
 327                 p2.set(pos);\r
 328                 p1.add(d);\r
 329                 p2.sub(d);\r
 330         }\r
 331         \r
 332         /**\r
 333          * Return positions (world) of an InlineComponents ends\r
 334          * @param pcp\r
 335          * @param p1\r
 336          * @param p2\r
 337          */\r
 338         public static void getInlineControlPointEnds(IEntity pcp, Point3d center, Point3d p1, Point3d p2, Vector3d dir) {\r
 339                 assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));\r
 340                 \r
 341                 Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));\r
 342                 center.set(pos);\r
 343                 dir.set(ControlPointTools.getPathLegDirection(pcp, Direction.NEXT));\r
 344                 double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);\r
 345                 Vector3d d = new Vector3d(dir);\r
 346                 d.scale(length * 0.5);\r
 347                 p1.set(pos);\r
 348                 p2.set(pos);\r
 349                 p1.add(d);\r
 350                 p2.sub(d);\r
 351         }\r
 352         \r
 353         public static double getInlineLength(IEntity pcp) {\r
 354         double l2 = 0.0;\r
 355         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.TurnControlPoint)) {\r
 356                 l2 += pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);\r
 357         } else if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {\r
 358                 l2 += pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength) * 0.5;\r
 359         }\r
 360         return l2;\r
 361     }\r
 362         \r
 363         /**\r
 364          * Returns position (world) of a single port\r
 365          * @param pcp\r
 366          * @param type\r
 367          * @return\r
 368          */\r
 369         public static Point3d getRealPosition(IEntity pcp, PositionType type) {\r
 370                 PipeControlPoint p= new PipeControlPoint(pcp);\r
 371                 Point3d pos = G3DTools.getPoint(p.getWorldPosition());\r
 372                 switch (type) {\r
 373                 case NEXT: {\r
 374                         Vector3d dir = ControlPointTools.getPathLegDirection(pcp,Direction.NEXT);\r
 375                         double length = getInlineLength(pcp);\r
 376                         dir.normalize();\r
 377                         dir.scale(length);\r
 378                         pos.add(dir);\r
 379                         break;\r
 380                 }\r
 381                 case PREVIOUS: {\r
 382                         Vector3d dir = ControlPointTools.getPathLegDirection(pcp,Direction.PREVIOUS);\r
 383                         double length = getInlineLength(pcp);\r
 384                         dir.normalize();\r
 385                         dir.scale(length);\r
 386                         pos.add(dir);\r
 387                         break;\r
 388                 }\r
 389                 case PORT:\r
 390                         // IEntity portDir = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasDirection);\r
 391                         // TODO : how we calculated needed space for a port; does it has an offset from control point's position or not?\r
 392                         break;\r
 393                 case SPLIT:\r
 394                         // do nothing\r
 395                         break;\r
 396                         \r
 397                 }\r
 398                 return pos;\r
 399         }\r
 400         \r
 401         public static void getInlineMovement(PipeControlPoint pcp, Point3d start, Point3d end) {\r
 402                 // FIXME : check type of neighbor components and allow movement on top of variable length components,\r
 403                 //         ffind proper range for movement (pcp's position is not)\r
 404                 PipeControlPoint prev = pcp.getPrevious().getPrevious();\r
 405                 PipeControlPoint next = pcp.getNext().getNext();\r
 406                 start.set(G3DTools.getPoint(prev.getWorldPosition()));\r
 407                 end.set(G3DTools.getPoint(next.getWorldPosition()));\r
 408         }\r
 409         \r
 410         public static AxisAngle4d getControlPointLocalRotation(IEntity pcp, double angle) {\r
 411                 Quat4d q1 = getControlPointLocalOrientationQuat(pcp, angle);\r
 412         AxisAngle4d aa= new AxisAngle4d();\r
 413                 aa.set(q1);\r
 414                 return aa;\r
 415         }\r
 416         \r
 417         public static AxisAngle4d getControlPointWorldRotation(IEntity pcp, double angle) {\r
 418                 Quat4d q1 = getControlPointWorldOrientationQuat(pcp, angle);\r
 419         AxisAngle4d aa= new AxisAngle4d();\r
 420                 aa.set(q1);\r
 421                 return aa;\r
 422         }\r
 423         \r
 424 \r
 425     public static Quat4d getControlPointLocalOrientationQuat(IEntity pcp, double angle) {\r
 426         return getControlPointLocalOrientationQuat(pcp, angle, false);\r
 427     }\r
 428     \r
 429     public static Quat4d getControlPointWorldOrientationQuat(IEntity pcp, double angle) {\r
 430         return getControlPointWorldOrientationQuat(pcp, angle, false);\r
 431     }\r
 432     \r
 433     public static Quat4d getControlPointLocalOrientationQuat(IEntity cp, double angle, boolean offset) {\r
 434         PipeControlPoint pcp = new PipeControlPoint(cp);\r
 435         PipeControlPoint next;\r
 436         // if pcp is size change control point with offset, next control point is not in line with previous and pcp control point (it's offsetted)\r
 437         // else it's more numerically stable to use next control point\r
 438         if (offset)\r
 439                 next = pcp;\r
 440         else\r
 441                 next = pcp.getNext();\r
 442         \r
 443         PipeControlPoint prev = pcp.getPrevious();\r
 444         assert (next != null || prev != null);\r
 445         if (prev == null)\r
 446                 prev = pcp;\r
 447         else if (next == null)\r
 448                 next = pcp;\r
 449         // TODO : check correct type\r
 450         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint) && pcp.getPrevious() == null) {\r
 451                 PipeControlPoint temp = next;\r
 452                 next = prev;\r
 453                 prev = temp;\r
 454         }\r
 455         Vector3d current = new Vector3d(G3DTools.getPoint(next.getLocalPosition()));\r
 456         current.sub(G3DTools.getPoint(prev.getLocalPosition()));\r
 457         current.normalize();\r
 458         return getControlPointOrientationQuat(current, angle);\r
 459     }\r
 460     \r
 461     public static Quat4d getControlPointWorldOrientationQuat(IEntity cp, double angle, boolean offset) {\r
 462         PipeControlPoint pcp = new PipeControlPoint(cp);\r
 463         PipeControlPoint next;\r
 464         // if pcp is size change control point with offset, next control point is not in line with previous and pcp control point (it's offsetted)\r
 465         // else it's more numerically stable to use next control point\r
 466         if (offset)\r
 467                 next = pcp;\r
 468         else\r
 469                 next = pcp.getNext();\r
 470         \r
 471         PipeControlPoint prev = pcp.getPrevious();\r
 472         assert (next != null || prev != null);\r
 473         if (prev == null)\r
 474                 prev = pcp;\r
 475         else if (next == null)\r
 476                 next = pcp;\r
 477         // TODO : check correct type\r
 478         if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint) && pcp.getPrevious() == null) {\r
 479                 PipeControlPoint temp = next;\r
 480                 next = prev;\r
 481                 prev = temp;\r
 482         }\r
 483         Vector3d current = new Vector3d(G3DTools.getPoint(next.getWorldPosition()));\r
 484         current.sub(G3DTools.getPoint(prev.getWorldPosition()));\r
 485         current.normalize();\r
 486         return getControlPointOrientationQuat(current, angle);\r
 487     }\r
 488     \r
 489     public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {\r
 490 \r
 491         \r
 492         final Vector3d front = new Vector3d(1.0,0.0,0.0);\r
 493         \r
 494         Quat4d q1 = new Quat4d();\r
 495 \r
 496                 Vector3d up = new Vector3d(0.0, 1.0, 0.0);\r
 497                 double a = up.angle(dir);\r
 498                 if (a < 0.1 || (Math.PI - a) < 0.1) {\r
 499                         up.set(1.0, 0.0, 0.0);\r
 500                 }\r
 501 \r
 502                 Vector3d right = new Vector3d();\r
 503 \r
 504                 right.cross(dir, up);\r
 505                 up.cross(right, dir);\r
 506                 right.normalize();\r
 507                 up.normalize();\r
 508 \r
 509                 Matrix3d m = new Matrix3d();\r
 510                 m.m00 = dir.x;\r
 511                 m.m10 = dir.y;\r
 512                 m.m20 = dir.z;\r
 513                 m.m01 = up.x;\r
 514                 m.m11 = up.y;\r
 515                 m.m21 = up.z;\r
 516                 m.m02 = right.x;\r
 517                 m.m12 = right.y;\r
 518                 m.m22 = right.z;\r
 519 \r
 520                 //q1.set(m); MathTools contains more stable conversion\r
 521                 MathTools.getQuat(m, q1);\r
 522 \r
 523                 if (DEBUG) System.out.println("PipingTools.getPipeComponentOrientationQuat() " + dir+ " " + up + " " + right);\r
 524 \r
 525                 Quat4d q2 = new Quat4d();\r
 526                 q2.set(new AxisAngle4d(front, angle));\r
 527                 q1.mul(q2);\r
 528                 return q1;\r
 529     }\r
 530     \r
 531     public static PipeControlPoint findPreviousEnd(PipeControlPoint tcp) {\r
 532         // TODO : optimize (we do not need the list here)\r
 533         ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();\r
 534         return findPreviousEnd(tcp, t);\r
 535     }\r
 536     \r
 537     public static PipeControlPoint findNextEnd(PipeControlPoint tcp) {\r
 538         // TODO : optimize (we do not need the list here)\r
 539         ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();\r
 540         return findNextEnd(tcp, t);\r
 541     }\r
 542     \r
 543     /**\r
 544      * Returns pipe leg's end using "nextControlPoint" relation and collects control point in the given list. \r
 545      * @param tcp\r
 546      * @param nextList\r
 547      * @return\r
 548      */\r
 549     public static PipeControlPoint findNextEnd(PipeControlPoint tcp, ArrayList<PipeControlPoint> nextList) {\r
 550         if (DEBUG) System.out.print("PipingTools.findNextEnd " + tcp.getResource());\r
 551         while (true) {\r
 552             PipeControlPoint pcp = null;\r
 553             PipeControlPoint p = null;\r
 554             if (nextList.size() == 0)\r
 555                 p = tcp;\r
 556                 \r
 557             else\r
 558                 p = nextList.get(nextList.size() - 1);\r
 559 \r
 560             pcp = p.getNext();\r
 561             if (pcp == null) {\r
 562                 pcp = p;\r
 563                 if (nextList.size() > 0)\r
 564                         nextList.remove(nextList.size() - 1);\r
 565                 if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");\r
 566                 return pcp;\r
 567                 //break;\r
 568             }\r
 569             if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {\r
 570                 if (DEBUG) System.out.println(" " + pcp.getResource());\r
 571                 return pcp;\r
 572             } else {\r
 573                 nextList.add(pcp);\r
 574                 if (DEBUG) System.out.print(" " + pcp.getResource());\r
 575             }\r
 576         }\r
 577     }\r
 578     \r
 579    \r
 580     \r
 581     /**\r
 582      * Returns pipe leg's end using "previousControlPoint" relation and collects control point in the given list.\r
 583      * @param tcp\r
 584      * @param prevList\r
 585      * @return\r
 586      */\r
 587     public static PipeControlPoint findPreviousEnd(PipeControlPoint tcp, ArrayList<PipeControlPoint> prevList) {\r
 588         if (DEBUG) System.out.print("PipingTools.findPreviousEnd " + tcp.getResource());\r
 589         while (true) {\r
 590             PipeControlPoint pcp = null;\r
 591             PipeControlPoint p = null;\r
 592             if (prevList.size() == 0)\r
 593                 p = tcp;\r
 594                 \r
 595             else\r
 596                 p = prevList.get(prevList.size() - 1);\r
 597 \r
 598             pcp = p.getPrevious();\r
 599             if (pcp == null) {\r
 600                 pcp = p;\r
 601                 if (prevList.size() > 0)\r
 602                         prevList.remove(prevList.size() - 1);\r
 603                 if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");\r
 604                 return pcp;\r
 605             }\r
 606             if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {\r
 607                 if (DEBUG) System.out.println(" " + pcp.getResource());\r
 608                 return pcp;\r
 609             } else {\r
 610                 prevList.add(pcp);\r
 611                 if (DEBUG) System.out.print(" " + pcp.getResource());\r
 612             }\r
 613         }\r
 614     }\r
 615     \r
 616     public static PipeRun getPipeRun(PipeControlPoint pcp) {\r
 617         return pcp.getControlPointOfPipeRun();\r
 618     }\r
 619     \r
 620     @Deprecated\r
 621     public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp) {\r
 622         Quat4d q = getControlPointWorldOrientationQuat(sccp, sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle),true);\r
 623         return getSizeChangeOffsetVector(sccp,q);\r
 624     }\r
 625     \r
 626     public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp, Vector3d dir) {\r
 627         Quat4d q = getControlPointOrientationQuat(dir, sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle));\r
 628         return getSizeChangeOffsetVector(sccp,q);\r
 629     }\r
 630     \r
 631     public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp, Quat4d q) {\r
 632         Vector3d v = new Vector3d(0.0,0.0,sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasOffset));\r
 633         Vector3d offset = new Vector3d();\r
 634         MathTools.rotate(q, v, offset);\r
 635         return offset;\r
 636     }\r
 637     \r
 638     public static Vector3d getDirectedControlPointDirection(IEntity dcp) {\r
 639         assert(dcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint));\r
 640         AxisAngle4d worldR = G3DTools.getOrientation(dcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldOrientation));\r
 641         Quat4d q = new Quat4d();\r
 642                 q.set(worldR);\r
 643                 Vector3d dir = new Vector3d();\r
 644                 MathTools.rotate(q, new Vector3d(1.0, 0.0, 0.0), dir);\r
 645                 dir.normalize();\r
 646                 return dir;\r
 647     }\r
 648     \r
 649     public static Vector3d getNozzleDirection(IEntity rotation) {\r
 650         AxisAngle4d worldR = G3DTools.getOrientation(rotation);\r
 651         Quat4d q = new Quat4d();\r
 652                 q.set(worldR);\r
 653                 Vector3d dir = new Vector3d();\r
 654                 MathTools.rotate(q, new Vector3d(1.0, 0.0, 0.0), dir);\r
 655                 return dir;\r
 656     }\r
 657     \r
 658     public static Vector3d getNozzleDirection(PipeControlPoint dcp) {\r
 659         return getNozzleDirection(dcp.getWorldOrientation());\r
 660     }\r
 661 \r
 662     public static void removeControlPoint(PipeControlPoint removed) {\r
 663         if (DEBUG) System.out.println("PipingTools.removeControlPoint() controlpoint " + removed.getResource());//FIXME : offset + size change control points !\r
 664         \r
 665         // this code is not valid anymore.\r
 666         \r
 667         // different removing cases:\r
 668         //\r
 669         // 1. Point is SizeChangeControlPoint  (this is currently ok)\r
 670         //    * remove offset point and component \r
 671         //    * do NOT link components together\r
 672         //\r
 673         // 2. Point is VariableLength\r
 674         //    * check if its a branch (TODO : ontology support?)\r
 675         //      * if so, also branch point in the other piperun may have to be removed\r
 676         //        (we cannot move other components next to branch)\r
 677         //      * if not, components next to removed one are moved next to each other\r
 678         // 3. Other\r
 679         //    * check if there is VariableLength on both sides,\r
 680         //              * if so, those must be unified to a single Variable Length component.\r
 681         //      * if not, components must be moved next to each other\r
 682         //\r
 683         // a) If removed Control Point is next to Nozzle and after the point is removed nozzle is not connected to anything\r
 684         //    * nozzle's link to piperun's specs must be removed\r
 685         //    \r
 686         \r
 687         \r
 688         if (removed.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)||\r
 689                 removed.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 690                 // sccp & ocp connect two pipeRuns to each other; when thoes are remove, pipeRuns are not connected to each other anymore.\r
 691                 removeDualPoint(removed);\r
 692                 return;\r
 693         } else {\r
 694                 PipeControlPoint prev = removed.getPrevious();\r
 695             PipeControlPoint next = removed.getNext();\r
 696                 PipeRun pipeRun = getPipeRun(removed);\r
 697                 if (pipeRun == null)\r
 698                         return;\r
 699                 if (next == null && prev == null) {\r
 700                                 if (removed.isInstanceOf(ProcessResource.plant3Dresource.NozzleControlPoint)) {\r
 701                                         // Nozzle's control point does not need to be removed, only unlinked\r
 702                                         // TODO : what about component ports?\r
 703                                         PipingTools2.unlinkNozzleAndPiperun(removed.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOf), ControlPointTools.getPipeRun(removed));\r
 704                                         return;\r
 705                                 }\r
 706                         } else {\r
 707 \r
 708                                 if (next != null && prev != null) {\r
 709                                         boolean link = true;\r
 710                                         if (next.isInstanceOf(ProcessResource.plant3Dresource.BranchEndControlPoint)){\r
 711                                                 link = false;\r
 712                                                 next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 713                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 714                                                 removeControlPoint(next);\r
 715                                         }\r
 716                                         if (prev.isInstanceOf(ProcessResource.plant3Dresource.BranchEndControlPoint)) {\r
 717                                                 link = false;\r
 718                                                 prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 719                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 720                                                 removeControlPoint(prev);\r
 721                                         }\r
 722                                         if (link && prev.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)&&\r
 723                                                                 next.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)) {\r
 724                                                 link = false;\r
 725                                         }\r
 726                                         if (next.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 727                                                 PipeControlPoint sccp = next;\r
 728                                                 PipeControlPoint ocp = sccp.getSubPoint().iterator().next();\r
 729                                                 if (ocp == null) {\r
 730                                                         ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource()+ " structure damaged, no offset control point",null);\r
 731                                                         return;\r
 732                                                 }\r
 733                                                 if (link) {\r
 734                                                         sccp.setPrevious(prev);\r
 735                                                         ocp.setPrevious(prev);\r
 736                                                 } else {\r
 737                                                         sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 738                                                         ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 739                                                         \r
 740                                                 }\r
 741                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 742                                         } else if (next.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 743                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource()+ " structure damaged, next control point is offset control point",null);\r
 744                                                 return;\r
 745                                         } else if (next.getPrevious().getResource().equals(removed.getResource())) {\r
 746                                                 if (link) {\r
 747                                                         next.setPrevious(prev);\r
 748                                                 } else {\r
 749                                                         next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 750                                                 }\r
 751                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 752                                         } else {\r
 753                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource()+ " structure damaged", null);\r
 754                                                 return;\r
 755                                         }\r
 756                                         if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 757                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, previous control point is size change control point", null);\r
 758                                                 return;\r
 759                                         } else if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 760                                                 PipeControlPoint ocp = prev;\r
 761                                                 PipeControlPoint sccp = ocp.getSubPointOf();\r
 762                                                 if (sccp == null) {\r
 763                                                         ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, no size change control point",null);\r
 764                                                         return;\r
 765                                                 }\r
 766                                                 if (link) {\r
 767                                                         ocp.setNext(next);\r
 768                                                         sccp.setNext(next);\r
 769                                                 } else {\r
 770                                                         ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 771                                                         sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 772                                                 }\r
 773                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 774                                         } else if (prev.getNext().getResource().equals(removed.getResource())) {\r
 775                                                 if (link)\r
 776                                                         prev.setNext(next);\r
 777                                                 else\r
 778                                                         prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 779                                                 removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 780                                         } else {\r
 781                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged", null);\r
 782                                                 return;\r
 783                                         }\r
 784                                         if (link) {\r
 785                                                 if (next.isInstanceOf(ProcessResource.plant3Dresource.VariableLengthControlPoint) &&\r
 786                                                         prev.isInstanceOf(ProcessResource.plant3Dresource.VariableLengthControlPoint)) {\r
 787                                                         // we have to join them into single variable length component.\r
 788                                                         removeControlPoint(prev);\r
 789                                                 }\r
 790                                         }\r
 791 \r
 792                                         \r
 793                                 } else if (next != null) {\r
 794                                         if (next.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 795                                                 PipeControlPoint sccp = next;\r
 796                                                 PipeControlPoint ocp = sccp.getSubPoint().iterator().next();\r
 797                                                 if (ocp == null) {\r
 798                                                         ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource()+ " structure damaged, no offset control point",null);\r
 799                                                         return;\r
 800                                                 }\r
 801                                                 next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 802                                                 ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 803                                         } else if (next.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 804                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, next control point is offset control point", null);\r
 805                                                 return;\r
 806                                         } else if (next.getPrevious().getResource().equals(removed.getResource())) {\r
 807                                                 next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 808                                         } else {\r
 809                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged", null);\r
 810                                                 return;\r
 811                                         }\r
 812                                         removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 813                                 } else { //(prev != null)\r
 814                                         if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 815                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, previous control point is size change control point", null);\r
 816                                                 return;\r
 817                                         } else if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {\r
 818                                                 PipeControlPoint ocp = prev;\r
 819                                                 PipeControlPoint sccp = ocp.getSubPointOf();\r
 820                                                 if (sccp == null) {\r
 821                                                         ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, no size change control point", null);\r
 822                                                         return;\r
 823                                                 }\r
 824                                                 prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 825                                                 sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 826                                         } else if (prev.getNext().getResource().equals(removed.getResource())) {\r
 827                                                 prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 828                                         } else {\r
 829                                                 ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource() + " structure damaged", null);\r
 830                                                 return;\r
 831                                         }\r
 832                                         removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 833                                 }\r
 834 \r
 835                         }\r
 836                         if (removed.getSubPoint().size() > 0 ) {\r
 837                                 removeSubPoints(removed);\r
 838                         } else if (removed.getSubPointOf() != null) {\r
 839                                 removeParentPoint(removed);\r
 840                         }\r
 841                                 \r
 842                         removeComponents(removed);\r
 843                         \r
 844                         pipeRun.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, removed);\r
 845                         if (pipeRun.getChild().size() == 0) {\r
 846                                 PipingTools2.removePipeRun(pipeRun);\r
 847                         } \r
 848                         else if (pipeRun.getControlPoints().size() == 1) {\r
 849                 removeControlPoint(pipeRun.getControlPoints().iterator().next());\r
 850             }\r
 851                 }\r
 852         \r
 853     }\r
 854     \r
 855     private static void removeDualPoint(PipeControlPoint removed) {\r
 856         PipeControlPoint prev = removed.getPrevious();\r
 857         PipeControlPoint next = removed.getNext();\r
 858         if (prev != null) {\r
 859                 prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 860                 }\r
 861                 if (next != null)\r
 862                         next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 863                 PipeControlPoint ocp;\r
 864                 PipeControlPoint sccp;\r
 865                 // get sccp / ocp pair\r
 866                 if (removed.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
 867                         sccp = removed;\r
 868                 ocp = sccp.getSubPoint().iterator().next();\r
 869                 \r
 870                 } else {\r
 871                         ocp = removed;\r
 872                 sccp = ocp.getSubPointOf();\r
 873                 }\r
 874                 PipeRun p1 = getPipeRun(ocp);\r
 875                 PipeRun p2 = getPipeRun(sccp);\r
 876                 \r
 877                 //  removes all components connected to control point\r
 878         \r
 879         removeComponents(ocp);\r
 880         \r
 881         removeComponents(sccp); \r
 882         \r
 883         // remove control points from pipeRuns\r
 884         p1.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, ocp);\r
 885         p2.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, sccp);\r
 886 \r
 887         // remove links to other control points\r
 888         ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);\r
 889         sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);            \r
 890         ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);\r
 891         sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);            \r
 892         \r
 893         // if pipeRuns contains no other components(control points), they can be removed too.\r
 894         if (p1.getControlPoints().size() == 0) {\r
 895                 PipingTools2.removePipeRun(p1);\r
 896         } else if (p1.getControlPoints().size() == 1) {\r
 897                 removeControlPoint(p1.getControlPoints().iterator().next());\r
 898         }\r
 899         if (p2.getControlPoints().size() == 0) {\r
 900                 PipingTools2.removePipeRun(p2);\r
 901         } else if (p2.getControlPoints().size() == 1) {\r
 902                 removeControlPoint(p2.getControlPoints().iterator().next());\r
 903         }\r
 904     }\r
 905     \r
 906     /**\r
 907      * Removes sub points of a point\r
 908      * @param removed\r
 909      * @throws TransactionException\r
 910      */\r
 911     private static void removeSubPoints(PipeControlPoint removed) {\r
 912         // if control point is branch control point, all branch of points must be removed too\r
 913                 Collection<PipeControlPoint> points = removed.getSubPoint();\r
 914                 \r
 915                 for (PipeControlPoint p : points) {\r
 916                         removed.removeStatement(ProcessResource.plant3Dresource.HasSubPoint, p);\r
 917                         removeControlPoint(p);\r
 918                 }\r
 919     }\r
 920     \r
 921     /**\r
 922      * Removed point is a subpoint of something, \r
 923      * @param removed\r
 924      */\r
 925     private static void removeParentPoint(PipeControlPoint removed)  {\r
 926         throw new RuntimeException("Subpoints cannot be removed");\r
 927         \r
 928         // if control point is branch it has to be removed from branch control point\r
 929 //              BranchEndControlPoint ecp = BranchEndControlPointFactory.create(removed);\r
 930 //              BranchControlPoint bcp = null;\r
 931 //              if (ecp.getBranchOfPointSet().size() == 1) {\r
 932 //                      bcp = ecp.getBranchOfPointSet().iterator().next();\r
 933 //              }\r
 934 //              if (bcp != null) {\r
 935 //                      bcp.getBranchPointSet().remove(ecp);\r
 936 //                      if (bcp.getBranchPointSet().size() == 0) {\r
 937 //                              // branch control point is not used and can be removed\r
 938 //                              removeControlPoint(bcp);\r
 939 //                      }\r
 940 //              }\r
 941     }\r
 942  \r
 943     \r
 944     private static void removeComponents(PipeControlPoint pcp) {\r
 945         IEntity component = pcp.getControlPointOf();\r
 946         if (component != null) {\r
 947                 PipeRun p1 = getPipeRun(pcp);\r
 948                 p1.removeStatement(ProcessResource.g3dResource.HasChild, component);\r
 949                 component.removeRelatedStatements(ProcessResource.plant3Dresource.HasControlPoint);\r
 950         }\r
 951     }\r
 952     \r
 953     private static void setStatement(IEntity subject, Resource relation, IEntity object) {\r
 954         subject.removeRelatedStatements(relation);\r
 955         subject.addStatement(relation, object);\r
 956     }\r
 957     \r
 958     \r
 959     public static void setWorldPosition(IEntity pcp, Tuple3d position) {\r
 960         IEntity wp = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);\r
 961         G3DTools.setTuple3(wp, position);\r
 962         tt.propagateWorldTransformChange(tt.getParent(pcp), pcp);\r
 963         \r
 964     }\r
 965     \r
 966     public static void setLocalPosition(IEntity pcp, Tuple3d position) {\r
 967         G3DTools.setTuple3(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition), position);\r
 968         tt.propagateLocalTransformChange(tt.getParent(pcp), pcp);\r
 969     }\r
 970     \r
 971     public static void setWorldOrientation(IEntity node, AxisAngle4d orientation) {\r
 972                 G3DTools.setOrientation(node.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldOrientation), orientation);\r
 973                 tt.propagateWorldTransformChange(tt.getParent(node), node);\r
 974                 \r
 975         }\r
 976         \r
 977         public static void setLocalOrientation(IEntity node, AxisAngle4d orientation) {\r
 978                 G3DTools.setOrientation(node.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalOrientation), orientation);\r
 979                 tt.propagateLocalTransformChange(tt.getParent(node), node);\r
 980         }\r
 981     \r
 982     private static boolean updatePosition(IEntity pcp) {\r
 983         return tt.transformationUpdate(pcp);\r
 984         \r
 985         \r
 986         // TODO : orientation is also needed, current code handles only position\r
 987         // TODO : reuse the code in G3DTools!\r
 988         /*\r
 989         IEntity worldPosition = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);\r
 990         IEntity localPosition = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition);\r
 991         \r
 992         \r
 993         Tuple3d worldP = G3DTools.getPoint(worldPosition);\r
 994         Tuple3d localP = G3DTools.getPoint(localPosition);\r
 995 \r
 996         if (localP == null || worldP == null)\r
 997             return false;\r
 998         if (!isValid(worldP) && !isValid(localP))\r
 999                 return false;\r
1000 \r
1001         Tuple3d cachedWorldP = (Tuple3d) getProperty(worldPosition.getResource());\r
1002         Tuple3d cachedLocalP = (Tuple3d) getProperty(localPosition.getResource());\r
1003 \r
1004         if (DEBUG) System.out.println("PipeControlPoint changed " + worldP + " " + cachedWorldP + " " + localP + " " + cachedLocalP);\r
1005         boolean changed = false;\r
1006 \r
1007         IEntity parent = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOf);\r
1008         \r
1009         if (parent == null) {\r
1010                 if (DEBUG) System.out.println("PipeControlPoint changed, no parent node");\r
1011             return false;\r
1012         }\r
1013 \r
1014         if (cachedLocalP == null) {\r
1015             storeProperty(localPosition.getResource(), localP);\r
1016             Tuple3d p = G3DTools.getWorldFromLocal(parent, new Point3d(localP)); \r
1017             storeProperty(worldPosition.getResource(), p);\r
1018             G3DTools.setTuple3(worldPosition, p);\r
1019             if (DEBUG) System.out.println("PipeControlPoint changed local " + worldP + " " + p);\r
1020             changed = true;\r
1021         } else {\r
1022             if (TransformationTools.changed(cachedLocalP, localP)) {\r
1023                 storeProperty(localPosition.getResource(), localP);\r
1024                 Tuple3d p = G3DTools.getWorldFromLocal(parent, new Point3d(localP));\r
1025                 storeProperty(worldPosition.getResource(), p);\r
1026                 G3DTools.setTuple3(worldPosition, p);\r
1027                 if (DEBUG) System.out.println("PipeControlPoint changed local " + worldP + " " + localP);\r
1028                 changed = true;\r
1029             }\r
1030             if (cachedWorldP == null) {\r
1031                 storeProperty(worldPosition.getResource(), worldP);\r
1032                 Tuple3d p = G3DTools.getLocalFromWorld(parent, new Point3d(worldP));\r
1033                 G3DTools.setTuple3(localPosition, p);\r
1034                 storeProperty(localPosition.getResource(), p);\r
1035                 if (DEBUG) System.out.println("PipeControlPoint changed world " + worldP + " " + p);\r
1036                 changed = true;\r
1037             } else {\r
1038                 if (TransformationTools.changed(cachedWorldP, worldP)) {\r
1039                     storeProperty(worldPosition.getResource(), worldP);\r
1040                     Tuple3d p = G3DTools.getLocalFromWorld(parent, new Point3d(worldP));\r
1041                     G3DTools.setTuple3(localPosition, p);\r
1042                     storeProperty(localPosition.getResource(), p);\r
1043                     if (DEBUG) System.out.println("PipeControlPoint changed world " + worldP + " " + p);\r
1044                     changed = true;\r
1045                 }\r
1046             }\r
1047         }\r
1048         return changed;\r
1049         //*/\r
1050     }\r
1051     \r
1052     static boolean isControlPointChanged(PipeControlPoint node) {\r
1053 \r
1054         long id = node.getResource().getResourceId();\r
1055         \r
1056         boolean changed = updatePosition(node);\r
1057         //if (!changed) {\r
1058                 if (node.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {\r
1059                         if (node.isInstanceOf(ProcessResource.plant3Dresource.TurnControlPoint)) {\r
1060                                 Pair<Long,Long> connected = (Pair<Long,Long>)getProperty(node.getResource().getResourceId());\r
1061                     PipeControlPoint next = node.getNext();\r
1062                     PipeControlPoint prev = node.getPrevious();\r
1063                     if ((next != null && prev != null) && (\r
1064                         connected == null || \r
1065                        (connected.first == null && prev != null) ||\r
1066                        (connected.second == null && next != null) ||\r
1067                         !connected.first.equals(prev.getResource().getResourceId()) ||\r
1068                         !connected.second.equals(next.getResource().getResourceId()))) {\r
1069                         storeProperty(id, new Pair<Long,Long>(prev.getResource().getResourceId(),next.getResource().getResourceId()));\r
1070                         changed = true;  \r
1071                     }\r
1072                     if (node.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint)) {\r
1073                         double r = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasTurnRadius);\r
1074                         Double d = (Double)getProperty(id + ":turnradius");\r
1075                         if (d == null || TransformationTools.changed(r, d)) {\r
1076                                 storeProperty(id + ":turnradius", r);\r
1077                                 changed = true;\r
1078                         }\r
1079                     }\r
1080                         }\r
1081                         else if (node.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)) {\r
1082                                 Vector3d dir = ControlPointTools.getDirectedControlPointDirection(node);\r
1083                                 Vector3d old = (Vector3d)getProperty(id + ":direction");\r
1084                                 if (old == null || TransformationTools.changed(dir, old)) {\r
1085                                         storeProperty(id + ":direction", dir);\r
1086                                         changed = true;\r
1087                                 }\r
1088                         }\r
1089                 } else { // InlineControlPoint\r
1090                         if (node.isInstanceOf(ProcessResource.plant3Dresource.FixedLengthControlPoint)) {\r
1091                                 double length = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);\r
1092                                 Double d = (Double)getProperty(id + ":length");\r
1093                                 if (d == null)\r
1094                                         changed = true;\r
1095                                 else\r
1096                                         changed = changed || TransformationTools.changed(length, d);\r
1097                                 \r
1098                                 if (changed) {\r
1099                                         storeProperty(id + ":length", length);\r
1100                                         return true;\r
1101                                 }\r
1102                         } else \r
1103                         if (node.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {\r
1104                                 \r
1105                                 double angle = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle);\r
1106                                 Double d = (Double)getProperty(id + ":rotationangle");\r
1107                                 if (d == null)\r
1108                                         changed = true;\r
1109                                 else\r
1110                                         changed = changed || TransformationTools.changed(angle, d);\r
1111                                 if (changed) {\r
1112                                         storeProperty(id + ":rotationangle", angle);\r
1113                                         return true;\r
1114                                 }\r
1115                                 Collection<PipeControlPoint> subpoints = node.getSubPoint();\r
1116                                 if (subpoints.size() != 1)\r
1117                                         throw new RuntimeException("Current implementation assumes that size change components are dual conmnected");\r
1118                                 PipeControlPoint ocp = subpoints.iterator().next();\r
1119                                 if (node.isInstanceOf(ProcessResource.plant3Dresource.OffsettingPoint)) {\r
1120                                         double offset = ocp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasOffset);\r
1121                                         d = (Double)getProperty(id +":offset");\r
1122                                         if (d == null)\r
1123                                                 changed = true;\r
1124                                         else\r
1125                                                 changed = TransformationTools.changed(offset, d);\r
1126                                         if (changed) {\r
1127                                                 storeProperty(id+":offset", offset);\r
1128                                                 return true;\r
1129                                         }\r
1130                                 }\r
1131 //                      } else if (node instanceof OffsetControlPoint) {\r
1132 //                              OffsetControlPoint ocp = (OffsetControlPoint)node;\r
1133 //                              //ocp.\r
1134 //                      } else if (node instanceof BranchControlPoint) {\r
1135 //                              BranchControlPoint bcp = (BranchControlPoint)node;\r
1136 //                              int size = bcp.getBranchPointSet().size();\r
1137 //                              Integer i = (Integer)getProperty(bcp.getResource().getId());\r
1138 //                              if (i == null)\r
1139 //                                      changed =true;\r
1140 //                              else\r
1141 //                                      changed = changed || i != size;\r
1142 //                              if (changed) {\r
1143 //                                      storeProperty(bcp.getResource().getId(), size);\r
1144 //                                      return true;\r
1145 //                              }\r
1146                         }\r
1147                 }\r
1148         //}\r
1149         \r
1150         return changed;\r
1151     }\r
1152     \r
1153     private static boolean isValid(Tuple3d v) {\r
1154         if (Double.isInfinite(v.x) || \r
1155                  Double.isNaN(v.x) ||\r
1156             Double.isInfinite(v.y) || \r
1157                  Double.isNaN(v.y) ||\r
1158             Double.isInfinite(v.z) || \r
1159                  Double.isNaN(v.z))\r
1160             return false;\r
1161         return true;\r
1162     }\r
1163     \r
1164     private static HashMap<Object, Object> properties = new HashMap<Object, Object>();\r
1165     \r
1166     public static Object getProperty(Object key) {\r
1167         return properties.get(key);\r
1168     }\r
1169     \r
1170     public static void storeProperty(Object key, Object value) {\r
1171         properties.put(key, value);\r
1172     }\r
1173     \r
1174     /**\r
1175      * Loads positions of controlpoint to rule cache\r
1176      * \r
1177      * TODO : this caches only transformation information : other info must be cached too\r
1178      * \r
1179      * @param root resource of the modeled plant\r
1180      */\r
1181     public static void reloadCache(Graph graph, Resource root) {\r
1182         \r
1183         Stack<IEntity> stack = new Stack<IEntity>();\r
1184         stack.add(EntityFactory.create(graph,root));\r
1185         while (!stack.isEmpty()) {\r
1186                 IEntity current = stack.pop();\r
1187                 IEntity pcp = current.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasControlPoint);\r
1188                 if (pcp == null) {\r
1189                         stack.addAll(current.getRelatedObjects(ProcessResource.g3dResource.HasChild));\r
1190                 } else {\r
1191                         if (DEBUG) System.out.println("Cached pcp " + pcp.getResource());\r
1192                         IEntity localPos = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition);\r
1193                         IEntity worldPos = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);\r
1194                         \r
1195                 tt.storeProperty(localPos.getResource(),G3DTools.getPoint(localPos));\r
1196                 tt.storeProperty(worldPos.getResource(),G3DTools.getPoint(worldPos));\r
1197                 \r
1198                 IEntity localOr = pcp.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasLocalOrientation);\r
1199                         IEntity worldOr = pcp.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasWorldOrientation);\r
1200                         \r
1201                         if (worldOr != null) {\r
1202                                 tt.storeProperty(localOr.getResource(),G3DTools.getOrientation(localOr));\r
1203                     tt.storeProperty(worldOr.getResource(),G3DTools.getOrientation(worldOr));\r
1204                         }\r
1205                         \r
1206                 stack.addAll(pcp.getRelatedObjects(ProcessResource.plant3Dresource.HasSubPoint));\r
1207                 }\r
1208         }\r
1209 \r
1210     }\r
1211 }\r