/** * Adds the given vectors to this vector angular and linear parts. * <p> * {@code this.angularPart += angular}<br> * {@code this.linearPart += linear} * </p> * * @param angular the vector to add to this vector's angular part. Not modified. * @param linear the vector to add to this vector's linear part. Not modified. */ default void add(Vector3DReadOnly angular, Vector3DReadOnly linear) { getAngularPart().add(angular); getLinearPart().add(linear); }
/** * Adds the given vectors to this vector angular and linear parts. * <p> * {@code this.angularPart += angular}<br> * {@code this.linearPart += linear} * </p> * * @param angular the vector to add to this vector's angular part. Not modified. * @param linear the vector to add to this vector's linear part. Not modified. * @throws ReferenceFrameMismatchException if any of the arguments are not expressed in the same * reference frame as {@code this}. */ default void add(FrameVector3DReadOnly angular, FrameVector3DReadOnly linear) { getAngularPart().add(angular); getLinearPart().add(linear); }
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyTransform(transform); centerOfMassOffset.applyTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, false, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
/** * Transforms this spatial inertia by the inverse of the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyInverseTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyInverseTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyInverseTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyInverseTransform(transform); centerOfMassOffset.applyInverseTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, true, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
/** * Sets this motion vector from a given motion measured at a different position. * <p> * Effectively, this motion is updated as follow: * * <pre> * ω<sub>this</sub> = ω<sub>new</sub> * ν<sub>this</sub> = ν<sub>new</sub> + P × ω<sub>new</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param angularPart the angular part of the motion. Not modified. * @param linearPart the linear part of the motion measured at the observer position. Not * modified. * @param observerPosition the location at which the motion is measured. Not modified. */ default void set(Vector3DReadOnly angularPart, Vector3DReadOnly linearPart, Point3DReadOnly observerPosition) { getAngularPart().set(angularPart); double linearPartX = linearPart.getX(); double linearPartY = linearPart.getY(); double linearPartZ = linearPart.getZ(); getLinearPart().cross(observerPosition, angularPart); getLinearPart().add(linearPartX, linearPartY, linearPartZ); }
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> * * @param transform the transform to use on this. Not modified. */ default void applyTransform(RigidBodyTransform transform) { if (transform.hasRotation()) { // Let's first apply the rotation onto the CoM and the mass moment of inertia: MecanoTools.transformSymmetricMatrix3D(transform.getRotationMatrix(), getMomentOfInertia()); getCenterOfMassOffset().applyTransform(transform); } if (transform.hasTranslation()) { // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(getMass(), getCenterOfMassOffset(), false, transform.getTranslationVector(), getMomentOfInertia()); getCenterOfMassOffset().add(transform.getTranslationVector()); } }
getAngularPart().add(x, y, z);