added == operator for VisionTargetSim

PhotonVision · Nov 26, 2023 · 63c4caa · 63c4caa
1 parent d43a124
commit 63c4caa
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h
@@ -50,6 +50,18 @@ class VisionTargetSim {
     return pose.Translation().Norm() < right.pose.Translation().Norm();
   }
 
+  bool operator==(const VisionTargetSim& other) const {
+    return units::math::abs(pose.Translation().X() -
+                            other.GetPose().Translation().X()) < 1_in &&
+           units::math::abs(pose.Translation().Y() -
+                            other.GetPose().Translation().Y()) < 1_in &&
+           units::math::abs(pose.Translation().Z() -
+                            other.GetPose().Translation().Z()) < 1_in &&
+           units::math::abs(pose.Rotation().Degrees() -
+                            other.GetPose().Degrees()) < 1_deg &&
+           pose.model.GetIsPlanar() == other.model.GetIsPlanar();
+  }
+
  private:
   frc::Pose3d pose;
   TargetModel model;