@Override public boolean epsilonEquals(Ellipsoid3d other, double epsilon) { return radius.epsilonEquals(other.radius, epsilon); }
/** equals - compare ecf position (x,y,z) for equality with an epsilon value */ public boolean epsilonEquals(final EarthVector otherEv, final double epsilon) { if (otherEv == null) { return false; } return ecfVector.epsilonEquals(otherEv.ecfVector, epsilon); }
/** * Returns true if this SpatialMotionVector and the input SpatialMotionVector * have the same HumanoidReferenceFrames and if the L-infinite distance between their * linear parts and angular parts are less than or equal to the epsilon parameter. * If any of the frames are different, throws a RuntimeException. * Otherwise returns false. The L-infinite * distance is equal to MAX[abs(x1-x2), abs(y1-y2), abs(z1-z2)]. * @param spatialMotionVector the SpatialAccelerationVector to be compared to this SpatialAccelerationVector * @param epsilon the threshold value * @return true or false */ public boolean epsilonEquals(SpatialMotionVector spatialMotionVector, double epsilon) { checkReferenceFramesMatch(spatialMotionVector); if (!linearPart.epsilonEquals(spatialMotionVector.linearPart, epsilon)) return false; if (!angularPart.epsilonEquals(spatialMotionVector.angularPart, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(EuclideanWaypoint other, double epsilon) { if (!position.epsilonEquals(other.position, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(SO3Waypoint other, double epsilon) { if (!orientation.epsilonEquals(other.orientation, epsilon)) return false; if (!angularVelocity.epsilonEquals(other.angularVelocity, epsilon)) return false; return true; }
@Override public boolean epsilonEquals(RawIMUPacket other, double epsilon) { boolean ret = MathTools.epsilonEquals(timestampInNanoSecond, other.timestampInNanoSecond, epsilon); ret &= linearAcceleration.epsilonEquals(other.linearAcceleration, epsilon); return ret; }
@Override public boolean epsilonEquals(HandRotateAboutAxisPacket other, double epsilon) { boolean sameRobotSide = other.robotSide == robotSide; boolean sameTrajectoryTime = other.trajectoryTime == trajectoryTime; boolean sameRotationAxisOrigin = other.rotationAxisOriginInWorld == rotationAxisOriginInWorld; boolean sameRotationAxis = other.rotationAxisInWorld == rotationAxisInWorld; boolean sameRotationAngle = other.rotationRightHandRule == rotationRightHandRule; boolean sameOrientationLockBoolean = other.controlHandAngleAboutAxis == controlHandAngleAboutAxis; boolean sameDesiredTangentialForce; boolean sameForceConstraint; if(dataType != null) { sameDesiredTangentialForce = MathTools.epsilonEquals(other.desiredTangentialForce, desiredTangentialForce, epsilon); sameForceConstraint = forceConstraint.epsilonEquals(other.forceConstraint, epsilon); } else { sameDesiredTangentialForce = true; sameForceConstraint = true; } return sameRobotSide && sameTrajectoryTime && sameRotationAxisOrigin && sameRotationAxis && sameRotationAngle && sameOrientationLockBoolean && sameDesiredTangentialForce && sameForceConstraint; } }
@Override public boolean epsilonEquals(SO3TrajectoryPointMessage other, double epsilon) { if (orientation == null && other.orientation != null) return false; if (orientation != null && other.orientation == null) return false; if (angularVelocity == null && other.angularVelocity != null) return false; if (angularVelocity != null && other.angularVelocity == null) return false; if (!MathTools.epsilonEquals(time, other.time, epsilon)) return false; if (!orientation.epsilonEquals(other.orientation, epsilon)) return false; if (!angularVelocity.epsilonEquals(other.angularVelocity, epsilon)) return false; return true; }
if (!orientation.epsilonEquals(other.orientation, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon)) return false; if (!angularVelocity.epsilonEquals(other.angularVelocity, epsilon)) return false;
@Override public boolean epsilonEquals(EuclideanTrajectoryPointMessage other, double epsilon) { if (position == null && other.position != null) return false; if (position != null && other.position == null) return false; if (linearVelocity == null && other.linearVelocity != null) return false; if (linearVelocity != null && other.linearVelocity == null) return false; if (!MathTools.epsilonEquals(time, other.time, epsilon)) return false; if (!position.epsilonEquals(other.position, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon)) return false; return true; }
public boolean epsilonEquals(ButtonData buttonData, double epsilon) { boolean vectorEquals = pushDirection.epsilonEquals(buttonData.getPushDirection(), epsilon); boolean positionEquals = pushPoint.epsilonEquals(buttonData.getPushPosition(), epsilon); return vectorEquals && positionEquals; }
if (!steeringWheelCenter.epsilonEquals(other.steeringWheelCenter, epsilon)) return false; if (!steeringWheelRotationAxis.epsilonEquals(other.steeringWheelRotationAxis, epsilon)) return false; if (!steeringWheelZeroAxis.epsilonEquals(other.steeringWheelZeroAxis, epsilon)) return false; if (!MathTools.epsilonEquals(steeringWheelRadius, other.steeringWheelRadius, epsilon))
@Override public boolean epsilonEquals(HeadPosePacket other, double epsilon) { boolean ret = true; ret &= MathTools.epsilonEquals(yaw, other.yaw, epsilon); ret &= MathTools.epsilonEquals(pitch, other.pitch, epsilon); ret &= MathTools.epsilonEquals(roll, other.roll, epsilon); ret &= measuredGravityInWorld.epsilonEquals(other.measuredGravityInWorld, epsilon); ret &= status.equals(other.status); return ret; } }
public boolean epsilonEquals(PointVelocityDataObject other, double epsilon) { if (this.bodyFixedReferenceFrameName != other.bodyFixedReferenceFrameName) return false; boolean validStateEqual = isPointVelocityValid == other.isPointVelocityValid; boolean rigidBodyEqual = other.rigidBodyName == this.rigidBodyName; boolean bodyPointsEqual = getMeasurementPointInBodyFrame().epsilonEquals(other.getMeasurementPointInBodyFrame(), epsilon); boolean worldVelocitiesEqual = getVelocityOfMeasurementPointInWorldFrame().epsilonEquals(other.getVelocityOfMeasurementPointInWorldFrame(), epsilon); return validStateEqual && rigidBodyEqual && bodyPointsEqual && worldVelocitiesEqual; }
public boolean isSolvable(double trajectoryTime, Vector3d omegaA, Vector3d omegaB) { omegaA.scale(trajectoryTime / 3); boolean omegaALength = omegaA.length() < 2 * Math.PI; omegaB.scale(trajectoryTime / 3); boolean omegaBLength = (omegaB.length() < 2 * Math.PI); omegaB.scale(-1); boolean opposite = omegaA.epsilonEquals(omegaB, 1e-2); return omegaALength && omegaBLength && !opposite; }
public boolean epsilonEquals(DebrisData debrisData, double epsilon) { boolean transformEquals = debrisTransform.epsilonEquals(debrisData.getDebrisTransform(), epsilon); boolean vectorEquals = graspVector.epsilonEquals(debrisData.getGraspVector(), epsilon); boolean positionEquals = graspVectorPosition.epsilonEquals(debrisData.getGraspVectorPosition(), epsilon); return transformEquals && vectorEquals && positionEquals; } }
footNormalVector.normalize(); lineOfContactVector.cross(normal, footNormalVector); if (lineOfContactVector.epsilonEquals(zero, 10E-12)) return; lineOfContact.setIncludingFrame(worldFrame, lineOfContactVector);