-
- public Quat4d getControlPointOrientationQuat(double angle) {
-
- 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 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);
+
+ public Quat4d getControlPointOrientationQuat(double angle) {
+
+ 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) {
+ Quat4d q2 = new Quat4d();
+ q2.set(new AxisAngle4d(MathTools.Y_AXIS, Math.PI));
+ q.mulInverse(q2);
+ }
+ 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;
+ }
+
+ 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;
+ }