/** * 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); }
/** * 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); }
alignAxisWithThis.changeFrame(currentXYZAxis.getReferenceFrame()); AxisAngle4d rotationToDesired = new AxisAngle4d(); GeometryTools.getAxisAngleFromFirstToSecondVector(currentXYZAxis.getVector(), alignAxisWithThis.getVector(), rotationToDesired); alignAxisWithThis.changeFrame(initialFrame);
rayThroughSphere.normalize(); GeometryTools.getAxisAngleFromFirstToSecondVector(xAxis, rayThroughSphere, rotationForXAxisAlignedWithRay); rotationMatrixForXAxisAlignedWithRay.set(rotationForXAxisAlignedWithRay);
public static ReferenceFrame constructReferenceFrameFromPointAndAxis(String frameName, FramePoint point, Axis axisToAlign, FrameVector alignAxisWithThis) { point.checkReferenceFrameMatch(alignAxisWithThis.getReferenceFrame()); FrameVector referenceNormal = new FrameVector(); switch (axisToAlign) { case X: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 1.0, 0.0, 0.0); break; case Y: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 0.0, 1.0, 0.0); break; case Z: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 0.0, 0.0, 1.0); break; default: break; } AxisAngle4d rotationToDesired = new AxisAngle4d(); alignAxisWithThis.changeFrame(referenceNormal.getReferenceFrame()); GeometryTools.getAxisAngleFromFirstToSecondVector(referenceNormal.getVectorCopy(), alignAxisWithThis.getVectorCopy(), rotationToDesired); RigidBodyTransform transformToDesired = new RigidBodyTransform(); transformToDesired.setRotation(rotationToDesired); Vector3d translation = new Vector3d(); point.get(translation); transformToDesired.setTranslation(translation); return constructFrameWithUnchangingTransformToParent(frameName, point.getReferenceFrame(), transformToDesired); }
GeometryTools.getAxisAngleFromFirstToSecondVector(expectedArmZeroConfiguration, tempVector.getVector(), rotation); armZeroJointAngleConfigurationOffset.setRotation(rotation);