bool SpatialGraphManager::GetPath( const Vector2& source, const Vector2& dest, Vector2List& path )
{
if( _spatialGraph == NULL )
return false;
//Get source cell
SpatialGraphKDNode* pSourceNode = _spatialGraph->FindNode( source );
if( pSourceNode == NULL )
return false;
SpatialGraphKDNode* pDestNode = _spatialGraph->FindNode( dest );
if( pDestNode == NULL )
return false;
path.push_back(source);
if( pSourceNode == pDestNode )
{
path.push_back(dest);
return true;
}
//Compute A*
bool retVal = ComputeAStar(pSourceNode, pDestNode, path);
if( retVal == false )
{
path.clear();
return false;
}
//otherwise, put dest on the pathlist
path.push_back(dest);
return true;
}
Vector2List generatePoints2D(unsigned int n = 4)
{
Vector2List v;
for (unsigned int i = 0; i < n; i++)
{
v.push_back(Vector2::Random());
}
return v;
}
void MazeFinder::GoTo(Vector2 newDestination)
{
Vector2List pathTest;
theSpatialGraph.GetPath(GetPosition(), newDestination, pathTest);
if (pathTest.size() > 0)
{
_pathPoints = pathTest;
_pathIndex = 0;
GetToNextPoint();
}
}
MY_TEST(MinAreaRectangleTest, Point) {
MY_TEST_RANDOM_STUFF(Point)
Vector2List t;
t.push_back(Vector2(1,0));
Matrix2Dyn v(2,t.size());
for(unsigned int i = 0; i<t.size(); ++i) {
v.col(i) = t[i];
}
minRectTest(testName,v);
}
MY_TEST(MinAreaRectangleTest, TwoPoints) {
MY_TEST_RANDOM_STUFF(TwoPoints)
Vector2List v;
v.push_back(Vector2(1,0));
v.push_back(Vector2(3,3));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(testName,t);
}
bool ComputeAStar( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode, Vector2List& path )
{
AStarSearch<SearchInterface> search;
SearchInterface pStart(pSourceNode);
SearchInterface pEnd(pDestNode);
search.SetStartAndGoalStates( pStart, pEnd );
while( AStarSearch<SearchInterface>::SEARCH_STATE_SEARCHING == search.SearchStep() )
{
}
int curState = search.GetState();
if( curState == AStarSearch<SearchInterface>::SEARCH_STATE_SUCCEEDED )
{
//Get path
for( SearchInterface* pCur = search.GetSolutionStart(); pCur != NULL; pCur = search.GetSolutionNext() )
{
path.push_back( pCur->pNode->BBox.Centroid() );
}
search.FreeSolutionNodes();
return true;
}
return false;
}
void SpatialGraphKDNode::GetGridPoints( Vector2List& points, int& xPoints, int& yPoints )
{
xPoints = 0;
yPoints = 0;
Vector2 vSmallestDimensions = Tree->GetSmallestDimensions();
Vector2 vMyBoxDimensions = BBox.Max - BBox.Min;
/*
if( vSmallestDimensions == vMyBoxDimensions )
{
xPoints = 1;
yPoints = 1;
points.push_back( BBox.Centroid() );
return;
}
*/
xPoints = static_cast<int>(vMyBoxDimensions.X / vSmallestDimensions.X);
yPoints = static_cast<int>(vMyBoxDimensions.Y / vSmallestDimensions.Y);
points.reserve(xPoints*yPoints);
Vector2 vBottomLeftStartBox( BBox.Min.X, BBox.Max.Y - vSmallestDimensions.Y );
BoundingBox startBox( vBottomLeftStartBox, vBottomLeftStartBox + vSmallestDimensions);
BoundingBox checkBox( startBox );
for( int yDim = 0; yDim < yPoints; yDim++ )
{
for( int xDim = 0; xDim < xPoints; xDim++ )
{
points.push_back( checkBox.Centroid() );
checkBox.Min.X += vSmallestDimensions.X;
checkBox.Max.X += vSmallestDimensions.X;
}
checkBox.Min.X = startBox.Min.X;
checkBox.Max.X = startBox.Max.X;
checkBox.Min.Y -= vSmallestDimensions.Y;
checkBox.Max.Y -= vSmallestDimensions.Y;
}
}
Vector2List getPointsFromFile2D(std::string filePath)
{
std::ifstream file; // creates stream myFile
file.open(filePath.c_str()); // opens .txt file
if(!file.is_open())
{ // check file is open, quit if not
ApproxMVBB_ERRORMSG("Could not open file: " << filePath)
}
PREC a, b;
Vector2List v;
while(file.good())
{
file >> a >> b;
v.emplace_back(a, b);
}
file.close();
return v;
}
MY_TEST(MinAreaRectangleTest, AlmostLine) {
MY_TEST_RANDOM_STUFF(AlmostLine)
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(1,1+1e-13));
v.push_back(Vector2(2,2));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(testName,t);
}
void SpatialGraphKDNode::Render()
{
if( HasChildren() )
{
LHC->Render();
RHC->Render();
return;
}
if( theSpatialGraph.GetDrawBlocked() )
{
if( bBlocked )
{
glColor4f(1,0,0,0.25f);
BBox.RenderBox();
}
}
if( theSpatialGraph.GetDrawBounds() )
{
glColor4f(0,0,0,1.f);
BBox.RenderOutline();
}
Vector2 centroid = BBox.Centroid();
if( theSpatialGraph.GetDrawNodeIndex() )
{
Vector2 screenCenter = MathUtil::WorldToScreen( centroid.X, centroid.Y );
//Print some vals
glColor3f(0,1.f,1.f);
DrawGameText( IntToString(Index), "ConsoleSmall", (unsigned int)screenCenter.X, (unsigned int)screenCenter.Y );
}
if( theSpatialGraph.GetDrawGraph() && !bBlocked )
{
glColor3f(1.f,0.f,0.f);
float linePoints[4];
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(2, GL_FLOAT, 0, linePoints);
for( unsigned int i = 0; i < Neighbors.size(); i++ )
{
if( Neighbors[i]->bBlocked || !NeighborLOS[i] )
continue;
//draw centroid to centroid half way point
Vector2 neighbor = Neighbors[i]->BBox.Centroid();
neighbor = centroid + ((neighbor - centroid) * 0.6f);
linePoints[0] = centroid.X;
linePoints[1] = centroid.Y;
linePoints[2] = neighbor.X;
linePoints[3] = neighbor.Y;
glDrawArrays(GL_LINES, 0, 2);
}
}
if( theSpatialGraph.GetDrawGridPoints() )
{
glColor3f(1.f,0.f,0.f);
Vector2List gridPoints;
int xPoints, yPoints;
GetGridPoints(gridPoints, xPoints, yPoints );
for( unsigned int i = 0; i < gridPoints.size(); i++ )
{
DrawPoint( gridPoints[i], Tree->GetSmallestDimensions().X * 0.15f );
}
}
}
void test() {
using namespace PointFunctions;
using namespace TestFunctions;
{
// generate points
ApproxMVBB::Matrix2Dyn t(2,10);
t.setRandom();
minRectTest(1,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(2,2));
v.push_back(Vector2(3,3));
v.push_back(Vector2(-1,1));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(2,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(2,2));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(3,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(4,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(1,-1));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(5,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(1,1+1e-13));
v.push_back(Vector2(2,2));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(6,t);
}
//
// {
// // generate points on circle
// unsigned int max = 100000;
// Vector2List v(max);
// for(unsigned int i=0; i<max; i++) {
// v[i] = Vector2(std::cos(0.0001/max * i) ,std::sin(0.0001/max * i) );
// }
// ApproxMVBB::Matrix2Dyn t(2,v.size());
// for(unsigned int i = 0; i<v.size(); ++i) {
// t.col(i) = v[i];
// }
// minRectTest(7,t);
// }
{
// generate points
Vector2List v;
v.push_back(Vector2(1,0));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(8,t);
}
{
// generate points
Vector2List v;
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(9,t);
}
{
// generate points
Vector2List v;
v.push_back(Vector2(0,0));
v.push_back(Vector2(1,0));
v.push_back(Vector2(1,1));
v.push_back(Vector2(0,1));
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(10,t);
}
{
// generate points
auto v = getPointsFromFile2D("./PointsSimulation2DRectFail.txt");
ApproxMVBB::Matrix2Dyn t(2,v.size());
for(unsigned int i = 0; i<v.size(); ++i) {
t.col(i) = v[i];
}
minRectTest(11,t);
}
#ifdef ApproxMVBB_TESTS_HIGH_PERFORMANCE
{
// generate points
ApproxMVBB::Matrix2Dyn t(2,10000000);
t.setRandom();
minRectTest(12,t);
}
#endif
{
// generate points
ApproxMVBB::Matrix2Dyn t(2,400);
getPointsFromFileBinary("./PointsBadProjection.bin",t);
minRectTest(13,t);
}
{
// generate points
ApproxMVBB::Matrix2Dyn t(2,400);
getPointsFromFileBinary("./PointsBadProjection2.bin",t);
minRectTest(14,t);
}
{
// generate points
ApproxMVBB::Matrix2Dyn t(2,400);
getPointsFromFileBinary("./PointsBadProjection3.bin",t);
minRectTest(15,t);
}
}
bool PixelsToPositions(const String& filename, Vector2List &positions, float gridSize, const Color& pixelColor, float tolerance)
{
#if _ANGEL_DISABLE_DEVIL
png_byte* pngData;
float* rawData;
png_uint_32 width, height;
if (!LoadPNG(filename, pngData, width, height))
{
//error was output by the load
return false;
}
// convert png ubyte data to float array
rawData = (float*)malloc(3 * width * height * sizeof(float));
int j = 0;
for (int i = 0; i < (4 * width * height); i++)
{
if ((i+1) % 4)
{
rawData[j] = (float)pngData[i] / 255.0f;
j++;
}
}
free(pngData);
#else
ILuint imgRef;
ilGenImages(1, &imgRef);
ilBindImage(imgRef);
// load image into DevIL
if (!ilLoadImage(filename.c_str()))
{
HandleDevILErrors(filename);
return false;
}
// get image datums
unsigned int width = ilGetInteger(IL_IMAGE_WIDTH);
unsigned int height = ilGetInteger(IL_IMAGE_HEIGHT);
// convert it to RGB floats for easy comparison
ilConvertImage(IL_RGB, IL_FLOAT);
// flip it if we need to
if (ilGetInteger(IL_IMAGE_ORIGIN) != IL_ORIGIN_LOWER_LEFT)
{
iluFlipImage();
}
// grab the raw data
ILfloat* rawData = (ILfloat*)ilGetData();
#endif //_ANGEL_DISABLE_DEVIL
// check every pixel, see if it's within the tolerance of the target
float w = -((float)width)*gridSize/2.f;
float h = -((float)height)*gridSize/2.f;
Vector2 offset(w, h);
float pixR, pixG, pixB;
unsigned int pixOffset = 0;
for (int y=0; y < height; y++)
{
for (int x=0; x < width; x++)
{
pixOffset = (y * width * 3) + (x * 3);
pixR = rawData[pixOffset];
pixG = rawData[pixOffset + 1];
pixB = rawData[pixOffset + 2];
if ( fabs(pixR - pixelColor.R) <= tolerance
&& fabs(pixG - pixelColor.G) <= tolerance
&& fabs(pixB - pixelColor.B) <= tolerance
)
{
positions.push_back(offset + Vector2(x * gridSize, y * gridSize));
}
}
}
// cleanup and return
#if _ANGEL_DISABLE_DEVIL
free(rawData);
#else
ilDeleteImages(1, &imgRef);
#endif
return true;
}
