/*! @brief Determines the IDs of mobile objects that should be visible for a given field position.
@param x x position of observer.
@param y y position of observer.
@param heading Heading of observer.
@param headYaw Additional yaw caused by head position relative to body.
@param headPitch Additional pitch caused by head position relative to body.
@param FoV_x Horizontal field of view of the observer.
@param FoV_y Vertical field of view of the observer.
@return A vector of IDs representing the stationary field objects expected to be visible.
*/
std::vector<FieldObjects::MobileFieldObjectID> FieldObjects::GetPossibleMobileObservationIds(float x, float y, float heading,
float headYaw, float headPitch,
float FoV_x, float FoV_y)
{
// Calculate limits.
// Note: These limits assume that the camera is flat horizontally.
float maxAngle = heading + headYaw + FoV_x / 2.0f; // Radians
float minAngle = heading + headYaw - FoV_x / 2.0f; // Radians
float minDistance = 0; // cm
float maxDistance = 200; // cm
// Create temporary variables for use inside loop.
std::vector<MobileFieldObjectID> visibleIds;
Vector2<float> basePos(x,y), targetPosition;
float distance, angle;
bool expectedVisible;
visibleIds.clear(); // Make sure list is empty.
std::vector<MobileObject>::iterator stat_obj_iterator;
// Check all objects.
for(stat_obj_iterator = mobileFieldObjects.begin();
stat_obj_iterator != mobileFieldObjects.end();
stat_obj_iterator++)
{
// Get position of current field object.
targetPosition = stat_obj_iterator->getEstimatedFieldLocation();
// Calculate expected measurments.
distance = DistanceBetweenPoints(basePos,targetPosition);
angle = AngleBetweenPoints(basePos,targetPosition);
// Check if within limits.
expectedVisible = (angle > minAngle) and (angle < maxAngle) and (distance > minDistance) and (distance < maxDistance);
// If expected to be visible put on list.
if(expectedVisible)
visibleIds.push_back(MobileFieldObjectID(stat_obj_iterator->getID()));
}
return visibleIds;
}
bool CircleToCircleCollision(double aX1, double aY1, double aRadius1, double aX2, double aY2, double aRadius2)
{
return (DistanceBetweenPoints(aX1, aY1, aX2, aY2) < aRadius1 + aRadius2) ? true : false;
}
double DistanceBetweenPoints(vec3 aPoint1, vec3 aPoint2)
{
return DistanceBetweenPoints(aPoint1.x, aPoint1.y, aPoint1.z, aPoint2.x, aPoint2.y, aPoint2.z);
}
double DistanceBetweenPoints(vec2 aPoint1, b2Vec2 aPoint2)
{
return DistanceBetweenPoints(aPoint1.x, aPoint1.y, aPoint2.x, aPoint2.y);
}
