SearchPattern.hpp

Go to the documentation of this file.

 
#ifndef SEARCH_PATTERN_HPP
#define SEARCH_PATTERN_HPP
 
#include "../util/GeospatialOperations.hpp"
 
#include <cmath>
#include <vector>
 
 
 
namespace searchpattern
{
 
    inline std::vector<geoops::Waypoint> CalculateSpiralPatternWaypoints(const geoops::UTMCoordinate& stStartingPoint,
                                                                         const double dAngularStepDegrees     = 57,
                                                                         const double dMaxRadius              = 25,
                                                                         const double dStartingHeadingDegrees = 0,
                                                                         const double dStartSpacing           = 1)
    {
        // Define variables.
        std::vector<geoops::Waypoint> vWaypoints;
        double dAngularStepRadians   = dAngularStepDegrees * M_PI / 180;
        double dAngleRadians         = (dStartingHeadingDegrees + 90) * M_PI / 180;
        double dCurrentSpacingWindUp = 0.0;
        double dStartingX            = stStartingPoint.dEasting;
        double dStartingY            = stStartingPoint.dNorthing;
        double dCurrentRadius        = 0.0;
 
        // Check if the MaxRadius is less than 1 meter. If so return an empty vector.
        if (dMaxRadius < 1)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "MaxRadius is less than 1 meter. Cannot create spiral pattern.");
            return vWaypoints;
        }
 
        // Calculate each waypoint. Stop when the radius exceeds the maximum.
        while (dCurrentRadius <= dMaxRadius)
        {
            // Get X and Y positions for the current point.
            double dCurrentX = dStartingX + dCurrentSpacingWindUp * cos(dAngleRadians);
            double dCurrentY = dStartingY + dCurrentSpacingWindUp * sin(dAngleRadians);
 
            // Add the current waypoint to the final vector.
            geoops::UTMCoordinate stCurrentCoordinate = stStartingPoint;
            stCurrentCoordinate.dEasting              = dCurrentX;
            stCurrentCoordinate.dNorthing             = dCurrentY;
            geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
            vWaypoints.push_back(stCurrentWaypoint);
 
            // Increment angle and radius for the next waypoint.
            dAngleRadians += dAngularStepRadians;
            dCurrentSpacingWindUp += dStartSpacing;
 
            // Calculate the current distance from the starting point. This is our radius.
            dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPoint, stCurrentCoordinate).dDistanceMeters;
        }
 
        // Write the search pattern points to the logger, just store the GPS lat/long.
        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
        for (geoops::Waypoint& stWaypoint : vWaypoints)
        {
            szSearchPatternPoints +=
                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
        }
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
 
        return vWaypoints;
    }
 
 
    inline std::vector<geoops::Waypoint> CalculateSpiralPatternWaypoints(const geoops::GPSCoordinate& stStartingPoint,
                                                                         const double dAngularStepDegrees     = 57,
                                                                         const double dMaxRadius              = 25,
                                                                         const double dStartingHeadingDegrees = 0,
                                                                         const double dStartSpacing           = 1)
    {
        // Define variables.
        std::vector<geoops::Waypoint> vWaypoints;
        geoops::UTMCoordinate stStartingPointUTM = geoops::ConvertGPSToUTM(stStartingPoint);
        double dAngularStepRadians               = dAngularStepDegrees * M_PI / 180;
        double dAngleRadians                     = (dStartingHeadingDegrees + 90) * M_PI / 180;
        double dCurrentSpacingWindUp             = 0.0;
        double dStartingX                        = stStartingPointUTM.dEasting;
        double dStartingY                        = stStartingPointUTM.dNorthing;
        double dCurrentRadius                    = 0.0;
 
        // Check if the MaxRadius is less than 1 meter. If so return an empty vector.
        if (dMaxRadius < 1)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "MaxRadius is less than 1 meter. Cannot create spiral pattern.");
            return vWaypoints;
        }
 
        // Calculate each waypoint. Stop when the radius exceeds the maximum.
        while (dCurrentRadius <= dMaxRadius)
        {
            // Get X and Y positions for the current point.
            double dCurrentX = dStartingX + dCurrentSpacingWindUp * cos(dAngleRadians);
            double dCurrentY = dStartingY + dCurrentSpacingWindUp * sin(dAngleRadians);
 
            // Add the current waypoint to the final vector.
            geoops::UTMCoordinate stCurrentCoordinate = stStartingPointUTM;
            stCurrentCoordinate.dEasting              = dCurrentX;
            stCurrentCoordinate.dNorthing             = dCurrentY;
            geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
            vWaypoints.push_back(stCurrentWaypoint);
 
            // Increment angle and radius for the next waypoint.
            dAngleRadians += dAngularStepRadians;
            dCurrentSpacingWindUp += dStartSpacing;
 
            // Calculate the current distance from the starting point. This is our radius.
            dCurrentRadius = geoops::CalculateGeoMeasurement(stStartingPointUTM, stCurrentCoordinate).dDistanceMeters;
        }
 
        // Write the search pattern points to the logger, just store the GPS lat/long.
        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
        for (geoops::Waypoint& stWaypoint : vWaypoints)
        {
            szSearchPatternPoints +=
                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
        }
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
 
        return vWaypoints;
    }
 
 
    inline std::vector<geoops::Waypoint> CalculateZigZagPatternWaypoints(const geoops::UTMCoordinate& stCenterPoint,
                                                                         const double dWidth   = 20.0,
                                                                         const double dHeight  = 20.0,
                                                                         const double dSpacing = 1.0,
                                                                         const bool bVertical  = true)
    {
        // Create instance variables.
        std::vector<geoops::Waypoint> vWaypoints;
        double dStartingX   = stCenterPoint.dEasting - (dWidth / 2);
        double dStartingY   = stCenterPoint.dNorthing - (dHeight / 2);
        double dCurrentX    = dStartingX;
        double dCurrentY    = dStartingY;
        bool bZigNotZag     = true;
        double bCalcSpacing = dSpacing;
 
        // Check if the width or height is less than 1 meter. If so return an empty vector.
        if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
            return vWaypoints;
        }
 
        // Limit spacing to the width or height.
        if (bCalcSpacing > dWidth / 2.0)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than width. Setting spacing to width / 2.");
            // Set spacing to half the width.
            bCalcSpacing = dWidth / 2.0 - 1.0;
        }
        if (bCalcSpacing > dHeight / 2.0)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than height. Setting spacing to height / 2.");
            // Set spacing to half the height.
            bCalcSpacing = dHeight / 2.0 - 1.0;
        }
 
        // Loop until covered entire space of width and height.
        while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
        {
            // Check if pattern should be vertical or horizontal.
            if (bVertical)
            {
                // Check step direction.
                if (bZigNotZag)
                {
                    // Zig.
                    dCurrentX = dStartingX + (dWidth / 2);
                }
                else
                {
                    // Zag.
                    dCurrentX = dStartingX - (dWidth / 2);
                }
            }
            else
            {
                // Check step direction.
                if (bZigNotZag)
                {
                    // Zig.
                    dCurrentY = dStartingY + (dHeight / 2);
                }
                else
                {
                    // Zag.
                    dCurrentY = dStartingY - (dHeight / 2);
                }
            }
 
            // Loop and add points along line until we reached the limit or width or height.
            while ((bZigNotZag && bVertical && dCurrentX <= dStartingX + dWidth) || (!bZigNotZag && bVertical && dCurrentX >= dStartingX) ||
                   (bZigNotZag && !bVertical && dCurrentY <= dStartingY + dHeight) || (!bZigNotZag && !bVertical && dCurrentY >= dStartingY))
            {
                // Construct UTMCoordinate.
                geoops::UTMCoordinate stCurrentCoordinate = stCenterPoint;
                stCurrentCoordinate.dEasting              = dCurrentX;
                stCurrentCoordinate.dNorthing             = dCurrentY;
                geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
                // Add current waypoint to final path.
                vWaypoints.push_back(stCurrentWaypoint);
 
                // Move to the next point based on spacing and direction.
                if (bVertical)
                {
                    // Increment current position.
                    dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
                }
                else
                {
                    // Increment current position.
                    dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
                }
            }
 
            // Now shift the opposite coordinate forward.
            if (bVertical)
            {
                dCurrentY += bCalcSpacing;
            }
            else
            {
                dCurrentX += bCalcSpacing;
            }
 
            // Toggle zigzag direction.
            bZigNotZag = !bZigNotZag;
        }
 
        // The path now contains the zigzag pattern with points spaced at the specified distance. This means that there
        // are many points potentially very close to each other. This is not ideal for navigation, so we will filter out
        // points that are too close to each other, by calculating their GeoMeasurement and testing if the distance is greater
        // then the CalcSpacing. If the CalcSpacing is greater a certain threshold, we will remove the point since it jumps to the other side.
        // This will reduce the number of points in the zigzag pattern, making it more efficient for navigation.
 
        // Create a vector to store the filtered waypoints.
        std::vector<geoops::Waypoint> vFilterWaypoints;
        // Always store the first point.
        vFilterWaypoints.push_back(vWaypoints[0]);
        // Loop through the waypoints and remove any that are too close to each other.
        for (size_t i = 0; i < vWaypoints.size() - 1; ++i)
        {
            // Calculate the GeoMeasurement between the current waypoint and the next waypoint.
            geoops::GeoMeasurement stGeoMeasurement = geoops::CalculateGeoMeasurement(vWaypoints[i].GetGPSCoordinate(), vWaypoints[i + 1].GetGPSCoordinate());
            if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
            {
                // Add the points to the filtered waypoints.
                vFilterWaypoints.push_back(vWaypoints[i]);
                vFilterWaypoints.push_back(vWaypoints[i + 1]);
            }
        }
 
        // Write the search pattern points to the logger, just store the GPS lat/long.
        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
        for (geoops::Waypoint& stWaypoint : vWaypoints)
        {
            szSearchPatternPoints +=
                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
        }
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
 
        // Return the final path.
        return vFilterWaypoints;
    }
 
 
    inline std::vector<geoops::Waypoint> CalculateZigZagPatternWaypoints(const geoops::GPSCoordinate& stCenterPoint,
                                                                         const double dWidth   = 20.0,
                                                                         const double dHeight  = 20.0,
                                                                         const double dSpacing = 1.0,
                                                                         const bool bVertical  = true)
    {
        // Create instance variables.
        std::vector<geoops::Waypoint> vWaypoints;
        geoops::UTMCoordinate stCenterPointUTM = geoops::ConvertGPSToUTM(stCenterPoint);
        double dStartingX                      = stCenterPointUTM.dEasting - (dWidth / 2);
        double dStartingY                      = stCenterPointUTM.dNorthing - (dHeight / 2);
        double dCurrentX                       = dStartingX;
        double dCurrentY                       = dStartingY;
        bool bZigNotZag                        = true;
        double bCalcSpacing                    = dSpacing;
 
        // Check if the width or height is less than 1 meter. If so return an empty vector.
        if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
            return vWaypoints;
        }
 
        // Limit spacing to the width or height.
        if (bCalcSpacing > dWidth / 2.0)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than width. Setting spacing to width / 2.");
            // Set spacing to half the width.
            bCalcSpacing = dWidth / 2.0 - 1.0;
        }
        if (bCalcSpacing > dHeight / 2.0)
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "Spacing is greater than height. Setting spacing to height / 2.");
            // Set spacing to half the height.
            bCalcSpacing = dHeight / 2.0 - 1.0;
        }
 
        // Loop until covered entire space of width and height.
        while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
        {
            // Check if pattern should be vertical or horizontal.
            if (bVertical)
            {
                // Check step direction.
                if (bZigNotZag)
                {
                    // Zig.
                    dCurrentX = dStartingX + bCalcSpacing;
                }
                else
                {
                    // Zag.
                    dCurrentX = dStartingX - bCalcSpacing;
                }
            }
            else
            {
                // Check step direction.
                if (bZigNotZag)
                {
                    // Zig.
                    dCurrentY = dStartingY + bCalcSpacing;
                }
                else
                {
                    // Zag.
                    dCurrentY = dStartingY - bCalcSpacing;
                }
            }
 
            // Loop and add points along line until we reached the limit or width or height.
            while ((bZigNotZag && bVertical && dCurrentX <= dStartingX + dWidth) || (!bZigNotZag && bVertical && dCurrentX >= dStartingX) ||
                   (bZigNotZag && !bVertical && dCurrentY <= dStartingY + dHeight) || (!bZigNotZag && !bVertical && dCurrentY >= dStartingY))
            {
                // Construct UTMCoordinate.
                geoops::UTMCoordinate stCurrentCoordinate = stCenterPointUTM;
                stCurrentCoordinate.dEasting              = dCurrentX;
                stCurrentCoordinate.dNorthing             = dCurrentY;
                geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
                // Add current waypoint to final path.
                vWaypoints.push_back(stCurrentWaypoint);
 
                // Move to the next point based on spacing and direction.
                if (bVertical)
                {
                    // Increment current position.
                    dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
                }
                else
                {
                    // Increment current position.
                    dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
                }
            }
 
            // Now shift the opposite coordinate forward.
            if (bVertical)
            {
                dCurrentY += bCalcSpacing;
            }
            else
            {
                dCurrentX += bCalcSpacing;
            }
 
            // Toggle zigzag direction.
            bZigNotZag = !bZigNotZag;
        }
 
        // The path now contains the zigzag pattern with points spaced at the specified distance. This means that there
        // are many points potentially very close to each other. This is not ideal for navigation, so we will filter out
        // points that are too close to each other, by calculating their GeoMeasurement and testing if the distance is greater
        // then the CalcSpacing. If the CalcSpacing is greater a certain threshold, we will remove the point since it jumps to the other side.
        // This will reduce the number of points in the zigzag pattern, making it more efficient for navigation.
 
        // Create a vector to store the filtered waypoints.
        std::vector<geoops::Waypoint> vFilterWaypoints;
        // Always store the first point.
        vFilterWaypoints.push_back(vWaypoints[0]);
        // Loop through the waypoints and remove any that are too close to each other.
        for (size_t i = 0; i < vWaypoints.size() - 1; ++i)
        {
            // Calculate the GeoMeasurement between the current waypoint and the next waypoint.
            geoops::GeoMeasurement stGeoMeasurement = geoops::CalculateGeoMeasurement(vWaypoints[i].GetGPSCoordinate(), vWaypoints[i + 1].GetGPSCoordinate());
            if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
            {
                // Add the points to the filtered waypoints.
                vFilterWaypoints.push_back(vWaypoints[i]);
                vFilterWaypoints.push_back(vWaypoints[i + 1]);
            }
        }
 
        // Write the search pattern points to the logger, just store the GPS lat/long.
        std::string szSearchPatternPoints = "Search Pattern Points (Spiral): ";
        for (geoops::Waypoint& stWaypoint : vWaypoints)
        {
            szSearchPatternPoints +=
                "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) + ", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) + "), ";
        }
        // Submit logger message.
        LOG_DEBUG(logging::g_qSharedLogger, "{}", szSearchPatternPoints);
 
        // Return the final path.
        return vFilterWaypoints;
    }
}    // namespace searchpattern
#endif