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)
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[] result) { for (int i = 0; i < sizes[parameters][order]; ++i) { result[i] = MathArrays.linearCombination(a1, c1[i], a2, c2[i], a3, c3[i]); }/* ww w .jav a2 s .c o m*/ }
From source file:de.tuberlin.uebb.jbop.example.DSCompiler.java
@Override @Optimizable//from w w w.j av a2 s . com @StrictLoops public void linearCombination(final double a1, final double[] c1, final double a2, final double[] c2, final double a3, final double[] c3, 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]); } }
From source file:msi.gama.common.geometry.Rotation3D.java
/** * Build the rotation that transforms a pair of vectors into another pair. * /*from w ww .j a va 2s . com*/ * <p> * Except for possible scale factors, if the instance were applied to the pair (u<sub>1</sub>, u<sub>2</sub>) it * will produce the pair (v<sub>1</sub>, v<sub>2</sub>). * </p> * * <p> * If the angular separation between u<sub>1</sub> and u<sub>2</sub> is not the same as the angular separation * between v<sub>1</sub> and v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than v<sub>2</sub>, * the corrected vector will be in the (±v<sub>1</sub>, +v<sub>2</sub>) half-plane. * </p> * * @param u1 * first vector of the origin pair * @param u2 * second vector of the origin pair * @param v1 * desired image of u1 by the rotation * @param v2 * desired image of u2 by the rotation */ public Rotation3D(final GamaPoint originVector1, final GamaPoint originVector2, final GamaPoint desiredVector1, final GamaPoint desiredVector2) { GamaPoint u1 = originVector1; GamaPoint u2 = originVector2; GamaPoint v1 = desiredVector1; GamaPoint v2 = desiredVector2; // build orthonormalized base from u1, u2 // this fails when vectors are null or collinear, which is forbidden to define a rotation final GamaPoint u3 = u1.crossProduct(u2).normalized(); u2 = u3.crossProduct(u1).normalized(); u1 = u1.normalized(); // build an orthonormalized base from v1, v2 // this fails when vectors are null or collinear, which is forbidden to define a rotation final GamaPoint v3 = v1.crossProduct(v2).normalized(); v2 = v3.crossProduct(v1).normalized(); v1 = v1.normalized(); // buid a matrix transforming the first base into the second one final double[][] m = new double[][] { { MathArrays.linearCombination(u1.x, v1.x, u2.x, v2.x, u3.x, v3.x), MathArrays.linearCombination(u1.y, v1.x, u2.y, v2.x, u3.y, v3.x), MathArrays.linearCombination(u1.z, v1.x, u2.z, v2.x, u3.z, v3.x) }, { MathArrays.linearCombination(u1.x, v1.y, u2.x, v2.y, u3.x, v3.y), MathArrays.linearCombination(u1.y, v1.y, u2.y, v2.y, u3.y, v3.y), MathArrays.linearCombination(u1.z, v1.y, u2.z, v2.y, u3.z, v3.y) }, { MathArrays.linearCombination(u1.x, v1.z, u2.x, v2.z, u3.x, v3.z), MathArrays.linearCombination(u1.y, v1.z, u2.y, v2.z, u3.y, v3.z), MathArrays.linearCombination(u1.z, v1.z, u2.z, v2.z, u3.z, v3.z) } }; final double[] quat = mat2quat(m); q0 = quat[0]; q1 = quat[1]; q2 = quat[2]; q3 = quat[3]; }
From source file:org.orekit.attitudes.BodyCenterPointing.java
/** {@inheritDoc} */ @Override//ww w . ja v a 2s . c o m protected TimeStampedPVCoordinates getTargetPV(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException { // spacecraft coordinates in body frame final TimeStampedPVCoordinates scInBodyFrame = pvProv.getPVCoordinates(date, getBodyFrame()); // central projection to ground (NOT the classical nadir point) final double u = scInBodyFrame.getPosition().getX() / ellipsoid.getA(); final double v = scInBodyFrame.getPosition().getY() / ellipsoid.getB(); final double w = scInBodyFrame.getPosition().getZ() / ellipsoid.getC(); final double d2 = u * u + v * v + w * w; final double d = FastMath.sqrt(d2); final double ratio = 1.0 / d; final Vector3D projectedP = new Vector3D(ratio, scInBodyFrame.getPosition()); // velocity final double uDot = scInBodyFrame.getVelocity().getX() / ellipsoid.getA(); final double vDot = scInBodyFrame.getVelocity().getY() / ellipsoid.getB(); final double wDot = scInBodyFrame.getVelocity().getZ() / ellipsoid.getC(); final double dDot = MathArrays.linearCombination(u, uDot, v, vDot, w, wDot) / d; final double ratioDot = -dDot / d2; final Vector3D projectedV = new Vector3D(ratio, scInBodyFrame.getVelocity(), ratioDot, scInBodyFrame.getPosition()); // acceleration final double uDotDot = scInBodyFrame.getAcceleration().getX() / ellipsoid.getA(); final double vDotDot = scInBodyFrame.getAcceleration().getY() / ellipsoid.getB(); final double wDotDot = scInBodyFrame.getAcceleration().getZ() / ellipsoid.getC(); final double dDotDot = (MathArrays.linearCombination(u, uDotDot, v, vDotDot, w, wDotDot) + uDot * uDot + vDot * vDot + wDot * wDot - dDot * dDot) / d; final double ratioDotDot = (2 * dDot * dDot - d * dDotDot) / (d * d2); final Vector3D projectedA = new Vector3D(ratio, scInBodyFrame.getAcceleration(), 2 * ratioDot, scInBodyFrame.getVelocity(), ratioDotDot, scInBodyFrame.getPosition()); final TimeStampedPVCoordinates projected = new TimeStampedPVCoordinates(date, projectedP, projectedV, projectedA); return getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(projected); }
From source file:org.orekit.attitudes.BodyCenterPointingTest.java
@Test public void testQDot() throws OrekitException { Utils.setDataRoot("regular-data"); final double ehMu = 3.9860047e14; final double ae = 6.378137e6; final double c20 = -1.08263e-3; final double c30 = 2.54e-6; final double c40 = 1.62e-6; final double c50 = 2.3e-7; final double c60 = -5.5e-7; final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.); final Vector3D position = new Vector3D(3220103., 69623., 6449822.); final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.); final CircularOrbit initialOrbit = new CircularOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, ehMu); EcksteinHechlerPropagator propagator = new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);//from w w w .j ava 2s .c o m propagator.setAttitudeProvider(earthCenterAttitudeLaw); List<WeightedObservedPoint> w0 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w1 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w2 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w3 = new ArrayList<WeightedObservedPoint>(); for (double dt = -1; dt < 1; dt += 0.01) { Rotation rP = propagator.propagate(date.shiftedBy(dt)).getAttitude().getRotation(); w0.add(new WeightedObservedPoint(1, dt, rP.getQ0())); w1.add(new WeightedObservedPoint(1, dt, rP.getQ1())); w2.add(new WeightedObservedPoint(1, dt, rP.getQ2())); w3.add(new WeightedObservedPoint(1, dt, rP.getQ3())); } double q0DotRef = PolynomialCurveFitter.create(2).fit(w0)[1]; double q1DotRef = PolynomialCurveFitter.create(2).fit(w1)[1]; double q2DotRef = PolynomialCurveFitter.create(2).fit(w2)[1]; double q3DotRef = PolynomialCurveFitter.create(2).fit(w3)[1]; Attitude a0 = propagator.propagate(date).getAttitude(); double q0 = a0.getRotation().getQ0(); double q1 = a0.getRotation().getQ1(); double q2 = a0.getRotation().getQ2(); double q3 = a0.getRotation().getQ3(); double oX = a0.getSpin().getX(); double oY = a0.getSpin().getY(); double oZ = a0.getSpin().getZ(); // first time-derivatives of the quaternion double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ); double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ); double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ); double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ); Assert.assertEquals(q0DotRef, q0Dot, 5.0e-9); Assert.assertEquals(q1DotRef, q1Dot, 5.0e-9); Assert.assertEquals(q2DotRef, q2Dot, 5.0e-9); Assert.assertEquals(q3DotRef, q3Dot, 5.0e-9); }
From source file:org.orekit.bodies.Ellipsoid.java
/** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane. * @param planePoint point belonging to the plane, in the ellipsoid frame * @param planeNormal normal of the plane, in the ellipsoid frame * @return plane section or null if there are no intersections *//*from w ww. ja v a2s . c om*/ public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal) { // we define the points Q in the plane using two free variables and as: // Q = P + u + v // where u and v are two unit vectors belonging to the plane // Q belongs to the 3D ellipsoid so: // (xQ / a) + (yQ / b) + (zQ / c) = 1 // combining both equations, we get: // (xP + 2 xP ( xU + xV) + ( xU + xV)) / a // + (yP + 2 yP ( yU + yV) + ( yU + yV)) / b // + (zP + 2 zP ( zU + zV) + ( zU + zV)) / c // = 1 // which can be rewritten: // + + 2 + 2 + 2 + = 0 // with // = xU / a + yU / b + zU / c > 0 // = xV / a + yV / b + zV / c > 0 // = xU xV / a + yU yV / b + zU zV / c // = xP xU / a + yP yU / b + zP zU / c // = xP xV / a + yP yV / b + zP zV / c // = xP / a + yP / b + zP / c - 1 // this is the equation of a conic (here an ellipse) // Of course, we note that if the point P belongs to the ellipsoid // then = 0 and the equation holds at point P since = 0 and = 0 final Vector3D u = planeNormal.orthogonal(); final Vector3D v = Vector3D.crossProduct(planeNormal, u).normalize(); final double xUOa = u.getX() / a; final double yUOb = u.getY() / b; final double zUOc = u.getZ() / c; final double xVOa = v.getX() / a; final double yVOb = v.getY() / b; final double zVOc = v.getZ() / c; final double xPOa = planePoint.getX() / a; final double yPOb = planePoint.getY() / b; final double zPOc = planePoint.getZ() / c; final double alpha = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc; final double beta = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc; final double gamma = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc); final double delta = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc); final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc); final double zeta = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1); // reduce the general equation + + 2 + 2 + 2 + = 0 // to canonical form (/l) + (/m) = 1 // using a coordinates change // = C + cos - sin // = C + sin + cos // or equivalently // = ( - C) cos + ( - C) sin // = - ( - C) sin + ( - C) cos // C and C are the coordinates of the 2D ellipse center with respect to P // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest) // is the angle of the 2D ellipse axis corresponding to axis with length 2l // choose in order to cancel the coupling term in // expanding the general equation, we get: A + B + C + D + E + F = 0 // with C = 2[( - ) cos sin + (cos - sin)] // hence the term is cancelled when = arctan(t), with t + ( - ) t - = 0 // As the solutions of the quadratic equation obey t?t = -1, they correspond to // angles in quadrature to each other. Selecting one solution or the other simply // exchanges the principal axes. As we don't care about which axis we want as the // first one, we select an arbitrary solution final double tanTheta; if (FastMath.abs(gamma) < Precision.SAFE_MIN) { tanTheta = 0.0; } else { final double bMA = beta - alpha; tanTheta = (bMA >= 0) ? (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) : (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))); } final double tan2 = tanTheta * tanTheta; final double cos2 = 1 / (1 + tan2); final double sin2 = tan2 * cos2; final double cosSin = tanTheta * cos2; final double cos = FastMath.sqrt(cos2); final double sin = tanTheta * cos; // choose C and C in order to cancel the linear terms in and // expanding the general equation, we get: A + B + C + D + E + F = 0 // with D = 2[ ( C + C + ) cos + ( C + C + ) sin] // E = 2[-( C + C + ) sin + ( C + C + ) cos] // can be eliminated by combining the equations // D cos - E sin = 2[ C + C + ] // E cos + D sin = 2[ C + C + ] // hence the terms D and E are both cancelled (regardless of ) when // C = ( - ) / ( - ) // C = ( - ) / ( - ) final double denom = MathArrays.linearCombination(gamma, gamma, -alpha, beta); final double tauC = MathArrays.linearCombination(beta, delta, -gamma, epsilon) / denom; final double nuC = MathArrays.linearCombination(alpha, epsilon, -gamma, delta) / denom; // compute l and m // expanding the general equation, we get: A + B + C + D + E + F = 0 // with A = cos + sin + 2 cos sin // B = sin + cos - 2 cos sin // F = C + C + 2 C C + 2 C + 2 C + // hence we compute directly l = (-F/A) and m = (-F/B) final double twogcs = 2 * gamma * cosSin; final double bigA = alpha * cos2 + beta * sin2 + twogcs; final double bigB = alpha * sin2 + beta * cos2 - twogcs; final double bigF = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC + (beta * nuC + 2 * epsilon) * nuC + zeta; final double l = FastMath.sqrt(-bigF / bigA); final double m = FastMath.sqrt(-bigF / bigB); if (Double.isNaN(l + m)) { // the plane does not intersect the ellipsoid return null; } if (l > m) { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(cos, u, sin, v), new Vector3D(-sin, u, cos, v), l, m, frame); } else { return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v), new Vector3D(sin, u, -cos, v), new Vector3D(cos, u, sin, v), m, l, frame); } }
From source file:org.orekit.time.AbsoluteDate.java
/** Build an instance corresponding to a Besselian Epoch (BE). * <p>According to Lieske paper: <a * href="http://articles.adsabs.harvard.edu/cgi-bin/nph-iarticle_query?1979A%26A....73..282L&defaultprint=YES&filetype=.pdf."> * Precession Matrix Based on IAU (1976) System of Astronomical Constants</a>, Astronomy and Astrophysics, * vol. 73, no. 3, Mar. 1979, p. 282-284, Besselian Epoch is related to Julian Ephemeris Date as:</p> * <pre>/*from w ww. j a va 2 s. c om*/ * BE = 1900.0 + (JED - 2415020.31352) / 365.242198781 * </pre> * <p> * This method reverts the formula above and computes an {@code AbsoluteDate} from the Besselian Epoch. * </p> * @param besselianEpoch Besselian epoch, like 1950 for defining the classical reference B1950.0 * @return a new instant * @see #createJulianEpoch(double) */ public static AbsoluteDate createBesselianEpoch(final double besselianEpoch) { return new AbsoluteDate(J2000_EPOCH, MathArrays.linearCombination(Constants.BESSELIAN_YEAR, besselianEpoch - 1900, Constants.JULIAN_DAY, -36525, Constants.JULIAN_DAY, 0.31352)); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Transform the instance to a {@link FieldRotation}<{@link DerivativeStructure}>. * <p>/* ww w . ja v a 2 s . co m*/ * The {@link DerivativeStructure} coordinates correspond to time-derivatives up * to the user-specified order. * </p> * @param order derivation order for the vector components * @return rotation with time-derivatives embedded within the coordinates * @exception OrekitException if the user specified order is too large */ public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) throws OrekitException { // quaternion components final double q0 = rotation.getQ0(); final double q1 = rotation.getQ1(); final double q2 = rotation.getQ2(); final double q3 = rotation.getQ3(); // first time-derivatives of the quaternion final double oX = rotationRate.getX(); final double oY = rotationRate.getY(); final double oZ = rotationRate.getZ(); final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ); final double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ); final double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ); final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ); // second time-derivatives of the quaternion final double oXDot = rotationAcceleration.getX(); final double oYDot = rotationAcceleration.getY(); final double oZDot = rotationAcceleration.getZ(); final double q0DotDot = -0.5 * MathArrays.linearCombination( new double[] { q1, q2, q3, q1Dot, q2Dot, q3Dot }, new double[] { oXDot, oYDot, oZDot, oX, oY, oZ }); final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q2, -q3, q0Dot, q2Dot, -q3Dot }, new double[] { oXDot, oZDot, oYDot, oX, oZ, oY }); final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q3, -q1, q0Dot, q3Dot, -q1Dot }, new double[] { oYDot, oXDot, oZDot, oY, oX, oZ }); final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q1, -q2, q0Dot, q1Dot, -q2Dot }, new double[] { oZDot, oYDot, oXDot, oZ, oY, oX }); final DerivativeStructure q0DS; final DerivativeStructure q1DS; final DerivativeStructure q2DS; final DerivativeStructure q3DS; switch (order) { case 0: q0DS = new DerivativeStructure(1, 0, q0); q1DS = new DerivativeStructure(1, 0, q1); q2DS = new DerivativeStructure(1, 0, q2); q3DS = new DerivativeStructure(1, 0, q3); break; case 1: q0DS = new DerivativeStructure(1, 1, q0, q0Dot); q1DS = new DerivativeStructure(1, 1, q1, q1Dot); q2DS = new DerivativeStructure(1, 1, q2, q2Dot); q3DS = new DerivativeStructure(1, 1, q3, q3Dot); break; case 2: q0DS = new DerivativeStructure(1, 2, q0, q0Dot, q0DotDot); q1DS = new DerivativeStructure(1, 2, q1, q1Dot, q1DotDot); q2DS = new DerivativeStructure(1, 2, q2, q2Dot, q2DotDot); q3DS = new DerivativeStructure(1, 2, q3, q3Dot, q3DotDot); break; default: throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order); } return new FieldRotation<DerivativeStructure>(q0DS, q1DS, q2DS, q3DS, false); }
From source file:org.orekit.utils.AngularCoordinates.java
/** Convert rotation, rate and acceleration to modified Rodrigues vector and derivatives. * <p>/*from w w w . j av a2s . c om*/ * The modified Rodrigues vector is tan(/4) u where and u are the * rotation angle and axis respectively. * </p> * @param sign multiplicative sign for quaternion components * @return modified Rodrigues vector and derivatives (vector on row 0, first derivative * on row 1, second derivative on row 2) * @see #createFromModifiedRodrigues(double[][]) */ public double[][] getModifiedRodrigues(final double sign) { final double q0 = sign * getRotation().getQ0(); final double q1 = sign * getRotation().getQ1(); final double q2 = sign * getRotation().getQ2(); final double q3 = sign * getRotation().getQ3(); final double oX = getRotationRate().getX(); final double oY = getRotationRate().getY(); final double oZ = getRotationRate().getZ(); final double oXDot = getRotationAcceleration().getX(); final double oYDot = getRotationAcceleration().getY(); final double oZDot = getRotationAcceleration().getZ(); // first time-derivatives of the quaternion final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ); final double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ); final double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ); final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ); // second time-derivatives of the quaternion final double q0DotDot = -0.5 * MathArrays.linearCombination( new double[] { q1, q2, q3, q1Dot, q2Dot, q3Dot }, new double[] { oXDot, oYDot, oZDot, oX, oY, oZ }); final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q2, -q3, q0Dot, q2Dot, -q3Dot }, new double[] { oXDot, oZDot, oYDot, oX, oZ, oY }); final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q3, -q1, q0Dot, q3Dot, -q1Dot }, new double[] { oYDot, oXDot, oZDot, oY, oX, oZ }); final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q1, -q2, q0Dot, q1Dot, -q2Dot }, new double[] { oZDot, oYDot, oXDot, oZ, oY, oX }); // 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 double inv = 1.0 / (1.0 + q0); final double mTwoInvQ0Dot = -2 * inv * q0Dot; final double r1 = inv * q1; final double r2 = inv * q2; final double r3 = inv * q3; final double mInvR1 = -inv * r1; final double mInvR2 = -inv * r2; final double mInvR3 = -inv * r3; final double r1Dot = MathArrays.linearCombination(inv, q1Dot, mInvR1, q0Dot); final double r2Dot = MathArrays.linearCombination(inv, q2Dot, mInvR2, q0Dot); final double r3Dot = MathArrays.linearCombination(inv, q3Dot, mInvR3, q0Dot); final double r1DotDot = MathArrays.linearCombination(inv, q1DotDot, mTwoInvQ0Dot, r1Dot, mInvR1, q0DotDot); final double r2DotDot = MathArrays.linearCombination(inv, q2DotDot, mTwoInvQ0Dot, r2Dot, mInvR2, q0DotDot); final double r3DotDot = MathArrays.linearCombination(inv, q3DotDot, mTwoInvQ0Dot, r3Dot, mInvR3, q0DotDot); return new double[][] { { r1, r2, r3 }, { r1Dot, r2Dot, r3Dot }, { r1DotDot, r2DotDot, r3DotDot } }; }
From source file:org.orekit.utils.AngularCoordinates.java
/** Convert a modified Rodrigues vector and derivatives to angular coordinates. * @param r modified Rodrigues vector (with first and second times derivatives) * @return angular coordinates//from w w w .j a v a 2 s. c o m * @see #getModifiedRodrigues(double) */ public static AngularCoordinates createFromModifiedRodrigues(final double[][] r) { // rotation final double rSquared = r[0][0] * r[0][0] + r[0][1] * r[0][1] + r[0][2] * r[0][2]; final double oPQ0 = 2 / (1 + rSquared); final double q0 = oPQ0 - 1; final double q1 = oPQ0 * r[0][0]; final double q2 = oPQ0 * r[0][1]; final double q3 = oPQ0 * r[0][2]; // rotation rate final double oPQ02 = oPQ0 * oPQ0; final double q0Dot = -oPQ02 * MathArrays.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1], r[0][2], r[1][2]); final double q1Dot = oPQ0 * r[1][0] + r[0][0] * q0Dot; final double q2Dot = oPQ0 * r[1][1] + r[0][1] * q0Dot; final double q3Dot = oPQ0 * r[1][2] + r[0][2] * q0Dot; final double oX = 2 * MathArrays.linearCombination(-q1, q0Dot, q0, q1Dot, q3, q2Dot, -q2, q3Dot); final double oY = 2 * MathArrays.linearCombination(-q2, q0Dot, -q3, q1Dot, q0, q2Dot, q1, q3Dot); final double oZ = 2 * MathArrays.linearCombination(-q3, q0Dot, q2, q1Dot, -q1, q2Dot, q0, q3Dot); // rotation acceleration final double q0DotDot = (1 - q0) / oPQ0 * q0Dot * q0Dot - oPQ02 * MathArrays.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]) - (q1Dot * q1Dot + q2Dot * q2Dot + q3Dot * q3Dot); final double q1DotDot = MathArrays.linearCombination(oPQ0, r[2][0], 2 * r[1][0], q0Dot, r[0][0], q0DotDot); final double q2DotDot = MathArrays.linearCombination(oPQ0, r[2][1], 2 * r[1][1], q0Dot, r[0][1], q0DotDot); final double q3DotDot = MathArrays.linearCombination(oPQ0, r[2][2], 2 * r[1][2], q0Dot, r[0][2], q0DotDot); final double oXDot = 2 * MathArrays.linearCombination(-q1, q0DotDot, q0, q1DotDot, q3, q2DotDot, -q2, q3DotDot); final double oYDot = 2 * MathArrays.linearCombination(-q2, q0DotDot, -q3, q1DotDot, q0, q2DotDot, q1, q3DotDot); final double oZDot = 2 * MathArrays.linearCombination(-q3, q0DotDot, q2, q1DotDot, -q1, q2DotDot, q0, q3DotDot); return new AngularCoordinates(new Rotation(q0, q1, q2, q3, false), new Vector3D(oX, oY, oZ), new Vector3D(oXDot, oYDot, oZDot)); }