public enum PointType{INLINE,TURN,END};
public enum Direction{NEXT,PREVIOUS};
public enum PositionType {SPLIT,NEXT,PREVIOUS,PORT}
public enum PointType{INLINE,TURN,END};
public enum Direction{NEXT,PREVIOUS};
public enum PositionType {SPLIT,NEXT,PREVIOUS,PORT}
-
- if (turnAxis == null) {
- Vector3d dir = getPathLegDirection(Direction.NEXT);
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- return getControlPointOrientationQuat(dir, angle);
- } else {
- Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
- dir.negate();
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- return getControlPointOrientationQuat(dir, turnAxis, angle);
- }
- }
-
- public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
-
- if (turnAxis == null) {
- Vector3d dir = getPathLegDirection(Direction.NEXT);
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- Quat4d q = getControlPointOrientationQuat(dir, angle);
- if (reversed) {
+
+ if (turnAxis == null) {
+ Vector3d dir = getPathLegDirection(Direction.NEXT);
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ return getControlPointOrientationQuat(dir, angle);
+ } else {
+ Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
+ dir.negate();
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ return getControlPointOrientationQuat(dir, turnAxis, angle);
+ }
+ }
+
+ public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
+
+ if (turnAxis == null) {
+ Vector3d dir = getPathLegDirection(Direction.NEXT);
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ Quat4d q = getControlPointOrientationQuat(dir, angle);
+ if (reversed) {
- }
- return q;
- } else {
- Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
- dir.negate();
- if (dir.lengthSquared() > MathTools.NEAR_ZERO)
- dir.normalize();
- return getControlPointOrientationQuat(dir, turnAxis, angle);
- }
- }
-
-
-
- public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
- if (dir.lengthSquared() < MathTools.NEAR_ZERO)
- return MathTools.getIdentityQuat();
-
-
- Vector3d up = new Vector3d(0.0, 1.0, 0.0);
- double a = up.angle(dir);
- if (a < 0.1 || (Math.PI - a) < 0.1) {
- up.set(1.0, 0.0, 0.0);
-
-
- return getControlPointOrientationQuat(dir, up, angle);
- }
-
- public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up, double angle) {
- if (dir.lengthSquared() < MathTools.NEAR_ZERO)
- return MathTools.getIdentityQuat();
-
- final Vector3d front = new Vector3d(1.0,0.0,0.0);
-
- Quat4d q1 = new Quat4d();
-
-
- Vector3d right = new Vector3d();
-
- right.cross(dir, up);
- up.cross(right, dir);
- right.normalize();
- up.normalize();
-
- Matrix3d m = new Matrix3d();
- m.m00 = dir.x;
- m.m10 = dir.y;
- m.m20 = dir.z;
- m.m01 = up.x;
- m.m11 = up.y;
- m.m21 = up.z;
- m.m02 = right.x;
- m.m12 = right.y;
- m.m22 = right.z;
-
- //q1.set(m); MathTools contains more stable conversion
- MathTools.getQuat(m, q1);
-
-// if (DEBUG) System.out.println("PipingTools.getPipeComponentOrientationQuat() " + dir+ " " + up + " " + right);
-
- Quat4d q2 = new Quat4d();
- q2.set(new AxisAngle4d(front, angle));
- q1.mul(q2);
- return q1;
- }
-
+ return q;
+ } else {
+ Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
+ dir.negate();
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ return getControlPointOrientationQuat(dir, turnAxis, angle);
+ }
+ }
+
+
+
+ public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
+ if (dir.lengthSquared() < MathTools.NEAR_ZERO)
+ return MathTools.getIdentityQuat();
+
+
+ Vector3d up = new Vector3d(0.0, 1.0, 0.0);
+ double a = up.angle(dir);
+ if (a < 0.1 || (Math.PI - a) < 0.1) {
+ up.set(1.0, 0.0, 0.0);
+ }
+
+
+ return getControlPointOrientationQuat(dir, up, angle);
+ }
+
+ public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up, double angle) {
+ if (dir.lengthSquared() < MathTools.NEAR_ZERO)
+ return MathTools.getIdentityQuat();
+
+ final Vector3d front = new Vector3d(1.0,0.0,0.0);
+
+ Quat4d q1 = new Quat4d();
+
+
+ Vector3d right = new Vector3d();
+
+ right.cross(dir, up);
+ up.cross(right, dir);
+ right.normalize();
+ up.normalize();
+
+ Matrix3d m = new Matrix3d();
+ m.m00 = dir.x;
+ m.m10 = dir.y;
+ m.m20 = dir.z;
+ m.m01 = up.x;
+ m.m11 = up.y;
+ m.m21 = up.z;
+ m.m02 = right.x;
+ m.m12 = right.y;
+ m.m22 = right.z;
+
+ //q1.set(m); MathTools contains more stable conversion
+ MathTools.getQuat(m, q1);
+
+ // if (DEBUG) System.out.println("PipingTools.getPipeComponentOrientationQuat() " + dir+ " " + up + " " + right);
+
+ Quat4d q2 = new Quat4d();
+ q2.set(new AxisAngle4d(front, angle));
+ q1.mul(q2);
+ return q1;
+ }
+
- 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;
- }
-
+ 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;
+ }
+
- while (true) {
- PipeControlPoint pcp = null;
- PipeControlPoint p = null;
- if (nextList.size() == 0)
- p = this;
-
- else
- p = nextList.get(nextList.size() - 1);
-
- pcp = p.getNext();
- if (pcp == null) {
- pcp = p;
- if (nextList.size() > 0)
- nextList.remove(nextList.size() - 1);
-// if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");
- return pcp;
- //break;
- }
- if (pcp.isPathLegEnd()) {
- //if (DEBUG) System.out.println(" " + pcp.getResource());
- return pcp;
- } else {
- nextList.add(pcp);
- // if (DEBUG) System.out.print(" " + pcp.getResource());
- }
- }
- }
-
+ while (true) {
+ PipeControlPoint pcp = null;
+ PipeControlPoint p = null;
+ if (nextList.size() == 0)
+ p = this;
+
+ else
+ p = nextList.get(nextList.size() - 1);
+
+ pcp = p.getNext();
+ if (pcp == null) {
+ pcp = p;
+ if (nextList.size() > 0)
+ nextList.remove(nextList.size() - 1);
+ // if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");
+ return pcp;
+ //break;
+ }
+ if (pcp.isPathLegEnd()) {
+ //if (DEBUG) System.out.println(" " + pcp.getResource());
+ return pcp;
+ } else {
+ nextList.add(pcp);
+ // if (DEBUG) System.out.print(" " + pcp.getResource());
+ }
+ }
+ }
+