From 66c60b66ede0ff709b63cd223eb417b18294490a Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 21 Nov 2023 17:38:48 +0100
Subject: [PATCH] Added perception

---
 Perception.h |  3 +++
 Roboid.h     |  2 +-
 Sensing.cpp  | 46 ++++++++++++++++++++++++++++++++++++++++------
 Sensing.h    | 13 +++++++++++--
 4 files changed, 55 insertions(+), 9 deletions(-)
 create mode 100644 Perception.h

diff --git a/Perception.h b/Perception.h
new file mode 100644
index 0000000..2158882
--- /dev/null
+++ b/Perception.h
@@ -0,0 +1,3 @@
+#include "Sensing.h"
+
+using Perception = Sensing;
diff --git a/Roboid.h b/Roboid.h
index 759d039..cbcd440 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -1,9 +1,9 @@
 #pragma once
 
 #include "Activation.h"
+#include "Perception.h"
 #include "Placement.h"
 #include "Propulsion.h"
-#include "Sensing.h"
 
 class Roboid {
  public:
diff --git a/Sensing.cpp b/Sensing.cpp
index 56b7723..84ec2c2 100644
--- a/Sensing.cpp
+++ b/Sensing.cpp
@@ -166,14 +166,48 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
   return false;
 }
 
+unsigned int Sensing::ToDepthMapIndex(float angle) {
+  unsigned int depthMapIx = (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * (float)resolution);
+  return depthMapIx;
+}
+
 float Sensing::GetDistance(float angle) {
-  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-    Placement placement = sensorPlacements[sensorIx];
-    float placementAngle = placement.direction.x;
-    if (placementAngle == angle) {
-      DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
-      return distanceSensor->GetDistance();
+  if (depthMap != nullptr) {
+    if (angle < rangeMinimum || angle > rangeMaximum)
+      return INFINITY;
+    unsigned int depthMapIx = ToDepthMapIndex(angle);
+    return depthMap[depthMapIx];
+  } else {
+    for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+      Placement placement = sensorPlacements[sensorIx];
+      float placementAngle = placement.direction.x;
+      if (placementAngle == angle) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
+        return distanceSensor->GetDistance();
+      }
     }
   }
   return INFINITY;
+}
+
+void Sensing::SetResolution(unsigned int resolution) {
+  this->resolution = resolution;
+  this->depthMap = new float[this->resolution];
+}
+
+void Sensing::SetRange(float min, float max) {
+  this->rangeMinimum = min;
+  this->rangeMaximum = max;
+}
+
+float* Sensing::GetDepthMap() {
+  return this->depthMap;
+}
+
+void Sensing::SetDepthMap(float angle, float distance) {
+  if (angle < rangeMinimum || angle > rangeMaximum)
+    return;
+
+  unsigned int depthMapIx = ToDepthMapIndex(angle);
+  depthMap[depthMapIx] = distance;
 }
\ No newline at end of file
diff --git a/Sensing.h b/Sensing.h
index 4f7f47b..d8fb1a6 100644
--- a/Sensing.h
+++ b/Sensing.h
@@ -72,10 +72,19 @@ class Sensing {
 
   bool SwitchOn(float fromAngle, float toAngle);
 
+  void SetResolution(unsigned int resolution);
+  void SetRange(float min, float max);
+  float* GetDepthMap();
+  unsigned int ToDepthMapIndex(float angle);
+  void SetDepthMap(float angle, float distance);
+
  protected:
   // SensorPlacement* sensors = nullptr;
   Placement* sensorPlacements = nullptr;
   unsigned int sensorCount = 0;
-};
 
-using Perception = Sensing;
+  unsigned int resolution;
+  float rangeMinimum;
+  float rangeMaximum;
+  float* depthMap = nullptr;
+};