List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D dotProduct
public double dotProduct(final Vector<Euclidean3D> v)
The implementation uses specific multiplication and addition algorithms to preserve accuracy and reduce cancellation effects.
From source file:org.jtrfp.trcl.beh.CollidesWithTerrain.java
@Override public void _tick(long tickTimeMillis) { if (tickCounter++ % 2 == 0 && !recentlyCollided) return;/*from w ww .j a va 2 s.co m*/ recentlyCollided = false; final WorldObject p = getParent(); final TR tr = p.getTr(); final World world = tr.getWorld(); final InterpolatingAltitudeMap aMap; final Mission mission = tr.getGame().getCurrentMission(); try { aMap = mission.getOverworldSystem().getAltitudeMap(); } catch (NullPointerException e) { return; } if (mission.getOverworldSystem().isTunnelMode()) return;//No terrain to collide with while in tunnel mode. if (aMap == null) return; final double[] thisPos = p.getPosition(); final double groundHeightNorm = aMap.heightAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize)); final double groundHeight = groundHeightNorm * (world.sizeY / 2); final double ceilingHeight = (1.99 - aMap.heightAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize))) * (world.sizeY / 2) + CEILING_Y_NUDGE; final Vector3D groundNormal = (aMap.normalAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize))); Vector3D downhillDirectionXZ = new Vector3D(groundNormal.getX(), 0, groundNormal.getZ()); if (downhillDirectionXZ.getNorm() != 0) downhillDirectionXZ = downhillDirectionXZ.normalize(); else downhillDirectionXZ = Vector3D.PLUS_J; final OverworldSystem overworldSystem = tr.getGame().getCurrentMission().getOverworldSystem(); if (overworldSystem == null) return; final boolean terrainMirror = overworldSystem.isChamberMode(); final double thisY = thisPos[1]; boolean groundImpact = thisY < (groundHeight + (autoNudge ? nudgePadding : 0)); final boolean ceilingImpact = (thisY > ceilingHeight && terrainMirror && !ignoreCeiling); final Vector3D ceilingNormal = new Vector3D(groundNormal.getX(), -groundNormal.getY(), groundNormal.getZ()); Vector3D surfaceNormal = groundImpact ? groundNormal : ceilingNormal; final double dot = surfaceNormal.dotProduct(getParent().getHeading()); if (terrainMirror && groundHeightNorm > .97) { groundImpact = true; surfaceNormal = downhillDirectionXZ; } //end if(smushed between floor and ceiling) if (groundLock) { recentlyCollided = true; thisPos[1] = groundHeight; p.notifyPositionChange(); return; } //end if(groundLock) if (tunnelEntryCapable && groundImpact && dot < 0) { final OverworldSystem os = mission.getOverworldSystem(); if (!os.isTunnelMode()) { TunnelEntranceObject teo = mission.getTunnelEntranceObject( new Point((int) (thisPos[0] / TR.mapSquareSize), (int) (thisPos[2] / TR.mapSquareSize))); if (teo != null && !mission.isBossFight()) { mission.enterTunnel(teo.getSourceTunnel()); return; } } //end if(above ground) } //end if(tunnelEntryCapable()) if (groundImpact || ceilingImpact) {// detect collision recentlyCollided = true; double padding = autoNudge ? nudgePadding : 0; padding *= groundImpact ? 1 : -1; thisPos[1] = (groundImpact ? groundHeight : ceilingHeight) + padding; p.notifyPositionChange(); if (dot < 0 || ignoreHeadingForImpact) {//If toward ground, call impact listeners. surfaceNormalVar = surfaceNormal; final Behavior behavior = p.getBehavior(); behavior.probeForBehaviors(sub, SurfaceImpactListener.class); } //end if(pointedTowardGround) } // end if(collision) }
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 ava 2 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.orekit.forces.gravity.Relativity.java
@Override public void addContribution(final SpacecraftState s, final TimeDerivativesEquations adder) throws OrekitException { final PVCoordinates pv = s.getPVCoordinates(); final Vector3D p = pv.getPosition(); final Vector3D v = pv.getVelocity(); //radius/*from ww w . java2 s . c o m*/ final double r2 = p.getNormSq(); final double r = FastMath.sqrt(r2); //speed final double s2 = v.getNormSq(); final double c2 = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT; //eq. 3.146 final Vector3D accel = new Vector3D(4 * this.gm / r - s2, p, 4 * p.dotProduct(v), v) .scalarMultiply(this.gm / (r2 * r * c2)); adder.addAcceleration(accel, s.getFrame()); }
From source file:org.orekit.forces.gravity.Relativity.java
@Override public FieldVector3D<DerivativeStructure> accelerationDerivatives(final SpacecraftState s, final String paramName) throws OrekitException { complainIfNotSupported(paramName);/* ww w .j ava 2 s .c o m*/ final DerivativeStructure gmDS = new DerivativeStructure(1, 1, 0, this.gm); final PVCoordinates pv = s.getPVCoordinates(); final Vector3D p = pv.getPosition(); final Vector3D v = pv.getVelocity(); //radius final double r2 = p.getNormSq(); final double r = FastMath.sqrt(r2); //speed final double s2 = v.getNormSq(); final double c2 = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT; //eq. 3.146 return new FieldVector3D<DerivativeStructure>(gmDS.multiply(4 / r).subtract(s2), p, new DerivativeStructure(1, 1, 4 * p.dotProduct(v)), v).scalarMultiply(gmDS.divide(r2 * r * c2)); }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTSolarRadiationPressure.java
/** {@inheritDoc} */ protected double[] getLLimits(final SpacecraftState state) throws OrekitException { // Default bounds without shadow [-PI, PI] final double[] ll = { -FastMath.PI + MathUtils.normalizeAngle(state.getLv(), 0), FastMath.PI + MathUtils.normalizeAngle(state.getLv(), 0) }; // Direction cosines of the Sun in the equinoctial frame final Vector3D sunDir = sun.getPVCoordinates(state.getDate(), state.getFrame()).getPosition().normalize(); final double alpha = sunDir.dotProduct(f); final double beta = sunDir.dotProduct(g); final double gamma = sunDir.dotProduct(w); // Compute limits only if the perigee is close enough from the central body to be in the shadow if (FastMath.abs(gamma * a * (1. - ecc)) < ae) { // Compute the coefficients of the quartic equation in cos(L) 3.5-(2) final double bet2 = beta * beta; final double h2 = h * h; final double k2 = k * k; final double m = ae / (a * B); final double m2 = m * m; final double m4 = m2 * m2; final double bb = alpha * beta + m2 * h * k; final double b2 = bb * bb; final double cc = alpha * alpha - bet2 + m2 * (k2 - h2); final double dd = 1. - bet2 - m2 * (1. + h2); final double[] a = new double[5]; a[0] = 4. * b2 + cc * cc;//from w ww.j av a 2 s . c o m a[1] = 8. * bb * m2 * h + 4. * cc * m2 * k; a[2] = -4. * b2 + 4. * m4 * h2 - 2. * cc * dd + 4. * m4 * k2; a[3] = -8. * bb * m2 * h - 4. * dd * m2 * k; a[4] = -4. * m4 * h2 + dd * dd; // Compute the real roots of the quartic equation 3.5-2 final double[] roots = new double[4]; final int nbRoots = realQuarticRoots(a, roots); if (nbRoots > 0) { // Check for consistency boolean entryFound = false; boolean exitFound = false; // Eliminate spurious roots for (int i = 0; i < nbRoots; i++) { final double cosL = roots[i]; final double sL = FastMath.sqrt((1. - cosL) * (1. + cosL)); // Check both angles: L and -L for (int j = -1; j <= 1; j += 2) { final double sinL = j * sL; final double cPhi = alpha * cosL + beta * sinL; // Is the angle on the shadow side of the central body (eq. 3.5-3) ? if (cPhi < 0.) { final double range = 1. + k * cosL + h * sinL; final double S = 1. - m2 * range * range - cPhi * cPhi; // Is the shadow equation 3.5-1 satisfied ? if (FastMath.abs(S) < S_ZERO) { // Is this the entry or exit angle ? final double dSdL = m2 * range * (k * sinL - h * cosL) + cPhi * (alpha * sinL - beta * cosL); if (dSdL > 0.) { // Exit from shadow: 3.5-4 exitFound = true; ll[0] = FastMath.atan2(sinL, cosL); } else { // Entry into shadow: 3.5-5 entryFound = true; ll[1] = FastMath.atan2(sinL, cosL); } } } } } // Must be one entry and one exit or none if (!(entryFound == exitFound)) { // entry or exit found but not both ! In this case, consider there is no eclipse... ll[0] = -FastMath.PI; ll[1] = FastMath.PI; } // Quadrature between L at exit and L at entry so Lexit must be lower than Lentry if (ll[0] > ll[1]) { // Keep the angles between [-2PI, 2PI] if (ll[1] < 0.) { ll[1] += 2. * FastMath.PI; } else { ll[0] -= 2. * FastMath.PI; } } } } return ll; }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTThirdBody.java
/** {@inheritDoc} */ @Override//from www . jav a 2 s. co m public void initializeStep(final AuxiliaryElements aux) throws OrekitException { // Equinoctial elements a = aux.getSma(); k = aux.getK(); h = aux.getH(); q = aux.getQ(); p = aux.getP(); // Retrograde factor I = aux.getRetrogradeFactor(); // Eccentricity ecc = aux.getEcc(); // Distance from center of mass of the central body to the 3rd body final Vector3D bodyPos = body.getPVCoordinates(aux.getDate(), aux.getFrame()).getPosition(); R3 = bodyPos.getNorm(); // Direction cosines final Vector3D bodyDir = bodyPos.normalize(); alpha = bodyDir.dotProduct(aux.getVectorF()); beta = bodyDir.dotProduct(aux.getVectorG()); gamma = bodyDir.dotProduct(aux.getVectorW()); // Equinoctial coefficients A = aux.getA(); B = aux.getB(); C = aux.getC(); meanMotion = aux.getMeanMotion(); //Χ<sup>-2</sup>. BB = B * B; //Χ<sup>-3</sup>. BBB = BB * B; //b = 1 / (1 + B) b = 1. / (1. + B); // Χ X = 1. / B; XX = X * X; XXX = X * XX; // -2 * a / A m2aoA = -2. * a / A; // B / A BoA = B / A; // 1 / AB ooAB = 1. / (A * B); // -C / 2AB mCo2AB = -C * ooAB / 2.; // B / A(1 + B) BoABpo = BoA / (1. + B); // mu3 / R3 muoR3 = gm / R3; //h * Χ hXXX = h * XXX; //k * Χ kXXX = k * XXX; }
From source file:org.rhwlab.dispim.nucleus.NamedNucleusFile.java
License:asdf
static public RealMatrix rotationMatrix(Vector3D A, Vector3D B) { Vector3D a = A.normalize();// w w w. j a va 2 s. c o m Vector3D b = B.normalize(); Vector3D v = a.crossProduct(b); double s = v.getNormSq(); double c = a.dotProduct(b); RealMatrix vx = MatrixUtils.createRealMatrix(3, 3); vx.setEntry(1, 0, v.getZ()); vx.setEntry(0, 1, -v.getZ()); vx.setEntry(2, 0, -v.getY()); vx.setEntry(0, 2, v.getY()); vx.setEntry(2, 1, v.getX()); vx.setEntry(1, 2, -v.getX()); RealMatrix vx2 = vx.multiply(vx); RealMatrix scaled = vx2.scalarMultiply((1.0 - c) / s); RealMatrix ident = MatrixUtils.createRealIdentityMatrix(3); RealMatrix sum = vx.add(scaled); RealMatrix ret = ident.add(sum); return ret; }
From source file:org.wallerlab.yoink.math.linear.CommonsMatrix.java
@Override public double dotProduct() { double[] v = this.internalMatrix.getRow(0); Vector3D vector = new Vector3D(v); return vector.dotProduct(vector); }