/** * 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); }
/** * Transform this twist using the given transform. Effectively, the new twist * (T<sub>body</sub><sup>des, base</sup> = [ω<sub>new</sub>; ν<sub>new</sub>]) is * calculated as follows: * * <pre> * ω<sub>new</sub> = R ω * ν<sub>new</sub> = R ν + P × ω<sub>new</sub> * </pre> * * where: * <ul> * <li>ω and ν are the current angular and linear parts of this spatial vector, * <li>R and P are the rotation and translation parts of the given transform. * </ul> * </p> * * @param transform the transform to use on this. Not modified. */ default void applyTransform(RigidBodyTransform transform) { if (transform.hasRotation()) { getAngularPart().applyTransform(transform); getLinearPart().applyTransform(transform); } if (transform.hasTranslation()) addCrossToLinearPart(transform.getTranslationVector(), getAngularPart()); }
getLinearPart().applyInverseTransform(transform);