void CreateRandomAvoidsSet()
{
cout << "Generating avoids" << endl;
int counter = 0;
int* output_seq_of_locations = (int*)malloc(100 * sizeof(int));
int outsize;
int numRobots = WORKSPACE_INFO->numOfRobots;
avoidsArr = (AvoidPositions*)malloc((numRobots - 1) * sizeof(AvoidPositions));
sizeof_avoids = 0;
while (counter < (numRobots - 1))
{
int st = GetStartLocation(counter);
int g = GetEndLocation(counter);
cout << st << endl;
cout << g << endl;
output_seq_of_locations = (int*)malloc(1000 * sizeof(int));
GenerateMotionPlanFor(*WORKSPACE_INFO, st, g, WORKSPACE_INFO->obstacles.locations, WORKSPACE_INFO->obstacles.size, avoidsArr, sizeof_avoids, output_seq_of_locations, &outsize);
cout << "Trajectory Length = " << outsize << endl;
cout << "Trajectory: " << endl;
for (int count = 0; count < outsize; count++)
{
cout << output_seq_of_locations[count] << " ";
}
cout << endl;
avoidsArr[counter].size = outsize;
for (int i = 0; i < outsize; i++)
{
avoidsArr[counter].PositionsOccupied[i] = output_seq_of_locations[i];
}
sizeof_avoids++;
counter++;
}
cout << "------------" << endl;
}
int main()
{
int count;
int* output_seq_of_locations = (int*)malloc(100 * sizeof(int));
int output_size = 0;
srand(time(NULL));
//Initialize the workspace
WORKSPACE_INFO = ParseWorkspaceConfig("C:\\Workspace\\Drona\\Src\\Workspaces\\Exp1\\Workspace.xml");
int exp = 3;
cout << "--------------------------------" << endl;
cout << "Num Robots: " << WORKSPACE_INFO->numOfRobots << endl;
CreateRandomAvoidsSet();
exp = 5;
while (exp > 0)
{
cout << "Experiment " << exp << endl;
int st = GetStartLocation(WORKSPACE_INFO->numOfRobots - 1);
int g = GetEndLocation(WORKSPACE_INFO->numOfRobots -1);
cout << st << endl;
cout << g << endl;
output_seq_of_locations = (int*)malloc(1000 * sizeof(int));
GenerateMotionPlanFor(*WORKSPACE_INFO, st, g, WORKSPACE_INFO->obstacles.locations, WORKSPACE_INFO->obstacles.size, avoidsArr, sizeof_avoids, output_seq_of_locations, &output_size);
cout << "Trajectory Length = " << output_size << endl;
cout << "Trajectory: " << endl;
for (count = 0; count < output_size; count++)
{
cout << output_seq_of_locations[count] << " ";
}
cout << endl;
free(output_seq_of_locations);
exp--;
cout << "--------------------------------" << endl;
}
return 0;
}
void FNavigationPath::DebugDraw(const ANavigationData* NavData, FColor PathColor, UCanvas* Canvas, bool bPersistent, const uint32 NextPathPointIndex) const
{
static const FColor Grey(100,100,100);
const int32 NumPathVerts = PathPoints.Num();
UWorld* World = NavData->GetWorld();
for (int32 VertIdx = 0; VertIdx < NumPathVerts-1; ++VertIdx)
{
// draw box at vert
FVector const VertLoc = PathPoints[VertIdx].Location + NavigationDebugDrawing::PathOffset;
DrawDebugSolidBox(World, VertLoc, NavigationDebugDrawing::PathNodeBoxExtent, VertIdx < int32(NextPathPointIndex) ? Grey : PathColor, bPersistent);
// draw line to next loc
FVector const NextVertLoc = PathPoints[VertIdx+1].Location + NavigationDebugDrawing::PathOffset;
DrawDebugLine(World, VertLoc, NextVertLoc, VertIdx < int32(NextPathPointIndex)-1 ? Grey : PathColor, bPersistent
, /*LifeTime*/-1.f, /*DepthPriority*/0
, /*Thickness*/NavigationDebugDrawing::PathLineThickness);
}
// draw last vert
if (NumPathVerts > 0)
{
DrawDebugBox(World, PathPoints[NumPathVerts-1].Location + NavigationDebugDrawing::PathOffset, FVector(15.f), PathColor, bPersistent);
}
// if observing goal actor draw a radius and a line to the goal
if (GoalActor.IsValid())
{
const FVector GoalLocation = GetGoalLocation() + NavigationDebugDrawing::PathOffset;
const FVector EndLocation = GetEndLocation() + NavigationDebugDrawing::PathOffset;
static const FVector CylinderHalfHeight = FVector::UpVector * 10.f;
DrawDebugCylinder(World, EndLocation - CylinderHalfHeight, EndLocation + CylinderHalfHeight, FMath::Sqrt(GoalActorLocationTetherDistanceSq), 16, PathColor, bPersistent);
DrawDebugLine(World, EndLocation, GoalLocation, Grey, bPersistent);
}
}
FVector FNavigationPath::GetGoalLocation() const
{
return GoalActor != NULL ? (GoalActorAsNavAgent != NULL ? GoalActorAsNavAgent->GetNavAgentLocation() : GoalActor->GetActorLocation()) : GetEndLocation();
}