Fix relative obs

Author: aaravpandya

src/sim.cpp

@@ -192,8 +192,8 @@ inline void collectPartnerObsSystem(Engine &ctx,
         const Rotation &other_rot = ctx.get<Rotation>(other);
         const VehicleSize &other_size = ctx.get<VehicleSize>(other);
 
-        Vector2 relative_pos = (other_position - pos).xy();
-        relative_pos = rot.inv().rotateVec({relative_pos.x, relative_pos.y, 0}).xy();
+        Vector3 relative_pos = (other_position - pos);
+        relative_pos = rot.inv().rotateVec(relative_pos);
         float relative_speed = other_velocity.linear.length(); // Design decision: return the speed of the other agent directly
 
         Rotation relative_orientation = rot.inv() * other_rot;

tests/EgocentricRoadObservationTests.cpp

@@ -7,16 +7,17 @@ using namespace madrona::math;
 using gpudrive::utils::ReferenceFrame;
 
 TEST(EgocentricRoadObservationTests, Relative) {
-  ReferenceFrame rf{Vector2{.x = 3, .y = 0},
+  ReferenceFrame rf{Vector3{.x = 3, .y = 0, .z=0},
                     Quat::angleAxis(toRadians(90), madrona::math::up)};
 
   auto mapObs =
-      rf.observationOf(Vector3{.x = 3, .y = 3, .z = 0},
+      rf.observationOf(Vector3{.x = 3, .y = 3, .z = 3},
                        Quat::angleAxis(toRadians(270), madrona::math::up),
                        Scale{{.d0 = 10, .d1 = 0.1, .d2 = 0.1}},
                        static_cast<gpudrive::EntityType>(0), 0);
 
   EXPECT_LT(mapObs.position.x - 3, 0.000001);
   EXPECT_LT(mapObs.position.y - 0, 0.000001);
+  EXPECT_LT(mapObs.position.z - 3, 0.000001);
   EXPECT_EQ(mapObs.heading, toRadians(180));
 }