List of usage examples for org.apache.commons.math3.util MathArrays linearCombination
public static double linearCombination(final double a1, final double b1, final double a2, final double b2, final double a3, final double b3, final double a4, final double b4)
From source file:de.tuberlin.uebb.jbop.example.DSCompilerOnlyCompose.java
@Override public void linearCombination(final double a1, final double[] c1, final double a2, final double[] c2, final double a3, final double[] c3, final double a4, final double[] c4, final double[] result) { for (int i = 0; i < sizes[parameters][order]; ++i) { result[i] = MathArrays.linearCombination(a1, c1[i], a2, c2[i], a3, c3[i], a4, c4[i]); }/* ww w .j av a 2 s . com*/ }
From source file:de.tuberlin.uebb.jbop.example.DSCompiler.java
@Override @Optimizable//from www .ja v a2s . c o m @StrictLoops public void linearCombination(final double a1, final double[] c1, final double a2, final double[] c2, final double a3, final double[] c3, final double a4, final double[] c4, final double[] result) { for (int i = 0; i < sizes[parameters][order]; ++i) { result[i] = MathArrays.linearCombination(a1, c1[i], a2, c2[i], a3, c3[i], a4, c4[i]); } }
From source file:org.orekit.bodies.EllipsoidTest.java
private double errorOnEllipsoid(Ellipse ps, Ellipsoid ellipsoid) { double max = 0; for (double theta = 0; theta < 2 * FastMath.PI; theta += 0.1) { Vector3D p = ps.pointAt(theta); double xOa = p.getX() / ellipsoid.getA(); double yOb = p.getY() / ellipsoid.getB(); double zOc = p.getZ() / ellipsoid.getC(); max = FastMath.max(max,//from w w w . j a v a2 s . c o m FastMath.abs(MathArrays.linearCombination(xOa, xOa, yOb, yOb, zOc, zOc, 1, -1))); } return max; }
From source file:org.orekit.utils.AngularCoordinates.java
/** Builds a AngularCoordinates from a {@link FieldRotation}<{@link DerivativeStructure}>. * <p>//w ww . j a v a2 s.c o m * The rotation components must have time as their only derivation parameter and * have consistent derivation orders. * </p> * @param r rotation with time-derivatives embedded within the coordinates */ public AngularCoordinates(final FieldRotation<DerivativeStructure> r) { final double q0 = r.getQ0().getReal(); final double q1 = r.getQ1().getReal(); final double q2 = r.getQ2().getReal(); final double q3 = r.getQ3().getReal(); rotation = new Rotation(q0, q1, q2, q3, false); if (r.getQ0().getOrder() >= 1) { final double q0Dot = r.getQ0().getPartialDerivative(1); final double q1Dot = r.getQ1().getPartialDerivative(1); final double q2Dot = r.getQ2().getPartialDerivative(1); final double q3Dot = r.getQ3().getPartialDerivative(1); rotationRate = new Vector3D( 2 * MathArrays.linearCombination(-q1, q0Dot, q0, q1Dot, q3, q2Dot, -q2, q3Dot), 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot, q0, q2Dot, q1, q3Dot), 2 * MathArrays.linearCombination(-q3, q0Dot, q2, q1Dot, -q1, q2Dot, q0, q3Dot)); if (r.getQ0().getOrder() >= 2) { final double q0DotDot = r.getQ0().getPartialDerivative(2); final double q1DotDot = r.getQ1().getPartialDerivative(2); final double q2DotDot = r.getQ2().getPartialDerivative(2); final double q3DotDot = r.getQ3().getPartialDerivative(2); rotationAcceleration = new Vector3D( 2 * MathArrays.linearCombination(-q1, q0DotDot, q0, q1DotDot, q3, q2DotDot, -q2, q3DotDot), 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot, q0, q2DotDot, q1, q3DotDot), 2 * MathArrays.linearCombination(-q3, q0DotDot, q2, q1DotDot, -q1, q2DotDot, q0, q3DotDot)); } else { rotationAcceleration = Vector3D.ZERO; } } else { rotationRate = Vector3D.ZERO; rotationAcceleration = Vector3D.ZERO; } }
From source file:org.orekit.utils.TimeStampedAngularCoordinates.java
/** Interpolate angular coordinates. * <p>// w w w . j a va2 s . c o m * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation. * </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 rotations * when this singularity is detected. * </p> * <p> * Note that even if first and second time derivatives (rotation rates and acceleration) * 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 rotations. * </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 * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedAngularCoordinates> sample) throws OrekitException { // 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 Vector3D meanRate; if (filter != AngularDerivativesFilter.USE_R) { Vector3D sum = Vector3D.ZERO; for (final TimeStampedAngularCoordinates datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new Vector3D(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } Vector3D sum = Vector3D.ZERO; TimeStampedAngularCoordinates previous = null; for (final TimeStampedAngularCoordinates datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.date))); } previous = datedAC; } meanRate = new Vector3D(1.0 / (sample.size() - 1), sum); } TimeStampedAngularCoordinates offset = new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.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 HermiteInterpolator interpolator = new HermiteInterpolator(); // add sample points double sign = +1.0; Rotation previous = Rotation.IDENTITY; for (final TimeStampedAngularCoordinates ac : sample) { // remove linear offset from the current coordinates final double dt = ac.date.durationFrom(date); final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt)); // make sure all interpolated points will be on the same branch final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), fixed.getRotation().getQ1(), previous.getQ1(), fixed.getRotation().getQ2(), previous.getQ2(), fixed.getRotation().getQ3(), previous.getQ3()); sign = FastMath.copySign(1.0, dot * sign); previous = fixed.getRotation(); // check modified Rodrigues vector singularity if (fixed.getRotation().getQ0() * sign < threshold) { // 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; } final double[][] rodrigues = fixed.getModifiedRodrigues(sign); 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 AngularCoordinates(new Rotation(Vector3D.PLUS_I, epsilon), Vector3D.ZERO, Vector3D.ZERO)); } else { // interpolation succeeded with the current offset final DerivativeStructure zero = new DerivativeStructure(1, 2, 0, 0.0); final DerivativeStructure[] p = interpolator.value(zero); final AngularCoordinates ac = createFromModifiedRodrigues( new double[][] { { p[0].getValue(), p[1].getValue(), p[2].getValue() }, { p[0].getPartialDerivative(1), p[1].getPartialDerivative(1), p[2].getPartialDerivative(1) }, { p[0].getPartialDerivative(2), p[1].getPartialDerivative(2), p[2].getPartialDerivative(2) } }); return new TimeStampedAngularCoordinates(offset.getDate(), ac.getRotation(), ac.getRotationRate(), ac.getRotationAcceleration()).addOffset(offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives. * <p>// www . ja v a 2 s . c om * The modified Rodrigues vector is tan(/4) u where and u are the * rotation angle and axis respectively. * </p> * @param fixed coordinates to convert, with offset already fixed * @param previous previous quaternion used * @param threshold threshold for rotations too close to 2 * @param <T> the type of the field elements * @return modified Rodrigues vector and derivative, or null if rotation is too close to 2 */ private static <T extends RealFieldElement<T>> T[][] getModifiedRodrigues( final TimeStampedFieldAngularCoordinates<T> fixed, final double[] previous, final double threshold) { // make sure all interpolated points will be on the same branch T q0 = fixed.getRotation().getQ0(); T q1 = fixed.getRotation().getQ1(); T q2 = fixed.getRotation().getQ2(); T q3 = fixed.getRotation().getQ3(); if (MathArrays.linearCombination(q0.getReal(), previous[0], q1.getReal(), previous[1], q2.getReal(), previous[2], q3.getReal(), previous[3]) < 0) { q0 = q0.negate(); q1 = q1.negate(); q2 = q2.negate(); q3 = q3.negate(); } previous[0] = q0.getReal(); previous[1] = q1.getReal(); previous[2] = q2.getReal(); previous[3] = q3.getReal(); // check modified Rodrigues vector singularity if (q0.getReal() < threshold) { // this is an intermediate point that happens to be 2PI away from reference // we need to change the linear offset model to avoid this point return null; } final Field<T> field = q0.getField(); final T oX = fixed.getRotationRate().getX(); final T oY = fixed.getRotationRate().getY(); final T oZ = fixed.getRotationRate().getZ(); final T oXDot = fixed.getRotationAcceleration().getX(); final T oYDot = fixed.getRotationAcceleration().getY(); final T oZDot = fixed.getRotationAcceleration().getZ(); // first time-derivatives of the quaternion final T q0Dot = q0.linearCombination(q1.negate(), oX, q2.negate(), oY, q3.negate(), oZ).multiply(0.5); final T q1Dot = q1.linearCombination(q0, oX, q3.negate(), oY, q2, oZ).multiply(0.5); final T q2Dot = q2.linearCombination(q3, oX, q0, oY, q1.negate(), oZ).multiply(0.5); final T q3Dot = q3.linearCombination(q2.negate(), oX, q1, oY, q0, oZ).multiply(0.5); // second time-derivatives of the quaternion final T q0DotDot = q0.linearCombination(array6(field, q1, q2, q3, q1Dot, q2Dot, q3Dot), array6(field, oXDot, oYDot, oZDot, oX, oY, oZ)).multiply(-0.5); final T q1DotDot = q1.linearCombination(array6(field, q0, q2, q3.negate(), q0Dot, q2Dot, q3Dot.negate()), array6(field, oXDot, oZDot, oYDot, oX, oZ, oY)).multiply(0.5); final T q2DotDot = q2.linearCombination(array6(field, q0, q3, q1.negate(), q0Dot, q3Dot, q1Dot.negate()), array6(field, oYDot, oXDot, oZDot, oY, oX, oZ)).multiply(0.5); final T q3DotDot = q3.linearCombination(array6(field, q0, q1, q2.negate(), q0Dot, q1Dot, q2Dot.negate()), array6(field, oZDot, oYDot, oXDot, oZ, oY, oX)).multiply(0.5); // the modified Rodrigues is tan(/4) u where and u are the rotation angle and axis respectively // this can be rewritten using quaternion components: // r (q? / (1+q), q / (1+q), q / (1+q)) // applying the derivation chain rule to previous expression gives rDot and rDotDot final T inv = q0.add(1.0).reciprocal(); final T mTwoInvQ0Dot = inv.multiply(q0Dot).multiply(-2); final T r1 = inv.multiply(q1); final T r2 = inv.multiply(q2); final T r3 = inv.multiply(q3); final T mInvR1 = inv.multiply(r1).negate(); final T mInvR2 = inv.multiply(r2).negate(); final T mInvR3 = inv.multiply(r3).negate(); final T r1Dot = r1.linearCombination(inv, q1Dot, mInvR1, q0Dot); final T r2Dot = r2.linearCombination(inv, q2Dot, mInvR2, q0Dot); final T r3Dot = r3.linearCombination(inv, q3Dot, mInvR3, q0Dot); final T r1DotDot = r1.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot); final T r2DotDot = r2.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot); final T r3DotDot = r3.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot); return matrix33(field, r1, r2, r3, r1Dot, r2Dot, r3Dot, r1DotDot, r2DotDot, r3DotDot); }