- if (next == null || prev == null)
- return Math.PI; // FIXME : argh
- Vector3d middlePoint = tcp.getWorldPosition();
- Vector3d nextPoint = next.getWorldPosition();
- Vector3d prevPoint = prev.getWorldPosition();
- return updateTurnControlPointTurn(tcp, middlePoint, prevPoint, nextPoint);
+
+ if (!tcp.isFixed()) {
+ 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();
+ }