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