Make GetDistance using direction/range

Perception.cpp

@@ -12,9 +12,7 @@ Perception::Perception(Placement* sensors, unsigned int sensorCount) {
   this->sensorPlacements = (Placement *)sensors;
 }
 
-unsigned int Perception::GetSensorCount() {
+unsigned int Perception::GetSensorCount() { return this->sensorCount; }
-  return this->sensorCount;
-}
 
 Sensor *Perception::GetSensor(unsigned int sensorId) {
   if (sensorId >= this->sensorCount)
@@ -115,16 +113,40 @@ float Perception::DistanceDown(float angle) {
 }
 */
 
-float Perception::GetDistance(float fromAngle, float toAngle) {
+// float Perception::GetDistance(float fromAngle, float toAngle) {
+//   float minDistance = INFINITY;
+//   if (toAngle < fromAngle)
+//     // Hmm. Can't look backward properly for now
+//     return minDistance;
+
+//   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+//     Placement placement = sensorPlacements[sensorIx];
+//     float angle = placement.horizontalDirection;
+//     if (angle > fromAngle && angle < toAngle) {
+//       Thing* thing = placement.thing;
+//       if (thing == nullptr)
+//         continue;
+
+//       if (thing->type == Thing::DistanceSensorType) {
+//         DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+//         if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
+//           minDistance = fmin(minDistance, distanceSensor->GetDistance());
+//       }
+//     }
+//   }
+//   return minDistance;
+// }
+
+float Perception::GetDistance(float direction, float range) {
   float minDistance = INFINITY;
-  if (toAngle < fromAngle)
+  if (range < 0)
-    // Hmm. Can't look backward properly for now
+    range = -range;
-    return minDistance;
 
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
-    float angle = placement.horizontalDirection;
+    // This still needs support for angles wrapping around 180 degrees !!!!
-    if (angle > fromAngle && angle < toAngle) {
+    if (placement.horizontalDirection > direction - range &&
+        placement.horizontalDirection < direction + range) {
       Thing *thing = placement.thing;
       if (thing == nullptr)
         continue;
@@ -138,23 +160,48 @@ float Perception::GetDistance(float fromAngle, float toAngle) {
   }
   return minDistance;
 }
+// float Perception::GetDistance(float fromHorizontalAngle,
+//                               float toHorizontalAngle, float
+//                               fromVerticalAngle, float toVerticalAngle) {
+//   float minDistance = INFINITY;
+//   if (toHorizontalAngle < fromHorizontalAngle ||
+//       toVerticalAngle < fromVerticalAngle)
+//     // Hmm. Can't look backward properly for now
+//     return minDistance;
 
-float Perception::GetDistance(float fromHorizontalAngle,
+//   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-                              float toHorizontalAngle,
+//     Placement placement = sensorPlacements[sensorIx];
-                              float fromVerticalAngle,
+//     if (placement.horizontalDirection > fromHorizontalAngle &&
-                              float toVerticalAngle) {
+//         placement.horizontalDirection < toHorizontalAngle &&
+//         placement.verticalDirection > fromVerticalAngle &&
+//         placement.verticalDirection < toVerticalAngle) {
+//       Thing *thing = placement.thing;
+//       if (thing == nullptr)
+//         continue;
+
+//       if (thing->type == Thing::DistanceSensorType) {
+//         DistanceSensor *distanceSensor = (DistanceSensor *)thing;
+//         if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
+//           minDistance = fmin(minDistance, distanceSensor->GetDistance());
+//       }
+//     }
+//   }
+//   return minDistance;
+// }
+
+float Perception::GetDistance(float horizontalDirection,
+                              float verticalDirection, float range) {
   float minDistance = INFINITY;
-  if (toHorizontalAngle < fromHorizontalAngle ||
+  if (range < 0)
-      toVerticalAngle < fromVerticalAngle)
+    range = -range;
-    // Hmm. Can't look backward properly for now
-    return minDistance;
 
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
-    if (placement.horizontalDirection > fromHorizontalAngle &&
+    // This still needs support for angles wrapping around 180 degrees !!!!
-        placement.horizontalDirection < toHorizontalAngle &&
+    if (placement.horizontalDirection > horizontalDirection - range &&
-        placement.verticalDirection > fromVerticalAngle &&
+        placement.horizontalDirection < horizontalDirection + range &&
-        placement.verticalDirection < toVerticalAngle) {
+        placement.verticalDirection > verticalDirection - range &&
+        placement.verticalDirection < verticalDirection + range) {
       Thing *thing = placement.thing;
       if (thing == nullptr)
         continue;
@@ -168,15 +215,14 @@ float Perception::GetDistance(float fromHorizontalAngle,
   }
   return minDistance;
 }
-
+bool Perception::ObjectNearby(float direction, float range) {
-bool Perception::ObjectNearby(float fromAngle, float toAngle) {
+  if (range < 0)
-  if (toAngle < fromAngle)
+    range = -range;
-    return false;
 
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Placement placement = sensorPlacements[sensorIx];
-    float angle = placement.horizontalDirection;
+    if (placement.horizontalDirection > direction - range &&
-    if (angle > fromAngle && angle < toAngle) {
+        placement.horizontalDirection < direction + range) {
       Thing *thing = placement.thing;
       if (thing == nullptr)
         continue;

Perception.h

@@ -29,21 +29,46 @@ class Perception {
   unsigned int GetSensorCount();
   Sensor *GetSensor(unsigned int sensorId);
 
-  float GetDistance(float fromAngle, float toAngle);
+  /// @brief Gets the distance to the closest object
-  float GetDistance(float fromHorizontalAngle,
+  /// @param direction The direction to look for objects
-                    float toHorizontalAngle,
+  /// @param range The range in which objects should be looked for
-                    float fromVerticalAngle,
+  /// @return The distance to the closest object in meters
-                    float toVerticalAngle);
+  float GetDistance(float direction, float range = 10.0F);
-  //   float GetDistance(float angle);
+  /// @brief Gets the distance to the closest object
+  /// @param horizontalDirection The direction in the horizontal plane to look
+  /// for objects
+  /// @param verticalDirection The direction in the vertical plane to look for
+  /// objects
+  /// @param range The range in which objects should be looked for
+  /// @return The distance to the closest object in meters
+  /// The directions can be thought of as the polar angle (vertical) and
+  /// azimuthal angle (horizontal) in the spherical coordinate system.
+  float GetDistance(float horizontalDirection, float verticalDirection,
+                    float range = 10.0F);
 
-  /// @brief Checks if an object is close within the give range in the
+  /// @brief Checks if an object is nearby
-  /// horizontal plane
+  /// @param direction The direction to look for objects
-  /// @param fromAngle Start angle in the horizontal plane
+  /// @param range The range in which objects should be looked for
-  /// @param toAngle End angle in the horizontal plane
+  /// @return True when an object is close, False otherwise
-  /// @return True is an object is closeby
+  /// Wether an object is closeby depends on the sensor. This can be a sensor
-  /// @note Whether an object is closeby depends on the Distance Sensor
+  /// like a Switch or a DistanceSensor. The latter uses the
-  /// @remark This function is likely to change in the near future
+  /// DistanceSensor::triggerDistance to check if an object is nearby.
-  bool ObjectNearby(float fromAngle, float toAngle);
+  bool ObjectNearby(float direction, float range = 10.0F);
+  /// @brief Checks if an object is nearby
+  /// @param horizontalDirection The direction in the horizontal plane to look
+  /// for objects
+  /// @param verticalDirection The direction in the vertical plane to look for
+  /// objects
+  /// @param range The range in which objects should be looked for
+  /// @return True when an object is close, False otherwise
+  /// Wether an object is closeby depends on the sensor. This can be a sensor
+  /// like a Switch or a DistanceSensor. The latter uses the
+  /// DistanceSensor::triggerDistance to check if an object is nearby.
+  ///
+  /// The directions can be thought of as the polar angle (vertical) and
+  /// azimuthal angle (horizontal) in the spherical coordinate system.
+  bool ObjectNearby(float horizontalDirection, float verticalDirection,
+                    float range = 10.0F);
 
 protected:
   /// @brief The Placement of the Sensors used for Perception