本文整理了Java中us.ihmc.robotics.geometry.GeometryTools
类的一些代码示例,展示了GeometryTools
类的具体用法。这些代码示例主要来源于Github
/Stackoverflow
/Maven
等平台,是从一些精选项目中提取出来的代码,具有较强的参考意义,能在一定程度帮忙到你。GeometryTools
类的具体详情如下:
包路径:us.ihmc.robotics.geometry.GeometryTools
类名称:GeometryTools
暂无
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit
public static void rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, FramePose3D framePoseToPack)
{
GeometryTools.rotatePoseAboutAxis(rotationAxisFrame, rotationAxis, angle, false, false, framePoseToPack);
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit
public static void rotatePoseAboutAxis(FrameVector3D rotatationAxis, FramePoint3D rotationAxisOrigin, double angle, FramePose3D framePoseToPack)
{
ReferenceFrame frameWhoseZAxisIsRotationAxis = constructReferenceFrameFromPointAndZAxis("rotationAxisFrame", rotationAxisOrigin,
rotatationAxis);
rotatePoseAboutAxis(frameWhoseZAxisIsRotationAxis, Axis.Z, angle, framePoseToPack);
}
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit
boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector);
if (!success)
return null;
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit-test
@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);
}
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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);
}
代码示例来源:origin: us.ihmc/simulation-construction-set-tools
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);
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit
/**
* 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);
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit-test
@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);
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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());
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector);
if (!success)
return null;
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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);
}
代码示例来源:origin: us.ihmc/simulation-construction-set-tools
this.cylinderReferenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(name, new FramePoint3D(world), cylinderZAxisExpressedInWorld);
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
/**
* 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;
}
代码示例来源:origin: us.ihmc/ihmc-robotics-toolkit-test
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);
}
代码示例来源:origin: us.ihmc/IHMCRoboticsToolkit
GeometryTools.clipToBoundingBox(pointToCheckAndPack, 0.0, getLength(), -getWidth() / 2.0, getWidth() / 2.0, 0.0, getHeight());
内容来源于网络,如有侵权,请联系作者删除!