- public Vector3d getDirection(Direction direction) {
- if (isDirected())
- return getDirectedControlPointDirection();
- if (isTurn() && isFixed()) {
- if (direction == Direction.NEXT) {
- if (previous != null) {
- PipeControlPoint pcp = this;
- Vector3d dir = new Vector3d();
- dir.sub(pcp.getWorldPosition(),previous.getWorldPosition());
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- else
- return null;
- Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
- AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
- Quat4d q2 = MathTools.getQuat(aa);
- Vector3d v = new Vector3d(1.,0.,0.);
- Vector3d offset = new Vector3d();
- MathTools.rotate(q2, v, offset);
- MathTools.rotate(q, offset, dir);
- return dir;
- }
- } else {
- if (next != null) {
- PipeControlPoint pcp = this;
- Vector3d dir = new Vector3d();
- dir.sub(next.getWorldPosition(),pcp.getWorldPosition());
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- else
- return null;
- Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
- AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
- Quat4d q2 = MathTools.getQuat(aa);
- Vector3d v = new Vector3d(1.,0.,0.);
- Vector3d offset = new Vector3d();
- MathTools.rotate(q2, v, offset);
- MathTools.rotate(q, offset, dir);
- return dir;
- }
- }
- }
- return null;
- }
-