- if (next == null || prev == null)
- return tcp.getTurnAngle();
- Vector3d middlePoint = tcp.getWorldPosition();
- Vector3d nextPoint = next.getWorldPosition();
- Vector3d prevPoint = prev.getWorldPosition();
- return updateTurnControlPointTurn(tcp, middlePoint, prevPoint, nextPoint);
- } else {
- // Verify that fixed turn is properly defined.
- // TODO : when reversed flag is changed, rotation angle should be recalculated, otherwise the orientation will change.
- // TODO : should reverse flag toggle to be done already when we are removing defining side?
- if (prev != null && next != null) {
- // Nothing to do
- } else if (prev == null) {
- if (!tcp._getReversed())
- tcp.setReversed(true);
- } else if (next == null) {
- if (tcp._getReversed())
- tcp.setReversed(false);
- }
-
- Vector3d dir;
- if (!tcp._getReversed()) {
- if (prev == null)
- return Math.PI; // FIXME : argh
-
- Vector3d middlePoint = tcp.getWorldPosition();
- Vector3d prevPoint = prev.getWorldPosition();
- dir = new Vector3d();
- dir.sub(middlePoint, prevPoint);
-
- } else {
- if (next == null)
- return Math.PI; // FIXME : argh
-
- Vector3d middlePoint = tcp.getWorldPosition();
- Vector3d nextPoint = next.getWorldPosition();
- dir = new Vector3d();
- dir.sub(middlePoint, nextPoint);
- }
- dir.normalize();
-
- Quat4d q = PipeControlPoint.getControlPointOrientationQuat(dir, tcp.getRotationAngle() != null ? tcp.getRotationAngle() : 0.0);
- Vector3d v = new Vector3d();
- MathTools.rotate(q, MathTools.Y_AXIS,v);
- tcp.setTurnAxis(v);
- tcp.setWorldOrientation(q);
- return tcp.getTurnAngle();
- }
- }
-
- /**
- * Recalculates turn control point's internal data (turn angle and offset)
- *
- * @param tcp
- * @param middlePoint
- * @param nextPoint
- * @param prevPoint
- */
- private static double updateTurnControlPointTurn(PipeControlPoint tcp, Vector3d middlePoint, Vector3d prevPoint, Vector3d nextPoint) {
-
- Vector3d dir1 = new Vector3d(middlePoint);
- dir1.sub(prevPoint);
- Vector3d dir2 = new Vector3d(nextPoint);
- dir2.sub(middlePoint);
- if (DEBUG)
- System.out.println("PipingTools.updateTurnControlPointTurn " + tcp + " " + prevPoint + " " + middlePoint + " " + nextPoint);
- return updateTurnControlPointTurn(tcp, dir1, dir2);
- }
-
- private static double updateTurnControlPointTurn(PipeControlPoint tcp, Vector3d dir1, Vector3d dir2) {
- double turnAngle = dir1.angle(dir2);
-
- double angle = Math.PI - turnAngle;
-
- Vector3d turnAxis = new Vector3d();
- turnAxis.cross(dir1, dir2);
- if (turnAxis.lengthSquared() > MathTools.NEAR_ZERO) {
- double elbowRadius = tcp.getPipelineComponent().getPipeRun().getTurnRadius();
- double R = elbowRadius / Math.tan(angle * 0.5);