#include "Perception.h" #include "Angle.h" #include "Switch.h" Perception::Perception() {} Perception::Perception(Placement* sensors, unsigned int sensorCount) { this->sensorCount = sensorCount; this->sensorPlacements = (Placement*)sensors; } void Perception::AddSensors(Placement* things, unsigned int thingCount) { sensorCount = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; if (thing->IsSensor()) sensorCount++; } sensorPlacements = new Placement[sensorCount]; unsigned int sensorIx = 0; for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { Thing* thing = things[thingIx].thing; if (thing->IsSensor()) sensorPlacements[sensorIx++] = things[thingIx]; } } unsigned int Perception::GetSensorCount() { return this->sensorCount; } Sensor* Perception::GetSensor(unsigned int sensorId) { if (sensorId >= this->sensorCount) return nullptr; Thing* thing = this->sensorPlacements[sensorId].thing; if (thing->IsSensor()) return (Sensor*)thing; return nullptr; } float Perception::DistanceForward(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; if (placement.verticalDirection > -angle && placement.verticalDirection < angle && placement.horizontalDirection > -angle && placement.horizontalDirection < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } return minDistance; } float Perception::DistanceLeft(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; float sensorAngle = placement.horizontalDirection; if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } return minDistance; } float Perception::DistanceRight(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; if (sensor->type != Thing::DistanceSensorType) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; float sensorAngle = placement.horizontalDirection; if (sensorAngle > 0 && sensorAngle < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } return minDistance; } float Perception::DistanceUp(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; float sensorAngle = placement.verticalDirection; if (sensorAngle > 0 && sensorAngle < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } return minDistance; } float Perception::DistanceDown(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; Sensor* sensor = (Sensor*)placement.thing; if (sensor->IsSensor()) continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; float sensorAngle = placement.verticalDirection; if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } return minDistance; } bool Perception::SwitchOn(float fromAngle, float toAngle) { if (toAngle < fromAngle) return false; 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->IsOn()) return true; } else if (thing->type == Thing::SwitchType) { Switch* switchSensor = (Switch*)thing; if (switchSensor != nullptr && switchSensor->IsOn()) return true; } } } return false; } unsigned int Perception::ToDepthMapIndex(float angle) { unsigned int depthMapIx = (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) * (float)resolution); return depthMapIx; } float Perception::GetDistance(float angle) { 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.horizontalDirection; if (placementAngle == angle) { DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; return distanceSensor->GetDistance(); } } } return INFINITY; } void Perception::SetResolution(unsigned int resolution) { this->resolution = resolution; this->depthMap = new float[this->resolution]; } void Perception::SetRange(float min, float max) { this->rangeMinimum = min; this->rangeMaximum = max; } float* Perception::GetDepthMap() { return this->depthMap; } void Perception::SetDepthMap(float angle, float distance) { if (angle < rangeMinimum || angle > rangeMaximum) return; unsigned int depthMapIx = ToDepthMapIndex(angle); depthMap[depthMapIx] = distance; } DistanceSensor* Perception::GetSensor(float angle) { angle = Angle::Normalize(angle); for (unsigned int ix = 0; ix < this->sensorCount; ix++) { Placement placement = this->sensorPlacements[ix]; if (abs(placement.horizontalDirection - angle) < 0.01F) return (DistanceSensor*)placement.thing; } DistanceSensor* distanceSensor = new DistanceSensor(); Placement* newPlacement = new Placement(distanceSensor, angle); AddSensors(newPlacement, 1); return distanceSensor; }