Created
July 16, 2023 16:59
-
-
Save JamesBremner/e4db46e3e3fa2ce386fccbe6bf47f187 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <iostream> | |
#include <vector> | |
#include <cmath> | |
#include <limits> | |
#include <queue> | |
#include <map> | |
#include <cfloat> | |
#include <set> | |
#include <string> | |
using namespace std; | |
struct Obstacle { | |
double x; | |
double y; | |
double diameter; | |
}; | |
struct Waypoint { | |
double x; | |
double y; | |
double cumulativeDistanceEuclidean; | |
double cumulativeDistanceManhattan; | |
int parentIndex; | |
}; | |
double calculateEuclideanDistance(const Waypoint& waypoint1, const Waypoint& waypoint2) { | |
return std::sqrt(std::pow(waypoint1.x - waypoint2.x, 2) + std::pow(waypoint1.y - waypoint2.y, 2)); | |
} | |
double calculateManhattanDistance(const Waypoint& waypoint1, const Waypoint& waypoint2) { | |
return std::abs(waypoint1.x - waypoint2.x) + std::abs(waypoint1.y - waypoint2.y); | |
} | |
bool isWaypointValid( | |
const Waypoint& waypoint, | |
const std::vector<Obstacle>& obstacles, | |
int maxDistFromOriginSquared ) { | |
if ( waypoint.x * waypoint.x + waypoint.y * waypoint.y > | |
maxDistFromOriginSquared ) | |
return false; | |
for (const Obstacle& obstacle : obstacles) { | |
double distance = std::sqrt(std::pow(obstacle.x - waypoint.x, 2) + std::pow(obstacle.y - waypoint.y, 2)); | |
if (distance <= obstacle.diameter / 2.0) { | |
// Waypoint lies inside an obstacle | |
return false; | |
} | |
} | |
return true; | |
} | |
// this part is generating the waypoints | |
std::vector<Waypoint> generateNeighboringWaypoints( | |
const Waypoint& currentWaypoint, | |
const std::vector<Obstacle>& obstacles, | |
int parentIndex, | |
int maxDistFromOriginSquared ) { | |
std::vector<Waypoint> neighboringWaypoints; | |
for (double dx : {-1.0, 0.0, 1.0}) { | |
for (double dy : {-1.0, 0.0, 1.0}) { | |
if (dx == 0.0 && dy == 0.0) { | |
continue; | |
} | |
Waypoint newWaypoint; | |
newWaypoint.x = currentWaypoint.x + dx; | |
newWaypoint.y = currentWaypoint.y + dy; | |
newWaypoint.cumulativeDistanceEuclidean = currentWaypoint.cumulativeDistanceEuclidean + calculateEuclideanDistance(currentWaypoint, newWaypoint); | |
newWaypoint.cumulativeDistanceManhattan = currentWaypoint.cumulativeDistanceManhattan + calculateManhattanDistance(currentWaypoint, newWaypoint); | |
newWaypoint.parentIndex = parentIndex; | |
if (isWaypointValid( | |
newWaypoint, | |
obstacles, | |
maxDistFromOriginSquared)) { | |
neighboringWaypoints.push_back(newWaypoint); | |
} | |
} | |
} | |
return neighboringWaypoints; | |
} | |
std::vector<Waypoint> exploreWorkspace( | |
const std::vector<Obstacle>& obstacles, | |
int maxDistFromOriginSquared ) { | |
std::vector<Waypoint> waypoints; | |
std::queue<int> queue; | |
std::set<std::pair<double, double>> visited; | |
Waypoint startWaypoint = {-1.0, -1.0, 0.0, 0.0, -1}; | |
waypoints.push_back(startWaypoint); | |
queue.push(0); | |
visited.insert({startWaypoint.x, startWaypoint.y}); | |
while (!queue.empty()) { | |
int currentIndex = queue.front(); | |
queue.pop(); | |
Waypoint currentWaypoint = waypoints[currentIndex]; | |
std::vector<Waypoint> neighboringWaypoints = generateNeighboringWaypoints( | |
currentWaypoint, | |
obstacles, | |
currentIndex, | |
maxDistFromOriginSquared ); | |
for (const Waypoint& newWaypoint : neighboringWaypoints) { | |
if (!visited.count({newWaypoint.x, newWaypoint.y})) { | |
waypoints.push_back(newWaypoint); | |
queue.push(waypoints.size() - 1); | |
visited.insert({newWaypoint.x, newWaypoint.y}); | |
} | |
} | |
} | |
return waypoints; | |
} | |
void printWaypoints(const std::vector<Waypoint>& waypoints) { | |
if (waypoints.empty()) { | |
std::cout << "No waypoints found." << std::endl; | |
return; | |
} | |
std::cout << "Waypoints: " << std::endl; | |
int i = 0; | |
for (const Waypoint& waypoint : waypoints) { | |
std::cout << "Waypoint " << i << ": (" << waypoint.x << ", " << waypoint.y << ")"; | |
std::cout << ", Euclidean Distance: " << waypoint.cumulativeDistanceEuclidean; | |
std::cout << ", Manhattan Distance: " << waypoint.cumulativeDistanceManhattan; | |
std::cout << ", Parent Index: " << waypoint.parentIndex << std::endl; | |
i++; | |
} | |
} | |
int main() { | |
// Define the obstacles | |
std::vector<Obstacle> obstacles = { | |
{0.0, 0.0, 0.5}, // Example obstacle 1 (center x, center y, diameter) | |
{0.5, 0.5, 0.3}, // Example obstacle 2 | |
{-0.5, 0.0, 0.4}, // Example obstacle 3 | |
{0.3, -0.2, 0.2} // Example obstacle 4 | |
}; | |
int maxDistFromOriginSquared = 20; | |
std::vector<Waypoint> waypoints = exploreWorkspace( | |
obstacles, | |
maxDistFromOriginSquared); | |
// Print obstacles | |
std::cout << "Obstacles: " << std::endl; | |
int obstacleIndex = 1; | |
for (const Obstacle& obstacle : obstacles) { | |
std::cout << "Obstacle " << obstacleIndex << ": (" << obstacle.x << ", " << obstacle.y << "), Diameter: " << obstacle.diameter << std::endl; | |
obstacleIndex++; | |
} | |
std::cout << std::endl; | |
// Print waypoints | |
printWaypoints(waypoints); | |
return 0; | |
} |
@Tejal-19 hello?
ohh I'm trying to run it using vscode and it prints nothing. Is it something vscode ?
can you please explain your file structure
Did you look at the build log I posted yesterday? All the code is in one file. Please post your build log ( 2nd time I have asked )
Hey, thankyou soo much for your help. The issue was with VS code. Resolved it. Thankyou
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
When I download the code in this gist, build and run it, then it runs perfectly.
Build log
Output
Please post your build log and output when you try to run my, unaltered, code.