@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(LidarPosePacket other, double epsilon) { try { boolean arePositionsEqual = position.epsilonEquals(other.position, epsilon); boolean areOrientationsEqual = orientation.epsilonEquals(other.orientation, epsilon); return arePositionsEqual && areOrientationsEqual; } catch (NullPointerException e) { return false; } } }
@Override public boolean epsilonEquals(DetectedFacesPacket other, double epsilon) { boolean ret = true; for(int i = 0; i < ids.length; i++) { ret &= ids[i].equals(other.getIds()[i]) && positions[i].epsilonEquals(other.getPositions()[i], epsilon); } return ret; } }
@Override public boolean epsilonEquals(UIPositionCheckerPacket other, double epsilon) { if (orientation != null) return (position.epsilonEquals(other.getPosition(), (float) epsilon)) && orientation.epsilonEquals(other.getOrientation(), epsilon); return position.epsilonEquals(other.getPosition(), (float) epsilon); } }
public boolean epsilonEquals(PointPositionDataObject other, double epsilon) { if (bodyFixedReferenceFrameName != other.bodyFixedReferenceFrameName) return false; boolean validStateEqual = isPointPositionValid == other.isPointPositionValid; boolean bodyPointsEqual = getMeasurementPointInBodyFrame().epsilonEquals(other.getMeasurementPointInBodyFrame(), epsilon); boolean worldPointsEqual = getMeasurementPointInWorldFrame().epsilonEquals(other.getMeasurementPointInWorldFrame(), epsilon); return validStateEqual && bodyPointsEqual && worldPointsEqual; }
@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; }
public boolean epsilonEquals(AdjustFootstepMessage footstepData, double epsilon) boolean locationEquals = location.epsilonEquals(footstepData.location, epsilon);
@Override public boolean epsilonEquals(HandstepPacket handstepPacket, double epsilon) { boolean robotSideEquals = robotSide == handstepPacket.robotSide; boolean locationEquals = location.epsilonEquals(handstepPacket.location, epsilon); boolean orientationEquals = orientation.epsilonEquals(handstepPacket.orientation, epsilon); if (!orientationEquals) { Quat4d temp = new Quat4d(); temp.negate(orientation); orientationEquals = temp.epsilonEquals(handstepPacket.orientation, epsilon); } return robotSideEquals && locationEquals && orientationEquals; }
public boolean epsilonEquals(SpigotPosePacket other, double epsilon) { boolean ret = RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
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; } }
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 epsilonEquals(VehiclePosePacket other, double epsilon) { boolean ret = RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
public boolean epsilonEquals(TorusPosePacket other, double epsilon) { boolean ret; final double thisRadius = this.getFingerHoleRadius(); final double otherRadius = other.getFingerHoleRadius(); final double delta = Math.abs(thisRadius - otherRadius); if (thisRadius == otherRadius) ret = true; else if ((thisRadius == 0) || (otherRadius == 0) || (delta < 1e-2)) ret = (delta < epsilon); else { ret = (delta / (thisRadius + otherRadius) < epsilon); } ret &= RotationTools.quaternionEpsilonEquals(this.getOrientation(), other.getOrientation(), epsilon); ret &= this.getPosition().epsilonEquals(other.getPosition(), epsilon); return ret; }
if (!position.epsilonEquals(other.position, epsilon)) return false; if (!orientation.epsilonEquals(other.orientation, epsilon))
ret &= this.desiredCapturePoint.epsilonEquals(other.desiredCapturePoint, epsilon); ret &= this.centerOfMass.epsilonEquals(other.centerOfMass, epsilon);
@Override public boolean epsilonEquals(VideoPacket other, double epsilon) if (!getPosition().epsilonEquals(other.getPosition(), epsilon))
public boolean epsilonEquals(Footstep otherFootstep, double epsilon) { boolean arePosesEqual = ankleReferenceFrame.epsilonEquals(otherFootstep.ankleReferenceFrame, epsilon); boolean bodiesHaveTheSameName = endEffector.getName().equals(otherFootstep.endEffector.getName()); boolean sameRobotSide = robotSide == otherFootstep.robotSide; boolean isTrustHeightTheSame = trustHeight == otherFootstep.trustHeight; boolean sameWaypoints = swingWaypoints.size() == otherFootstep.swingWaypoints.size(); if (sameWaypoints) { for (int i = 0; i < swingWaypoints.size(); i++) { Point3d waypoint = swingWaypoints.get(i); Point3d otherWaypoint = otherFootstep.swingWaypoints.get(i); sameWaypoints = sameWaypoints && waypoint.epsilonEquals(otherWaypoint, epsilon); } } return arePosesEqual && bodiesHaveTheSameName && sameRobotSide && isTrustHeightTheSame && sameWaypoints; }
public boolean epsilonEquals(FootstepDataMessage footstepData, double epsilon) boolean locationEquals = location.epsilonEquals(footstepData.location, epsilon);
if (!steeringWheelCenter.epsilonEquals(other.steeringWheelCenter, epsilon)) return false; if (!steeringWheelRotationAxis.epsilonEquals(other.steeringWheelRotationAxis, epsilon))