WiimoteEmu: Update IR camera status from DesiredWiimoteState.

5 files changed, 69 insertions, 42 deletions

diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp
index f2a9885fc7..854c9e8577 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp
@@ -52,36 +52,14 @@ int CameraLogic::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in)
   return RawWrite(&m_reg_data, addr, count, data_in);
 }
 
-void CameraLogic::Update(const Common::Matrix44& transform, Common::Vec2 field_of_view)
+std::array<CameraPoint, CameraLogic::NUM_POINTS>
+CameraLogic::GetCameraPoints(const Common::Matrix44& transform, Common::Vec2 field_of_view)
 {
-  // IR data is read from offset 0x37 on real hardware.
-  auto& data = m_reg_data.camera_data;
-  data.fill(0xff);
-
-  constexpr u8 OBJECT_TRACKING_ENABLE = 0x08;
-
-  // If Address 0x30 is not 0x08 the camera will return 0xFFs.
-  // The Wii seems to write 0x01 here before changing modes/sensitivities.
-  if (m_reg_data.enable_object_tracking != OBJECT_TRACKING_ENABLE)
-    return;
-
-  // If the sensor bar is off the camera will see no LEDs and return 0xFFs.
-  if (!IOS::g_gpio_out[IOS::GPIO::SENSOR_BAR])
-    return;
-
   using Common::Matrix33;
   using Common::Matrix44;
   using Common::Vec3;
   using Common::Vec4;
 
-  // FYI: A real wiimote normally only returns 1 point for each LED cluster (2 total).
-  // Sending all 4 points can actually cause some stuttering issues.
-  constexpr int NUM_POINTS = 2;
-
-  // Range from 0-15. Small values (2-4) seem to be very typical.
-  // This is reduced based on distance from sensor bar.
-  constexpr int MAX_POINT_SIZE = 15;
-
   const std::array<Vec3, NUM_POINTS> leds{
       Vec3{-SENSOR_BAR_LED_SEPARATION / 2, 0, 0},
       Vec3{SENSOR_BAR_LED_SEPARATION / 2, 0, 0},
@@ -91,12 +69,6 @@ void CameraLogic::Update(const Common::Matrix44& transform, Common::Vec2 field_o
       Matrix44::Perspective(field_of_view.y, field_of_view.x / field_of_view.y, 0.001f, 1000) *
       Matrix44::FromMatrix33(Matrix33::RotateX(float(MathUtil::TAU / 4))) * transform;
 
-  struct CameraPoint
-  {
-    IRBasic::IRObject position;
-    u8 size = 0;
-  };
-
   std::array<CameraPoint, leds.size()> camera_points;
 
   std::transform(leds.begin(), leds.end(), camera_points.begin(), [&](const Vec3& v) {
@@ -112,13 +84,32 @@ void CameraLogic::Update(const Common::Matrix44& transform, Common::Vec2 field_o
       const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2);
 
       if (x >= 0 && y >= 0 && x < CAMERA_RES_X && y < CAMERA_RES_Y)
-        return CameraPoint{{u16(x), u16(y)}, u8(point_size)};
+        return CameraPoint({u16(x), u16(y)}, u8(point_size));
     }
 
-    // 0xFFFFs are interpreted as "not visible".
-    return CameraPoint{{0xffff, 0xffff}, 0xff};
+    return CameraPoint();
   });
 
+  return camera_points;
+}
+
+void CameraLogic::Update(const std::array<CameraPoint, NUM_POINTS>& camera_points)
+{
+  // IR data is read from offset 0x37 on real hardware.
+  auto& data = m_reg_data.camera_data;
+  data.fill(0xff);
+
+  constexpr u8 OBJECT_TRACKING_ENABLE = 0x08;
+
+  // If Address 0x30 is not 0x08 the camera will return 0xFFs.
+  // The Wii seems to write 0x01 here before changing modes/sensitivities.
+  if (m_reg_data.enable_object_tracking != OBJECT_TRACKING_ENABLE)
+    return;
+
+  // If the sensor bar is off the camera will see no LEDs and return 0xFFs.
+  if (!IOS::g_gpio_out[IOS::GPIO::SENSOR_BAR])
+    return;
+
   switch (m_reg_data.mode)
   {
   case IR_MODE_BASIC:
diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.h b/Source/Core/Core/HW/WiimoteEmu/Camera.h
index 5c2a186ced..693b07e515 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Camera.h
+++ b/Source/Core/Core/HW/WiimoteEmu/Camera.h
@@ -16,11 +16,26 @@ class Matrix44;
 
 namespace WiimoteEmu
 {
+using IRObject = Common::TVec2<u16>;
+
+struct CameraPoint
+{
+  IRObject position;
+  u8 size;
+
+  // 0xFFFFs are interpreted as "not visible".
+  constexpr CameraPoint() : position({0xffff, 0xffff}), size(0xff) {}
+  constexpr CameraPoint(IRObject position_, u8 size_) : position(position_), size(size_) {}
+  constexpr bool operator==(const CameraPoint& other) const
+  {
+    return this->position == other.position && this->size == other.size;
+  }
+  constexpr bool operator!=(const CameraPoint& other) const { return !(*this == other); }
+};
+
 // Four bytes for two objects. Filled with 0xFF if empty
 struct IRBasic
 {
-  using IRObject = Common::TVec2<u16>;
-
   u8 x1;
   u8 y1;
   u8 x2hi : 2;
@@ -59,8 +74,8 @@ struct IRExtended
   u8 xhi : 2;
   u8 yhi : 2;
 
-  auto GetPosition() const { return IRBasic::IRObject(xhi << 8 | x, yhi << 8 | y); }
-  void SetPosition(const IRBasic::IRObject& obj)
+  auto GetPosition() const { return IRObject(xhi << 8 | x, yhi << 8 | y); }
+  void SetPosition(const IRObject& obj)
   {
     x = obj.x;
     xhi = obj.x >> 8;
@@ -109,9 +124,19 @@ public:
     IR_MODE_FULL = 5,
   };
 
+  // FYI: A real wiimote normally only returns 1 point for each LED cluster (2 total).
+  // Sending all 4 points can actually cause some stuttering issues.
+  static constexpr int NUM_POINTS = 2;
+
+  // Range from 0-15. Small values (2-4) seem to be very typical.
+  // This is reduced based on distance from sensor bar.
+  static constexpr int MAX_POINT_SIZE = 15;
+
   void Reset();
   void DoState(PointerWrap& p);
-  void Update(const Common::Matrix44& transform, Common::Vec2 field_of_view);
+  static std::array<CameraPoint, NUM_POINTS> GetCameraPoints(const Common::Matrix44& transform,
+                                                             Common::Vec2 field_of_view);
+  void Update(const std::array<CameraPoint, NUM_POINTS>& camera_points);
   void SetEnabled(bool is_enabled);
 
   static constexpr u8 I2C_ADDR = 0x58;
diff --git a/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h b/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h
index 699ed05045..ac23c77567 100644
--- a/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h
+++ b/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h
@@ -3,7 +3,10 @@
 
 #pragma once
 
+#include <array>
+
 #include "Core/HW/WiimoteCommon/WiimoteReport.h"
+#include "Core/HW/WiimoteEmu/Camera.h"
 #include "Core/HW/WiimoteEmu/WiimoteEmu.h"
 
 namespace WiimoteEmu
@@ -14,7 +17,11 @@ struct DesiredWiimoteState
   static constexpr WiimoteCommon::AccelData DEFAULT_ACCELERATION = WiimoteCommon::AccelData(
       {Wiimote::ACCEL_ZERO_G << 2, Wiimote::ACCEL_ZERO_G << 2, Wiimote::ACCEL_ONE_G << 2});
 
+  // No light detected by the IR camera.
+  static constexpr std::array<CameraPoint, 2> DEFAULT_CAMERA = {CameraPoint(), CameraPoint()};
+
   WiimoteCommon::ButtonData buttons{};  // non-button state in this is ignored
   WiimoteCommon::AccelData acceleration = DEFAULT_ACCELERATION;
+  std::array<CameraPoint, 2> camera_points = DEFAULT_CAMERA;
 };
 }  // namespace WiimoteEmu
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index e279f8a38e..d3c365bb42 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -454,6 +454,12 @@ DesiredWiimoteState Wiimote::BuildDesiredWiimoteState()
   wiimote_state.acceleration =
       ConvertAccelData(GetTotalAcceleration(), ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2);
 
+  // Calculate IR camera state.
+  wiimote_state.camera_points = CameraLogic::GetCameraPoints(
+      GetTotalTransformation(),
+      Common::Vec2(m_fov_x_setting.GetValue(), m_fov_y_setting.GetValue()) / 360 *
+          float(MathUtil::TAU));
+
   return wiimote_state;
 }
 
@@ -545,9 +551,7 @@ void Wiimote::SendDataReport(const DesiredWiimoteState& target_state)
     {
       // Note: Camera logic currently contains no changing state so we can just update it here.
       // If that changes this should be moved to Wiimote::Update();
-      m_camera_logic.Update(GetTotalTransformation(),
-                            Common::Vec2(m_fov_x_setting.GetValue(), m_fov_y_setting.GetValue()) /
-                                360 * float(MathUtil::TAU));
+      m_camera_logic.Update(target_state.camera_points);
 
       // The real wiimote reads camera data from the i2c bus starting at offset 0x37:
       const u8 camera_data_offset =
diff --git a/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp b/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp
index 6ab4e599cb..6b92bb9b16 100644
--- a/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp
+++ b/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp
@@ -1256,7 +1256,7 @@ void Device::IRState::ProcessData(const std::array<WiimoteEmu::IRBasic, 2>& data
   // A better implementation might extrapolate points when they fall out of camera view.
   // But just averaging visible points actually seems to work very well.
 
-  using IRObject = WiimoteEmu::IRBasic::IRObject;
+  using IRObject = WiimoteEmu::IRObject;
 
   MathUtil::RunningVariance<Common::Vec2> points;