List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldVector3D add
public FieldVector3D<T> add(final Vector3D v)
From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> radiationPressureAcceleration(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass, final FieldVector3D<DerivativeStructure> flux) throws OrekitException { if (flux.getNormSq().getValue() < Precision.SAFE_MIN) { // null illumination (we are probably in umbra) return new FieldVector3D<DerivativeStructure>(0.0, flux); }// ww w . j ava2 s. c o m // radiation flux in spacecraft frame final FieldVector3D<DerivativeStructure> fluxSat = rotation.applyTo(flux); // solar array contribution FieldVector3D<DerivativeStructure> normal = getNormal(date, frame, position, rotation); DerivativeStructure dot = FieldVector3D.dotProduct(normal, fluxSat); if (dot.getValue() > 0) { // the solar array is illuminated backward, // fix signs to compute contribution correctly dot = dot.negate(); normal = normal.negate(); } FieldVector3D<DerivativeStructure> force = facetRadiationAcceleration(normal, solarArrayArea, fluxSat, dot); // body facets contribution for (final Facet bodyFacet : facets) { normal = new FieldVector3D<DerivativeStructure>(mass.getField().getOne(), bodyFacet.getNormal()); dot = FieldVector3D.dotProduct(normal, fluxSat); if (dot.getValue() < 0) { // the facet intercepts the incoming flux force = force.add(facetRadiationAcceleration(normal, bodyFacet.getArea(), fluxSat, dot)); } } // convert to inertial frame return rotation.applyInverseTo(new FieldVector3D<DerivativeStructure>(mass.reciprocal(), force)); }
From source file:org.orekit.forces.BoxAndSolarArraySpacecraft.java
/** {@inheritDoc} */ public FieldVector3D<DerivativeStructure> radiationPressureAcceleration(final AbsoluteDate date, final Frame frame, final Vector3D position, final Rotation rotation, final double mass, final Vector3D flux, final String paramName) throws OrekitException { if (flux.getNormSq() < Precision.SAFE_MIN) { // null illumination (we are probably in umbra) final DerivativeStructure zero = new DerivativeStructure(1, 1, 0.0); return new FieldVector3D<DerivativeStructure>(zero, zero, zero); }/*from w w w. j av a 2 s. c om*/ final DerivativeStructure absorptionCoeffDS; final DerivativeStructure specularReflectionCoeffDS; if (ABSORPTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, 0, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, specularReflectionCoeff); } else if (REFLECTION_COEFFICIENT.equals(paramName)) { absorptionCoeffDS = new DerivativeStructure(1, 1, absorptionCoeff); specularReflectionCoeffDS = new DerivativeStructure(1, 1, 0, specularReflectionCoeff); } else { throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, paramName, ABSORPTION_COEFFICIENT + ", " + REFLECTION_COEFFICIENT); } final DerivativeStructure diffuseReflectionCoeffDS = absorptionCoeffDS.add(specularReflectionCoeffDS) .subtract(1).negate(); // radiation flux in spacecraft frame final Vector3D fluxSat = rotation.applyTo(flux); // solar array contribution Vector3D normal = getNormal(date, frame, position, rotation); double dot = Vector3D.dotProduct(normal, fluxSat); if (dot > 0) { // the solar array is illuminated backward, // fix signs to compute contribution correctly dot = -dot; normal = normal.negate(); } FieldVector3D<DerivativeStructure> force = facetRadiationAcceleration(normal, solarArrayArea, fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS); // body facets contribution for (final Facet bodyFacet : facets) { normal = bodyFacet.getNormal(); dot = Vector3D.dotProduct(normal, fluxSat); if (dot < 0) { // the facet intercepts the incoming flux force = force.add(facetRadiationAcceleration(normal, bodyFacet.getArea(), fluxSat, dot, specularReflectionCoeffDS, diffuseReflectionCoeffDS)); } } // convert to inertial return FieldRotation.applyInverseTo(rotation, new FieldVector3D<DerivativeStructure>(1.0 / mass, force)); }
From source file:org.orekit.frames.Transform.java
/** Transform a position vector (including translation effects). * @param position vector to transform// w w w. j a v a 2 s . com * @param <T> the type of the field elements * @return transformed position */ public <T extends RealFieldElement<T>> FieldVector3D<T> transformPosition(final FieldVector3D<T> position) { return FieldRotation.applyTo(angular.getRotation(), position.add(cartesian.getPosition())); }
From source file:org.orekit.utils.FieldAngularCoordinatesTest.java
@Test public void testSpin() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); FieldAngularCoordinates<DerivativeStructure> angularCoordinates = new FieldAngularCoordinates<DerivativeStructure>( createRotation(0.48, 0.64, 0.36, 0.48, false), new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4)); Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10); double dt = 10.0; FieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt); Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10); Assert.assertEquals(rate * dt,//from w ww. ja v a 2 s . c o m FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation() .applyInverseTo(createVector(1, 0, 0, 4)); FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation() .applyInverseTo(createVector(0, 1, 0, 4)); FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation() .applyInverseTo(createVector(0, 0, 1, 4)); FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation() .applyInverseTo(createVector(1, 0, 0, 4)); FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation() .applyInverseTo(createVector(0, 1, 0, 4)); FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation() .applyInverseTo(createVector(0, 0, 1, 4)); Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10); Assert.assertEquals(FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10); Assert.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10); Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10); Assert.assertEquals(1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates .estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt); Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt); Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Interpolate angular coordinates. * <p>// www .j ava 2 s . c o m * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>. * </p> * <p> * This method is based on Sergei Tanygin's paper <a * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues * vector as described in Malcolm D. Shuster's paper <a * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A * Survey of Attitude Representations</a>. This change avoids the singularity at . * There is still a singularity at 2, which is handled by slightly offsetting all FieldRotation<T>s * when this singularity is detected. * </p> * <p> * Note that even if first time derivatives (FieldRotation<T> rates) * from sample can be ignored, the interpolated instance always includes * interpolated derivatives. This feature can be used explicitly to * compute these derivatives when it would be too complex to compute them * from an analytical formula: just compute a few sample points from the * explicit formula and set the derivatives to zero in these sample points, * then use interpolation to add derivatives consistent with the FieldRotation<T>s. * </p> * @param date interpolation date * @param filter filter for derivatives from the sample to use in interpolation * @param sample sample points on which interpolation should be done * @param <T> the type of the field elements * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ @SuppressWarnings("unchecked") public static <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> interpolate( final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedFieldAngularCoordinates<T>> sample) throws OrekitException { // get field properties final Field<T> field = sample.iterator().next().getRotation().getQ0().getField(); final T zero = field.getZero(); final T one = field.getOne(); // set up safety elements for 2 singularity avoidance final double epsilon = 2 * FastMath.PI / sample.size(); final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4)); // set up a linear model canceling mean rotation rate final FieldVector3D<T> meanRate; if (filter != AngularDerivativesFilter.USE_R) { FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new FieldVector3D<T>(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); TimeStampedFieldAngularCoordinates<T> previous = null; for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.getDate()))); } previous = datedAC; } meanRate = new FieldVector3D<T>(1.0 / (sample.size() - 1), sum); } TimeStampedFieldAngularCoordinates<T> offset = new TimeStampedFieldAngularCoordinates<T>(date, new FieldRotation<T>(one, zero, zero, zero, false), meanRate, new FieldVector3D<T>(zero, zero, zero)); boolean restart = true; for (int i = 0; restart && i < sample.size() + 2; ++i) { // offset adaptation parameters restart = false; // set up an interpolator taking derivatives into account final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>(); // add sample points final double[] previous = new double[] { 1.0, 0.0, 0.0, 0.0 }; for (final TimeStampedFieldAngularCoordinates<T> ac : sample) { // remove linear offset from the current coordinates final T dt = zero.add(ac.date.durationFrom(date)); final TimeStampedFieldAngularCoordinates<T> fixed = ac .subtractOffset(offset.shiftedBy(dt.getReal())); final T[][] rodrigues = getModifiedRodrigues(fixed, previous, threshold); if (rodrigues == null) { // the sample point is close to a modified Rodrigues vector singularity // we need to change the linear offset model to avoid this restart = true; break; } switch (filter) { case USE_RRA: // populate sample with rotation, rotation rate and acceleration data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]); break; case USE_RR: // populate sample with rotation and rotation rate data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]); break; case USE_R: // populate sample with rotation data only interpolator.addSamplePoint(dt, rodrigues[0]); break; default: // this should never happen throw new OrekitInternalError(null); } } if (restart) { // interpolation failed, some intermediate rotation was too close to 2 // we need to offset all rotations to avoid the singularity offset = offset.addOffset(new FieldAngularCoordinates<T>( new FieldRotation<T>(new FieldVector3D<T>(one, zero, zero), zero.add(epsilon)), new FieldVector3D<T>(zero, zero, zero), new FieldVector3D<T>(zero, zero, zero))); } else { // interpolation succeeded with the current offset final T[][] p = interpolator.derivatives(field.getZero(), 2); return createFromModifiedRodrigues(p, offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinatesTest.java
@Test public void testSpin() throws OrekitException { double rate = 2 * FastMath.PI / (12 * 60); TimeStampedFieldAngularCoordinates<DerivativeStructure> angularCoordinates = new TimeStampedFieldAngularCoordinates<DerivativeStructure>( AbsoluteDate.J2000_EPOCH, createRotation(0.48, 0.64, 0.36, 0.48, false), new FieldVector3D<DerivativeStructure>(rate, createVector(0, 0, 1, 4)), createVector(0, 0, 0, 4)); Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm().getReal(), 1.0e-10); double dt = 10.0; TimeStampedFieldAngularCoordinates<DerivativeStructure> shifted = angularCoordinates.shiftedBy(dt); Assert.assertEquals(rate, shifted.getRotationRate().getNorm().getReal(), 1.0e-10); Assert.assertEquals(rate * dt,// w w w . j av a2 s .co m FieldRotation.distance(angularCoordinates.getRotation(), shifted.getRotation()).getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> shiftedX = shifted.getRotation() .applyInverseTo(createVector(1, 0, 0, 4)); FieldVector3D<DerivativeStructure> shiftedY = shifted.getRotation() .applyInverseTo(createVector(0, 1, 0, 4)); FieldVector3D<DerivativeStructure> shiftedZ = shifted.getRotation() .applyInverseTo(createVector(0, 0, 1, 4)); FieldVector3D<DerivativeStructure> originalX = angularCoordinates.getRotation() .applyInverseTo(createVector(1, 0, 0, 4)); FieldVector3D<DerivativeStructure> originalY = angularCoordinates.getRotation() .applyInverseTo(createVector(0, 1, 0, 4)); FieldVector3D<DerivativeStructure> originalZ = angularCoordinates.getRotation() .applyInverseTo(createVector(0, 0, 1, 4)); Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10); Assert.assertEquals(FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10); Assert.assertEquals(-FastMath.sin(rate * dt), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10); Assert.assertEquals(FastMath.cos(rate * dt), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10); Assert.assertEquals(0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10); Assert.assertEquals(1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> forward = FieldAngularCoordinates .estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt); Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10); FieldVector3D<DerivativeStructure> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt); Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm().getReal(), 1.0e-10); }