50 const double dAngularStepDegrees = 57,
51 const double dMaxRadius = 25,
52 const double dStartingHeadingDegrees = 0,
53 const double dStartSpacing = 1)
56 std::vector<geoops::Waypoint> vWaypoints;
57 double dAngularStepRadians = dAngularStepDegrees * M_PI / 180;
58 double dAngleRadians = (dStartingHeadingDegrees + 90) * M_PI / 180;
59 double dCurrentSpacingWindUp = 0.0;
62 double dCurrentRadius = 0.0;
68 LOG_WARNING(logging::g_qSharedLogger,
"MaxRadius is less than 1 meter. Cannot create spiral pattern.");
73 while (dCurrentRadius <= dMaxRadius)
76 double dCurrentX = dStartingX + dCurrentSpacingWindUp *
cos(dAngleRadians);
77 double dCurrentY = dStartingY + dCurrentSpacingWindUp *
sin(dAngleRadians);
81 stCurrentCoordinate.dEasting = dCurrentX;
82 stCurrentCoordinate.dNorthing = dCurrentY;
83 geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
84 vWaypoints.push_back(stCurrentWaypoint);
87 dAngleRadians += dAngularStepRadians;
88 dCurrentSpacingWindUp += dStartSpacing;
95 std::string szSearchPatternPoints =
"Search Pattern Points (Spiral): ";
98 szSearchPatternPoints +=
99 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) +
", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) +
"), ";
102 LOG_DEBUG(logging::g_qSharedLogger,
"{}", szSearchPatternPoints);
125 const double dWidth = 20.0,
126 const double dHeight = 20.0,
127 const double dSpacing = 1.0,
128 const bool bVertical =
true)
131 std::vector<geoops::Waypoint> vWaypoints;
132 double dStartingX = stCenterPoint.
GetUTMCoordinate().dEasting - (dWidth / 2);
133 double dStartingY = stCenterPoint.
GetUTMCoordinate().dNorthing - (dHeight / 2);
134 double dCurrentX = dStartingX;
135 double dCurrentY = dStartingY;
136 bool bZigNotZag =
true;
137 double bCalcSpacing = dSpacing;
140 if (dWidth < 1 || dHeight < 1 || dSpacing < 1)
143 LOG_WARNING(logging::g_qSharedLogger,
"Width or height or spacing is less than 1 meter. Cannot create zigzag pattern.");
148 if (bCalcSpacing > dWidth / 2.0)
151 LOG_WARNING(logging::g_qSharedLogger,
"Spacing is greater than width. Setting spacing to width / 2.");
153 bCalcSpacing = dWidth / 2.0 - 1.0;
155 if (bCalcSpacing > dHeight / 2.0)
158 LOG_WARNING(logging::g_qSharedLogger,
"Spacing is greater than height. Setting spacing to height / 2.");
160 bCalcSpacing = dHeight / 2.0 - 1.0;
164 while ((bVertical && dCurrentY <= dStartingY + dHeight) || (!bVertical && dCurrentX <= dStartingX + dWidth))
173 dCurrentX = dStartingX + bCalcSpacing;
178 dCurrentX = dStartingX - bCalcSpacing;
187 dCurrentY = dStartingY + bCalcSpacing;
192 dCurrentY = dStartingY - bCalcSpacing;
197 while ((bZigNotZag && bVertical && dCurrentX <= dStartingX + dWidth) || (!bZigNotZag && bVertical && dCurrentX >= dStartingX) ||
198 (bZigNotZag && !bVertical && dCurrentY <= dStartingY + dHeight) || (!bZigNotZag && !bVertical && dCurrentY >= dStartingY))
202 stCurrentCoordinate.dEasting = dCurrentX;
203 stCurrentCoordinate.dNorthing = dCurrentY;
204 geoops::Waypoint stCurrentWaypoint(stCurrentCoordinate, geoops::WaypointType::eNavigationWaypoint);
206 vWaypoints.push_back(stCurrentWaypoint);
212 dCurrentX += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
217 dCurrentY += bZigNotZag ? bCalcSpacing : -bCalcSpacing;
224 dCurrentY += bCalcSpacing;
228 dCurrentX += bCalcSpacing;
232 bZigNotZag = !bZigNotZag;
242 std::vector<geoops::Waypoint> vFilterWaypoints;
244 vFilterWaypoints.push_back(vWaypoints[0]);
246 for (
size_t i = 0; i < vWaypoints.size() - 1; ++i)
250 if (stGeoMeasurement.dDistanceMeters > bCalcSpacing * 1.5)
253 vFilterWaypoints.push_back(vWaypoints[i]);
254 vFilterWaypoints.push_back(vWaypoints[i + 1]);
259 std::string szSearchPatternPoints =
"Search Pattern Points (Spiral): ";
262 szSearchPatternPoints +=
263 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) +
", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) +
"), ";
266 LOG_DEBUG(logging::g_qSharedLogger,
"{}", szSearchPatternPoints);
269 return vFilterWaypoints;
287 const double dWidth = 20.0,
288 const double dHeight = 20.0,
289 const double dSpacing = 1.0,
290 const int nNumberOfSlithers = 1.0,
291 const bool bVertical =
true)
294 std::vector<geoops::Waypoint> vWaypoints;
296 double dStartingX = (bVertical) ? stStartUTM.dEasting : stStartUTM.dEasting - (dWidth / 2);
297 double dStartingY = (bVertical) ? stStartUTM.dNorthing - (dHeight / 2) : stStartUTM.dNorthing;
300 if (dWidth < 1.0 || dHeight < 1.0 || dSpacing < 0.1 || nNumberOfSlithers < 1)
303 LOG_WARNING(logging::g_qSharedLogger,
304 "Invalid parameters for snake pattern: width={}, height={}, spacing={}, number of slithers={}",
314 int nPoints =
static_cast<int>(dWidth + dHeight / dSpacing);
317 for (
int nIter = 0; nIter < nPoints; ++nIter)
319 double dTime =
static_cast<double>(nIter) / (nPoints - 1);
320 double dCurrentX, dCurrentY;
325 dCurrentY = dStartingY + dTime * dWidth;
327 dCurrentX = dStartingX + (dHeight / 2.0) * std::cos(2.0 * nNumberOfSlithers * M_PI * dTime);
332 dCurrentX = dStartingX + dTime * dWidth;
334 dCurrentY = dStartingY + (dHeight / 2.0) * std::cos(2.0 * nNumberOfSlithers * M_PI * dTime);
339 stCurrentUTM.dEasting = dCurrentX;
340 stCurrentUTM.dNorthing = dCurrentY;
341 geoops::Waypoint stCurrentWaypoint(stCurrentUTM, geoops::WaypointType::eNavigationWaypoint);
342 vWaypoints.push_back(stCurrentWaypoint);
346 std::string szSearchPatternPoints =
"Search Pattern Points (Snake): ";
349 szSearchPatternPoints +=
350 "(" + std::to_string(stWaypoint.GetGPSCoordinate().dLatitude) +
", " + std::to_string(stWaypoint.GetGPSCoordinate().dLongitude) +
"), ";
352 LOG_DEBUG(logging::g_qSharedLogger,
"{}", szSearchPatternPoints);