public static void rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, FramePose3D framePoseToPack) { GeometryTools.rotatePoseAboutAxis(rotationAxisFrame, rotationAxis, angle, false, false, framePoseToPack); }
public static void rotatePoseAboutAxis(FrameVector3D rotatationAxis, FramePoint3D rotationAxisOrigin, double angle, FramePose3D framePoseToPack) { ReferenceFrame frameWhoseZAxisIsRotationAxis = constructReferenceFrameFromPointAndZAxis("rotationAxisFrame", rotationAxisOrigin, rotatationAxis); rotatePoseAboutAxis(frameWhoseZAxisIsRotationAxis, Axis.Z, angle, framePoseToPack); } }
/** * Computes the complete minimum rotation from {@code zUp = (0, 0, 1)} to the given {@code vector} and packs it into an {@link AxisAngle4d}. * The rotation axis if perpendicular to both vectors. * The rotation angle is computed as the angle from the {@code zUp} to the {@code vector}: * <br> {@code rotationAngle = zUp.angle(vector)}. </br> * Note: the vector does not need to be unit length. * <p> * Edge cases: * <ul> * <li> the vector is aligned with {@code zUp}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * <li> the vector is collinear pointing opposite direction of {@code zUp}: the rotation angle is equal to {@code Math.PI} and the rotation axis is set to: (1, 0, 0). * <li> if the length of the given normal is below {@code 1.0E-7}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * </ul> * </p> * <p> * Note: The calculation becomes less accurate as the two vectors are more collinear. * </p> * <p> * WARNING: This method generates garbage. * </p> * * @param vector the 3D vector that is rotated with respect to {@code zUp}. Not modified. * @return the minimum rotation from {@code zUp} to the given {@code vector}. */ public static AxisAngle4d getAxisAngleFromZUpToVector(Vector3d vector) { AxisAngle4d axisAngle = new AxisAngle4d(); getAxisAngleFromZUpToVector(vector, axisAngle); return axisAngle; }
/** * Tests if the two given planes are coincident: * <ul> * <li> {@code planeNormal1} and {@code planeNormal2} are collinear given the tolerance {@code angleEpsilon}. * <li> the distance of {@code pointOnPlane2} from the first plane is less than {@code distanceEpsilon}. * </ul> * <p> * Edge cases: * <ul> * <li> if the length of either normal is below {@code 1.0E-7}, this method fails and returns {@code false}. * </ul> * </p> * * @param pointOnPlane1 a point on the first plane. Not modified. * @param planeNormal1 the normal of the first plane. Not modifed. * @param pointOnPlane2 a point on the second plane. Not modified. * @param planeNormal2 the normal of the second plane. Not modified. * @param angleEpsilon tolerance on the angle in radians to determine if the plane normals are collinear. * @param distanceEpsilon tolerance on the distance to determine if {@code pointOnPlane2} belongs to the first plane. * @return {@code true} if the two planes are coincident, {@code false} otherwise. */ public static boolean arePlanesCoincident(Point3d pointOnPlane1, Vector3d planeNormal1, Point3d pointOnPlane2, Vector3d planeNormal2, double angleEpsilon, double distanceEpsilon) { if (!areVectorsCollinear(planeNormal1, planeNormal2, angleEpsilon)) return false; else return distanceFromPointToPlane(pointOnPlane2, pointOnPlane1, planeNormal1) < distanceEpsilon; }
/** * Computes the normal of a plane that is defined by three points. * <p> * Edge cases: * <ul> * <li> Returns a {@code null} if the three points are on a line. * <li> Returns {@code null} if two or three points are equal. * </ul> * </p> * <p> * WARNING: This method generates garbage. * </p> * * @param firstPointOnPlane first point on the plane. Not modified. * @param secondPointOnPlane second point on the plane. Not modified. * @param thirdPointOnPlane third point on the plane. Not modified. * @return the plane normal or {@code null} when the normal could not be determined. */ public static Vector3d getPlaneNormalGivenThreePoints(Point3d firstPointOnPlane, Point3d secondPointOnPlane, Point3d thirdPointOnPlane) { Vector3d normal = new Vector3d(); boolean success = getPlaneNormalGivenThreePoints(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normal); if (!success) return null; else return normal; }
boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector); if (!success) return null;
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout = 30000) public void testConstructFrameFromPointAndAxis() { Random random = new Random(1776L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePoint3D randomPoint = new FramePoint3D(worldFrame); FrameVector3D randomVector = new FrameVector3D(worldFrame); int numberOfTests = 100000; for (int i = 0; i < numberOfTests; i++) { randomPoint.setIncludingFrame(EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame, 10.0, 10.0, 10.0)); randomVector.setIncludingFrame(EuclidFrameRandomTools.nextFrameVector3D(random, worldFrame, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0)); ReferenceFrame frameA = GeometryTools.constructReferenceFrameFromPointAndZAxis("frameA", randomPoint, randomVector); ReferenceFrame frameB = GeometryTools.constructReferenceFrameFromPointAndAxis("frameB", randomPoint, Axis.Z, randomVector); EuclidCoreTestTools.assertRigidBodyTransformEquals(frameA.getTransformToRoot(), frameB.getTransformToRoot(), 1.0e-2); } }
/** * Computes the complete minimum rotation from {@code zUp = (0, 0, 1)} to the given {@code vector} and packs it into an {@link AxisAngle4d}. * The rotation axis if perpendicular to both vectors. * The rotation angle is computed as the angle from the {@code zUp} to the {@code vector}: * <br> {@code rotationAngle = zUp.angle(vector)}. </br> * Note: the vector does not need to be unit length. * <p> * Edge cases: * <ul> * <li> the vector is aligned with {@code zUp}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * <li> the vector is collinear pointing opposite direction of {@code zUp}: the rotation angle is equal to {@code Math.PI} and the rotation axis is set to: (1, 0, 0). * <li> if the length of the given normal is below {@code 1.0E-7}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * </ul> * </p> * <p> * Note: The calculation becomes less accurate as the two vectors are more collinear. * </p> * * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. Modified. */ public static void getAxisAngleFromZUpToVector(Vector3d vector, AxisAngle4d rotationToPack) { getAxisAngleFromFirstToSecondVector(0.0, 0.0, 1.0, vector.getX(), vector.getY(), vector.getZ(), rotationToPack); }
public JointAxisVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); if(joint instanceof OneDoFJointBasics) { FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis(); ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis)); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold()); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); } List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
/** * Computes the orthogonal projection of a 3D point on a given 3D plane defined by a 3D point and 3D normal. * <p> * Edge cases: * <ul> * <li> if the length of the plane normal is too small, i.e. less than {@link Epsilons#ONE_TRILLIONTH}, * this method fails and returns {@code false}. * </ul> * </p> * * @param pointToProject the point to compute the projection of. Not modified. * @param pointOnPlane a point on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @return the projection of the point onto the plane, or {@code null} if the method failed. */ public static Point3d getOrthogonalProjectionOnPlane(Point3d pointToProject, Point3d pointOnPlane, Vector3d planeNormal) { Point3d projection = new Point3d(); boolean success = getOrthogonalProjectionOnPlane(pointToProject, pointOnPlane, planeNormal, projection); if (!success) return null; else return projection; }
/** * Creates a new reference frame such that it is centered at the given {@code point} and with its * z-axis aligned with the given {@code zAxis} vector. * <p> * Note that the parent frame is set to the reference frame the given {@code point} and * {@code zAxis} are expressed in. * </p> * * @param frameName the name of the new frame. * @param point location of the reference frame's origin. Not modified. * @param zAxis orientation the reference frame's z-axis. Not modified. * @return the new reference frame. * @throws ReferenceFrameMismatchException if {@code point} and {@code zAxis} are not expressed * in the same reference frame. */ public static ReferenceFrame constructReferenceFrameFromPointAndZAxis(String frameName, FramePoint3D point, FrameVector3D zAxis) { return constructReferenceFrameFromPointAndAxis(frameName, point, Axis.Z, zAxis); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testClipToBoundingBox() { Tuple3DBasics tuple3d = new Point3D(1.0, -1.0, 0.0); GeometryTools.clipToBoundingBox(tuple3d, -0.5, 0.5, 0.5, -0.5, 0.0, 0.0); EuclidCoreTestTools.assertTuple3DEquals("not equal", new Point3D(0.5, -0.5, 0.0), tuple3d, 0.0); tuple3d.set(1.0, -1.0, 0.0); GeometryTools.clipToBoundingBox(tuple3d, 0.5, -0.5, -0.5, 0.5, -0.1, 0.1); EuclidCoreTestTools.assertTuple3DEquals("not equal", new Point3D(0.5, -0.5, 0.0), tuple3d, 0.0); tuple3d.set(1.0, -1.0, 2.0); GeometryTools.clipToBoundingBox(tuple3d, 0.5, -0.5, -0.5, 0.5, -0.1, 1.0); EuclidCoreTestTools.assertTuple3DEquals("not equal", new Point3D(0.5, -0.5, 1.0), tuple3d, 0.0); }
/** * Computes the minimum distance between a given point and a plane. * * @param point the 3D query. Not modified. * @param pointOnPlane a point located on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @return the distance between the point and the plane. * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference frame. */ public static double distanceFromPointToPlane(FramePoint point, FramePoint pointOnPlane, FrameVector planeNormal) { point.checkReferenceFrameMatch(pointOnPlane); point.checkReferenceFrameMatch(planeNormal); return distanceFromPointToPlane(point.getPoint(), pointOnPlane.getPoint(), planeNormal.getVector()); }
/** * Computes the normal of a plane that is defined by three points. * <p> * Edge cases: * <ul> * <li> Returns a {@code null} if the three points are on a line. * <li> Returns {@code null} if two or three points are equal. * </ul> * </p> * <p> * WARNING: This method generates garbage. * </p> * * @param firstPointOnPlane first point on the plane. Not modified. * @param secondPointOnPlane second point on the plane. Not modified. * @param thirdPointOnPlane third point on the plane. Not modified. * @return the plane normal or {@code null} when the normal could not be determined. * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference frame. */ public static FrameVector3D getPlaneNormalGivenThreePoints(FramePoint3D firstPointOnPlane, FramePoint3D secondPointOnPlane, FramePoint3D thirdPointOnPlane) { FrameVector3D normal = new FrameVector3D(); boolean success = getPlaneNormalGivenThreePoints(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normal); if (!success) return null; else return normal; }
boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector); if (!success) return null;
/** * Computes the complete minimum rotation from {@code firstVector} to the {@code secondVector} and packs it into an {@link AxisAngle4d}. * The rotation axis if perpendicular to both vectors. * The rotation angle is computed as the angle from the {@code firstVector} to the {@code secondVector}: * <br> {@code rotationAngle = firstVector.angle(secondVector)}. </br> * Note: the vectors do not need to be unit length. * <p> * Edge cases: * <ul> * <li> the vectors are the same: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * <li> the vectors are collinear pointing opposite directions: the rotation angle is equal to {@code Math.PI} and the rotation axis is set to: (1, 0, 0). * <li> if the length of either normal is below {@code 1.0E-7}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * </ul> * </p> * <p> * Note: The calculation becomes less accurate as the two vectors are more collinear. * </p> * * @param firstVector the first vector. Not modified. * @param secondVector the second vector that is rotated with respect to the first vector. Not modified. * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. Modified. */ public static void getAxisAngleFromFirstToSecondVector(Vector3d firstVector, Vector3d secondVector, AxisAngle4d rotationToPack) { getAxisAngleFromFirstToSecondVector(firstVector.getX(), firstVector.getY(), firstVector.getZ(), secondVector.getX(), secondVector.getY(), secondVector.getZ(), rotationToPack); }
this.cylinderReferenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(name, new FramePoint3D(world), cylinderZAxisExpressedInWorld);
/** * Computes the orthogonal projection of a 3D point on a given 3D plane defined by a 3D point and 3D normal. * <p> * Edge cases: * <ul> * <li> if the length of the plane normal is too small, i.e. less than {@link Epsilons#ONE_TRILLIONTH}, * this method fails and returns {@code false}. * </ul> * </p> * * @param pointToProject the point to compute the projection of. Not modified. * @param pointOnPlane a point on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @return the projection of the point onto the plane, or {@code null} if the method failed. * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference frame. */ public static FramePoint getOrthogonalProjectionOnPlane(FramePoint pointToProject, FramePoint pointOnPlane, FrameVector planeNormal) { FramePoint projection = new FramePoint(); boolean success = getOrthogonalProjectionOnPlane(pointToProject, pointOnPlane, planeNormal, projection); if (!success) return null; else return projection; }
private void initializeFourBarWithRandomlyRotatedJointFrames(FramePoint3D jointAPosition, FramePoint3D jointBPosition, FramePoint3D jointCPosition, FramePoint3D jointDPosition, FrameVector3D jointAxisA, FrameVector3D jointAxisB, FrameVector3D jointAxisC, FrameVector3D jointAxisD) { ReferenceFrame jointAFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointAFrame", jointAPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointBFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointBFrame", jointBPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointCFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointCFrame", jointCPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); ReferenceFrame jointDFrame = GeometryTools.constructReferenceFrameFromPointAndAxis("jointDFrame", jointDPosition, Axis.Z, new FrameVector3D(worldFrame, RandomGeometry.nextVector3D(random, 1.0))); Vector3D jointAxisAFrameA = new Vector3D(); Vector3D jointAxisBFrameB = new Vector3D(); Vector3D jointAxisCFrameC = new Vector3D(); Vector3D jointAxisDFrameD = new Vector3D(); jointAxisA.changeFrame(jointAFrame); jointAxisAFrameA.set(jointAxisA); jointAxisB.changeFrame(jointBFrame); jointAxisBFrameB.set(jointAxisB); jointAxisC.changeFrame(jointCFrame); jointAxisCFrameC.set(jointAxisC); jointAxisD.changeFrame(jointDFrame); jointAxisDFrameD.set(jointAxisD); RigidBodyTransform jointAtoElevator = jointAFrame.getTransformToDesiredFrame(worldFrame); RigidBodyTransform jointBtoA = jointBFrame.getTransformToDesiredFrame(jointAFrame); RigidBodyTransform jointCtoB = jointCFrame.getTransformToDesiredFrame(jointBFrame); RigidBodyTransform jointDtoC = jointDFrame.getTransformToDesiredFrame(jointCFrame); initializeFourBar(jointAtoElevator, jointBtoA, jointCtoB, jointDtoC, jointAxisAFrameA, jointAxisBFrameB, jointAxisCFrameC, jointAxisDFrameD); }
GeometryTools.clipToBoundingBox(pointToCheckAndPack, 0.0, getLength(), -getWidth() / 2.0, getWidth() / 2.0, 0.0, getHeight());