dev/org.simantics.proconf.processeditor/src/fi/vtt/simantics/processeditor/common/ControlPointTools.java

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