checkReferenceFrameMatch(other); if (!getAngularPart().geometricallyEquals(getAngularPart(), epsilon)) return false; if (!getLinearPart().geometricallyEquals(getLinearPart(), epsilon)) return false; return true;
/** * Gets a representative {@code String} of {@code spatialForce} given a specific format to use. * <p> * Using the default format {@link #DEFAULT_FORMAT}, this provides a {@code String} as follows: * * <pre> * Spatial Force: [angular = ( 0.174, 0.732, -0.222 ), linear = ( 0.174, 0.732, -0.222 )] - expressedInFrame * </pre> * </p> * * @param format the format to use for each number. * @param spatialForce the object to get the {@code String} of. Not modified. * @return the representative {@code String}. */ public static String getSpatialForceString(String format, SpatialForceReadOnly spatialForce) { if (spatialForce == null) return "null"; else return getSpatialForceVectorString(format, spatialForce.getReferenceFrame(), spatialForce.getAngularPart(), spatialForce.getLinearPart()); }
/** * Tests on a per component basis if this vector is equal to the given {@code other} to an * {@code epsilon} and both vectors have the same frames. * * @param other the other wrench to compare against this. Not modified. * @param epsilon the tolerance to use when comparing each component. * @return {@code true} if the two vectors are equal, {@code false} otherwise. */ default boolean epsilonEquals(WrenchReadOnly other, double epsilon) { if (getBodyFrame() != other.getBodyFrame()) return false; return SpatialForceReadOnly.super.epsilonEquals(other, epsilon); }
/** * Tests on a per component basis, if this wrench is exactly equal to {@code other} and with the * same reference frames. * * @param other the other wrench to compare against this. Not modified. * @return {@code true} if the two wrenches are exactly equal component-wise, {@code false} * otherwise. */ default boolean equals(WrenchReadOnly other) { if (other == null) return false; if (getBodyFrame() != other.getBodyFrame()) return false; return SpatialForceReadOnly.super.equals(other); } }
/** * Gets the center of mass acceleration of the multi-body system. * * @return the center of mass acceleration. */ public FrameVector3DReadOnly getCenterOfMassAcceleration() { if (!isCenterOfMassAccelerationUpToDate) { centerOfMassAcceleration.setAndScale(1.0 / getTotalMass(), getMomentumRate().getLinearPart()); isCenterOfMassAccelerationUpToDate = true; } return centerOfMassAcceleration; }
/** * Asserts on a per component basis that the two spatial force vectors are equal to an * {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the automated message. * @param expected the expected spatial force vector. Not modified. * @param actual the actual spatial force vector. Not modified. * @param epsilon the tolerance to use. * @param format the format to use for printing each component when an {@code AssertionError} is * thrown. * @throws AssertionError if the two spatial force vectors are not equal. If only one of the * arguments is equal to {@code null}. */ public static void assertSpatialForceEquals(String messagePrefix, SpatialForceReadOnly expected, SpatialForceReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }