List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D normalize
public Vector3D normalize() throws MathArithmeticException
From source file:org.orekit.propagation.analytical.AdapterPropagatorTest.java
private BoundedPropagator getEphemeris(final Orbit orbit, final double mass, final int nbOrbits, final AttitudeProvider law, final AbsoluteDate t0, final Vector3D dV, final double f, final double isp, final boolean sunAttraction, final boolean moonAttraction, final NormalizedSphericalHarmonicsProvider gravityField) throws OrekitException, ParseException, IOException { SpacecraftState initialState = new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass); // add some dummy additional states initialState = initialState.addAdditionalState("dummy 1", 1.25, 2.5); initialState = initialState.addAdditionalState("dummy 2", 5.0); // set up numerical propagator final double dP = 1.0; double[][] tolerances = NumericalPropagator.tolerances(dP, orbit, orbit.getType()); AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 1000, tolerances[0], tolerances[1]);/*from www . j a v a2s . co m*/ integrator.setInitialStepSize(orbit.getKeplerianPeriod() / 100.0); final NumericalPropagator propagator = new NumericalPropagator(integrator); propagator.addAdditionalStateProvider(new AdditionalStateProvider() { public String getName() { return "dummy 2"; } public double[] getAdditionalState(SpacecraftState state) { return new double[] { 5.0 }; } }); propagator.setInitialState(initialState); propagator.setAttitudeProvider(law); if (dV.getNorm() > 1.0e-6) { // set up maneuver final double vExhaust = Constants.G0_STANDARD_GRAVITY * isp; final double dt = -(mass * vExhaust / f) * FastMath.expm1(-dV.getNorm() / vExhaust); final ConstantThrustManeuver maneuver = new ConstantThrustManeuver(t0.shiftedBy(-0.5 * dt), dt, f, isp, dV.normalize()); propagator.addForceModel(maneuver); } if (sunAttraction) { propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun())); } if (moonAttraction) { propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon())); } if (gravityField != null) { propagator.addForceModel( new HolmesFeatherstoneAttractionModel(FramesFactory.getGTOD(false), gravityField)); } propagator.setEphemerisMode(); propagator.propagate(t0.shiftedBy(nbOrbits * orbit.getKeplerianPeriod())); final BoundedPropagator ephemeris = propagator.getGeneratedEphemeris(); // both the initial propagator and generated ephemeris manage one of the two // additional states, but they also contain unmanaged copies of the other one Assert.assertFalse(propagator.isAdditionalStateManaged("dummy 1")); Assert.assertTrue(propagator.isAdditionalStateManaged("dummy 2")); Assert.assertFalse(ephemeris.isAdditionalStateManaged("dummy 1")); Assert.assertTrue(ephemeris.isAdditionalStateManaged("dummy 2")); Assert.assertEquals(2, ephemeris.getInitialState().getAdditionalState("dummy 1").length); Assert.assertEquals(1, ephemeris.getInitialState().getAdditionalState("dummy 2").length); return ephemeris; }
From source file:org.orekit.propagation.semianalytical.dsst.forces.DSSTThirdBody.java
/** {@inheritDoc} */ @Override//from ww w . j a va2 s . c o 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(); Vector3D b = B.normalize();/* w ww. jav a 2s . co m*/ 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:services.SimulationService.java
public Ray convertRayToModelSpace(Point3D origin, Point3D end, SWGObject object) { Point3D position = object.getPosition(); WB_M44 translateMatrix = new WB_M44(1, 0, 0, position.x, 0, 1, 0, position.y, 0, 0, 1, position.z, 0, 0, 0, 1);//from ww w . ja v a 2 s . co m float radians = object.getRadians(); float sin = (float) Math.sin(radians); float cos = (float) Math.cos(radians); WB_M44 rotationMatrix = new WB_M44(cos, 0, sin, 0, 0, 1, 0, 0, -sin, 0, cos, 0, 0, 0, 0, 1); WB_M44 modelSpace = null; try { modelSpace = translateMatrix.mult(rotationMatrix).inverse(); } catch (Exception ex) { // It's usually a bank terminal causing this //System.out.println("The object " + object.getTemplate() + " at x:" + object.getWorldPosition().x + " z:" + object.getWorldPosition().z + " causes a problem during modelspaceconversion. Can be safely ignored."); if (modelSpace == null) return new Ray(origin, new Vector3D(0, 0, 0)); } float originX = (float) (modelSpace.m11 * origin.x + modelSpace.m12 * origin.y + modelSpace.m13 * origin.z + modelSpace.m14); float originY = (float) (modelSpace.m21 * origin.x + modelSpace.m22 * origin.y + modelSpace.m23 * origin.z + modelSpace.m24); float originZ = (float) (modelSpace.m31 * origin.x + modelSpace.m32 * origin.y + modelSpace.m33 * origin.z + modelSpace.m34); origin = new Point3D(originX, originY, originZ); float endX = (float) (modelSpace.m11 * end.x + modelSpace.m12 * end.y + modelSpace.m13 * end.z + modelSpace.m14); float endY = (float) (modelSpace.m21 * end.x + modelSpace.m22 * end.y + modelSpace.m23 * end.z + modelSpace.m24); float endZ = (float) (modelSpace.m31 * end.x + modelSpace.m32 * end.y + modelSpace.m33 * end.z + modelSpace.m34); end = new Point3D(endX, endY, endZ); Vector3D direction = new Vector3D(end.x - origin.x, end.y - origin.y, end.z - origin.z); if (direction.getX() > 0 && direction.getY() > 0 && direction.getZ() > 0) direction.normalize(); return new Ray(origin, direction); }
From source file:services.SimulationService.java
public boolean checkLineOfSightInBuilding(SWGObject obj1, SWGObject obj2, SWGObject building) { PortalVisitor portalVisitor = building.getPortalVisitor(); if (portalVisitor == null) return true; Point3D position1 = obj1.getPosition(); Point3D position2 = obj2.getPosition(); Point3D origin = new Point3D(position1.x, position1.y + 1, position1.z); Point3D end = new Point3D(position2.x, position2.y + 1, position2.z); Vector3D direction = new Vector3D(end.x - origin.x, end.y - origin.y, end.z - origin.z); if (direction.getNorm() != 0) { direction.normalize(); } else {//w w w . j a v a 2s .c om System.out.println("WARNING: checkLineOfSightInBuilding: Vector norm was 0."); } float distance = position1.getDistance2D(position2); Ray ray = new Ray(origin, direction); for (int i = 1; i < portalVisitor.cells.size(); i++) { Cell cell = portalVisitor.cells.get(i); try { MeshVisitor meshVisitor; if (!cellMeshes.containsKey(cell.mesh)) { meshVisitor = ClientFileManager.loadFile(cell.mesh, MeshVisitor.class); meshVisitor.getTriangles(); cellMeshes.put(cell.mesh, meshVisitor); } else { meshVisitor = cellMeshes.get(cell.mesh); } if (meshVisitor == null) continue; List<Mesh3DTriangle> tris = meshVisitor.getTriangles(); if (tris.isEmpty()) continue; for (Mesh3DTriangle tri : tris) { if (ray.intersectsTriangle(tri, distance) != null) { // System.out.println("Collision with: " + cell.name); return false; } } } catch (InstantiationException | IllegalAccessException e) { e.printStackTrace(); } } return true; }