/** * Transforms this plane using the inverse of the given homogeneous transformation matrix. * * @param transform the transform to apply on this plane's point and normal. Not modified. * @throws RuntimeException if this plane has not been initialized yet. */ @Override public void applyInverseTransform(Transform transform) { checkHasBeenInitialized(); point.applyInverseTransform(transform); normal.applyInverseTransform(transform); }
public ConnectionPoint3D applyInverseTransform(Transform transform) { Point3D transformed = new Point3D(this); transformed.applyInverseTransform(transform); return new ConnectionPoint3D(transformed, regionId); } }
public static boolean isPlanarRegionIntersectingWithCapsule(LineSegment3D capsuleSegmentInWorld, double capsuleRadius, PlanarRegion query) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); query.getTransformToWorld(transformToWorld); Point3D firstEndPointInLocal = new Point3D(capsuleSegmentInWorld.getFirstEndpoint()); Point3D secondEndPointInLocal = new Point3D(capsuleSegmentInWorld.getSecondEndpoint()); firstEndPointInLocal.applyInverseTransform(transformToWorld); secondEndPointInLocal.applyInverseTransform(transformToWorld); Point2D[] concaveHull = query.getConcaveHull(); List<Point3D> convexPolygon3D = new ArrayList<>(); for (int i = 0; i < concaveHull.length; i++) convexPolygon3D.add(new Point3D(concaveHull[i])); double minDistanceToEdge = getDistanceFromLineSegment3DToConvexPolygon(firstEndPointInLocal, secondEndPointInLocal, convexPolygon3D); return minDistanceToEdge <= capsuleRadius; }
observerPosition.applyInverseTransform(transform); spatialVector.applyInverseTransform(transform); addCrossToLinearPart(observerPosition, getAngularPart());
observerPosition.applyInverseTransform(transform); spatialVector.applyInverseTransform(transform); addCrossToLinearPart(observerPosition, getAngularPart());
pointOfApplication.applyInverseTransform(transform); // p spatialVector.applyInverseTransform(transform);
/** * 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); } }
pointOnPlaneInRegionFrame.applyInverseTransform(transformFromRegionToWorld); planeNormalInRegionFrame.applyInverseTransform(transformFromRegionToWorld);
pointOnPlane.applyInverseTransform(transform);
pointOnPlane.applyInverseTransform(transform);
pointOnPlane.applyInverseTransform(transform);
transform.invertRotation(); point0.applyInverseTransform(transform); point1.applyInverseTransform(transform); point2.applyInverseTransform(transform); point3.applyInverseTransform(transform);
intersectionInPlaneFrame.applyInverseTransform(regionToWorld); Point2D intersectionInPlaneFrame2D = new Point2D(intersectionInPlaneFrame);
public static Point3D intersectRegionWithRay(PlanarRegion region, Point3D rayStart, Vector3D rayDirection) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3D planeNormal = new Vector3D(0.0, 0.0, 1.0); planeNormal.applyTransform(regionToWorld); Point3D pointOnPlane = new Point3D(); pointOnPlane.set(region.getConvexPolygon(0).getVertex(0)); pointOnPlane.applyTransform(regionToWorld); Point3D intersectionWithPlane = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, rayStart, rayDirection); if (intersectionWithPlane == null) { return null; } Point3D intersectionInPlaneFrame = new Point3D(intersectionWithPlane); intersectionInPlaneFrame.applyInverseTransform(regionToWorld); // checking convex hull here - might be better to check all polygons to avoid false positive if (!region.getConvexHull().isPointInside(intersectionInPlaneFrame.getX(), intersectionInPlaneFrame.getY())) { return null; } Vector3D rayToIntersection = new Vector3D(); rayToIntersection.sub(intersectionWithPlane, rayStart); if (rayToIntersection.dot(rayDirection) < 0.0) { return null; } return intersectionWithPlane; }