public static boolean quaternionEpsilonEquals(Quat4f original, Quat4f result, float epsilon) { if (original.epsilonEquals(result, epsilon)) { return true; } else { Quat4f originalNegated = quaternionFloatForEpsilonEquals.get(); originalNegated.negate(original); return originalNegated.epsilonEquals(result, epsilon); } }
@Override public boolean epsilonEquals(LidarScanMessage other, double epsilon) { if (!lidarPosition.epsilonEquals(other.lidarPosition, (float) epsilon)) return false; if (!lidarOrientation.epsilonEquals(other.lidarOrientation, (float) epsilon)) return false; if (scan.length != other.scan.length) return false; for (int i = 0; i < scan.length; i++) { if (!MathTools.epsilonEquals(scan[i], other.scan[i], epsilon)) return false; } return true; } }
@Override public boolean epsilonEquals(IMUPacket other, double epsilon) { boolean ret = linearAcceleration.epsilonEquals(other.linearAcceleration, (float) epsilon); ret &= orientation.epsilonEquals(other.orientation, (float) epsilon); ret &= angularVelocity.epsilonEquals(other.angularVelocity, (float) epsilon); ret &= MathTools.epsilonEquals(time, other.time, epsilon); return ret; } }