diff --git a/Perception.cpp b/Perception.cpp index 67b3a30..ee577c1 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -52,8 +52,10 @@ float Perception::DistanceForward(float angle) { continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.z; - if (sensorAngle > 0 && sensorAngle < angle) { + if (placement.verticalDirection > -angle && + placement.verticalDirection < angle && + placement.horizontalDirection > -angle && + placement.horizontalDirection < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } } @@ -69,7 +71,7 @@ float Perception::DistanceLeft(float angle) { continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; + float sensorAngle = placement.horizontalDirection; // Serial.printf(" distance sensor: %f %f 0\n", -angle, sensorAngle); if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); @@ -87,7 +89,7 @@ float Perception::DistanceRight(float angle) { continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; + float sensorAngle = placement.horizontalDirection; if (sensorAngle > 0 && sensorAngle < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -104,7 +106,7 @@ float Perception::DistanceUp(float angle) { continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; // not correct! + float sensorAngle = placement.verticalDirection; if (sensorAngle > 0 && sensorAngle < angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -121,7 +123,7 @@ float Perception::DistanceDown(float angle) { continue; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; - float sensorAngle = placement.direction.y; // not correct! + float sensorAngle = placement.verticalDirection; if (sensorAngle < 0 && sensorAngle > -angle) { minDistance = fmin(minDistance, distanceSensor->GetDistance()); } @@ -135,7 +137,7 @@ bool Perception::SwitchOn(float fromAngle, float toAngle) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; - float angle = placement.direction.y; + float angle = placement.horizontalDirection; if (angle > fromAngle && angle < toAngle) { Thing* thing = placement.thing; if (thing == nullptr) @@ -171,7 +173,7 @@ float Perception::GetDistance(float angle) { } else { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { Placement placement = sensorPlacements[sensorIx]; - float placementAngle = placement.direction.x; + float placementAngle = placement.horizontalDirection; if (placementAngle == angle) { DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; return distanceSensor->GetDistance(); @@ -208,7 +210,7 @@ DistanceSensor* Perception::GetSensor(float angle) { for (unsigned int ix = 0; ix < this->sensorCount; ix++) { Placement placement = this->sensorPlacements[ix]; - if (abs(placement.direction.y - angle) < 0.01F) + if (abs(placement.horizontalDirection - angle) < 0.01F) return (DistanceSensor*)placement.thing; } diff --git a/Placement.cpp b/Placement.cpp index 16e8da7..32f59e0 100644 --- a/Placement.cpp +++ b/Placement.cpp @@ -5,20 +5,17 @@ Placement::Placement() { this->thing = nullptr; } -Placement::Placement(Thing* thing, Vector3 position) { - this->position = position; - this->direction = Vector3::zero; +Placement::Placement(Thing* thing, + Vector3 position, + float horizontalDirection, + float verticalDirection) { this->thing = thing; + this->position = position; + this->horizontalDirection = horizontalDirection; + this->verticalDirection = verticalDirection; } -Placement::Placement(Thing* thing, Vector3 position, float horizontalDirection) { - this->position = position; - this->direction = Vector3(0, horizontalDirection, 0); - this->thing = thing; -} - -Placement::Placement(Thing* thing, float horizontalDirection) { - this->position = Vector3::zero; - this->direction = Vector3(0, horizontalDirection, 0); - this->thing = thing; -} +Placement::Placement(Thing* thing, + float horizontalDirection, + float verticalDirection) + : Placement(thing, Vector3::zero, horizontalDirection, verticalDirection) {} diff --git a/Placement.h b/Placement.h index c1d67f9..b95ba50 100644 --- a/Placement.h +++ b/Placement.h @@ -10,20 +10,46 @@ namespace Passer { namespace RoboidControl { +/// @brief A plament is used to specify where a Thing is placed on the Roboid. class Placement { public: + /// @brief Default constructor with a zero placement Placement(); - Placement(Thing* thing, Vector3 position); - Placement(Thing* thing, Vector3 position, float horizontalAngle); - Placement(Thing* thing, float horizontalAngle); + /// @brief Placement of a Thing on the Roboid + /// @param thing The Thing which is placed + /// @param position The position of the Thing in carthesian coordinated + /// @param horizontalDirection The horizontal direction angle of the Thing + /// @param verticalAngle The vertical direction angle of the Thing + Placement(Thing* thing, + Vector3 position = Vector3::zero, + float horizontalDirection = 0.0F, + float verticalAngle = 0.0F); + /// @brief Placement of a Thing on the roboit without position + /// @param thing The Thing which is place + /// @param horizontalDirection The horizontal direction angle of the Thing + /// @param verticalAngle The vertical direction angle of the Thing + Placement(Thing* thing, + float horizontalDirection, + float verticalAnlge = 0.0F); + /// @brief The parent placement in the Roboid hierarchy + /// @remark Reserved for future use Placement* parent = nullptr; + /// @brief An array of children of this placement in the Roboid hierarchy + /// @remark Reserved for future use Placement** children = nullptr; + /// @brief The number of children of this placemet in the Roboid hierarchy + /// @remark Reserved for future use unsigned int childCount = 0; - Vector3 position; - Vector3 direction; + /// @brief The Thing which is placed Thing* thing; + /// @brief The position of the Thing in carthesian coordinates + Vector3 position; + /// @brief The angle or direction of the Thing in the horizontal plane + float horizontalDirection; + /// @brief The angle or direction of the Thing in the vertical plane + float verticalDirection; }; } // namespace RoboidControl