List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D crossProduct
public static Vector3D crossProduct(final Vector3D v1, final Vector3D v2)
From source file:org.orekit.utils.AngularCoordinates.java
/** Find a vector from two known cross products. * <p>//from w ww .j av a2s .c o m * We want to find such that: v? = c? and v = c * </p> * <p> * The first equation ( v? = c?) will always be fulfilled exactly, * and the second one will be fulfilled if possible. * </p> * @param v1 vector forming the first known cross product * @param c1 know vector for cross product v? * @param v2 vector forming the second known cross product * @param c2 know vector for cross product v * @param tolerance relative tolerance factor used to check singularities * @return vector such that: v? = c? and v = c * @exception MathIllegalArgumentException if vectors are inconsistent and * no solution can be found */ private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1, final Vector3D v2, final Vector3D c2, final double tolerance) throws MathIllegalArgumentException { final double v12 = v1.getNormSq(); final double v1n = FastMath.sqrt(v12); final double v22 = v2.getNormSq(); final double v2n = FastMath.sqrt(v22); final double threshold = tolerance * FastMath.max(v1n, v2n); Vector3D omega; try { // create the over-determined linear system representing the two cross products final RealMatrix m = MatrixUtils.createRealMatrix(6, 3); m.setEntry(0, 1, v1.getZ()); m.setEntry(0, 2, -v1.getY()); m.setEntry(1, 0, -v1.getZ()); m.setEntry(1, 2, v1.getX()); m.setEntry(2, 0, v1.getY()); m.setEntry(2, 1, -v1.getX()); m.setEntry(3, 1, v2.getZ()); m.setEntry(3, 2, -v2.getY()); m.setEntry(4, 0, -v2.getZ()); m.setEntry(4, 2, v2.getX()); m.setEntry(5, 0, v2.getY()); m.setEntry(5, 1, -v2.getX()); final RealVector rhs = MatrixUtils.createRealVector( new double[] { c1.getX(), c1.getY(), c1.getZ(), c2.getX(), c2.getY(), c2.getZ() }); // find the best solution we can final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver(); final RealVector v = solver.solve(rhs); omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2)); } catch (SingularMatrixException sme) { // handle some special cases for which we can compute a solution final double c12 = c1.getNormSq(); final double c1n = FastMath.sqrt(c12); final double c22 = c2.getNormSq(); final double c2n = FastMath.sqrt(c22); if (c1n <= threshold && c2n <= threshold) { // simple special case, velocities are cancelled return Vector3D.ZERO; } else if (v1n <= threshold && c1n >= threshold) { // this is inconsistent, if v? is zero, c? must be 0 too throw new NumberIsTooLargeException(c1n, 0, true); } else if (v2n <= threshold && c2n >= threshold) { // this is inconsistent, if v is zero, c must be 0 too throw new NumberIsTooLargeException(c2n, 0, true); } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) { // simple special case, v is redundant with v?, we just ignore it // use the simplest : orthogonal to both v? and c? omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1)); } else { throw sme; } } // check results final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1); if (d1 > threshold) { throw new NumberIsTooLargeException(d1, 0, true); } final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2); if (d2 > threshold) { throw new NumberIsTooLargeException(d2, 0, true); } return omega; }
From source file:org.orekit.utils.AngularCoordinates.java
/** Add an offset from the instance. * <p>/*from w w w. j ava 2 s.co m*/ * We consider here that the offset rotation is applied first and the * instance is applied afterward. Note that angular coordinates do <em>not</em> * commute under this operation, i.e. {@code a.addOffset(b)} and {@code * b.addOffset(a)} lead to <em>different</em> results in most cases. * </p> * <p> * The two methods {@link #addOffset(AngularCoordinates) addOffset} and * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed * so that round trip applications are possible. This means that both {@code * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1. * </p> * @param offset offset to subtract * @return new instance, with offset subtracted * @see #subtractOffset(AngularCoordinates) */ public AngularCoordinates addOffset(final AngularCoordinates offset) { final Vector3D rOmega = rotation.applyTo(offset.rotationRate); final Vector3D rOmegaDot = rotation.applyTo(offset.rotationAcceleration); return new AngularCoordinates(rotation.applyTo(offset.rotation), rotationRate.add(rOmega), new Vector3D(1.0, rotationAcceleration, 1.0, rOmegaDot, -1.0, Vector3D.crossProduct(rotationRate, rOmega))); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Apply the rotation to a pv coordinates. * @param pv vector to apply the rotation to * @return a new pv coordinates which is the image of u by the rotation *//* w w w. j a va2 s .co m*/ public PVCoordinates applyTo(final PVCoordinates pv) { final Vector3D transformedP = rotation.applyTo(pv.getPosition()); final Vector3D crossP = Vector3D.crossProduct(rotationRate, transformedP); final Vector3D transformedV = rotation.applyTo(pv.getVelocity()).subtract(crossP); final Vector3D crossV = Vector3D.crossProduct(rotationRate, transformedV); final Vector3D crossCrossP = Vector3D.crossProduct(rotationRate, crossP); final Vector3D crossDotP = Vector3D.crossProduct(rotationAcceleration, transformedP); final Vector3D transformedA = new Vector3D(1, rotation.applyTo(pv.getAcceleration()), -2, crossV, -1, crossCrossP, -1, crossDotP); return new PVCoordinates(transformedP, transformedV, transformedA); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Apply the rotation to a pv coordinates. * @param pv vector to apply the rotation to * @return a new pv coordinates which is the image of u by the rotation *//*from w w w. ja va2s .com*/ public TimeStampedPVCoordinates applyTo(final TimeStampedPVCoordinates pv) { final Vector3D transformedP = getRotation().applyTo(pv.getPosition()); final Vector3D crossP = Vector3D.crossProduct(getRotationRate(), transformedP); final Vector3D transformedV = getRotation().applyTo(pv.getVelocity()).subtract(crossP); final Vector3D crossV = Vector3D.crossProduct(getRotationRate(), transformedV); final Vector3D crossCrossP = Vector3D.crossProduct(getRotationRate(), crossP); final Vector3D crossDotP = Vector3D.crossProduct(getRotationAcceleration(), transformedP); final Vector3D transformedA = new Vector3D(1, getRotation().applyTo(pv.getAcceleration()), -2, crossV, -1, crossCrossP, -1, crossDotP); return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV, transformedA); }
From source file:org.orekit.utils.AngularCoordinatesTest.java
private void checkInverse(Vector3D omega, Vector3D v1, Vector3D v2) throws OrekitException { checkInverse(omega, v1, Vector3D.crossProduct(omega, v1), v2, Vector3D.crossProduct(omega, v2)); }
From source file:org.orekit.utils.PVCoordinates.java
/** Gets the momentum. * <p>This vector is the p ⊗ v where p is position, v is velocity * and ⊗ is cross product. To get the real physical angular momentum * you need to multiply this vector by the mass.</p> * <p>The returned vector is recomputed each time this method is called, it * is not cached.</p>/* w w w .j a v a 2 s . c om*/ * @return a new instance of the momentum vector (m/s). */ public Vector3D getMomentum() { return Vector3D.crossProduct(position, velocity); }
From source file:org.orekit.utils.PVCoordinates.java
/** Compute the cross-product of two instances. * @param pv1 first instances/*from w w w . ja v a2 s .c o m*/ * @param pv2 second instances * @return the cross product v1 ^ v2 as a new instance */ public static PVCoordinates crossProduct(final PVCoordinates pv1, final PVCoordinates pv2) { final Vector3D p1 = pv1.position; final Vector3D v1 = pv1.velocity; final Vector3D a1 = pv1.acceleration; final Vector3D p2 = pv2.position; final Vector3D v2 = pv2.velocity; final Vector3D a2 = pv2.acceleration; return new PVCoordinates(Vector3D.crossProduct(p1, p2), new Vector3D(1, Vector3D.crossProduct(p1, v2), 1, Vector3D.crossProduct(v1, p2)), new Vector3D(1, Vector3D.crossProduct(p1, a2), 2, Vector3D.crossProduct(v1, v2), 1, Vector3D.crossProduct(a1, p2))); }
From source file:org.orekit.utils.PVCoordinatesTest.java
@Test public void testCrossProduct() { RandomGenerator generator = new Well19937a(0x85c592b3be733d23l); FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 1.0e-3); for (int i = 0; i < 200; ++i) { final PVCoordinates pv1 = randomPVCoordinates(generator, 1.0, 1.0, 1.0); final PVCoordinates pv2 = randomPVCoordinates(generator, 1.0, 1.0, 1.0); DerivativeStructure x = differentiator.differentiate(new UnivariateFunction() { public double value(double t) { return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(), pv2.shiftedBy(t).getPosition()) .getX();/* w ww . j av a2 s.c o m*/ } }).value(new DerivativeStructure(1, 2, 0, 0.0)); DerivativeStructure y = differentiator.differentiate(new UnivariateFunction() { public double value(double t) { return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(), pv2.shiftedBy(t).getPosition()) .getY(); } }).value(new DerivativeStructure(1, 2, 0, 0.0)); DerivativeStructure z = differentiator.differentiate(new UnivariateFunction() { public double value(double t) { return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(), pv2.shiftedBy(t).getPosition()) .getZ(); } }).value(new DerivativeStructure(1, 2, 0, 0.0)); PVCoordinates product = PVCoordinates.crossProduct(pv1, pv2); Assert.assertEquals(x.getValue(), product.getPosition().getX(), 1.0e-16); Assert.assertEquals(y.getValue(), product.getPosition().getY(), 1.0e-16); Assert.assertEquals(z.getValue(), product.getPosition().getZ(), 1.0e-16); Assert.assertEquals(x.getPartialDerivative(1), product.getVelocity().getX(), 9.0e-10); Assert.assertEquals(y.getPartialDerivative(1), product.getVelocity().getY(), 9.0e-10); Assert.assertEquals(z.getPartialDerivative(1), product.getVelocity().getZ(), 9.0e-10); Assert.assertEquals(x.getPartialDerivative(2), product.getAcceleration().getX(), 3.0e-9); Assert.assertEquals(y.getPartialDerivative(2), product.getAcceleration().getY(), 3.0e-9); Assert.assertEquals(z.getPartialDerivative(2), product.getAcceleration().getZ(), 3.0e-9); } }
From source file:org.orekit.utils.TimeStampedAngularCoordinates.java
/** Add an offset from the instance. * <p>/* w w w . jav a 2s .com*/ * We consider here that the offset rotation is applied first and the * instance is applied afterward. Note that angular coordinates do <em>not</em> * commute under this operation, i.e. {@code a.addOffset(b)} and {@code * b.addOffset(a)} lead to <em>different</em> results in most cases. * </p> * <p> * The two methods {@link #addOffset(AngularCoordinates) addOffset} and * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed * so that round trip applications are possible. This means that both {@code * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1. * </p> * @param offset offset to subtract * @return new instance, with offset subtracted * @see #subtractOffset(AngularCoordinates) */ @Override public TimeStampedAngularCoordinates addOffset(final AngularCoordinates offset) { final Vector3D rOmega = getRotation().applyTo(offset.getRotationRate()); final Vector3D rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration()); return new TimeStampedAngularCoordinates(date, getRotation().applyTo(offset.getRotation()), getRotationRate().add(rOmega), new Vector3D(1.0, getRotationAcceleration(), 1.0, rOmegaDot, -1.0, Vector3D.crossProduct(getRotationRate(), rOmega))); }