List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D normalize
public Vector3D normalize() throws MathArithmeticException
From source file:edu.mit.fss.examples.member.OrekitSurfaceElement.java
/** * Gets the relative speed (in m/s) from this element * to the specified element./* www. j av a2 s . co m*/ * * @param element the element * @return the relative speed */ public double getRelativeSpeed(Element element) { if (element.getFrame() == ReferenceFrame.UNKNOWN) { logger.warn("Unknown reference frame for element " + element + ", cannot compute elevation."); return 0; } try { Transform t = element.getFrame().getOrekitFrame().getTransformTo(frame.getOrekitFrame(), getDate()); Vector3D relPosition = t.transformVector(element.getPosition()).subtract(getPosition()); Vector3D relVelocity = t.transformVector(element.getVelocity()).subtract(getVelocity()); // compute relative speed return relVelocity.dotProduct(relPosition.normalize()); } catch (OrekitException e) { logger.error(e.getMessage()); } return 0; }
From source file:edu.mit.fss.examples.member.OrekitOrbitalElement.java
/** * Gets the relative speed (in m/s) from this element * to the specified element./*from ww w.jav a 2 s . c o m*/ * * @param element the element * @return the relative speed */ public double getRelativeSpeed(Element element) { if (element.getFrame() == ReferenceFrame.UNKNOWN) { logger.warn("Unknown reference frame for element " + element + ", cannot compute elevation."); return 0; } try { Transform t = element.getFrame().getOrekitFrame().getTransformTo(getFrame().getOrekitFrame(), getDate()); Vector3D relPosition = t.transformVector(element.getPosition()).subtract(getPosition()); Vector3D relVelocity = t.transformVector(element.getVelocity()).subtract(getVelocity()); // compute relative speed return relVelocity.dotProduct(relPosition.normalize()); } catch (OrekitException e) { logger.error(e.getMessage()); } return 0; }
From source file:lambertmrev.Lambert.java
/** Constructs and solves a Lambert problem. * * \param[in] R1 first cartesian position * \param[in] R2 second cartesian position * \param[in] tof time of flight/*w w w. j a v a2 s .c o m*/ * \param[in] mu gravity parameter * \param[in] cw when 1 a retrograde orbit is assumed * \param[in] multi_revs maximum number of multirevolutions to compute */ public void lambert_problem(Vector3D r1, Vector3D r2, double tof, double mu, Boolean cw, int multi_revs) { // sanity checks if (tof <= 0) { System.out.println("ToF is negative! \n"); } if (mu <= 0) { System.out.println("mu is below zero"); } // 1 - getting lambda and T double m_c = FastMath.sqrt((r2.getX() - r1.getX()) * (r2.getX() - r1.getX()) + (r2.getY() - r1.getY()) * (r2.getY() - r1.getY()) + (r2.getZ() - r1.getZ()) * (r2.getZ() - r1.getZ())); double R1 = r1.getNorm(); double R2 = r2.getNorm(); double m_s = (m_c + R1 + R2) / 2.0; Vector3D ir1 = r1.normalize(); Vector3D ir2 = r2.normalize(); Vector3D ih = Vector3D.crossProduct(ir1, ir2); ih = ih.normalize(); if (ih.getZ() == 0) { System.out.println("angular momentum vector has no z component \n"); } double lambda2 = 1.0 - m_c / m_s; double m_lambda = FastMath.sqrt(lambda2); Vector3D it1 = new Vector3D(0.0, 0.0, 0.0); Vector3D it2 = new Vector3D(0.0, 0.0, 0.0); if (ih.getZ() < 0.0) { // Transfer angle is larger than 180 degrees as seen from abive the z axis m_lambda = -m_lambda; it1 = Vector3D.crossProduct(ir1, ih); it2 = Vector3D.crossProduct(ir2, ih); } else { it1 = Vector3D.crossProduct(ih, ir1); it2 = Vector3D.crossProduct(ih, ir2); } it1.normalize(); it2.normalize(); if (cw) { // Retrograde motion m_lambda = -m_lambda; it1.negate(); it2.negate(); } double lambda3 = m_lambda * lambda2; double T = FastMath.sqrt(2.0 * mu / m_s / m_s / m_s) * tof; // 2 - We now hava lambda, T and we will find all x // 2.1 - let us first detect the maximum number of revolutions for which there exists a solution int m_Nmax = FastMath.toIntExact(FastMath.round(T / FastMath.PI)); double T00 = FastMath.acos(m_lambda) + m_lambda * FastMath.sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * FastMath.PI); double T1 = 2.0 / 3.0 * (1.0 - lambda3); double DT = 0.0; double DDT = 0.0; double DDDT = 0.0; if (m_Nmax > 0) { if (T < T0) { // We use Halley iterations to find xM and TM int it = 0; double err = 1.0; double T_min = T0; double x_old = 0.0, x_new = 0.0; while (true) { ArrayRealVector deriv = dTdx(x_old, T_min, m_lambda); DT = deriv.getEntry(0); DDT = deriv.getEntry(1); DDDT = deriv.getEntry(2); if (DT != 0.0) { x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); } err = FastMath.abs(x_old - x_new); if ((err < 1e-13) || (it > 12)) { break; } tof = x2tof(x_new, m_Nmax, m_lambda); x_old = x_new; it++; } if (T_min > T) { m_Nmax -= 1; } } } // We exit this if clause with Mmax being the maximum number of revolutions // for which there exists a solution. We crop it to multi_revs m_Nmax = FastMath.min(multi_revs, m_Nmax); // 2.2 We now allocate the memory for the output variables m_v1 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_v2 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_iters = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); //RealMatrix m_x = MatrixUtils.createRealMatrix(m_Nmax*2+1, 3); ArrayRealVector m_x = new ArrayRealVector(m_Nmax * 2 + 1); // 3 - We may now find all solution in x,y // 3.1 0 rev solution // 3.1.1 initial guess if (T >= T00) { m_x.setEntry(0, -(T - T00) / (T - T00 + 4)); } else if (T <= T1) { m_x.setEntry(0, T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1); } else { m_x.setEntry(0, FastMath.pow((T / T00), 0.69314718055994529 / FastMath.log(T1 / T00)) - 1.0); } // 3.1.2 Householder iterations //m_iters.setEntry(0, 0, housOutTmp.getEntry(0)); m_x.setEntry(0, householder(T, m_x.getEntry(0), 0, 1e-5, 15, m_lambda)); // 3.2 multi rev solutions double tmp; double x0; for (int i = 1; i < m_Nmax + 1; i++) { // 3.2.1 left householder iterations tmp = FastMath.pow((i * FastMath.PI + FastMath.PI) / (8.0 * T), 2.0 / 3.0); m_x.setEntry(2 * i - 1, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i - 1), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i - 1, x0); //m_iters.setEntry(2*i-1, 0, housOutTmp.getEntry(0)); //3.2.1 right Householder iterations tmp = FastMath.pow((8.0 * T) / (i * FastMath.PI), 2.0 / 3.0); m_x.setEntry(2 * i, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i, x0); //m_iters.setEntry(2*i, 0, housOutTmp.getEntry(0)); } // 4 - For each found x value we recontruct the terminal velocities double gamma = FastMath.sqrt(mu * m_s / 2.0); double rho = (R1 - R2) / m_c; double sigma = FastMath.sqrt(1 - rho * rho); double vr1, vt1, vr2, vt2, y; ArrayRealVector ir1_vec = new ArrayRealVector(3); ArrayRealVector ir2_vec = new ArrayRealVector(3); ArrayRealVector it1_vec = new ArrayRealVector(3); ArrayRealVector it2_vec = new ArrayRealVector(3); // set Vector3D values to a mutable type ir1_vec.setEntry(0, ir1.getX()); ir1_vec.setEntry(1, ir1.getY()); ir1_vec.setEntry(2, ir1.getZ()); ir2_vec.setEntry(0, ir2.getX()); ir2_vec.setEntry(1, ir2.getY()); ir2_vec.setEntry(2, ir2.getZ()); it1_vec.setEntry(0, it1.getX()); it1_vec.setEntry(1, it1.getY()); it1_vec.setEntry(2, it1.getZ()); it2_vec.setEntry(0, it2.getX()); it2_vec.setEntry(1, it2.getY()); it2_vec.setEntry(2, it2.getZ()); for (int i = 0; i < m_x.getDimension(); i++) { y = FastMath.sqrt(1.0 - lambda2 + lambda2 * m_x.getEntry(i) * m_x.getEntry(i)); vr1 = gamma * ((m_lambda * y - m_x.getEntry(i)) - rho * (m_lambda * y + m_x.getEntry(i))) / R1; vr2 = -gamma * ((m_lambda * y - m_x.getEntry(i)) + rho * (m_lambda * y + m_x.getEntry(i))) / R2; double vt = gamma * sigma * (y + m_lambda * m_x.getEntry(i)); vt1 = vt / R1; vt2 = vt / R2; for (int j = 0; j < 3; ++j) m_v1.setEntry(i, j, vr1 * ir1_vec.getEntry(j) + vt1 * it1_vec.getEntry(j)); for (int j = 0; j < 3; ++j) m_v2.setEntry(i, j, vr2 * ir2_vec.getEntry(j) + vt2 * it2_vec.getEntry(j)); } }
From source file:org.gearvrf.controls.model.Apple.java
private Vector3D setNewApplePosition(GVRContext context, float angle, float distance) { float[] vecDistance = context.getMainScene().getMainCameraRig().getLookAt(); if (vecDistance[1] > CAMERA_DIRECTION_THREASHOLD || vecDistance[1] < -CAMERA_DIRECTION_THREASHOLD) { setAppleRandomPosition(context); return null; }/*from www . j a v a2 s . c o m*/ vecDistance[0] *= distance; vecDistance[2] *= distance; Vector3D instanceApple = new Vector3D(vecDistance[0], 0, vecDistance[2]); instanceApple.normalize(); this.getTransform().setPositionX((float) instanceApple.getX()); this.getTransform().setPositionZ((float) instanceApple.getZ()); this.getTransform().rotateByAxisWithPivot(angle, 0, 1, 0, 0, 0, 0); instanceApple = new Vector3D(this.getTransform().getPositionX(), this.getTransform().getPositionY(), this.getTransform().getPositionZ()); shadow.getTransform().setPosition((float) instanceApple.getX(), -0.9999f, (float) instanceApple.getZ()); shadow.getTransform().setScale(1, 1, 1); return instanceApple; }
From source file:org.gearvrf.keyboard.util.Util.java
public static void rotateWithOpenGLLookAt(Vector3D cameraVector, Vector3D parentVector, GVRSceneObject object) { Vector3D globalUpVector = new Vector3D(0, 1, 0); Vector3D lookVector = parentVector.normalize(); Vector3D rightVector = lookVector.crossProduct(globalUpVector); Vector3D upVector = rightVector.crossProduct(lookVector); Vector3D zAxis = cameraVector.subtract(parentVector).normalize(); // Vector3D xAxis = upVector.crossProduct(zAxis).normalize(); Vector3D xAxis = zAxis.crossProduct(upVector).normalize(); Vector3D yAxis = xAxis.crossProduct(zAxis).normalize(); // Vector3D yAxis = xAxis.crossProduct(zAxis).normalize(); zAxis = zAxis.scalarMultiply(-1.f);// w ww. ja va 2 s . c om float angle = (float) Vector3D.angle(parentVector, cameraVector); angle = (float) Math.toDegrees(angle); object.getTransform().rotateByAxis(angle, (float) xAxis.getX(), (float) xAxis.getY(), (float) xAxis.getZ()); object.getTransform().rotateByAxis(angle, (float) yAxis.getX(), (float) yAxis.getY(), (float) yAxis.getZ()); object.getTransform().rotateByAxis(angle, (float) zAxis.getX(), (float) zAxis.getY(), (float) zAxis.getZ()); }
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(); 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();//from w ww. java 2 s . c o m 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.AutoLeveling.java
/** * @param levelingVector/*from w w w .ja v a 2s . c om*/ * the levelingVector to set */ public AutoLeveling setLevelingVector(Vector3D levelingVector) { if (levelingVector.getNorm() == 0) throw new RuntimeException("Intolerable zero leveling vector."); this.levelingVector = levelingVector.normalize(); return this; }
From source file:org.jtrfp.trcl.beh.CollidesWithTerrain.java
@Override public void _tick(long tickTimeMillis) { if (tickCounter++ % 2 == 0 && !recentlyCollided) return;//from w w w . j ava 2 s. c om 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. ja v a 2s . c o m 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.ProjectileBehavior.java
public void reset(Vector3D heading, double speed) { this.speed = speed; honingTarget = null;//from w w w . j a va 2s .c om final WorldObject parent = getParent(); final Behavior beh = parent.getBehavior(); parent.setHeading(heading); if (honing) { // Find target WorldObject closestObject = null; double closestDistance = Double.POSITIVE_INFINITY; List<WorldObject> possibleTargets = getParent().getTr().getCollisionManager() .getCurrentlyActiveCollisionList(); synchronized (possibleTargets) { for (WorldObject possibleTarget : possibleTargets) { if (possibleTarget instanceof DEFObject) { DEFObject possibleDEFTarget = (DEFObject) possibleTarget; if (!possibleDEFTarget.isIgnoringProjectiles() && !possibleDEFTarget.isRuin()) { final Vector3D targetPos = new Vector3D(possibleTarget.getPositionWithOffset()); final Vector3D delta = targetPos.subtract(new Vector3D(getParent().getPosition())); final double dist = delta.getNorm(); final Vector3D proposedHeading = delta.normalize(); final Vector3D headingDelta = getParent().getHeading().subtract(proposedHeading); final double compositeHeadingDelta = headingDelta.getNorm(); if (compositeHeadingDelta < .5) { final double compositeDistance = dist; if (compositeDistance < closestDistance) { closestDistance = dist; closestObject = possibleTarget; parent.setHeading(proposedHeading); getParent().getBehavior().probeForBehavior(AutoLeveling.class) .setLevelingVector(heading); } // end if(closesObject) } // end if(headingDelta<1) } // end if(isIgnoringProjectiles) } // end if(DEFObject) } } // end for(WorldObject others) honingTarget = new WeakReference<WorldObject>(closestObject); // if(honingTarget==null){ getParent().getBehavior().probeForBehavior(AutoLeveling.class).setLevelingVector(heading); movesByVelocity.setVelocity(getParent().getHeading().scalarMultiply(speed)); // }//end if(honingTarget==null) } // end if(honingTarget) beh.probeForBehavior(LimitedLifeSpan.class).reset(LIFESPAN_MILLIS); beh.probeForBehavior(DeathBehavior.class).reset(); }