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

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