本文整理了Java中us.ihmc.robotics.geometry.GeometryTools.getAxisAngleFromZUpToVector()方法的一些代码示例，展示了GeometryTools.getAxisAngleFromZUpToVector()的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。GeometryTools.getAxisAngleFromZUpToVector()方法的具体详情如下：
包路径：us.ihmc.robotics.geometry.GeometryTools
类名称：GeometryTools
方法名：getAxisAngleFromZUpToVector

GeometryTools.getAxisAngleFromZUpToVector介绍

[英]Computes the complete minimum rotation from zUp = (0, 0, 1) to the given vector and packs it into an AxisAngle4d. The rotation axis if perpendicular to both vectors. The rotation angle is computed as the angle from the zUp to the vector:
rotationAngle = zUp.angle(vector).
Note: the vector does not need to be unit length.

Edge cases:

• the vector is aligned with zUp: the rotation angle is equal to 0.0 and the rotation axis is set to: (1, 0, 0).
• the vector is collinear pointing opposite direction of zUp: the rotation angle is equal to Math.PI and the rotation axis is set to: (1, 0, 0).
• if the length of the given normal is below 1.0E-7: the rotation angle is equal to 0.0 and the rotation axis is set to: (1, 0, 0).

Note: The calculation becomes less accurate as the two vectors are more collinear.

WARNING: This method generates garbage.
[中]计算从zUp=（0，0，1）到给定向量的完整最小旋转，并将其打包为AxisAngle4d。如果垂直于两个矢量，则旋转轴。旋转角度计算为从zUp到矢量的角度：
旋转角度=zUp。角度（矢量）。
注：矢量不需要是单位长度。
边缘情况：
*向量与zUp对齐：旋转角度等于0.0，旋转轴设置为：（1，0，0）。
*矢量共线指向zUp的相反方向：旋转角度等于Math。PI，旋转轴设置为：（1,0,0）。
*如果给定法线的长度低于1.0E-7：旋转角度等于0.0，旋转轴设置为：（1，0，0）。
注：由于两个向量更为共线，因此计算的精度降低。
警告：此方法生成垃圾。

代码示例

代码示例来源：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

@Override
  protected void updateTransformToParent(RigidBodyTransform transformToParent)
  {
   positionError.changeFrame(bodyFrame);
   GeometryTools.getAxisAngleFromZUpToVector(positionError.getVector(), rotationToControlFrame);
   transformToParent.setRotationAndZeroTranslation(rotationToControlFrame);
  }
};

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
  protected void updateTransformToParent(RigidBodyTransform transformToParent)
  {
   circleOrigin.get(localTranslation);
   rotationAxis.get(localRotationAxis);
   GeometryTools.getAxisAngleFromZUpToVector(localRotationAxis, localAxisAngle);
   transformToParent.set(localAxisAngle, localTranslation);
  }
};

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack)
{
 yoPlaneContactState.getContactNormalFrameVector(contactNormalVector);
 contactNormalVector.changeFrame(planeFrame);
 contactNormalVector.normalize();
 GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation);
 normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation);
}

代码示例来源：origin: us.ihmc/IHMCCommunication

public static PlanarRegion convertToPlanarRegion(PlanarRegionMessage planarRegionMessage)
{
 RigidBodyTransform transformToWorld = new RigidBodyTransform();
 Vector3d regionOrigin = new Vector3d(planarRegionMessage.getRegionOrigin());
 Vector3d regionNormal = new Vector3d(planarRegionMessage.getRegionNormal());
 AxisAngle4d regionOrientation = GeometryTools.getAxisAngleFromZUpToVector(regionNormal);
 transformToWorld.set(regionOrientation, regionOrigin);
 List<Point2f[]> messageHullsVertices = planarRegionMessage.getConcaveHullsVertices();
 List<Point2d[]> concaveHullsVertices = new ArrayList<>();
 for (int hullIndex = 0; hullIndex < messageHullsVertices.size(); hullIndex++)
 {
   Point2f[] messageHullVertices = messageHullsVertices.get(hullIndex);
   Point2d[] hullVertices = new Point2d[messageHullVertices.length];
   for (int vertexIndex = 0; vertexIndex < messageHullVertices.length; vertexIndex++)
   {
    hullVertices[vertexIndex] = new Point2d(messageHullVertices[vertexIndex]);
   }
   concaveHullsVertices.add(hullVertices);
 }
 List<ConvexPolygon2d> planarRegionConvexPolygons = new ArrayList<>();
 List<Point2f[]> convexPolygonsVertices = planarRegionMessage.getConvexPolygonsVertices();
 for (int polygonIndex = 0; polygonIndex < convexPolygonsVertices.size(); polygonIndex++)
 {
   ConvexPolygon2d convexPolygon = new ConvexPolygon2d(convexPolygonsVertices.get(polygonIndex));
   planarRegionConvexPolygons.add(convexPolygon);
 }
 PlanarRegion planarRegion = new PlanarRegion(transformToWorld, concaveHullsVertices, planarRegionConvexPolygons);
 planarRegion.setRegionId(planarRegionMessage.getRegionId());
 return planarRegion;
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

/**
* Creates a transform that transforms to the given point and rotates to make the z axis align with the
* normal vector.
*/
private static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint point, FrameVector zAxis)
{
 RigidBodyTransform ret = new RigidBodyTransform();
 ret.setRotation(GeometryTools.getAxisAngleFromZUpToVector(zAxis.getVectorCopy()));
 Vector3d translation = new Vector3d();
 point.get(translation);
 ret.setTranslation(translation);
 return ret;
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setInitialClearance(FrameVector initialDirection, double leaveDistance)
{
 this.initialDirection.set(initialDirection);
 this.initialDirection.normalize();
 this.initialDirection.get(tempVector);
 GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld);
 rotationPlane.setIncludingFrame(this.initialDirection.getReferenceFrame(), axisAngleToWorld);
 this.leaveDistance.set(leaveDistance);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setFinalApproach(FrameVector finalDirection, double approachDistance)
{
 this.finalDirection.set(finalDirection);
 this.finalDirection.normalize();
 this.finalDirection.get(tempVector);
 tempVector.negate();
 GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld);
 rotationPlane.setIncludingFrame(this.finalDirection.getReferenceFrame(), axisAngleToWorld);
 this.approachDistance.set(approachDistance);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance)
{
 this.initialPosition.set(initialPosition);
 this.initialDirection.set(initialDirection);
 this.initialDirection.normalize();
 this.initialDirection.get(tempVector);
 GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle);
 initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame());
 initialDistortionPose.setPosition(initialPosition);
 initialDistortionPose.setOrientation(tempAxisAngle);
 this.leaveDistance.set(leaveDistance);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance)
{
 this.finalPosition.set(finalPosition);
 this.finalDirection.set(finalDirection);
 this.finalDirection.normalize();
 this.finalDirection.get(tempVector);
 tempVector.negate();
 GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle);
 finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame());
 finalDistortionPose.setPosition(finalPosition);
 finalDistortionPose.setOrientation(tempAxisAngle);
 this.approachDistance.set(approachDistance);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
  protected void updateTransformToParent(RigidBodyTransform transformToParent)
  {
   tempPoint.setToZero(frameBeforeHipPitchJoint);
   tempPoint.changeFrame(endEffectorFrame);
   footToHipAxis.setIncludingFrame(tempPoint);
   footToHipAxis.changeFrame(getParent());
   GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), anklePitchRotationToParentFrame);
   anklePitchPosition.setToZero(endEffectorFrame);
   anklePitchPosition.changeFrame(getParent());
   anklePitchPosition.get(anklePitchToParentFrame);
   transformToParent.setRotationAndZeroTranslation(anklePitchRotationToParentFrame);
   transformToParent.setTranslation(anklePitchToParentFrame);
  }
};

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
  protected void updateTransformToParent(RigidBodyTransform transformToParent)
  {
   tempPoint.setToZero(frameBeforeHipPitchJoint);
   tempPoint.changeFrame(endEffectorFrame);
   footToHipAxis.setIncludingFrame(tempPoint);
   footToHipAxis.changeFrame(getParent());
   GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), hipPitchRotationToParentFrame);
   hipPitchPosition.setToZero(frameBeforeHipPitchJoint);
   hipPitchPosition.changeFrame(getParent());
   hipPitchPosition.get(hipPitchToParentFrame);
   transformToParent.setRotationAndZeroTranslation(hipPitchRotationToParentFrame);
   transformToParent.setTranslation(hipPitchToParentFrame);
  }
};

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public static RigidBodyTransform computeHandstepTransform(boolean rotateZIntoX, Tuple3d position, Vector3d surfaceNormal, double rotationAngleAboutNormal)
  {
   surfaceNormal.normalize();
   AxisAngle4d rotationAxisAngle = new AxisAngle4d();
   GeometryTools.getAxisAngleFromZUpToVector(surfaceNormal, rotationAxisAngle);

   AxisAngle4d rotationAboutNormal = new AxisAngle4d(surfaceNormal, rotationAngleAboutNormal);

   RigidBodyTransform transformOne = new RigidBodyTransform();
   transformOne.setRotationAndZeroTranslation(rotationAboutNormal);

   RigidBodyTransform transformTwo = new RigidBodyTransform();
   transformTwo.setRotationAndZeroTranslation(rotationAxisAngle);
   transformOne.multiply(transformTwo);

   if (rotateZIntoX)
   {
     RigidBodyTransform transformThree = new RigidBodyTransform();
     transformThree.setRotationPitchAndZeroTranslation(Math.PI / 2.0);
     transformOne.multiply(transformThree);
   }

   transformOne.setTranslation(new Vector3d(position));

   return transformOne;
  }
}