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; }