How to use

GeometryTools

in

us.ihmc.robotics.geometry

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());