public void setIncludingFrame(double time, FramePoint3DReadOnly position, FrameQuaternionReadOnly orientation, FrameVector3DReadOnly linearVelocity, FrameVector3DReadOnly angularVelocity) { position.checkReferenceFrameMatch(orientation); position.checkReferenceFrameMatch(linearVelocity); position.checkReferenceFrameMatch(angularVelocity); setToZero(position.getReferenceFrame()); geometryObject.set(time, position, orientation, linearVelocity, angularVelocity); }
public static void pitchAboutPoint(FramePoint3DReadOnly pointToTransform, FramePoint3DReadOnly pointToPitchAbout, double pitch, FramePoint3D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToPitchAbout); double tempX = pointToTransform.getX() - pointToPitchAbout.getX(); double tempY = pointToTransform.getY() - pointToPitchAbout.getY(); double tempZ = pointToTransform.getZ() - pointToPitchAbout.getZ(); double cosAngle = Math.cos(pitch); double sinAngle = Math.sin(pitch); double x = cosAngle * tempX + sinAngle * tempZ; tempZ = -sinAngle * tempX + cosAngle * tempZ; tempX = x; resultToPack.setIncludingFrame(pointToPitchAbout); resultToPack.add(tempX, tempY, tempZ); }
/** * Sets the next ball to the given location, and gives it the given appearance. If all the balls * have been set, then does nothing. * * @param location FramePoint to set the next ball to. * @param appearance Appearance to give the next ball. */ public void setBall(FramePoint3DReadOnly location, AppearanceDefinition appearance, int ballIndex) { //TODO: PDN, note that with current implementation of JME, you can only "set" the appearance once. After that, it will ignore all appearance sets if (!location.getReferenceFrame().isWorldFrame()) throw new RuntimeException(location + " must be in a World Frame!"); setBall(location.getX(), location.getY(), location.getZ(), appearance, ballIndex); }
public void setCubicUsingIntermediatePoints(double t0, double tIntermediate1, double tIntermediate2, double tFinal, FramePoint3DReadOnly z0, FramePoint3DReadOnly zIntermediate1, FramePoint3DReadOnly zIntermediate2, FramePoint3DReadOnly zFinal) { z0.checkReferenceFrameMatch(referenceFrame); zIntermediate1.checkReferenceFrameMatch(referenceFrame); zIntermediate2.checkReferenceFrameMatch(referenceFrame); zFinal.checkReferenceFrameMatch(referenceFrame); super.setCubicUsingIntermediatePoints(t0, tIntermediate1, tIntermediate2, tFinal, z0, zIntermediate1, zIntermediate2, zFinal); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(FramePoint3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { setIncludingFrame(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
FramePoint3DReadOnly secondPointOnLine2, FramePoint3D intersectionToPack) firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1); firstPointOnLine2.checkReferenceFrameMatch(secondPointOnLine2); firstPointOnLine1.checkReferenceFrameMatch(firstPointOnLine2); intersectionToPack.changeFrame(firstPointOnLine1.getReferenceFrame()); double pointOnLine1x = firstPointOnLine1.getX(); double pointOnLine1y = firstPointOnLine1.getY(); double lineDirection1x = secondPointOnLine1.getX() - firstPointOnLine1.getX(); double lineDirection1y = secondPointOnLine1.getY() - firstPointOnLine1.getY(); double pointOnLine2x = firstPointOnLine2.getX(); double pointOnLine2y = firstPointOnLine2.getY(); double lineDirection2x = secondPointOnLine2.getX() - firstPointOnLine2.getX(); double lineDirection2y = secondPointOnLine2.getY() - firstPointOnLine2.getY();
/** * Tests on a per-vertex basis if this supplier and {@code other} are equal to an * {@code epsilon}. * * @param other the other supplier to compare against this. * @param epsilon the tolerance to use. * @return {@code true} if the two suppliers are equal. */ default boolean epsilonEquals(FrameVertex3DSupplier other, double epsilon) { if (getNumberOfVertices() != other.getNumberOfVertices()) return false; for (int i = 0; i < getNumberOfVertices(); i++) { if (!getVertex(i).epsilonEquals(other.getVertex(i), epsilon)) return false; } return true; }
public void setQuadraticUsingIntermediatePoint(double t0, double tIntermediate, double tFinal, FramePoint3DReadOnly z0, FramePoint3DReadOnly zIntermediate, FramePoint3DReadOnly zFinal) { z0.checkReferenceFrameMatch(referenceFrame); zIntermediate.checkReferenceFrameMatch(referenceFrame); zFinal.checkReferenceFrameMatch(referenceFrame); super.setQuadraticUsingIntermediatePoint(t0, tIntermediate, tFinal, z0, zIntermediate, zFinal); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(FramePoint3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { setIncludingFrame(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FramePoint3DReadOnly expectedPosition, FrameVector3DReadOnly expectedLinearVelocity, FrameEuclideanTrajectoryPoint testedFrameEuclideanTrajectoryPoint, double epsilon) { assertTrue(expectedFrame == testedFrameEuclideanTrajectoryPoint.getReferenceFrame()); assertEquals(expectedTime, testedFrameEuclideanTrajectoryPoint.getTime(), epsilon); assertTrue(expectedPosition.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getPosition(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getLinearVelocity(), epsilon)); Point3D actualPosition = new Point3D(); Vector3D actualLinearVelocity = new Vector3D(); testedFrameEuclideanTrajectoryPoint.getPosition(actualPosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); FramePoint3D actualFramePosition = new FramePoint3D(); FrameVector3D actualFrameLinearVelocity = new FrameVector3D(); testedFrameEuclideanTrajectoryPoint.getPositionIncludingFrame(actualFramePosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); actualFramePosition = new FramePoint3D(expectedFrame); actualFrameLinearVelocity = new FrameVector3D(expectedFrame); testedFrameEuclideanTrajectoryPoint.getPosition(actualFramePosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); }
/** * Redefines this line with a new point and a new direction vector. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code pointOnLine}, * {@code lineDirection} are not expressed in the same reference frame. */ default void set(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); set(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
public void setCubicUsingIntermediatePoint(double t0, double tIntermediate1, double tFinal, FramePoint3DReadOnly z0, FramePoint3DReadOnly zIntermediate1, FramePoint3DReadOnly zFinal) { z0.checkReferenceFrameMatch(referenceFrame); zIntermediate1.checkReferenceFrameMatch(referenceFrame); zFinal.checkReferenceFrameMatch(referenceFrame); super.setCubicUsingIntermediatePoint(t0, tIntermediate1, tFinal, z0, zIntermediate1, zFinal); }
/** * Redefines this line such that it goes through the two given points in the given reference frame. * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. */ default void setIncludingFrame(FramePoint3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine) { setIncludingFrame(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint * @param yaw double * @return CartesianPositionFootstep */ public static void yawAboutPoint(FramePoint3DReadOnly pointToTransform, FramePoint3DReadOnly pointToYawAbout, double yaw, FramePoint3D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToYawAbout); double tempX = pointToTransform.getX() - pointToYawAbout.getX(); double tempY = pointToTransform.getY() - pointToYawAbout.getY(); double tempZ = pointToTransform.getZ() - pointToYawAbout.getZ(); double cosAngle = Math.cos(yaw); double sinAngle = Math.sin(yaw); double x = cosAngle * tempX + -sinAngle * tempY; tempY = sinAngle * tempX + cosAngle * tempY; tempX = x; resultToPack.setIncludingFrame(pointToYawAbout); resultToPack.add(tempX, tempY, tempZ); }
assertEquals(expectedNamePrefix, testedYoFrameEuclideanTrajectoryPoint.getNamePrefix()); assertEquals(expectedNameSuffix, testedYoFrameEuclideanTrajectoryPoint.getNameSuffix()); assertTrue(expectedPosition.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getPosition(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(), epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); testedYoFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
/** * Redefines this line segment with new endpoints. * * @param firstEndpoint new endpoint of this line segment. Not modified * @param secondEndpoint new second endpoint of this line segment. Not modified. * @throws ReferenceFrameMismatchException if {@code firstEndpoint} and {@code secondEndpoint} are * not expressed in the same reference frame. */ default void setIncludingFrame(FramePoint3DReadOnly firstEndpoint, FramePoint3DReadOnly secondEndpoint) { firstEndpoint.checkReferenceFrameMatch(secondEndpoint); setIncludingFrame(firstEndpoint.getReferenceFrame(), firstEndpoint, secondEndpoint); }
public void setQuinticWithZeroTerminalVelocityAndAcceleration(double t0, double tFinal, FramePoint3DReadOnly z0, FramePoint3DReadOnly zFinal) { z0.checkReferenceFrameMatch(referenceFrame); zFinal.checkReferenceFrameMatch(referenceFrame); super.setQuinticWithZeroTerminalVelocityAndAcceleration(t0, tFinal, z0, zFinal); }
/** * Redefines this line such that it goes through the two given points. * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code firstPointOnLine} are not * expressed in the same reference frame. */ default void set(FramePoint3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine) { set(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
assertTrue(expectedPosition.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getPosition(), epsilon)); assertTrue(expectedOrientation.geometricallyEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getSO3Waypoint().getOrientation(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameSE3TrajectoryPoint.getGeometryObject().getEuclideanWaypoint().getLinearVelocity(), epsilon)); testedFrameSE3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); testedFrameSE3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); testedFrameSE3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon));
/** * Redefines this line with a new point and a new direction vector. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code pointOnLine}, * {@code lineDirection} are not expressed in the same reference frame. */ default void set(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); set(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }