bool CPathFinder::Find(int iFromX, int iFromY, int iToX, int iToY, bool (*fnSetWaypoint)(int, int, int), int iWaypointParameter)
{
// Prepare
Clear();
// Parameter safety
if (!fnSetWaypoint) return false;
SetWaypoint=fnSetWaypoint;
WaypointParameter=iWaypointParameter;
// Start & target coordinates must be free
if (!PointFree(iFromX,iFromY,PointFreeParameter) || !PointFree(iToX,iToY,PointFreeParameter)) return false;
// Add the first two rays
if (!AddRay(iFromX,iFromY,iToX,iToY,0,PF_Direction_Left,NULL)) return false;
if (!AddRay(iFromX,iFromY,iToX,iToY,0,PF_Direction_Right,NULL)) return false;
// Run
Run();
// Success
return Success;
}
int CPathFinderRay::FindCrawlAttach(int iX, int iY)
{
if (!PointFree(iX,iY-1)) return PF_Crawl_Top;
if (!PointFree(iX,iY+1)) return PF_Crawl_Bottom;
if (!PointFree(iX-1,iY)) return PF_Crawl_Left;
if (!PointFree(iX+1,iY)) return PF_Crawl_Right;
return PF_Crawl_NoAttach;
}
void test_point() {
int ndim = 5;
int i;
double diff;
int retval = 0;
printf("\nTesting points\n");
struct Point* p1 = PointAlloc(ndim);
struct Point* p2 = PointAlloc(ndim);
struct Point* p3;
for (i=0; i<p1->x->size; i++) {
p1->x->data[i] = (double) i;
p2->x->data[i] = (double) 2*i;
printf("p1->x->data[%d]: %lf\n", i, p1->x->data[i]);
printf("p2->x->data[%d]: %lf\n", i, p2->x->data[i]);
}
if (PointEqual(p1,p2)) {
printf("Points are equal\n");
} else {
printf("Points differ\n");
}
diff = PointDist(p1,p2);
if (diff < 0.0) {
retval = 1;
} else {
printf("diff: %lf\n", diff);
}
printf("copying Points\n");
PointCopy(p2,p1);
if (PointEqual(p1,p2)) {
printf("Points are equal\n");
} else {
printf("Points differ\n");
}
// this would fail
//PointEqual(p1,p3);
PointFree(p1);
PointFree(p2);
}
int CPathFinderRay::FindCrawlAttachDiagonal(int iX, int iY, int iDirection)
{
// Going left
if (iDirection==PF_Direction_Left)
{
// Top Left
if (!PointFree(iX-1,iY-1)) return PF_Crawl_Top;
// Bottom left
if (!PointFree(iX-1,iY+1)) return PF_Crawl_Left;
// Top right
if (!PointFree(iX+1,iY-1)) return PF_Crawl_Right;
// Bottom right
if (!PointFree(iX+1,iY+1)) return PF_Crawl_Bottom;
}
// Going right
if (iDirection==PF_Direction_Right)
{
// Top Left
if (!PointFree(iX-1,iY-1)) return PF_Crawl_Left;
// Bottom left
if (!PointFree(iX-1,iY+1)) return PF_Crawl_Bottom;
// Top right
if (!PointFree(iX+1,iY-1)) return PF_Crawl_Top;
// Bottom right
if (!PointFree(iX+1,iY+1)) return PF_Crawl_Right;
}
return PF_Crawl_NoAttach;
}
void test_hcube() {
printf("\nTesting points\n");
printf("NDIM = %d\n", NDIM);
int ndim=2;
struct Point* low = PointAlloc(ndim);
struct Point* high = PointAlloc(ndim);
low->x->data[0] = 0.0;
low->x->data[1] = 0.0;
high->x->data[0] = 1.0;
high->x->data[1] = 1.0;
struct HCube* h = HCubeAllocWithBounds(ndim, low, high);
struct Point* inside = PointAlloc(ndim);
inside->x->data[0] = 0.5;
inside->x->data[1] = 0.5;
if (HCubeContains(h, inside)) {
printf("inside point correctly found to be contained\n");
} else {
printf("inside point incorrectly found to be outside\n");
}
struct Point* outside = PointAlloc(ndim);
outside->x->data[0] = 1.5;
outside->x->data[1] = 1.5;
if (HCubeContains(h, outside)) {
printf("outside point correctly found to be outside\n");
} else {
printf("outside point incorrectly found to be inside\n");
}
PointFree(low);
PointFree(high);
PointFree(inside);
HCubeFree(h);
}
bool CPathFinderRay::PathFree(int &rX, int &rY, int iToX, int iToY)
{
int d,dx,dy,aincr,bincr,xincr,yincr,x,y;
// Y based
if (Abs(iToX-rX)<Abs(iToY-rY))
{
xincr=(iToX>rX) ? +1 : -1;
yincr=(iToY>rY) ? +1 : -1;
dy=Abs(iToY-rY); dx=Abs(iToX-rX);
d=2*dx-dy; aincr=2*(dx-dy); bincr=2*dx; x=rX; y=rY;
for (y=rY; y!=iToY; y+=yincr)
{
if (PointFree(x,y)) { rY=y; rX=x; }
else return false;
if (d>=0) { x+=xincr; d+=aincr; }
else d+=bincr;
}
}
// X based
else
{
yincr=(iToY>rY) ? +1 : -1;
xincr=(iToX>rX) ? +1 : -1;
dx=Abs(iToX-rX); dy=Abs(iToY-rY);
d=2*dy-dx; aincr=2*(dy-dx); bincr=2*dy; x=rX; y=rY;
for (x=rX; x!=iToX; x+=xincr)
{
if (PointFree(x,y)) { rY=y; rX=x; }
else return false;
if (d>=0) { y+=yincr; d+=aincr; }
else d+=bincr;
}
}
return true;
}
bool CPathFinderRay::IsCrawlAttach(int iX, int iY, int iAttach)
{
CrawlToAttach(iX,iY,iAttach);
return !PointFree(iX,iY);
}
bool CPathFinderRay::CrawlTargetFree(int iX, int iY, int iAttach, int iDirection)
{
CrawlByAttach(iX,iY,iAttach,iDirection);
return PointFree(iX,iY);
}
