public static boolean isInsideBoundsInclusive(double val, double max) { return isInsideBoundsInclusive(val, -max, max); }
public static void checkIfInRange(long argument, long min, long max) { if (!isInsideBoundsInclusive(argument, min, max)) { throw new RuntimeException("Argument " + argument + " not in range [" + min + ", " + max + "]."); } }
public static void checkIfInRange(double argument, double min, double max) { if (!isInsideBoundsInclusive(argument, min, max)) { throw new RuntimeException("Argument " + argument + " not in range [" + min + ", " + max + "]."); } }
public boolean isBetweenEndpointsInclusive(double point) { return MathTools.isInsideBoundsInclusive(point, getMinPoint(), getMaxPoint()); }
/** * Given a 3D point in world coordinates, computes whether the point is in this region. * @param point3dInWorld query expressed in world coordinates. * @param maximumOrthogonalDistance tolerance expressed as maximum orthogonal distance from the region. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(Point3d point3dInWorld, double maximumOrthogonalDistance) { Point3d localPoint = new Point3d(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); if (!MathTools.isInsideBoundsInclusive(localPoint.getZ(), maximumOrthogonalDistance)) return false; else return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Compute every angle of the quadrilateral AND crop the range of the input angle so that the four bar doesn't flip. * @param angleDABInRadians is the angle formed by the sides a and b (see scheme in this class) * @return true if the angle DAB is out of range making the quadrilateral non-convex */ public boolean updateAnglesGivenAngleDAB(double angleDABInRadians) { // Solve angles double A = clipToMinMax(angleDABInRadians, minA, maxA); double e = getUnknownTriangleSideLengthByLawOfCosine(a, b, A); double C = FourbarCalculatorTools.getAngleWithCosineLaw(c, d, e); double angleDBA = FourbarCalculatorTools.getAngleWithCosineLaw(b, e, a); double angleDBC = FourbarCalculatorTools.getAngleWithCosineLaw(c, e, d); double B = angleDBA + angleDBC; double D = 2 * PI - A - B - C; this.angleDAB = A; this.angleABC = B; this.angleBCD = C; this.angleCDA = D; return !MathTools.isInsideBoundsInclusive(angleDABInRadians, minA, maxA); }
if (Double.isNaN(newAlphaPosition) || !MathTools.isInsideBoundsInclusive(newAlphaPosition, 0.0, 1.0)) newAlphaPosition = defaultAlphaPositionIntegration.getDoubleValue(); if (Double.isNaN(newAlphaVelocity) || !MathTools.isInsideBoundsInclusive(newAlphaVelocity, 0.0, 1.0)) newAlphaVelocity = defaultAlphaVelocityIntegration.getDoubleValue();
public static boolean checkJointspaceTrajectoryPointList(OneDoFJoint joint, SimpleTrajectoryPoint1DList trajectoryPointList) { for (int i = 0; i < trajectoryPointList.getNumberOfTrajectoryPoints(); i++) { double waypointPosition = trajectoryPointList.getTrajectoryPoint(i).getPosition(); double jointLimitLower = joint.getJointLimitLower(); double jointLimitUpper = joint.getJointLimitUpper(); if (!MathTools.isInsideBoundsInclusive(waypointPosition, jointLimitLower, jointLimitUpper)) { PrintTools.warn("Joint out of bounds: "+joint.getName()+" (" +jointLimitLower+", "+jointLimitUpper+ ") = "+waypointPosition+" (t="+i+")"); return false; } } return true; } }
public boolean updateAnglesGivenAngleDAB(double angleDABInRadians) { // Solve angles double A = clipToMinMax(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue()); double e = getUnknownTriangleSideLengthByLawOfCosine(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), A); double C = getAngleWithCosineLaw(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), e); double angleDBA = getAngleWithCosineLaw(lengthBA.getDoubleValue(), e, lengthAD.getDoubleValue()); double angleDBC = getAngleWithCosineLaw(lengthCB.getDoubleValue(), e, lengthDC.getDoubleValue()); double B = angleDBA + angleDBC; double D = 2 * PI - A - B - C; this.angleDAB = A; this.angleABC = B; this.angleBCD = C; this.angleCDA = D; return !MathTools.isInsideBoundsInclusive(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue()); }