int main() {
const float PIF = 3.141592653589793238;
for (float x=-2; x<2.2; x+=0.5)
for (float y=-2.6; y<2.6; y+=0.5) {
auto in = octantIndex(x,y);
float oo = 4.f*std::atan2(y,x)/PIF; if (oo<0) oo = 8+oo;
printf("%f %f %f %d %d %d\n",x,y, std::atan2(y,x), int(oo), in,octant(in));
}
std::mt19937 eng;
std::uniform_real_distribution<float> rgen(-5.,5.);
//std::cout << rgen(eng) << std::endl;
for (int i=0; i!=100000; ++i) {
float x1=rgen(eng);
float y1=rgen(eng);
float x2=rgen(eng);
float y2=rgen(eng);
float p1 = std::atan2(y1,x1);
float p2 = std::atan2(y2,x2);
float dp = std::abs(p2-p1);
if (dp>PIF) dp = (2.f*PIF)-dp;
if (dp<(PIF/4.f) && !sameQuadrant( x1,y1, x2,y2) ) printf("%f %f %f\n", p1*180./PIF, p2*180./PIF, dp*180./PIF);
auto o1 = octant(octantIndex(x1,y1));
auto o2 = octant(octantIndex(x2,y2));
if (dp<(PIF/4.f) && !sameQuadrant(o1,o2) ) printf("%f %f %f\n", p1*180./PIF, p2*180./PIF, dp*180./PIF);
}
return 0;
}
void check_nn
(
long n,
Point* pt,
nn_array* nn
)
{
long i, j, oct;
nn_array* nn1;
nn1 = (nn_array*)calloc( (size_t)n, (size_t)sizeof(nn_array) );
brute_force_nearest_neighbors( n, pt, nn1 );
for( i = 0; i < n; i++ )
{
for( oct = 0; oct < 8; oct++ )
{
if( nn[i][oct] == -1 )
{
assert( nn1[i][oct] == -1 );
}
else
{
assert( nn1[i][oct] != -1 );
if( octant(pt[i], pt[ nn[i][oct] ]) != oct )
{
printf( "WRONG OCTANT!\noct=%ld\n", oct );
printf( "i=%ld, x=%ld, y=%ld\n", i, pt[i].x, pt[i].y );
j = nn[i][oct];
printf( "nn=%ld, x=%ld, y=%ld, dist = %ld\n", j, pt[j].x, pt[j].y,
dist(pt[i], pt[j ]) );
}
// assert( octant(pt[i], pt[ nn[i][oct] ]) == oct );
assert( octant(pt[i], pt[ nn1[i][oct] ]) == oct );
if( dist(pt[i], pt[ nn[i][oct] ]) !=
dist(pt[i], pt[ nn1[i][oct] ]) )
{
printf( "NNs DON'T MATCH!\noct=%ld\n", oct );
printf( "i=%ld, x=%ld, y=%ld\n", i, pt[i].x, pt[i].y );
j = nn[i][oct];
printf( "nn=%ld, x=%ld, y=%ld, dist = %ld\n", j, pt[j].x, pt[j].y,
dist(pt[i], pt[j ]) );
j = nn1[i][oct];
printf( "nn1=%ld, x=%ld, y=%ld, dist = %ld\n", j, pt[j].x, pt[j].y,
dist(pt[i], pt[ j ]) );
}
// assert( dist(pt[i], pt[ nn[i][oct] ]) ==
// dist(pt[i], pt[ nn1[i][oct] ]) );
}
}
}
free( nn1 );
}
void brute_force_nearest_neighbors
(
long n,
Point* pt,
nn_array* nn
)
{
long i, j, oct;
long d;
/*
compute nearest neighbors by inspecting all pairs of points
*/
for( i = 0; i < n; i++ )
{
for( oct = 0; oct < 8; oct++ )
{
nn[i][oct] = -1;
}
}
for( i = 0; i < n; i++ )
{
for( j = i+1; j < n; j++ )
{
d = dist(pt[i], pt[j]);
oct = octant( pt[i], pt[j] );
if( ( nn[i][oct] == -1 ) ||
( d < dist(pt[i], pt[ nn[i][oct] ]) )
)
{
nn[i][oct] = j;
}
oct = (oct + 4) % 8;
if( ( nn[j][oct] == -1 ) ||
( d < dist(pt[j], pt[ nn[j][oct] ]) )
)
{
nn[j][oct] = i;
}
}
}
}
void
DualStepper::moveTo(int ax, int ay, float speed)
{
xStepper->targetPos = ax;
yStepper->targetPos = ay;
long dx = ax - xStepper->pos;
long dy = ay - yStepper->pos;
majorAxisSpeed = min(speed, maxSpeed) * max(abs(dx), abs(dy)) / sqrt(dx * dx + dy * dy);
switch (octant(dx, dy)) {
case 0:
_xStepper = xStepper;
_yStepper = yStepper;
xdir = FORWARD;
ydir = FORWARD;
plotLine(dx, dy);
break;
case 1:
_xStepper = yStepper;
_yStepper = xStepper;
xdir = FORWARD;
ydir = FORWARD;
plotLine(dy, dx);
break;
case 2:
_xStepper = yStepper;
_yStepper = xStepper;
xdir = FORWARD;
ydir = BACKWARD;
plotLine(dy, -dx);
break;
case 3:
_xStepper = xStepper;
_yStepper = yStepper;
xdir = BACKWARD;
ydir = FORWARD;
plotLine(-dx, dy);
break;
case 4:
_xStepper = xStepper;
_yStepper = yStepper;
xdir = BACKWARD;
ydir = BACKWARD;
plotLine(-dx, -dy);
break;
case 5:
_xStepper = yStepper;
_yStepper = xStepper;
xdir = BACKWARD;
ydir = BACKWARD;
plotLine(-dy, -dx);
break;
case 6:
_xStepper = yStepper;
_yStepper = xStepper;
xdir = BACKWARD;
ydir = FORWARD;
plotLine(-dy, dx);
break;
case 7:
_xStepper = xStepper;
_yStepper = yStepper;
xdir = FORWARD;
ydir = BACKWARD;
plotLine(dx, -dy);
break;
}
}
int xyz(const Vector3i & halfCell)
{
return xyz(octant(halfCell));
}
