- 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;
- }
- }
- }
- return null;