public static boolean quaternionEpsilonEquals(Quat4d original, Quat4d result, double epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { Quat4d originalNegated = quaternionForEpsilonEquals.get(); originalNegated.negate(original); return originalNegated.epsilonEquals(result, epsilon); } }
@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(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; }
@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; }
boolean locationEquals = location.epsilonEquals(footstepData.location, epsilon); boolean orientationEquals = orientation.epsilonEquals(footstepData.orientation, epsilon); if (!orientationEquals) orientationEquals = temp.epsilonEquals(footstepData.orientation, epsilon);
private void sendUpdateStart(FootstepDataMessage updatedLocation){ if (updatedLocation.orientation.epsilonEquals(new Quat4d(), .003)) return; FootstepPlanRequestPacket updateStartPacket = new FootstepPlanRequestPacket(FootstepPlanRequestPacket.RequestType.UPDATE_START, updatedLocation, RotationTools.computeYaw(updatedLocation.orientation), null, 10); communicationBridge.sendPacket(updateStartPacket); }
@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); } }
boolean locationEquals = location.epsilonEquals(footstepData.location, epsilon); boolean orientationEquals = orientation.epsilonEquals(footstepData.orientation, epsilon); if (!orientationEquals) orientationEquals = temp.epsilonEquals(footstepData.orientation, epsilon);
if (!position.epsilonEquals(other.position, epsilon)) return false; if (!orientation.epsilonEquals(other.orientation, epsilon)) return false; if (!linearVelocity.epsilonEquals(other.linearVelocity, epsilon))