List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation Rotation
public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException
From source file:jtrace.texture.TransparentFinish.java
/** * Obtain ray refracted according to Snell's law. * // w ww . j a v a2 s . com * @param incidentRay * @param normalRay * @return refracted ray */ Ray getRefractedRay(Ray incidentRay, Ray normalRay) { // Check direction of incident ray (inside to out or outside to in) // and adjust ior accordingly: double snellFactor; Vector3D axis; if (incidentRay.direction.dotProduct(normalRay.direction) < 0.0) { snellFactor = 1.0 / ior; axis = incidentRay.direction.crossProduct(normalRay.direction); } else { snellFactor = ior; axis = normalRay.direction.crossProduct(incidentRay.direction); } if (axis.getNorm() > 0) { axis = axis.normalize(); } else { return new Ray(normalRay.getOrigin(), incidentRay.direction); } double angle = Math .asin((incidentRay.direction.normalize().crossProduct(normalRay.direction).normalize()).getNorm()); double newAngle = Math.asin(snellFactor * Math.sin(angle)); Rotation rotation = new Rotation(axis, newAngle - angle); Vector3D refractedDir = rotation.applyTo(incidentRay.direction); return new Ray(normalRay.getOrigin(), refractedDir); }
From source file:com.rvantwisk.cnctools.controls.opengl.ArrowsActor.java
private void addArrow(double[][] arrow, double[] loc, double rA, final Vector3D p1, final Vector3D p2) { try {//from w w w. j av a2 s . c om double angleZ = Point.angleBetween2Lines(new Point(p1.getX(), p1.getY()), new Point(p2.getX(), p2.getY()), new Point(0.0, 0.0), new Point(0.0, 1.0)); // double angleX = Point.angleBetween2Lines(new Point(lastY, lastZ), new Point(rY, rZ), new Point(0.0, 0.0), new Point(0.0, 1.0)); double dx = p1.getX() - p2.getX(); double dy = p1.getY() - p2.getY(); double d = Math.sqrt(dx * dx + dy * dy); double angle; Rotation myRotation; if (d != 0.0) { angle = Point.angleBetween2Lines(new Point(0.0, 0.0), new Point(1.0, 0.0), new Point(0.0, 0.0), new Point(d, p1.getZ() - p2.getZ())); myRotation = new Rotation(new Vector3D(1, 0, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } else if ((p1.getZ() - p2.getZ()) < 0.0) { angle = (90.0 / 360.0 * Math.PI * 2.0); myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } else { angle = (-90.0 / 360.0 * Math.PI * 2.0); myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0)); } Rotation myRotationZ = new Rotation(new Vector3D(0, 0, 1.0), angleZ + (-90.0 / 360.0 * Math.PI * 2.0)); Rotation myRotationA = new Rotation(new Vector3D(1.0, 0.0, 0.0), (rA / 360.0 * Math.PI * 2.0)); double[] out = new double[3]; double[] out2 = new double[3]; for (double[] v : arrow) { myRotationZ.applyTo(v, out); myRotation.applyTo(out, out2); out2[0] = out2[0] + loc[0]; out2[1] = out2[1] + loc[1]; out2[2] = out2[2] + loc[2]; myRotationA.applyTo(out2, out2); data.add((float) (out2[0] + 0.0)); data.add((float) (out2[1] + 0.0)); data.add((float) (out2[2] + 0.0)); setMotionColor(machine.getMotionMode()); } } catch (Exception e) { // If for some reason we get a exception, we just don't add the arrow logger.warn("Add arrow : This should normally not happen, please report back.", e); } }
From source file:com.kircherelectronics.androidlinearacceleration.sensor.AccelerationSensor.java
/** * To avoid anomalies at the poles with Euler angles and Gimbal lock, * quaternions are used instead.//from w w w . j av a2s . c om */ private void initQuaternionRotations() { // Rotate by 90 degrees or pi/2 radians. double rotation = Math.PI / 2; // Create the rotation around the x-axis Vector3D xV = new Vector3D(1, 0, 0); xQuaternion = new Rotation(xV, rotation); // Create the rotation around the y-axis Vector3D yV = new Vector3D(0, 1, 0); yQuaternion = new Rotation(yV, -rotation); // Create the composite rotation. rotationQuaternion = yQuaternion.applyTo(xQuaternion); }
From source file:msi.gama.metamodel.shape.GamaShape.java
/** * Same as above, but applies a (optional) rotation along a given vector and * (optional) translation to the geometry * // w w w . ja v a2s . c om * @param source * cannot be null * @param geom * can be null * @param rotation * can be null, expressed in degrees * @param newLocation * can be null */ public GamaShape(final IShape source, final Geometry geom, final Double rotation, final GamaPoint vector, final ILocation newLocation) { this(source, geom); if (!isPoint() && rotation != null) { if (vector == null) { final Coordinate c = geometry.getCentroid().getCoordinate(); geometry.apply(AffineTransform3D.createRotationOz(FastMath.toRadians(rotation), c.x, c.y)); } else { final Vector3D v3D = new Vector3D(vector.getX(), vector.getY(), vector.getZ()); final Rotation rot = new Rotation(v3D, FastMath.toRadians(rotation)); for (final Coordinate c : this.getInnerGeometry().getCoordinates()) { final Vector3D result = rot.applyTo(new Vector3D(c.x, c.y, c.z)); c.x = result.getX(); c.y = result.getY(); c.z = result.getZ(); } } } if (newLocation != null) { setLocation(newLocation); } }
From source file:nova.core.util.math.MatrixStack.java
/** * Rotates transformation matrix around rotateVector axis by angle radians. * * @param rotateVector Vector serving as rotation axis. * @param angle in radians.//from w ww. j a v a 2 s. c o m * @return The rotated matrix */ public MatrixStack rotate(Vector3D rotateVector, double angle) { return rotate(new Rotation(rotateVector, angle)); }
From source file:nova.core.wrapper.mc.forge.v17.wrapper.entity.forward.BWRigidBody.java
void updateRotation(double deltaTime) { //Integrate angular velocity to angular displacement Rotation angularVel = angularVelocity(); Rotation deltaRotation = RotationUtil.slerp(Rotation.IDENTITY, angularVel, deltaTime); entity().transform().setRotation(entity().rotation().applyTo(deltaRotation)); //Integrate torque to angular velocity Vector3D torque = netTorque.scalarMultiply(deltaTime); if (!Vector3D.ZERO.equals(torque)) { setAngularVelocity(angularVelocity().applyTo(new Rotation(Vector3DUtil.FORWARD, torque))); }/*from ww w. java 2 s . com*/ //Clear net torque netTorque = Vector3D.ZERO; //Apply drag Vector3D eulerAngularVel = angularVelocity().applyInverseTo(Vector3DUtil.FORWARD); addTorque(eulerAngularVel.negate().scalarMultiply(angularDrag())); }
From source file:org.jtrfp.trcl.beh.AutoLeveling.java
@Override public void _tick(long timeInMillis) { final WorldObject parent = getParent(); final Vector3D oldHeading = parent.getHeading(); final Vector3D oldTop = parent.getTop(); final Vector3D oldCross = oldHeading.crossProduct(oldTop); final Vector3D newHeading = levelingAxis == LevelingAxis.HEADING ? new Vector3D( oldHeading.getX() * retainmentCoeff[0] + levelingVector.getX() * inverseRetainmentCoeff[0], oldHeading.getY() * retainmentCoeff[1] + levelingVector.getY() * inverseRetainmentCoeff[1], oldHeading.getZ() * retainmentCoeff[2] + levelingVector.getZ() * inverseRetainmentCoeff[2]) .normalize() : oldHeading.normalize();//from www.j a v a 2s . c om final Vector3D newTop = levelingAxis == LevelingAxis.TOP ? new Vector3D( oldTop.getX() * retainmentCoeff[0] + levelingVector.getX() * inverseRetainmentCoeff[0], oldTop.getY() * retainmentCoeff[1] + levelingVector.getY() * inverseRetainmentCoeff[1], oldTop.getZ() * retainmentCoeff[2] + levelingVector.getZ() * inverseRetainmentCoeff[2]).normalize() : oldTop.normalize(); final Vector3D newCross = levelingAxis == LevelingAxis.CROSS ? new Vector3D( oldCross.getX() * retainmentCoeff[0] + levelingVector.getX() * inverseRetainmentCoeff[0], oldCross.getY() * retainmentCoeff[1] + levelingVector.getY() * inverseRetainmentCoeff[1], oldCross.getZ() * retainmentCoeff[2] + levelingVector.getZ() * inverseRetainmentCoeff[2]) .normalize() : oldCross.normalize(); final Rotation topDelta = new Rotation(oldTop, newTop); final Rotation headingDelta = new Rotation(oldHeading, newHeading); final Rotation crossDelta = new Rotation(oldCross, newCross); parent.setHeading(crossDelta.applyTo(headingDelta.applyTo(topDelta.applyTo(oldHeading)))); parent.setTop(crossDelta.applyTo(headingDelta.applyTo(topDelta.applyTo(oldTop)))); }
From source file:org.jtrfp.trcl.beh.HorizAimAtPlayerBehavior.java
@Override public void _tick(long timeInMillis) { if (chaseTarget != null) { WorldObject thisObject = getParent(); final Player player = thisObject.getTr().getGame().getPlayer(); if (player.getBehavior().probeForBehavior(Cloakable.class).isCloaked()) return; final RotationalMomentumBehavior rmb = thisObject.getBehavior() .probeForBehavior(RotationalMomentumBehavior.class); assert !Vect3D.isAnyEqual(chaseTarget.getPosition(), Double.POSITIVE_INFINITY); assert !Vect3D.isAnyEqual(thisObject.getPosition(), Double.NEGATIVE_INFINITY); TR.twosComplimentSubtract(chaseTarget.getPosition(), thisObject.getPosition(), vectorToTargetVar); assert !Vect3D.isAnyNaN(vectorToTargetVar); assert !Vect3D.isAnyEqual(vectorToTargetVar, Double.POSITIVE_INFINITY); assert !Vect3D.isAnyEqual(vectorToTargetVar, Double.NEGATIVE_INFINITY); vectorToTargetVar[1] = 0;/*from w ww . jav a 2 s . c o m*/ Vect3D.normalize(vectorToTargetVar, vectorToTargetVar); final Vector3D thisHeading = new Vector3D(thisObject.getHeading().getX(), 0, thisObject.getHeading().getZ()).normalize(); Vect3D.subtract(thisHeading.toArray(), vectorToTargetVar, headingVarianceDelta); if (Math.sqrt(headingVarianceDelta[2] * headingVarianceDelta[2] + headingVarianceDelta[0] * headingVarianceDelta[0]) < hysteresis) return; if (!reverse) Vect3D.negate(vectorToTargetVar); Rotation rot = new Rotation(new Vector3D(vectorToTargetVar), thisHeading); final Vector3D deltaVector = rot.applyTo(Vector3D.PLUS_K); if ((deltaVector.getZ() > 0 || deltaVector.getX() < 0) == leftHanded) { rmb.accellerateEquatorialMomentum(-equatorialAccelleration); } else { rmb.accellerateEquatorialMomentum(equatorialAccelleration); } } //end if(target!null) }
From source file:org.jtrfp.trcl.beh.phy.BouncesOffSurfaces.java
@Override public void collidedWithSurface(WorldObject wo, double[] surfaceNormal) { final WorldObject parent = getParent(); final Vector3D oldHeading = parent.getHeading(); final Vector3D oldTop = parent.getTop(); final Vector3D _surfaceNormal = new Vector3D(surfaceNormal); if (oldHeading == null) throw new NullPointerException("Parent heading is null."); if (surfaceNormal == null) throw new NullPointerException("Surface normal is null."); if (reflectHeading && new Rotation(oldHeading, _surfaceNormal).getAngle() > Math.PI / 2.) { Vector3D newHeading = (_surfaceNormal.scalarMultiply(_surfaceNormal.dotProduct(oldHeading) * -2) .add(oldHeading));/*from w w w . j a va2 s. com*/ parent.setHeading(newHeading); final Rotation resultingRotation = new Rotation(oldHeading, newHeading); Vector3D newTop = resultingRotation.applyTo(oldTop); //if(newTop.getY()<0)newTop=newTop.negate(); parent.setTop(newTop); } //end if(should reflect) //if(parent instanceof Velocible){ final Velocible velocible = (Velocible) parent.probeForBehavior(Velocible.class); Vector3D oldVelocity = velocible.getVelocity(); if (oldVelocity.getNorm() == 0) oldVelocity = Vector3D.PLUS_I; if (new Rotation(oldVelocity.normalize(), _surfaceNormal).getAngle() > Math.PI / 2.) { velocible.setVelocity( (_surfaceNormal.scalarMultiply(_surfaceNormal.dotProduct(oldVelocity) * -2).add(oldVelocity)) .scalarMultiply(velocityRetainmentCoefficient)); //Nudge parent.setPosition( new Vector3D(parent.getPosition()).add(_surfaceNormal.scalarMultiply(1000.)).toArray()); } //end if(should bounce) //}//end if(Velocible) }
From source file:org.jtrfp.trcl.beh.phy.RotatingObjectBehavior.java
@Override protected void _tick(long tickTimeInMillis) { angleAnimator.updateAnimation();//w w w .j ava2s .c o m super.getParent().setTop(new Rotation(rotationAxisTop, angle.get()).applyTo(originalTop)); super.getParent().setHeading(new Rotation(rotationAxisTop, angle.get()).applyTo(originalHeading)); }