List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D scalarMultiply
public Vector3D scalarMultiply(double a)
From source file:org.jtrfp.trcl.obj.ProjectileFactory.java
public Projectile fire(double[] newPosition, Vector3D heading, WorldObject objectOfOrigin) { assert !Vect3D.isAnyNaN(newPosition); assert heading.getNorm() != 0 && heading.getNorm() != Double.NaN; final Projectile result = projectiles[projectileIndex]; result.destroy();/*www .j a v a 2s . com*/ result.reset(newPosition, heading.scalarMultiply(projectileSpeed), objectOfOrigin); ((WorldObject) result).setTop(objectOfOrigin.getTop()); tr.getDefaultGrid().add((WorldObject) result); tr.mainRenderer.get().temporarilyMakeImmediatelyRelevant((PositionedRenderable) result); if (soundTexture != null) tr.soundSystem.get() .enqueuePlaybackEvent(tr.soundSystem.get().getPlaybackFactory().create(soundTexture, (WorldObject) result, tr.mainRenderer.get().getCamera(), (objectOfOrigin instanceof Player ? .6 : 1) * SoundSystem.DEFAULT_SFX_VOLUME));//TODO: Use configuration volume instead final List<WorldObject> cL = tr.getCollisionManager().getCurrentlyActiveCollisionList(); synchronized (cL) { cL.add((WorldObject) result); } projectileIndex++; projectileIndex %= projectiles.length; return result; }
From source file:org.jtrfp.trcl.Tunnel.java
/** * Tunnel items: FANBODY.BIN - fan IRIS.BIN - animated iris BEAM.BIN / * PIPE.BIN JAW1.BIN (right) JAW2.BIN (left) - jaws ELECTRI[0-3].RAW - force * field TP1.RAW - good enough for blastable door? * /*from w w w . j a v a 2 s. c om*/ * @throws IOException * @throws FileLoadException * * */ private void installObstacles(Segment s, ColorPaletteVectorList tunnelColorPalette, ColorPaletteVectorList ESTuTvPalette, TextureDescription[] tunnelTexturePalette, Vector3D heading, Vector3D top, Vector3D wPos, double width, double height, TR tr) throws IllegalAccessException, FileLoadException, IOException { final ColorPaletteVectorList palette = tr.getGlobalPaletteVL(); Obstacle obs = s.getObstacle(); WorldObject wo; Model m; switch (obs) { case none0: break; case doorway: { m = Model.buildCube(tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, .5, .5, 1, 1, tr); wo = new WorldObject(tr, m); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); add(wo); break; } case closedDoor: { BarrierCube bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, .5, .5, 0, 1, false); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.addBehavior(new DamageableBehavior().setHealth(4096)); bc.addBehavior(new ExplodesOnDeath(ExplosionType.Blast)); bc.addBehavior(new DeathBehavior()); bc.addBehavior(new DebrisOnDeathBehavior()); bc.addBehavior(new DestructibleWallBehavior()); bc.setTop(top); add(bc); break; } case blownOpenDoor://TODO: This is not displaying alpha BarrierCube bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, .5, .5, 1, 1, true); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); add(bc); break; case movingWallLeft: { Vector3D endPos = wPos.add(heading.crossProduct(top).scalarMultiply(tunnelDia)); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.addBehavior(new ShiftingObjectBehavior(3000, wPos, endPos)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case movingWallRight: { Vector3D endPos = wPos.subtract(heading.crossProduct(top).scalarMultiply(tunnelDia)); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.addBehavior(new ShiftingObjectBehavior(3000, wPos, endPos)); bc.addBehavior(new CubeCollisionBehavior(bc)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case movingWallDown: { Vector3D endPos = wPos.subtract(top.scalarMultiply(tunnelDia)); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.addBehavior(new ShiftingObjectBehavior(3000, wPos, endPos)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case movingWallUp: { Vector3D endPos = wPos.add(top.scalarMultiply(tunnelDia)); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.addBehavior(new ShiftingObjectBehavior(3000, wPos, endPos)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case wallLeftSTUB: case wallLeft: bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { 0., tunnelDia / 2., 0 }, false); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; case wallRightSTUB: case wallRight: bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia, tunnelDia / 2., 0 }, false); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; case wallDownSTUB: case wallDown: bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.setPosition((wPos.subtract(top.scalarMultiply(tunnelDia / 2))).toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; case wallUpSTUB: case wallUp: bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { tunnelDia / 2., tunnelDia / 2., 0 }, false); bc.setPosition((wPos.add(top.scalarMultiply(tunnelDia / 2))).toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; case rotatingHalfWall: { final double rotPeriod = 32768. / (double) s.getRotationSpeed(); final boolean rotate = !Double.isInfinite(rotPeriod); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { 0, tunnelDia / 2., 0 }, false); if (rotate) { bc.addBehavior(new RotatingObjectBehavior(heading, heading, top, (int) (rotPeriod * 1000.), 0)); bc.setTop(top); } else bc.setTop(new Rotation(heading, Math.PI + Math.PI / 2).applyTo(top)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.setTop(top); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case rotating34Wall: { final double rotPeriod = 32768. / (double) s.getRotationSpeed(); final boolean rotate = !Double.isInfinite(rotPeriod); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { 0, tunnelDia / 2., 10 }, false); if (rotate) { bc.addBehavior( new RotatingObjectBehavior(heading, heading, top, (int) (rotPeriod * 1000.), Math.PI)); bc.setTop(top); } else bc.setTop(new Rotation(heading, Math.PI + Math.PI / 2).applyTo(top)); bc.setPosition(wPos.toArray()); bc.setHeading(heading); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); bc = new BarrierCube(tr, tunnelDia, tunnelDia, wallThickness, tunnelTexturePalette[s.getObstacleTextureIndex()], new double[] { 0, tunnelDia / 2., 0 }, false); if (rotate) { bc.addBehavior(new RotatingObjectBehavior(heading, heading, top, (int) (rotPeriod * 1000.), Math.PI + Math.PI / 2)); bc.setTop(top); } else bc.setTop(new Rotation(heading, Math.PI * 2).applyTo(top)); bc.setPosition((wPos.add(new Vector3D(100, 0, 0))).toArray()); bc.setHeading(heading); bc.addBehavior(new CubeCollisionBehavior(bc)); add(bc); break; } case fan: wo = new WorldObject(tr, tr.getResourceManager().getBINModel("BLADE.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 28, false, palette, ESTuTvPalette)); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); wo.addBehavior(new RotatingObjectBehavior(heading, heading, top, 6000, Math.random() * 2 * Math.PI)); add(wo); wo = new WorldObject(tr, tr.getResourceManager().getBINModel("FANBODY.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 28, false, palette, null));//No ESTuTv for fan for now. wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case jawsVertical: // Up jaw wo = new WorldObject(tr, tr.getResourceManager().getBINModel("JAW2.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.addBehavior(new ShiftingObjectBehavior(3000, wPos, wPos.add(top.scalarMultiply(tunnelDia / 2)))); wo.addBehavior(new CubeCollisionBehavior(wo)); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(heading.crossProduct(top).negate()); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); // Down jaw wo = new WorldObject(tr, tr.getResourceManager().getBINModel("JAW1.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.addBehavior( new ShiftingObjectBehavior(3000, wPos, wPos.subtract(top.scalarMultiply(tunnelDia / 2)))); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(heading.crossProduct(top).negate()); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case jawsHorizontal: // Left jaw wo = new WorldObject(tr, tr.getResourceManager().getBINModel("JAW2.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.addBehavior(new ShiftingObjectBehavior(3000, wPos, wPos.add(heading.crossProduct(top).scalarMultiply(tunnelDia / 2)))); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); // Right jaw wo = new WorldObject(tr, tr.getResourceManager().getBINModel("JAW1.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.addBehavior(new ShiftingObjectBehavior(3000, wPos, wPos.subtract(heading.crossProduct(top).scalarMultiply(tunnelDia / 2)))); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case metalBeamUp: wo = new WorldObject(tr, tr.getResourceManager().getBINModel("BEAM.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.setPosition(wPos.add(new Vector3D(0, tunnelDia / 6, 0)).toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case metalBeamDown: wo = new WorldObject(tr, tr.getResourceManager().getBINModel("BEAM.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.setPosition(wPos.add(new Vector3D(0, -tunnelDia / 6, 0)).toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case metalBeamLeft: wo = new WorldObject(tr, tr.getResourceManager().getBINModel("BEAM.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.setPosition(wPos.add(new Vector3D(-tunnelDia / 6, 0, 0)).toArray()); wo.setHeading(heading); wo.setTop(top.crossProduct(heading)); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case metalBeamRight: wo = new WorldObject(tr, tr.getResourceManager().getBINModel("BEAM.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 8, false, palette, ESTuTvPalette)); wo.setPosition(wPos.add(new Vector3D(tunnelDia / 6, 0, 0)).toArray()); wo.setHeading(heading); wo.setTop(top.crossProduct(heading)); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; case forceField: { //ELECTRI[0-3].RAW final ForceField ff = new ForceField(tr, (int) tunnelDia, (int) wallThickness); ff.setPosition(wPos.toArray()); ff.setHeading(heading); ff.setTop(top); add(ff); break; } // Invisible walls, as far as I know, are never used. // This makes sense: There is nothing fun about trying to get through a // tunnel and crashing into invisible walls. case invisibleWallUp:// TODO break; case invisibleWallDown:// TODO break; case invisibleWallLeft:// TODO break; case invisibleWallRight:// TODO break; case iris: { wo = new WorldObject(tr, tr.getResourceManager().getBINModel("IRIS.BIN", tunnelTexturePalette[s.getObstacleTextureIndex()], 4 * 256, false, palette, ESTuTvPalette)); final Model mod = wo.getModel(); wo.addBehavior(new IrisBehavior(new Sequencer(mod.getFrameDelayInMillis(), 2, true), width)); wo.setPosition(wPos.toArray()); wo.setHeading(heading); wo.setTop(top); wo.addBehavior(new CubeCollisionBehavior(wo)); add(wo); break; } }// end switch(obstruction) }
From source file:org.micromanager.plugins.magellan.propsandcovariants.LaserPredNet.java
/** * * @return return distance to surface interpolation based on x y and z points *//*from w w w . j a v a 2 s .com*/ private static double getSampledDistancesToSurface(int angleIndex, double x, double y, double z, SurfaceInterpolator surface) throws InterruptedException { double dTheta = Math.PI * 2.0 / (double) N_THETA_ANGLES; Vector3D initialPoint = new Vector3D(x, y, z); double[] distances = new double[N_THETA_ANGLES]; double theta = angleIndex * dTheta; //calculate unit vector in theta phi direction Vector3D directionUnitVec = new Vector3D(Math.cos(theta) * Math.sin(PHI), Math.sin(theta) * Math.sin(PHI), Math.cos(PHI)).scalarMultiply(-1); //binary search double initialDist = SEARCH_START_DIST; //start with a point outside and then binary line search for the distance while (isWithinSurace(surface, initialPoint.add(directionUnitVec.scalarMultiply(initialDist)))) { initialDist *= 2; } return binarySearch(initialPoint, directionUnitVec, 0, initialDist, surface); }
From source file:org.micromanager.plugins.magellan.propsandcovariants.LaserPredNet.java
private static double binarySearch(Vector3D initialPoint, Vector3D direction, double minDistance, double maxDistance, SurfaceInterpolator surf) throws InterruptedException { double halfDistance = (minDistance + maxDistance) / 2; //if the distance has been narrowed to a sufficiently small interval, return if (maxDistance - minDistance < SEARCH_TOLERANCE) { return halfDistance; }/*from w w w . java2 s .c o m*/ //check if point is above surface in Vector3D searchPoint = initialPoint.add(direction.scalarMultiply(halfDistance)); boolean withinSurface = isWithinSurace(surf, searchPoint); if (!withinSurface) { return binarySearch(initialPoint, direction, minDistance, halfDistance, surf); } else { return binarySearch(initialPoint, direction, halfDistance, maxDistance, surf); } }
From source file:org.orekit.models.earth.GeoidTest.java
/** * check {@link Geoid#getIntersectionPoint(Line, Vector3D, Frame, * AbsoluteDate)} with several points./* w w w . j ava 2s. c o m*/ * * @throws OrekitException on error */ @Test public void testGetIntersectionPoint() throws OrekitException { // setup Geoid geoid = getComponent(); Frame frame = geoid.getBodyFrame(); for (double[] point : reference) { GeodeticPoint gp = new GeodeticPoint(FastMath.toRadians(point[0]), FastMath.toRadians(point[1]), 0); Vector3D expected = geoid.transform(gp); // glancing line: 10% vertical and 90% north (~6 deg elevation) Vector3D slope = gp.getZenith().scalarMultiply(0.1).add(gp.getNorth().scalarMultiply(0.9)); Vector3D close = expected.add(slope.scalarMultiply(100e3)); Vector3D pointOnLine = expected.add(slope); Line line = new Line(close, pointOnLine, 0); // line directed the other way Line otherDirection = new Line(pointOnLine, close, 0); // action GeodeticPoint actual = geoid.getIntersectionPoint(line, close, frame, date); // other direction GeodeticPoint actualReversed = geoid.getIntersectionPoint(otherDirection, close, frame, date); // verify String message = String.format("point: %s%n", Arrays.toString(point)); // position accuracy on Earth's surface to 1 um. assertThat(message, actualReversed, geodeticPointCloseTo(gp, 1e-6)); assertThat(message, actual, geodeticPointCloseTo(gp, 1e-6)); } }
From source file:org.orekit.models.earth.GeoidTest.java
/** * check {@link Geoid#getIntersectionPoint(Line, Vector3D, Frame, * AbsoluteDate)} handles frame transformations correctly * * @throws OrekitException on error//w ww . j a v a2 s.c o m */ @Test public void testGetIntersectionPointFrame() throws OrekitException { // setup Geoid geoid = getComponent(); Frame frame = new Frame(geoid.getBodyFrame(), new Transform(date, new Transform(date, new Vector3D(-1, 2, -3), new Vector3D(4, -5, 6)), new Transform(date, new Rotation(-7, 8, -9, 10, true), new Vector3D(-11, 12, -13))), "test frame"); GeodeticPoint gp = new GeodeticPoint(FastMath.toRadians(46.8743190), FastMath.toRadians(102.4487290), 0); Vector3D expected = geoid.transform(gp); // glancing line: 10% vertical and 90% north (~6 deg elevation) Vector3D slope = gp.getZenith().scalarMultiply(0.1).add(gp.getNorth().scalarMultiply(0.9)); Vector3D close = expected.add(slope.scalarMultiply(100)); Line line = new Line(expected.add(slope), close, 0); Transform xform = geoid.getBodyFrame().getTransformTo(frame, date); // transform to test frame close = xform.transformPosition(close); line = xform.transformLine(line); // action GeodeticPoint actual = geoid.getIntersectionPoint(line, close, frame, date); // verify, 1 um position accuracy at Earth's surface assertThat(actual, geodeticPointCloseTo(gp, 1e-6)); }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>// w ww . j a v a 2s . c o m * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianElliptical() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final Vector3D momentum = pvc.getMomentum(); final double v2 = velocity.getNormSq(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double px = position.getX(); final double py = position.getY(); final double pz = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double sqrtMuA = FastMath.sqrt(a * mu); final double sqrtAoMu = FastMath.sqrt(a / mu); final double a2 = a * a; final double twoA = 2 * a; final double rOnA = r / a; final double oMe2 = 1 - e * e; final double epsilon = FastMath.sqrt(oMe2); final double sqrtRec = 1 / epsilon; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosPA = FastMath.cos(pa); final double sinPA = FastMath.sin(pa); final double pv = Vector3D.dotProduct(position, velocity); final double cosE = (a - r) / (a * e); final double sinE = pv / (e * sqrtMuA); // da final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(2 * a2 / mu); fillHalfRow(1, vectorAR, jacobian[0], 0); fillHalfRow(1, vectorARDot, jacobian[0], 3); // de final double factorER3 = pv / twoA; final Vector3D vectorER = new Vector3D(cosE * v2 / (r * mu), position, sinE / sqrtMuA, velocity, -factorER3 * sinE / sqrtMuA, vectorAR); final Vector3D vectorERDot = new Vector3D(sinE / sqrtMuA, position, cosE * 2 * r / mu, velocity, -factorER3 * sinE / sqrtMuA, vectorARDot); fillHalfRow(1, vectorER, jacobian[1], 0); fillHalfRow(1, vectorERDot, jacobian[1], 3); // dE / dr (Eccentric anomaly) final double coefE = cosE / (e * sqrtMuA); final Vector3D vectorEAnR = new Vector3D(-sinE * v2 / (e * r * mu), position, coefE, velocity, -factorER3 * coefE, vectorAR); // dE / drDot final Vector3D vectorEAnRDot = new Vector3D(-sinE * 2 * r / (e * mu), velocity, coefE, position, -factorER3 * coefE, vectorARDot); // precomputing some more factors final double s1 = -sinE * pz / r - cosE * vz * sqrtAoMu; final double s2 = -cosE * pz / r3; final double s3 = -sinE * vz / (2 * sqrtMuA); final double t1 = sqrtRec * (cosE * pz / r - sinE * vz * sqrtAoMu); final double t2 = sqrtRec * (-sinE * pz / r3); final double t3 = sqrtRec * (cosE - e) * vz / (2 * sqrtMuA); final double t4 = sqrtRec * (e * sinI * cosPA * sqrtRec - vz * sqrtAoMu); final Vector3D s = new Vector3D(cosE / r, Vector3D.PLUS_K, s1, vectorEAnR, s2, position, s3, vectorAR); final Vector3D sDot = new Vector3D(-sinE * sqrtAoMu, Vector3D.PLUS_K, s1, vectorEAnRDot, s3, vectorARDot); final Vector3D t = new Vector3D(sqrtRec * sinE / r, Vector3D.PLUS_K) .add(new Vector3D(t1, vectorEAnR, t2, position, t3, vectorAR, t4, vectorER)); final Vector3D tDot = new Vector3D(sqrtRec * (cosE - e) * sqrtAoMu, Vector3D.PLUS_K, t1, vectorEAnRDot, t3, vectorARDot, t4, vectorERDot); // di final double factorI1 = -sinI * sqrtRec / sqrtMuA; final double i1 = factorI1; final double i2 = -factorI1 * mz / twoA; final double i3 = factorI1 * mz * e / oMe2; final double i4 = cosI * sinPA; final double i5 = cosI * cosPA; fillHalfRow(i1, new Vector3D(vy, -vx, 0), i2, vectorAR, i3, vectorER, i4, s, i5, t, jacobian[2], 0); fillHalfRow(i1, new Vector3D(-py, px, 0), i2, vectorARDot, i3, vectorERDot, i4, sDot, i5, tDot, jacobian[2], 3); // dpa fillHalfRow(cosPA / sinI, s, -sinPA / sinI, t, jacobian[3], 0); fillHalfRow(cosPA / sinI, sDot, -sinPA / sinI, tDot, jacobian[3], 3); // dRaan final double factorRaanR = 1 / (mu * a * oMe2 * sinI * sinI); fillHalfRow(-factorRaanR * my, new Vector3D(0, vz, -vy), factorRaanR * mx, new Vector3D(-vz, 0, vx), jacobian[4], 0); fillHalfRow(-factorRaanR * my, new Vector3D(0, -pz, py), factorRaanR * mx, new Vector3D(pz, 0, -px), jacobian[4], 3); // dM fillHalfRow(rOnA, vectorEAnR, -sinE, vectorER, jacobian[5], 0); fillHalfRow(rOnA, vectorEAnRDot, -sinE, vectorERDot, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Compute the Jacobian of the orbital parameters with respect to the cartesian parameters. * <p>// w w w .j a va 2s.c om * Element {@code jacobian[i][j]} is the derivative of parameter i of the orbit with * respect to cartesian coordinate j (x for j=0, y for j=1, z for j=2, xDot for j=3, * yDot for j=4, zDot for j=5). * </p> * @return 6x6 Jacobian matrix */ private double[][] computeJacobianMeanWrtCartesianHyperbolic() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final Vector3D momentum = pvc.getMomentum(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double mx = momentum.getX(); final double my = momentum.getY(); final double mz = momentum.getZ(); final double mu = getMu(); final double absA = -a; final double sqrtMuA = FastMath.sqrt(absA * mu); final double a2 = a * a; final double rOa = r / absA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double pv = Vector3D.dotProduct(position, velocity); // da final Vector3D vectorAR = new Vector3D(-2 * a2 / r3, position); final Vector3D vectorARDot = velocity.scalarMultiply(-2 * a2 / mu); fillHalfRow(-1, vectorAR, jacobian[0], 0); fillHalfRow(-1, vectorARDot, jacobian[0], 3); // differentials of the momentum final double m = momentum.getNorm(); final double oOm = 1 / m; final Vector3D dcXP = new Vector3D(0, vz, -vy); final Vector3D dcYP = new Vector3D(-vz, 0, vx); final Vector3D dcZP = new Vector3D(vy, -vx, 0); final Vector3D dcXV = new Vector3D(0, -z, y); final Vector3D dcYV = new Vector3D(z, 0, -x); final Vector3D dcZV = new Vector3D(-y, x, 0); final Vector3D dCP = new Vector3D(mx * oOm, dcXP, my * oOm, dcYP, mz * oOm, dcZP); final Vector3D dCV = new Vector3D(mx * oOm, dcXV, my * oOm, dcYV, mz * oOm, dcZV); // dp final double mOMu = m / mu; final Vector3D dpP = new Vector3D(2 * mOMu, dCP); final Vector3D dpV = new Vector3D(2 * mOMu, dCV); // de final double p = m * mOMu; final double moO2ae = 1 / (2 * absA * e); final double m2OaMu = -p / absA; fillHalfRow(moO2ae, dpP, m2OaMu * moO2ae, vectorAR, jacobian[1], 0); fillHalfRow(moO2ae, dpV, m2OaMu * moO2ae, vectorARDot, jacobian[1], 3); // di final double cI1 = 1 / (m * sinI); final double cI2 = cosI * cI1; fillHalfRow(cI2, dCP, -cI1, dcZP, jacobian[2], 0); fillHalfRow(cI2, dCV, -cI1, dcZV, jacobian[2], 3); // dPA final double cP1 = y * oOm; final double cP2 = -x * oOm; final double cP3 = -(mx * cP1 + my * cP2); final double cP4 = cP3 * oOm; final double cP5 = -1 / (r2 * sinI * sinI); final double cP6 = z * cP5; final double cP7 = cP3 * cP5; final Vector3D dacP = new Vector3D(cP1, dcXP, cP2, dcYP, cP4, dCP, oOm, new Vector3D(-my, mx, 0)); final Vector3D dacV = new Vector3D(cP1, dcXV, cP2, dcYV, cP4, dCV); final Vector3D dpoP = new Vector3D(cP6, dacP, cP7, Vector3D.PLUS_K); final Vector3D dpoV = new Vector3D(cP6, dacV); final double re2 = r2 * e * e; final double recOre2 = (p - r) / re2; final double resOre2 = (pv * mOMu) / re2; final Vector3D dreP = new Vector3D(mOMu, velocity, pv / mu, dCP); final Vector3D dreV = new Vector3D(mOMu, position, pv / mu, dCV); final Vector3D davP = new Vector3D(-resOre2, dpP, recOre2, dreP, resOre2 / r, position); final Vector3D davV = new Vector3D(-resOre2, dpV, recOre2, dreV); fillHalfRow(1, dpoP, -1, davP, jacobian[3], 0); fillHalfRow(1, dpoV, -1, davV, jacobian[3], 3); // dRAAN final double cO0 = cI1 * cI1; final double cO1 = mx * cO0; final double cO2 = -my * cO0; fillHalfRow(cO1, dcYP, cO2, dcXP, jacobian[4], 0); fillHalfRow(cO1, dcYV, cO2, dcXV, jacobian[4], 3); // dM final double s2a = pv / (2 * absA); final double oObux = 1 / FastMath.sqrt(m * m + mu * absA); final double scasbu = pv * oObux; final Vector3D dauP = new Vector3D(1 / sqrtMuA, velocity, -s2a / sqrtMuA, vectorAR); final Vector3D dauV = new Vector3D(1 / sqrtMuA, position, -s2a / sqrtMuA, vectorARDot); final Vector3D dbuP = new Vector3D(oObux * mu / 2, vectorAR, m * oObux, dCP); final Vector3D dbuV = new Vector3D(oObux * mu / 2, vectorARDot, m * oObux, dCV); final Vector3D dcuP = new Vector3D(oObux, velocity, -scasbu * oObux, dbuP); final Vector3D dcuV = new Vector3D(oObux, position, -scasbu * oObux, dbuV); fillHalfRow(1, dauP, -e / (1 + rOa), dcuP, jacobian[5], 0); fillHalfRow(1, dauV, -e / (1 + rOa), dcuV, jacobian[5], 3); return jacobian; }
From source file:org.rhwlab.dispim.nucleus.NamedNucleusFile.java
License:asdf
static public void nameChildren(Nucleus nuc) { Nucleus last = LinkedNucleusFile.lastNucleusInCell(nuc); if (last.isLeaf()) return; // no children Division div = divisionMap.get(last.getCellName()); if (div != null) { if (R != null) { Nucleus c1 = last.getChild1(); Nucleus c2 = last.getChild2(); Vector3D direction = divisionDirection(c1, c2); double d0 = new Vector3D(R.operate(direction.toArray())).dotProduct(div.getV()); double d1 = new Vector3D(R.operate(direction.scalarMultiply(-1.0).toArray())) .dotProduct(div.getV()); if (d0 < d1) { Nucleus parent = c1.getParent(); parent.setDaughters(c2, c1); //swap the daughters }//from www . j a v a2 s . co m } Nucleus.nameCellRecursive(last.getChild1(), div.child1, false); Nucleus.nameCellRecursive(last.getChild2(), div.child2, false); } else { Nucleus.nameCellRecursive(last.getChild1(), null, false); Nucleus.nameCellRecursive(last.getChild2(), null, false); } // rename nuclei in the subtrees nameChildren(last.getChild1()); nameChildren(last.getChild2()); }