Merge branch 'master' of https://git-wip-us.apache.org/repos/asf/commons-math
This commit is contained in:
Phil Steitz
commit
85a4fdd0ea
|
|
@ -54,6 +54,9 @@ If the output is not quite correct, check for invisible trailing spaces!
|
|||
</release>
|
||||
|
||||
<release version="4.0" date="XXXX-XX-XX" description="">
|
||||
<action dev="luc" type="fix" issue="MATH-1297"> <!-- backported to 3.6 -->
|
||||
Detect start failures with multi-step ODE integrators.
|
||||
</action>
|
||||
<action dev="luc" type="add" issue="MATH-1302,MATH-1303"> <!-- backported to 3.6 -->
|
||||
Added a RotationConvention enumerate to allow specifying the semantics
|
||||
or axis/angle for rotations. This enumerate has two values:
|
||||
|
|
@ -67,6 +70,7 @@ If the output is not quite correct, check for invisible trailing spaces!
|
|||
</action>
|
||||
<action dev="erans" type="fix" issue="MATH-1301">
|
||||
"JDKRandomGenerator": Method "nextInt(int)" now throws a "NotStrictlyPositiveException".
|
||||
The class now delegates to (rather inherits from) "java.util.Random".
|
||||
</action>
|
||||
<action dev="erans" type="update" issue="MATH-1305" due-to="Rostislav Krasny">
|
||||
"AbstractRandomGenerator" and "BitsStreamGenerator": Slight performance
|
||||
|
|
|
|||
|
|
@ -162,6 +162,7 @@ public enum LocalizedFormats implements Localizable {
|
|||
LOWER_BOUND_NOT_BELOW_UPPER_BOUND("lower bound ({0}) must be strictly less than upper bound ({1})"), /* keep */
|
||||
LOWER_ENDPOINT_ABOVE_UPPER_ENDPOINT("lower endpoint ({0}) must be less than or equal to upper endpoint ({1})"),
|
||||
MAP_MODIFIED_WHILE_ITERATING("map has been modified while iterating"),
|
||||
MULTISTEP_STARTER_STOPPED_EARLY("multistep integrator starter stopped early, maybe too large step size"),
|
||||
EVALUATIONS("evaluations"), /* keep */
|
||||
MAX_COUNT_EXCEEDED("maximal count ({0}) exceeded"), /* keep */
|
||||
MAX_ITERATIONS_EXCEEDED("maximal number of iterations ({0}) exceeded"),
|
||||
|
|
|
|||
|
|
@ -363,7 +363,8 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
* widespread in the aerospace business where Roll, Pitch and Yaw angles
|
||||
* are often wrongly tagged as Euler angles).</p>
|
||||
|
||||
* @param order order of rotations to use
|
||||
* @param order order of rotations to compose, from left to right
|
||||
* (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)})
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @param alpha1 angle of the first elementary rotation
|
||||
* @param alpha2 angle of the second elementary rotation
|
||||
|
|
@ -376,9 +377,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1, convention);
|
||||
final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2, convention);
|
||||
final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3, convention);
|
||||
final FieldRotation<T> composed = convention == RotationConvention.FRAME_TRANSFORM ?
|
||||
r3.applyTo(r2.applyTo(r1)) :
|
||||
r1.applyTo(r2.applyTo(r3));
|
||||
final FieldRotation<T> composed = r1.compose(r2.compose(r3, convention), convention);
|
||||
q0 = composed.q0;
|
||||
q1 = composed.q1;
|
||||
q2 = composed.q2;
|
||||
|
|
@ -1271,15 +1270,53 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
}
|
||||
|
||||
/** Apply the instance to another rotation.
|
||||
* Applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let u be any
|
||||
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
|
||||
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
|
||||
* where comp = applyTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #compose(FieldRotation, RotationConvention)
|
||||
* compose(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public FieldRotation<T> applyTo(final FieldRotation<T> r) {
|
||||
return compose(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let {@code u} be any
|
||||
* vector and {@code v} its image by {@code r1} (i.e.
|
||||
* {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
|
||||
* rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
|
||||
* {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public FieldRotation<T> compose(final FieldRotation<T> r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInternal(r) : r.composeInternal(this);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
* using vector operator convention
|
||||
*/
|
||||
private FieldRotation<T> composeInternal(final FieldRotation<T> r) {
|
||||
return new FieldRotation<T>(r.q0.multiply(q0).subtract(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))),
|
||||
r.q1.multiply(q0).add(r.q0.multiply(q1)).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))),
|
||||
r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))),
|
||||
|
|
@ -1288,20 +1325,58 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
}
|
||||
|
||||
/** Apply the instance to another rotation.
|
||||
* Applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let u be any
|
||||
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
|
||||
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
|
||||
* where comp = applyTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #compose(Rotation, RotationConvention)
|
||||
* compose(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public FieldRotation<T> applyTo(final Rotation r) {
|
||||
return compose(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let {@code u} be any
|
||||
* vector and {@code v} its image by {@code r1} (i.e.
|
||||
* {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
|
||||
* rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
|
||||
* {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public FieldRotation<T> compose(final Rotation r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInternal(r) : applyTo(r, this);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
* using vector operator convention
|
||||
*/
|
||||
private FieldRotation<T> composeInternal(final Rotation r) {
|
||||
return new FieldRotation<T>(q0.multiply(r.getQ0()).subtract(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))),
|
||||
q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))),
|
||||
q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))),
|
||||
q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))),
|
||||
false);
|
||||
q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))),
|
||||
q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))),
|
||||
q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))),
|
||||
false);
|
||||
}
|
||||
|
||||
/** Apply a rotation to another rotation.
|
||||
|
|
@ -1324,17 +1399,57 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
}
|
||||
|
||||
/** Apply the inverse of the instance to another rotation.
|
||||
* Applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
|
||||
* let w be the inverse image of v by the instance
|
||||
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
|
||||
* comp = applyInverseTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #composeInverse(FieldRotation<T>, RotationConvention)
|
||||
* composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public FieldRotation<T> applyInverseTo(final FieldRotation<T> r) {
|
||||
return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let {@code u} be any vector and {@code v} its image by {@code r1}
|
||||
* (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
|
||||
* of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
|
||||
* Then {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.composeInverse(r1)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed, which means it is the
|
||||
* <em>innermost</em> rotation that will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public FieldRotation<T> composeInverse(final FieldRotation<T> r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInverseInternal(r) : r.composeInternal(revert());
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation
|
||||
* using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance using vector operator convention
|
||||
*/
|
||||
private FieldRotation<T> composeInverseInternal(FieldRotation<T> r) {
|
||||
return new FieldRotation<T>(r.q0.multiply(q0).add(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))).negate(),
|
||||
r.q0.multiply(q1).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))).subtract(r.q1.multiply(q0)),
|
||||
r.q0.multiply(q2).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))).subtract(r.q2.multiply(q0)),
|
||||
|
|
@ -1343,17 +1458,57 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
}
|
||||
|
||||
/** Apply the inverse of the instance to another rotation.
|
||||
* Applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
|
||||
* let w be the inverse image of v by the instance
|
||||
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
|
||||
* comp = applyInverseTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #composeInverse(Rotation, RotationConvention)
|
||||
* composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public FieldRotation<T> applyInverseTo(final Rotation r) {
|
||||
return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let {@code u} be any vector and {@code v} its image by {@code r1}
|
||||
* (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
|
||||
* of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
|
||||
* Then {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.composeInverse(r1)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed, which means it is the
|
||||
* <em>innermost</em> rotation that will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public FieldRotation<T> composeInverse(final Rotation r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInverseInternal(r) : applyTo(r, revert());
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation
|
||||
* using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance using vector operator convention
|
||||
*/
|
||||
private FieldRotation<T> composeInverseInternal(Rotation r) {
|
||||
return new FieldRotation<T>(q0.multiply(r.getQ0()).add(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))).negate(),
|
||||
q1.multiply(r.getQ0()).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))).subtract(q0.multiply(r.getQ1())),
|
||||
q2.multiply(r.getQ0()).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))).subtract(q0.multiply(r.getQ2())),
|
||||
|
|
@ -1502,7 +1657,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
|
|||
* @return <i>distance</i> between r1 and r2
|
||||
*/
|
||||
public static <T extends RealFieldElement<T>> T distance(final FieldRotation<T> r1, final FieldRotation<T> r2) {
|
||||
return r1.applyInverseTo(r2).getAngle();
|
||||
return r1.composeInverseInternal(r2).getAngle();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -151,22 +151,11 @@ public class Rotation implements Serializable {
|
|||
}
|
||||
|
||||
/** Build a rotation from an axis and an angle.
|
||||
* <p>We use the convention that angles are oriented according to
|
||||
* the effect of the rotation on vectors around the axis. That means
|
||||
* that if (i, j, k) is a direct frame and if we first provide +k as
|
||||
* the axis and π/2 as the angle to this constructor, and then
|
||||
* {@link #applyTo(Vector3D) apply} the instance to +i, we will get
|
||||
* +j.</p>
|
||||
* <p>Another way to represent our convention is to say that a rotation
|
||||
* of angle θ about the unit vector (x, y, z) is the same as the
|
||||
* rotation build from quaternion components { cos(-θ/2),
|
||||
* x * sin(-θ/2), y * sin(-θ/2), z * sin(-θ/2) }.
|
||||
* Note the minus sign on the angle!</p>
|
||||
* <p>On the one hand this convention is consistent with a vectorial
|
||||
* perspective (moving vectors in fixed frames), on the other hand it
|
||||
* is different from conventions with a frame perspective (fixed vectors
|
||||
* viewed from different frames) like the ones used for example in spacecraft
|
||||
* attitude community or in the graphics community.</p>
|
||||
* <p>
|
||||
* Calling this constructor is equivalent to call
|
||||
* {@link #Rotation(Vector3D, double, RotationConvention)
|
||||
* new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)}
|
||||
* </p>
|
||||
* @param axis axis around which to rotate
|
||||
* @param angle rotation angle.
|
||||
* @exception MathIllegalArgumentException if the axis norm is zero
|
||||
|
|
@ -370,17 +359,11 @@ public class Rotation implements Serializable {
|
|||
|
||||
/** Build a rotation from three Cardan or Euler elementary rotations.
|
||||
|
||||
* <p>Cardan rotations are three successive rotations around the
|
||||
* canonical axes X, Y and Z, each axis being used once. There are
|
||||
* 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler
|
||||
* rotations are three successive rotations around the canonical
|
||||
* axes X, Y and Z, the first and last rotations being around the
|
||||
* same axis. There are 6 such sets of rotations (XYX, XZX, YXY,
|
||||
* YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p>
|
||||
* <p>Beware that many people routinely use the term Euler angles even
|
||||
* for what really are Cardan angles (this confusion is especially
|
||||
* widespread in the aerospace business where Roll, Pitch and Yaw angles
|
||||
* are often wrongly tagged as Euler angles).</p>
|
||||
* <p>
|
||||
* Calling this constructor is equivalent to call
|
||||
* {@link #Rotation(RotationOrder, RotationConvention, double, double, double)
|
||||
* new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)}
|
||||
* </p>
|
||||
|
||||
* @param order order of rotations to use
|
||||
* @param alpha1 angle of the first elementary rotation
|
||||
|
|
@ -409,7 +392,8 @@ public class Rotation implements Serializable {
|
|||
* widespread in the aerospace business where Roll, Pitch and Yaw angles
|
||||
* are often wrongly tagged as Euler angles).</p>
|
||||
|
||||
* @param order order of rotations to use
|
||||
* @param order order of rotations to compose, from left to right
|
||||
* (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)})
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @param alpha1 angle of the first elementary rotation
|
||||
* @param alpha2 angle of the second elementary rotation
|
||||
|
|
@ -421,9 +405,7 @@ public class Rotation implements Serializable {
|
|||
Rotation r1 = new Rotation(order.getA1(), alpha1, convention);
|
||||
Rotation r2 = new Rotation(order.getA2(), alpha2, convention);
|
||||
Rotation r3 = new Rotation(order.getA3(), alpha3, convention);
|
||||
Rotation composed = convention == RotationConvention.FRAME_TRANSFORM ?
|
||||
r3.applyTo(r2.applyTo(r1)) :
|
||||
r1.applyTo(r2.applyTo(r3));
|
||||
Rotation composed = r1.compose(r2.compose(r3, convention), convention);
|
||||
q0 = composed.q0;
|
||||
q1 = composed.q1;
|
||||
q2 = composed.q2;
|
||||
|
|
@ -531,6 +513,10 @@ public class Rotation implements Serializable {
|
|||
}
|
||||
|
||||
/** Get the normalized axis of the rotation.
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #getAxis(RotationConvention) getAxis(RotationConvention.VECTOR_OPERATOR)}
|
||||
* </p>
|
||||
* @return normalized axis of the rotation
|
||||
* @see #Rotation(Vector3D, double, RotationConvention)
|
||||
* @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)}
|
||||
|
|
@ -581,33 +567,11 @@ public class Rotation implements Serializable {
|
|||
|
||||
/** Get the Cardan or Euler angles corresponding to the instance.
|
||||
|
||||
* <p>The equations show that each rotation can be defined by two
|
||||
* different values of the Cardan or Euler angles set. For example
|
||||
* if Cardan angles are used, the rotation defined by the angles
|
||||
* a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as
|
||||
* the rotation defined by the angles π + a<sub>1</sub>, π
|
||||
* - a<sub>2</sub> and π + a<sub>3</sub>. This method implements
|
||||
* the following arbitrary choices:</p>
|
||||
* <ul>
|
||||
* <li>for Cardan angles, the chosen set is the one for which the
|
||||
* second angle is between -π/2 and π/2 (i.e its cosine is
|
||||
* positive),</li>
|
||||
* <li>for Euler angles, the chosen set is the one for which the
|
||||
* second angle is between 0 and π (i.e its sine is positive).</li>
|
||||
* </ul>
|
||||
|
||||
* <p>Cardan and Euler angle have a very disappointing drawback: all
|
||||
* of them have singularities. This means that if the instance is
|
||||
* too close to the singularities corresponding to the given
|
||||
* rotation order, it will be impossible to retrieve the angles. For
|
||||
* Cardan angles, this is often called gimbal lock. There is
|
||||
* <em>nothing</em> to do to prevent this, it is an intrinsic problem
|
||||
* with Cardan and Euler representation (but not a problem with the
|
||||
* rotation itself, which is perfectly well defined). For Cardan
|
||||
* angles, singularities occur when the second angle is close to
|
||||
* -π/2 or +π/2, for Euler angle singularities occur when the
|
||||
* second angle is close to 0 or π, this implies that the identity
|
||||
* rotation is always singular for Euler angles!</p>
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #getAngles(RotationOrder, RotationConvention)
|
||||
* getAngles(order, RotationConvention.VECTOR_OPERATOR)}
|
||||
* </p>
|
||||
|
||||
* @param order rotation order to use
|
||||
* @return an array of three angles, in the order specified by the set
|
||||
|
|
@ -1217,15 +1181,53 @@ public class Rotation implements Serializable {
|
|||
}
|
||||
|
||||
/** Apply the instance to another rotation.
|
||||
* Applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let u be any
|
||||
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
|
||||
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
|
||||
* where comp = applyTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #compose(Rotation, RotationConvention)
|
||||
* compose(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public Rotation applyTo(Rotation r) {
|
||||
return compose(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the instance to a rotation is computing the composition
|
||||
* in an order compliant with the following rule : let {@code u} be any
|
||||
* vector and {@code v} its image by {@code r1} (i.e.
|
||||
* {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
|
||||
* rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
|
||||
* {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
*/
|
||||
public Rotation compose(final Rotation r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInternal(r) : r.composeInternal(this);
|
||||
}
|
||||
|
||||
/** Compose the instance with another rotation using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the instance
|
||||
* using vector operator convention
|
||||
*/
|
||||
private Rotation composeInternal(final Rotation r) {
|
||||
return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
|
||||
r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
|
||||
r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
|
||||
|
|
@ -1234,17 +1236,57 @@ public class Rotation implements Serializable {
|
|||
}
|
||||
|
||||
/** Apply the inverse of the instance to another rotation.
|
||||
* Applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
|
||||
* let w be the inverse image of v by the instance
|
||||
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
|
||||
* comp = applyInverseTo(r).
|
||||
* <p>
|
||||
* Calling this method is equivalent to call
|
||||
* {@link #composeInverse(Rotation, RotationConvention)
|
||||
* composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public Rotation applyInverseTo(Rotation r) {
|
||||
return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation.
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
|
||||
* applying the inverse of the instance to a rotation is computing
|
||||
* the composition in an order compliant with the following rule :
|
||||
* let {@code u} be any vector and {@code v} its image by {@code r1}
|
||||
* (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
|
||||
* of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
|
||||
* Then {@code w = comp.applyTo(u)}, where
|
||||
* {@code comp = r2.composeInverse(r1)}.
|
||||
* </p>
|
||||
* <p>
|
||||
* If the semantics of the rotations composition corresponds to a
|
||||
* {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
|
||||
* the application order will be reversed, which means it is the
|
||||
* <em>innermost</em> rotation that will be reversed. So keeping the exact same
|
||||
* meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
|
||||
* and {@code comp} as above, {@code comp} could also be computed as
|
||||
* {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
|
||||
* </p>
|
||||
* @param r rotation to apply the rotation to
|
||||
* @param convention convention to use for the semantics of the angle
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance
|
||||
*/
|
||||
public Rotation composeInverse(final Rotation r, final RotationConvention convention) {
|
||||
return convention == RotationConvention.VECTOR_OPERATOR ?
|
||||
composeInverseInternal(r) : r.composeInternal(revert());
|
||||
}
|
||||
|
||||
/** Compose the inverse of the instance with another rotation
|
||||
* using vector operator convention.
|
||||
* @param r rotation to apply the rotation to
|
||||
* @return a new rotation which is the composition of r by the inverse
|
||||
* of the instance using vector operator convention
|
||||
*/
|
||||
private Rotation composeInverseInternal(Rotation r) {
|
||||
return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
|
||||
-r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
|
||||
-r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
|
||||
|
|
@ -1376,7 +1418,7 @@ public class Rotation implements Serializable {
|
|||
* @return <i>distance</i> between r1 and r2
|
||||
*/
|
||||
public static double distance(Rotation r1, Rotation r2) {
|
||||
return r1.applyInverseTo(r2).getAngle();
|
||||
return r1.composeInverseInternal(r2).getAngle();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
package org.apache.commons.math4.ode;
|
||||
|
||||
import org.apache.commons.math4.exception.DimensionMismatchException;
|
||||
import org.apache.commons.math4.exception.MathIllegalStateException;
|
||||
import org.apache.commons.math4.exception.MaxCountExceededException;
|
||||
import org.apache.commons.math4.exception.NoBracketingException;
|
||||
import org.apache.commons.math4.exception.NumberIsTooSmallException;
|
||||
|
|
@ -248,6 +249,9 @@ public abstract class MultistepIntegrator extends AdaptiveStepsizeIntegrator {
|
|||
}, t0, y0, t, new double[y0.length]);
|
||||
}
|
||||
|
||||
// we should not reach this step
|
||||
throw new MathIllegalStateException(LocalizedFormats.MULTISTEP_STARTER_STOPPED_EARLY);
|
||||
|
||||
} catch (InitializationCompletedMarkerException icme) { // NOPMD
|
||||
// this is the expected nominal interruption of the start integrator
|
||||
|
||||
|
|
|
|||
|
|
@ -20,50 +20,100 @@ import java.util.Random;
|
|||
import org.apache.commons.math4.exception.NotStrictlyPositiveException;
|
||||
|
||||
/**
|
||||
* Extension of <code>java.util.Random</code> to implement
|
||||
* {@link RandomGenerator}.
|
||||
* A {@link RandomGenerator} adapter that delegates the random number
|
||||
* generation to the standard {@link java.util.Random} class.
|
||||
*
|
||||
* @since 1.1
|
||||
*/
|
||||
public class JDKRandomGenerator extends Random implements RandomGenerator {
|
||||
|
||||
public class JDKRandomGenerator
|
||||
implements RandomGenerator {
|
||||
/** Serializable version identifier. */
|
||||
private static final long serialVersionUID = -7745277476784028798L;
|
||||
private static final long serialVersionUID = 20151227L;
|
||||
/** JDK's RNG. */
|
||||
private final Random delegate;
|
||||
|
||||
/**
|
||||
* Create a new JDKRandomGenerator with a default seed.
|
||||
* Creates an instance with an arbitrary seed.
|
||||
*/
|
||||
public JDKRandomGenerator() {
|
||||
super();
|
||||
delegate = new Random();
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new JDKRandomGenerator with the given seed.
|
||||
* Creates an instance with the given seed.
|
||||
*
|
||||
* @param seed initial seed
|
||||
* @param seed Initial seed.
|
||||
* @since 3.6
|
||||
*/
|
||||
public JDKRandomGenerator(long seed) {
|
||||
setSeed(seed);
|
||||
delegate = new Random(seed);
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public void setSeed(int seed) {
|
||||
setSeed((long) seed);
|
||||
delegate.setSeed((long) seed);
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public void setSeed(long seed) {
|
||||
delegate.setSeed( seed);
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public void setSeed(int[] seed) {
|
||||
setSeed(RandomGeneratorFactory.convertToLong(seed));
|
||||
delegate.setSeed(RandomGeneratorFactory.convertToLong(seed));
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public void nextBytes(byte[] bytes) {
|
||||
delegate.nextBytes(bytes);
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public int nextInt() {
|
||||
return delegate.nextInt();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public long nextLong() {
|
||||
return delegate.nextLong();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public boolean nextBoolean() {
|
||||
return delegate.nextBoolean();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public float nextFloat() {
|
||||
return delegate.nextFloat();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public double nextDouble() {
|
||||
return delegate.nextDouble();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public double nextGaussian() {
|
||||
return delegate.nextGaussian();
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public int nextInt(int n) {
|
||||
try {
|
||||
return super.nextInt(n);
|
||||
return delegate.nextInt(n);
|
||||
} catch (IllegalArgumentException e) {
|
||||
throw new NotStrictlyPositiveException(n);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,7 +24,12 @@ import org.apache.commons.math4.util.FastMath;
|
|||
/** This class implements a powerful pseudo-random number generator
|
||||
* developed by Makoto Matsumoto and Takuji Nishimura during
|
||||
* 1996-1997.
|
||||
|
||||
*
|
||||
* <b>Caveat:</b> It is recommended to use one of WELL generators rather
|
||||
* than the MersenneTwister generator (see
|
||||
* <a href="http://www.iro.umontreal.ca/~panneton/WELLRNG.html">
|
||||
* this paper</a> for more information).
|
||||
*
|
||||
* <p>This generator features an extremely long period
|
||||
* (2<sup>19937</sup>-1) and 623-dimensional equidistribution up to 32
|
||||
* bits accuracy. The home page for this generator is located at <a
|
||||
|
|
|
|||
|
|
@ -265,7 +265,7 @@ public final class CombinatoricsUtils {
|
|||
* <p>
|
||||
* <Strong>Preconditions</strong>:
|
||||
* <ul>
|
||||
* <li> {@code n ≥ 0} (otherwise
|
||||
* <li> {@code n >= 0} (otherwise
|
||||
* {@code MathIllegalArgumentException} is thrown)</li>
|
||||
* <li> The result is small enough to fit into a {@code long}. The
|
||||
* largest value of {@code n} for which {@code n!} does not exceed
|
||||
|
|
|
|||
|
|
@ -141,6 +141,7 @@ MAX_ITERATIONS_EXCEEDED = nombre maximal d''it\u00e9rations ({0}) d\u00e9pass\u0
|
|||
MINIMAL_STEPSIZE_REACHED_DURING_INTEGRATION = pas minimal ({1,number,0.00E00}) atteint, l''int\u00e9gration n\u00e9cessite {0,number,0.00E00}
|
||||
MISMATCHED_LOESS_ABSCISSA_ORDINATE_ARRAYS = Loess a besoin de tableaux d''abscisses et d''ordonn\u00e9es de m\u00eame taille, mais il y a {0} points en abscisse et {1} en ordonn\u00e9e
|
||||
MUTATION_RATE = proportion de mutation ({0})
|
||||
MULTISTEP_STARTER_STOPPED_EARLY = arr\u00eat pr\u00e9matur\u00e9 du d\u00e9marrage de l''int\u00e9grateur multi-pas, taille de pas peut-\u00eatre trop grande"),
|
||||
NAN_ELEMENT_AT_INDEX = l''\u00e9l\u00e9ment {0} est un NaN
|
||||
NAN_VALUE_CONVERSION = les valeurs NaN ne peuvent \u00eatre converties
|
||||
NEGATIVE_BRIGHTNESS_EXPONENT = l''exposant de brillance devrait \u00eatre positif ou null, or e = {0}
|
||||
|
|
|
|||
|
|
@ -187,8 +187,12 @@
|
|||
previous notations, we would say we can apply <code>r<sub>1</sub></code> to
|
||||
<code>r<sub>2</sub></code> and the result we get is <code>r =
|
||||
r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the class
|
||||
provides the methods: <code>applyTo(Rotation)</code> and
|
||||
<code>applyInverseTo(Rotation)</code>.
|
||||
provides the methods: <code>compose(Rotation, RotationConvention)</code> and
|
||||
<code>composeInverse(Rotation, RotationConvention)</code>. There are also
|
||||
shortcuts <code>applyTo(Rotation)</code> which is equivalent to
|
||||
<code>compose(Rotation, RotationConvention.VECTOR_OPERATOR)</code> and
|
||||
<code>applyInverseTo(Rotation)</code> which is equivalent to
|
||||
<code>composeInverse(Rotation, RotationConvention.VECTOR_OPERATOR)</code>.
|
||||
</p>
|
||||
</subsection>
|
||||
<subsection name="11.3 n-Sphere" href="sphere">
|
||||
|
|
|
|||
|
|
@ -16,9 +16,11 @@
|
|||
*/
|
||||
package org.apache.commons.math4;
|
||||
|
||||
import java.util.Random;
|
||||
import java.util.concurrent.Callable;
|
||||
|
||||
import org.apache.commons.math4.util.MathArrays;
|
||||
import org.apache.commons.math4.random.RandomGenerator;
|
||||
import org.apache.commons.math4.random.Well19937c;
|
||||
import org.apache.commons.math4.exception.MathIllegalStateException;
|
||||
import org.apache.commons.math4.exception.util.LocalizedFormats;
|
||||
import org.apache.commons.math4.stat.descriptive.StatisticalSummary;
|
||||
|
|
@ -35,7 +37,7 @@ public class PerfTestUtils {
|
|||
/** Default number of code repeats for computing the average run time. */
|
||||
private static final int DEFAULT_REPEAT_STAT = 10000;
|
||||
/** RNG. */
|
||||
private static Random rng = new Random();
|
||||
private static RandomGenerator rng = new Well19937c();
|
||||
|
||||
/**
|
||||
* Timing.
|
||||
|
|
@ -104,9 +106,16 @@ public class PerfTestUtils {
|
|||
final int numMethods = methods.length;
|
||||
final double[][][] timesAndResults = new double[numMethods][repeatStat][2];
|
||||
|
||||
// Indices into the array containing the methods to benchmark.
|
||||
// The purpose is that at each repeat, the "methods" are called in a different order.
|
||||
final int[] methodSequence = MathArrays.natural(numMethods);
|
||||
|
||||
try {
|
||||
for (int k = 0; k < repeatStat; k++) {
|
||||
for (int j = 0; j < numMethods; j++) {
|
||||
MathArrays.shuffle(methodSequence, rng);
|
||||
for (int n = 0; n < numMethods; n++) {
|
||||
final int j = methodSequence[n]; // Index of the timed method.
|
||||
|
||||
if (runGC) {
|
||||
// Try to perform GC outside the timed block.
|
||||
System.gc();
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ public class LocalizedFormatsTest {
|
|||
|
||||
@Test
|
||||
public void testMessageNumber() {
|
||||
Assert.assertEquals(327, LocalizedFormats.values().length);
|
||||
Assert.assertEquals(328, LocalizedFormats.values().length);
|
||||
}
|
||||
|
||||
@Test
|
||||
|
|
|
|||
|
|
@ -246,6 +246,150 @@ public class FieldRotationDSTest {
|
|||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertVectorOperator() {
|
||||
double a = 0.001;
|
||||
double b = 0.36;
|
||||
double c = 0.48;
|
||||
double d = 0.8;
|
||||
FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
|
||||
double a2 = a * a;
|
||||
double b2 = b * b;
|
||||
double c2 = c * c;
|
||||
double d2 = d * d;
|
||||
double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
|
||||
Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
FieldRotation<DerivativeStructure> reverted = r.revert();
|
||||
FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
|
||||
checkRotationDS(rrT, 1, 0, 0, 0);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
|
||||
checkRotationDS(rTr, 1, 0, 0, 0);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
|
||||
Assert.assertEquals(-1,
|
||||
FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
|
||||
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
|
||||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertFrameTransform() {
|
||||
double a = 0.001;
|
||||
double b = 0.36;
|
||||
double c = 0.48;
|
||||
double d = 0.8;
|
||||
FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
|
||||
double a2 = a * a;
|
||||
double b2 = b * b;
|
||||
double c2 = c * c;
|
||||
double d2 = d * d;
|
||||
double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
|
||||
Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
FieldRotation<DerivativeStructure> reverted = r.revert();
|
||||
FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
|
||||
checkRotationDS(rrT, 1, 0, 0, 0);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
|
||||
checkRotationDS(rTr, 1, 0, 0, 0);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
|
||||
Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
|
||||
Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
|
||||
Assert.assertEquals(-1,
|
||||
FieldVector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
|
||||
reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
|
||||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -642,7 +786,7 @@ public class FieldRotationDSTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testCompose() throws MathIllegalArgumentException {
|
||||
public void testApplyToRotation() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
|
|
@ -652,10 +796,10 @@ public class FieldRotationDSTest {
|
|||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.applyTo(r1);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false));
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false));
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
|
|
@ -670,7 +814,65 @@ public class FieldRotationDSTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverse() throws MathIllegalArgumentException {
|
||||
public void testComposeVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
|
||||
checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeFrameTransform() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testApplyInverseToRotation() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
|
|
@ -680,10 +882,10 @@ public class FieldRotationDSTest {
|
|||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.applyInverseTo(r1);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false));
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false));
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
|
|
@ -697,6 +899,64 @@ public class FieldRotationDSTest {
|
|||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
|
||||
checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseframeTransform() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
|
||||
checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testDoubleVectors() throws MathIllegalArgumentException {
|
||||
|
||||
|
|
@ -752,9 +1012,9 @@ public class FieldRotationDSTest {
|
|||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2);
|
||||
FieldRotation<DerivativeStructure> rB = r1Prime.applyTo(r2);
|
||||
FieldRotation<DerivativeStructure> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<DerivativeStructure> rC = FieldRotation.applyInverseTo(r1, r2);
|
||||
FieldRotation<DerivativeStructure> rD = r1Prime.applyInverseTo(r2);
|
||||
FieldRotation<DerivativeStructure> rD = r1Prime.composeInverse(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
|
|
|
|||
|
|
@ -192,6 +192,44 @@ public class FieldRotationDfpTest {
|
|||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertVectorOperator() {
|
||||
double a = 0.001;
|
||||
double b = 0.36;
|
||||
double c = 0.48;
|
||||
double d = 0.8;
|
||||
FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
|
||||
FieldRotation<Dfp> reverted = r.revert();
|
||||
FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
|
||||
checkRotationDS(rrT, 1, 0, 0, 0);
|
||||
FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
|
||||
checkRotationDS(rTr, 1, 0, 0, 0);
|
||||
Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
|
||||
Assert.assertEquals(-1,
|
||||
FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
|
||||
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
|
||||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertFrameTransform() {
|
||||
double a = 0.001;
|
||||
double b = 0.36;
|
||||
double c = 0.48;
|
||||
double d = 0.8;
|
||||
FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
|
||||
FieldRotation<Dfp> reverted = r.revert();
|
||||
FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
|
||||
checkRotationDS(rrT, 1, 0, 0, 0);
|
||||
FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
|
||||
checkRotationDS(rTr, 1, 0, 0, 0);
|
||||
Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
|
||||
Assert.assertEquals(-1,
|
||||
FieldVector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
|
||||
reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
|
||||
1.0e-15);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -585,7 +623,7 @@ public class FieldRotationDfpTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testCompose() throws MathIllegalArgumentException {
|
||||
public void testApplyToRotation() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
|
|
@ -613,7 +651,67 @@ public class FieldRotationDfpTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverse() throws MathIllegalArgumentException {
|
||||
public void testComposeVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<Dfp> u = createVector(x, y, z);
|
||||
checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeFrameTransform() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
Assert.assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<Dfp> u = createVector(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testApplyInverseToRotation() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
|
|
@ -640,6 +738,66 @@ public class FieldRotationDfpTest {
|
|||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<Dfp> u = createVector(x, y, z);
|
||||
checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
|
||||
|
||||
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
|
||||
createAngle(1.7),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
|
||||
createAngle(0.3),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
|
||||
r1.getQ1().getReal(),
|
||||
r1.getQ2().getReal(),
|
||||
r1.getQ3().getReal(),
|
||||
false),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
FieldRotation<Dfp> r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
|
||||
Assert.assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
FieldVector3D<Dfp> u = createVector(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
|
||||
checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testDoubleVectors() throws MathIllegalArgumentException {
|
||||
|
||||
|
|
@ -696,9 +854,9 @@ public class FieldRotationDfpTest {
|
|||
RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2);
|
||||
FieldRotation<Dfp> rB = r1Prime.applyTo(r2);
|
||||
FieldRotation<Dfp> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
FieldRotation<Dfp> rC = FieldRotation.applyInverseTo(r1, r2);
|
||||
FieldRotation<Dfp> rD = r1Prime.applyInverseTo(r2);
|
||||
FieldRotation<Dfp> rD = r1Prime.composeInverse(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.4) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.4) {
|
||||
|
|
|
|||
|
|
@ -152,7 +152,7 @@ public class RotationTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testRevert() {
|
||||
public void testRevertDeprecated() {
|
||||
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
|
||||
Rotation reverted = r.revert();
|
||||
checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
|
||||
|
|
@ -164,6 +164,32 @@ public class RotationTest {
|
|||
1.0e-12);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertVectorOperator() {
|
||||
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
|
||||
Rotation reverted = r.revert();
|
||||
checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
|
||||
checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
|
||||
Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
|
||||
Assert.assertEquals(-1,
|
||||
Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
|
||||
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
|
||||
1.0e-12);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRevertFrameTransform() {
|
||||
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
|
||||
Rotation reverted = r.revert();
|
||||
checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
|
||||
checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
|
||||
Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
|
||||
Assert.assertEquals(-1,
|
||||
Vector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
|
||||
reverted.getAxis(RotationConvention.FRAME_TRANSFORM)),
|
||||
1.0e-12);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -529,7 +555,7 @@ public class RotationTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testCompose() throws MathIllegalArgumentException {
|
||||
public void testApplyTo() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
|
||||
|
|
@ -547,7 +573,45 @@ public class RotationTest {
|
|||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverse() throws MathIllegalArgumentException {
|
||||
public void testComposeVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
Vector3D u = new Vector3D(x, y, z);
|
||||
checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeFrameTransform() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
|
||||
Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
Vector3D u = new Vector3D(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testApplyInverseToRotation() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
|
||||
|
|
@ -564,6 +628,44 @@ public class RotationTest {
|
|||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
|
||||
Rotation r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
Vector3D u = new Vector3D(x, y, z);
|
||||
checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
|
||||
|
||||
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
|
||||
Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
|
||||
Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
|
||||
|
||||
for (double x = -0.9; x < 0.9; x += 0.2) {
|
||||
for (double y = -0.9; y < 0.9; y += 0.2) {
|
||||
for (double z = -0.9; z < 0.9; z += 0.2) {
|
||||
Vector3D u = new Vector3D(x, y, z);
|
||||
checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testArray() throws MathIllegalArgumentException {
|
||||
|
||||
|
|
@ -696,7 +798,9 @@ public class RotationTest {
|
|||
final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
|
||||
final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM);
|
||||
final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM);
|
||||
final Rotation composite = r3.applyTo(r2.applyTo(r1));
|
||||
final Rotation composite = r1.compose(r2.compose(r3,
|
||||
RotationConvention.FRAME_TRANSFORM),
|
||||
RotationConvention.FRAME_TRANSFORM);
|
||||
final Vector3D good = composite.applyTo(startingVector);
|
||||
|
||||
Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);
|
||||
|
|
|
|||
|
|
@ -22,12 +22,14 @@ import java.io.ObjectInput;
|
|||
import java.io.ObjectOutput;
|
||||
|
||||
import org.apache.commons.math4.exception.DimensionMismatchException;
|
||||
import org.apache.commons.math4.exception.MathIllegalStateException;
|
||||
import org.apache.commons.math4.exception.MaxCountExceededException;
|
||||
import org.apache.commons.math4.exception.NoBracketingException;
|
||||
import org.apache.commons.math4.exception.NumberIsTooSmallException;
|
||||
import org.apache.commons.math4.ode.AbstractIntegrator;
|
||||
import org.apache.commons.math4.ode.ExpandableStatefulODE;
|
||||
import org.apache.commons.math4.ode.FirstOrderIntegrator;
|
||||
import org.apache.commons.math4.ode.MultistepIntegrator;
|
||||
import org.apache.commons.math4.ode.TestProblem1;
|
||||
import org.apache.commons.math4.ode.TestProblem5;
|
||||
import org.apache.commons.math4.ode.TestProblem6;
|
||||
|
|
@ -163,6 +165,29 @@ public class AdamsBashforthIntegratorTest {
|
|||
|
||||
}
|
||||
|
||||
@Test(expected=MathIllegalStateException.class)
|
||||
public void testStartFailure() {
|
||||
TestProblem1 pb = new TestProblem1();
|
||||
double minStep = 0.0001 * (pb.getFinalTime() - pb.getInitialTime());
|
||||
double maxStep = pb.getFinalTime() - pb.getInitialTime();
|
||||
double scalAbsoluteTolerance = 1.0e-6;
|
||||
double scalRelativeTolerance = 1.0e-7;
|
||||
|
||||
MultistepIntegrator integ =
|
||||
new AdamsBashforthIntegrator(6, minStep, maxStep,
|
||||
scalAbsoluteTolerance,
|
||||
scalRelativeTolerance);
|
||||
integ.setStarterIntegrator(new DormandPrince853Integrator(0.5 * (pb.getFinalTime() - pb.getInitialTime()),
|
||||
pb.getFinalTime() - pb.getInitialTime(),
|
||||
0.1, 0.1));
|
||||
TestProblemHandler handler = new TestProblemHandler(pb, integ);
|
||||
integ.addStepHandler(handler);
|
||||
integ.integrate(pb,
|
||||
pb.getInitialTime(), pb.getInitialState(),
|
||||
pb.getFinalTime(), new double[pb.getDimension()]);
|
||||
|
||||
}
|
||||
|
||||
private static class PerfectStarter extends AbstractIntegrator {
|
||||
|
||||
private final PerfectInterpolator interpolator;
|
||||
|
|
|
|||
Loading…
Reference in New Issue