139 lines
4.4 KiB
C++
139 lines
4.4 KiB
C++
#include "Perception.h"
|
|
#include "Angle.h"
|
|
#include "DistanceSensor.h"
|
|
#include "Switch.h"
|
|
|
|
Perception::Perception() {}
|
|
|
|
Perception::Perception(Placement* sensors, unsigned int sensorCount) {
|
|
this->sensorCount = sensorCount;
|
|
this->sensorPlacements = (Placement*)sensors;
|
|
}
|
|
|
|
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::ObjectNearby(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->ObjectNearby())
|
|
return true;
|
|
} else if (thing->type == Thing::SwitchType) {
|
|
Switch* switchSensor = (Switch*)thing;
|
|
if (switchSensor != nullptr && switchSensor->IsOn())
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
} |