/* static */
void fillLine(point p, point q, PointSet * ps)
{
int x1 = p.x;
int y1 = p.y;
int x2 = q.x;
int y2 = q.y;
int d, x, y, ax, ay, sx, sy, dx, dy;
dx = x2 - x1;
ax = ABS(dx) << 1;
sx = SGN(dx);
dy = y2 - y1;
ay = ABS(dy) << 1;
sy = SGN(dy);
/* fprintf (stderr, "fillLine %d %d - %d %d\n", x1,y1,x2,y2); */
x = x1;
y = y1;
if (ax > ay) { /* x dominant */
d = ay - (ax >> 1);
for (;;) {
/* fprintf (stderr, " addPS %d %d\n", x,y); */
addPS(ps, x, y);
if (x == x2)
return;
if (d >= 0) {
y += sy;
d -= ax;
}
x += sx;
d += ay;
}
} else { /* y dominant */
/*-----------------------------------------------------------
* Draws the outline of a line
*/
static void
XORDrawAttachedLine (LocationType x1, LocationType y1, LocationType x2,
LocationType y2, BDimension thick)
{
LocationType dx, dy, ox, oy;
float h;
dx = x2 - x1;
dy = y2 - y1;
if (dx != 0 || dy != 0)
h = 0.5 * thick / sqrt (SQUARE (dx) + SQUARE (dy));
else
h = 0.0;
ox = dy * h + 0.5 * SGN (dy);
oy = -(dx * h + 0.5 * SGN (dx));
gui->draw_line (Crosshair.GC, x1 + ox, y1 + oy, x2 + ox, y2 + oy);
if (abs (ox) >= pixel_slop || abs (oy) >= pixel_slop)
{
LocationType angle = atan2 ((float) dx, (float) dy) * 57.295779;
gui->draw_line (Crosshair.GC, x1 - ox, y1 - oy, x2 - ox, y2 - oy);
gui->draw_arc (Crosshair.GC,
x1, y1, thick / 2, thick / 2, angle - 180, 180);
gui->draw_arc (Crosshair.GC, x2, y2, thick / 2, thick / 2, angle, 180);
}
}
/* modified lambda (mlambda) search (ref. [2]) -------------------------------*/
static int search(int n, int m, const double *L, const double *D,
const double *zs, double *zn, double *s)
{
int i,j,k,c,nn=0,imax=0;
double newdist,maxdist=1E99,y;
double *S=zeros(n,n),*dist=mat(n,1),*zb=mat(n,1),*z=mat(n,1),*step=mat(n,1);
k=n-1; dist[k]=0.0;
zb[k]=zs[k];
z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);
for (c=0;c<LOOPMAX;c++) {
newdist=dist[k]+y*y/D[k];
if (newdist<maxdist) {
if (k!=0) {
dist[--k]=newdist;
for (i=0;i<=k;i++)
S[k+i*n]=S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];
zb[k]=zs[k]+S[k+k*n];
z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);
}
else {
if (nn<m) {
if (nn==0||newdist>s[imax]) imax=nn;
for (i=0;i<n;i++) zn[i+nn*n]=z[i];
s[nn++]=newdist;
}
else {
if (newdist<s[imax]) {
for (i=0;i<n;i++) zn[i+imax*n]=z[i];
s[imax]=newdist;
for (i=imax=0;i<m;i++) if (s[imax]<s[i]) imax=i;
}
maxdist=s[imax];
}
z[0]+=step[0]; y=zb[0]-z[0]; step[0]=-step[0]-SGN(step[0]);
}
}
else {
if (k==n-1) break;
else {
k++;
z[k]+=step[k]; y=zb[k]-z[k]; step[k]=-step[k]-SGN(step[k]);
}
}
}
for (i=0;i<m-1;i++) { /* sort by s */
for (j=i+1;j<m;j++) {
if (s[i]<s[j]) continue;
SWAP(s[i],s[j]);
for (k=0;k<n;k++) SWAP(zn[k+i*n],zn[k+j*n]);
}
}
free(S); free(dist); free(zb); free(z); free(step);
if (c>=LOOPMAX) {
fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
return -1;
}
return 0;
}
void
NWindowScreen::draw_line(int x1, int y1, int x2, int y2, int color)
{
// Simple Bresenham's line drawing algorithm
int d,x,y,ax,ay,sx,sy,dx,dy;
#define ABS(x) (((x)<0) ? -(x) : (x))
#define SGN(x) (((x)<0) ? -1 : 1)
dx=x2-x1; ax=ABS(dx)<<1; sx=SGN(dx);
dy=y2-y1; ay=ABS(dy)<<1; sy=SGN(dy);
x=x1;
y=y1;
if(ax>ay)
{
d=ay-(ax>>1);
for(;;)
{
set_pixel(x,y,color);
if(x==x2) return;
if(d>=0)
{
y+=sy;
d-=ax;
}
x+=sx;
d+=ay;
}
}
void draw_line(int c1, int r1, int c2, int r2) {
uint8_t *hud = hud_image+c1+r1*128;
int dc=(c2-c1), dr=(r2-r1);
int cs=SGN(dc), rs=SGN(dr)*128;
int h = abs(dc) > abs(dr); // line is more horizontal
int ml = h ? abs(dc) : abs(dr);
int ol = h ? abs(dr) : abs(dc);
int ms = h ? cs : rs;
int os = h ? rs : cs;
int state = ml/2;
int pl = ml;
while (pl>=0) {
hud[0] = fg_color;
hud+=ms;
state -= ol;
if (state < 0) {
state += ml;
hud+=os;
}
pl --;
}
}
/*
* PathIsClear: Return True iff the path from p1 to p2 is clear, with these
* 2 exceptions:
* 1) p1 may contain a piece
* 2) p2 may contain a piece NOT of the given color
* Assumes that p1 and p2 are connected by a horizontal, vertical, or diagonal
* line.
*/
Bool PathIsClear(Board *b, POINT p1, POINT p2, BYTE color)
{
int dx, dy;
int x, y;
dx = SGN(p2.x - p1.x);
dy = SGN(p2.y - p1.y);
// Start at first square piece enters
x = p1.x + dx;
y = p1.y + dy;
while (x != p2.x || y != p2.y)
{
if (b->squares[y][x].piece != NONE)
return False;
x += dx;
y += dy;
}
// p2 can't contain piece of same color
if (b->squares[y][x].piece != NONE && b->squares[y][x].color == color)
return False;
return True;
}
void
check_one (const char *name, mpz_srcptr x, double y, int cmp, int cmpabs)
{
int got;
got = mpz_cmp_d (x, y);
if (SGN(got) != cmp)
{
unsigned i;
printf ("mpz_cmp_d wrong (from %s)\n", name);
printf (" got %d\n", got);
printf (" want %d\n", cmp);
fail:
printf (" x=");
mpz_out_str (stdout, 10, x);
printf ("\n y %g\n", y);
printf (" x=0x");
mpz_out_str (stdout, -16, x);
printf ("\n y %g\n", y);
printf (" y");
for (i = 0; i < sizeof(y); i++)
printf (" %02X", (unsigned) ((unsigned char *) &y)[i]);
printf ("\n");
abort ();
}
got = mpz_cmpabs_d (x, y);
if (SGN(got) != cmpabs)
{
printf ("mpz_cmpabs_d wrong\n");
printf (" got %d\n", got);
printf (" want %d\n", cmpabs);
goto fail;
}
}
void convex_hull_marching(Bezier src_bz, Bezier bz,
std::vector<double> &solutions,
double left_t,
double right_t)
{
while(bz.order() > 0 and bz[0] == 0) {
std::cout << "deflate\n";
bz = bz.deflate();
solutions.push_back(left_t);
}
if (bz.order() > 0) {
int old_sign = SGN(bz[0]);
int sign;
double left_bound = 0;
double dt = 0;
for (size_t i = 1; i < bz.size(); i++)
{
sign = SGN(bz[i]);
if (sign != old_sign)
{
dt = double(i) / bz.order();
left_bound = dt * bz[0] / (bz[0] - bz[i]);
break;
}
old_sign = sign;
}
if (dt == 0) return;
std::cout << bz << std::endl;
std::cout << "dt = " << dt << std::endl;
std::cout << "left_t = " << left_t << std::endl;
std::cout << "right_t = " << right_t << std::endl;
std::cout << "left bound = " << left_bound
<< " = " << bz(left_bound) << std::endl;
double new_left_t = left_bound * (right_t - left_t) + left_t;
std::cout << "new_left_t = " << new_left_t << std::endl;
Bezier bzr = subRight(src_bz, new_left_t);
while(bzr.order() > 0 and bzr[0] == 0) {
std::cout << "deflate\n";
bzr = bzr.deflate();
solutions.push_back(new_left_t);
}
if (left_t < new_left_t) {
convex_hull_marching(src_bz, bzr,
solutions,
new_left_t, right_t);
} else {
std::cout << "epsilon reached\n";
while(bzr.order() > 0 and fabs(bzr[0]) <= 1e-10) {
std::cout << "deflate\n";
bzr = bzr.deflate();
std::cout << bzr << std::endl;
solutions.push_back(new_left_t);
}
}
}
}
static inline double __plm_minmod(double ul, double u0, double ur)
{
const double a = plm_theta * (u0 - ul);
const double b = 0.5 * (ur - ul);
const double c = plm_theta * (ur - u0);
const double fabc[3] = { fabs(a), fabs(b), fabs(c) };
return 0.25*fabs(SGN(a)+SGN(b))*(SGN(a)+SGN(c))*min3(fabc);
}
/**
* Compare intervals x and y. Intervals are sorted according to their
* lower bound; ties are broken by considering also the upper
* bound.
*/
static int int_compare( const struct interval* x, const struct interval* y, uint16_t current_dim )
{
if ( x == y ) return 0;
if ( x->lower[current_dim] != y->lower[current_dim])
return SGN(x->lower[current_dim] - y->lower[current_dim]);
else
return SGN(x->upper[current_dim] - y->upper[current_dim]);
}
//Trying to pull points out of a tripoint vector is messy and
//probably slow, so leaving two full functions for now
std::vector <point> line_to(const int x1, const int y1, const int x2, const int y2, int t)
{
std::vector<point> ret;
// Preallocate the number of cells we need instead of allocating them piecewise.
const int numCells = square_dist(tripoint(x1, y1, 0),tripoint(x2, y2, 0));
ret.reserve(numCells);
const int dx = x2 - x1;
const int dy = y2 - y1;
point cur;
cur.x = x1;
cur.y = y1;
// Draw point
if (dx==0 && dy==0) {
ret.push_back(cur);
// Should exit here
return ret;
}
// Any ideas why we're multiplying the abs distance by two here?
const int ax = abs(dx) << 1; // bitshift one place, functional *2
const int ay = abs(dy) << 1;
const int sx = (dx == 0 ? 0 : SGN(dx)), sy = (dy == 0 ? 0 : SGN(dy));
// The old version of this algorithm would generate points on the line and check min/max for each point
// to determine whether or not to continue generating the line. Since we already know how many points
// we need, this method saves us a half-dozen variables and a few calculations.
if (ax == ay) {
for (int i = 0; i < numCells; i++) {
cur.y += sy;
cur.x += sx;
ret.push_back(cur);
} ;
} else if (ax > ay) {
for (int i = 0; i < numCells; i++) {
if (t > 0) {
cur.y += sy;
t -= ax;
}
cur.x += sx;
t += ay;
ret.push_back(cur);
} ;
} else {
for (int i = 0; i < numCells; i++) {
if (t > 0) {
cur.x += sx;
t -= ay;
}
cur.y += sy;
t += ax;
ret.push_back(cur);
} ;
}
return ret;
}
std::vector <point> line_to(int x0, int y0, int x1, int y1)
{
int t = 0;
std::vector<point> ret;
int dx = x1 - x0;
int dy = y1 - y0;
int ax = abs(dx)<<1;
int ay = abs(dy)<<1;
int sx = SGN(dx);
int sy = SGN(dy);
if (dy == 0) sy = 0;
if (dx == 0) sx = 0;
point cur;
cur.x = x0;
cur.y = y0;
int xmin = (x0 < x1 ? x0 : x1), ymin = (y0 < y1 ? y0 : y1),
xmax = (x0 > x1 ? x0 : x1), ymax = (y0 > y1 ? y0 : y1);
xmin -= abs(dx);
ymin -= abs(dy);
xmax += abs(dx);
ymax += abs(dy);
if (ax == ay) {
do {
cur.y += sy;
cur.x += sx;
ret.push_back(cur);
} while ((cur.x != x1 || cur.y != y1) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
} else if (ax > ay) {
do {
if (t > 0) {
cur.y += sy;
t -= ax;
}
cur.x += sx;
t += ay;
ret.push_back(cur);
} while ((cur.x != x1 || cur.y != y1) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
} else {
do {
if (t > 0) {
cur.x += sx;
t -= ay;
}
cur.y += sy;
t += ax;
ret.push_back(cur);
} while ((cur.x != x1 || cur.y != y1) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
}
return ret;
}
int DPLL_add_binary_implications( int lit1, int lit2 )
{
int i, *bImp;
if( IS_FIXED(lit1) )
{
if( !FIXED_ON_COMPLEMENT(lit1) ) return SAT;
else if( IS_FIXED(lit2) )
return( !FIXED_ON_COMPLEMENT(lit2) );
else return look_fix_binary_implications(lit2);
}
else if( IS_FIXED(lit2) )
{
if( !FIXED_ON_COMPLEMENT(lit2) ) return SAT;
else return look_fix_binary_implications(lit1);
}
#ifdef BIEQ
while( (VeqDepends[ NR(lit1) ] != INDEPENDENT) &&
(VeqDepends[ NR(lit1) ] != EQUIVALENT) )
lit1 = VeqDepends[ NR(lit1) ] * SGN(lit1);
while( (VeqDepends[ NR(lit2) ] != INDEPENDENT) &&
(VeqDepends[ NR(lit2) ] != EQUIVALENT) )
lit2 = VeqDepends[ NR(lit2) ] * SGN(lit2);
if( lit1 == -lit2 ) return SAT;
if( lit1 == lit2 ) return look_fix_binary_implications(lit1);
#endif
STAMP_IMPLICATIONS( -lit1 );
if( bImp_stamps[ -lit2 ] == current_bImp_stamp )
return look_fix_binary_implications( lit1 );
if( bImp_stamps[lit2] != current_bImp_stamp )
{
int _result;
bImp_stamps[ BinaryImp[-lit1][ BinaryImp[-lit1][0] - 1] ] = current_bImp_stamp;
_result = DPLL_add_compensation_resolvents( lit1, lit2 );
if( _result != UNKNOWN )
return _result;
STAMP_IMPLICATIONS( -lit2 );
if( bImp_stamps[ -lit1 ] == current_bImp_stamp )
return look_fix_binary_implications( lit2 );
_result = DPLL_add_compensation_resolvents( lit2, lit1 );
if( _result != UNKNOWN )
return _result;
ADD_BINARY_IMPLICATIONS( lit1, lit2 );
}
return SAT;
}
std::vector <point> line_to(int x1, int y1, int x2, int y2, int t)
{
std::vector<point> ret;
int dx = x2 - x1;
int dy = y2 - y1;
int ax = abs(dx)<<1;
int ay = abs(dy)<<1;
int sx = SGN(dx);
int sy = SGN(dy);
if (dy == 0) sy = 0;
if (dx == 0) sx = 0;
point cur;
cur.x = x1;
cur.y = y1;
int xmin = (x1 < x2 ? x1 : x2), ymin = (y1 < y2 ? y1 : y2),
xmax = (x1 > x2 ? x1 : x2), ymax = (y1 > y2 ? y1 : y2);
xmin -= abs(dx);
ymin -= abs(dy);
xmax += abs(dx);
ymax += abs(dy);
if (ax == ay) {
do {
cur.y += sy;
cur.x += sx;
ret.push_back(cur);
} while ((cur.x != x2 || cur.y != y2) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
} else if (ax > ay) {
do {
if (t > 0) {
cur.y += sy;
t -= ax;
}
cur.x += sx;
t += ay;
ret.push_back(cur);
} while ((cur.x != x2 || cur.y != y2) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
} else {
do {
if (t > 0) {
cur.x += sx;
t -= ay;
}
cur.y += sy;
t += ax;
ret.push_back(cur);
} while ((cur.x != x2 || cur.y != y2) &&
(cur.x >= xmin && cur.x <= xmax && cur.y >= ymin && cur.y <= ymax));
}
return ret;
}
/* ---------------------------------------------------------------------------
* adjusts the insert lines to make them 45 degrees as necessary
*/
void
AdjustTwoLine (int way)
{
LocationType dx, dy;
AttachedLineTypePtr line = &Crosshair.AttachedLine;
if (Crosshair.AttachedLine.State == STATE_FIRST)
return;
/* don't draw outline when ctrl key is pressed */
if (gui->control_is_pressed ())
{
line->draw = false;
return;
}
else
line->draw = true;
if (TEST_FLAG (ALLDIRECTIONFLAG, PCB))
{
line->Point2.X = Crosshair.X;
line->Point2.Y = Crosshair.Y;
return;
}
/* swap the modes if shift is held down */
if (gui->shift_is_pressed ())
way = !way;
dx = Crosshair.X - line->Point1.X;
dy = Crosshair.Y - line->Point1.Y;
if (!way)
{
if (abs (dx) > abs (dy))
{
line->Point2.X = Crosshair.X - SGN (dx) * abs (dy);
line->Point2.Y = line->Point1.Y;
}
else
{
line->Point2.X = line->Point1.X;
line->Point2.Y = Crosshair.Y - SGN (dy) * abs (dx);
}
}
else
{
if (abs (dx) > abs (dy))
{
line->Point2.X = line->Point1.X + SGN (dx) * abs (dy);
line->Point2.Y = Crosshair.Y;
}
else
{
line->Point2.X = Crosshair.X;
line->Point2.Y = line->Point1.Y + SGN (dy) * abs (dx);;
}
}
}
/*-----------------------------------------------------------
* Draws the outline of an arc
*/
static void
XORDrawAttachedArc (BDimension thick)
{
ArcType arc;
BoxTypePtr bx;
LocationType wx, wy;
int sa, dir;
BDimension wid = thick / 2;
wx = Crosshair.X - Crosshair.AttachedBox.Point1.X;
wy = Crosshair.Y - Crosshair.AttachedBox.Point1.Y;
if (wx == 0 && wy == 0)
return;
arc.X = Crosshair.AttachedBox.Point1.X;
arc.Y = Crosshair.AttachedBox.Point1.Y;
if (XOR (Crosshair.AttachedBox.otherway, abs (wy) > abs (wx)))
{
arc.X = Crosshair.AttachedBox.Point1.X + abs (wy) * SGNZ (wx);
sa = (wx >= 0) ? 0 : 180;
#ifdef ARC45
if (abs (wy) >= 2 * abs (wx))
dir = (SGNZ (wx) == SGNZ (wy)) ? 45 : -45;
else
#endif
dir = (SGNZ (wx) == SGNZ (wy)) ? 90 : -90;
}
else
{
arc.Y = Crosshair.AttachedBox.Point1.Y + abs (wx) * SGNZ (wy);
sa = (wy >= 0) ? -90 : 90;
#ifdef ARC45
if (abs (wx) >= 2 * abs (wy))
dir = (SGNZ (wx) == SGNZ (wy)) ? -45 : 45;
else
#endif
dir = (SGNZ (wx) == SGNZ (wy)) ? -90 : 90;
wy = wx;
}
wy = abs (wy);
arc.StartAngle = sa;
arc.Delta = dir;
arc.Width = arc.Height = wy;
bx = GetArcEnds (&arc);
/* sa = sa - 180; */
gui->draw_arc (Crosshair.GC, arc.X, arc.Y, wy + wid, wy + wid, sa, dir);
if (wid > pixel_slop)
{
gui->draw_arc (Crosshair.GC, arc.X, arc.Y, wy - wid, wy - wid, sa, dir);
gui->draw_arc (Crosshair.GC, bx->X1, bx->Y1,
wid, wid, sa, -180 * SGN (dir));
gui->draw_arc (Crosshair.GC, bx->X2, bx->Y2,
wid, wid, sa + dir, 180 * SGN (dir));
}
}
//---------------------------------------------------------------------
int __fastcall TWireEditDlg::Execute(WDEF *p, int n, int max)
{
char bf[80];
sprintf(bf, "ワイヤ(極座標)編集 - Wire No.%d", n + 1);
Caption = bf;
int RmdSel = exeenv.RmdSel;
ChkRmd->Checked = exeenv.RmdSel;
memcpy(&NowW, &p[n], sizeof(NowW));
memcpy(&NewW, &p[n], sizeof(NewW));
if( (SGN(NowW.Y1)*SGN(NowW.Y2) == -1) || (SGN(NowW.Z1)*SGN(NowW.Z2) == -1) || (SGN(NowW.X1)*SGN(NowW.X2) == -1) ){
BaseSel->ItemIndex = 2;
}
else if( ((NowW.Y1==0)&&(NowW.Y2)) || ((NowW.Z1==0)&&(NowW.Z2)) || ((NowW.X1==0)&&(NowW.X2)) ){
BaseSel->ItemIndex = 0;
}
else if( ((NowW.Y2==0)&&(NowW.Y1)) || ((NowW.Z2==0)&&(NowW.Z1)) || ((NowW.X2==0)&&(NowW.X1)) ){
BaseSel->ItemIndex = 1;
}
CalcDegLen();
DispItem(-1);
if( ShowModal() == IDOK ){
int f = 0;
NewW.X1 = RoundUpStr(NewW.X1);
NewW.Y1 = RoundUpStr(NewW.Y1);
NewW.Z1 = RoundUpStr(NewW.Z1);
NewW.X2 = RoundUpStr(NewW.X2);
NewW.Y2 = RoundUpStr(NewW.Y2);
NewW.Z2 = RoundUpStr(NewW.Z2);
NewW.R = RoundUpStr(NewW.R);
if( (NowW.X1 != NewW.X1)||(NowW.Y1 != NewW.Y1)||(NowW.Z1 != NewW.Z1) ){
f = 1;
if( ChkChen->Checked == TRUE ){
AdjWireChen(p, max, NowW.X1, NowW.Y1, NowW.Z1, NewW.X1, NewW.Y1, NewW.Z1);
}
}
if( (NowW.X2 != NewW.X2)||(NowW.Y2 != NewW.Y2)||(NowW.Z2 != NewW.Z2) ){
f = 1;
if( ChkChen->Checked == TRUE ){
AdjWireChen(p, max, NowW.X2, NowW.Y2, NowW.Z2, NewW.X2, NewW.Y2, NewW.Z2);
}
}
if( (NowW.R != NewW.R) || (NowW.SEG != NewW.SEG) ) f = 1;
if( f == 0 ) return FALSE; // 変更なし
memcpy(&p[n], &NewW, sizeof(WDEF));
exeenv.RmdSel = RmdSel;
return TRUE;
}
else {
exeenv.RmdSel = RmdSel;
return FALSE;
}
}
//#define DEBUG_EULER
void Euler_matrix2angles(const Matrix2D<DOUBLE> &A, DOUBLE &alpha,
DOUBLE &beta, DOUBLE &gamma)
{
DOUBLE abs_sb, sign_sb;
if (MAT_XSIZE(A) != 3 || MAT_YSIZE(A) != 3)
REPORT_ERROR("Euler_matrix2angles: The Euler matrix is not 3x3");
abs_sb = sqrt(A(0, 2) * A(0, 2) + A(1, 2) * A(1, 2));
if (abs_sb > 16 * FLT_EPSILON)
{
gamma = atan2(A(1, 2), -A(0, 2));
alpha = atan2(A(2, 1), A(2, 0));
if (ABS(sin(gamma)) < FLT_EPSILON)
sign_sb = SGN(-A(0, 2) / cos(gamma));
// if (sin(alpha)<FLT_EPSILON) sign_sb=SGN(-A(0,2)/cos(gamma));
// else sign_sb=(sin(alpha)>0) ? SGN(A(2,1)):-SGN(A(2,1));
else
sign_sb = (sin(gamma) > 0) ? SGN(A(1, 2)) : -SGN(A(1, 2));
beta = atan2(sign_sb * abs_sb, A(2, 2));
}
else
{
if (SGN(A(2, 2)) > 0)
{
// Let's consider the matrix as a rotation around Z
alpha = 0;
beta = 0;
gamma = atan2(-A(1, 0), A(0, 0));
}
else
{
alpha = 0;
beta = PI;
gamma = atan2(A(1, 0), -A(0, 0));
}
}
gamma = RAD2DEG(gamma);
beta = RAD2DEG(beta);
alpha = RAD2DEG(alpha);
#ifdef DEBUG_EULER
std::cout << "abs_sb " << abs_sb << std::endl;
std::cout << "A(1,2) " << A(1, 2) << " A(0,2) " << A(0, 2) << " gamma "
<< gamma << std::endl;
std::cout << "A(2,1) " << A(2, 1) << " A(2,0) " << A(2, 0) << " alpha "
<< alpha << std::endl;
std::cout << "sign sb " << sign_sb << " A(2,2) " << A(2, 2)
<< " beta " << beta << std::endl;
#endif
}
/*
* CrossingCount :
* Count the number of times a Bezier control polygon
* crosses the 0-axis. This number is >= the number of roots.
*
*/
int Measurement::CrossingCount(QPointF *V,int degree)
{
int i;
int n_crossings = 0; /* Number of zero-crossings */
int sign, old_sign; /* Sign of coefficients */
sign = old_sign = SGN(V[0].ry());
for (i = 1; i <= degree; i++) {
sign = SGN(V[i].ry());
if (sign != old_sign) n_crossings++;
old_sign = sign;
}
return n_crossings;
}
void exafmm_kernel::M2M(std::vector<real>& CiM, const std::vector<real>& CjM, const std::array<real, NDIM>& dist,
const integer N) {
std::vector<real> Ynm(FMM_P * FMM_P);
std::vector<real> M_r(N), M_i(N);
real rho, theta, phi;
cart2sph(rho, theta, phi, dist);
evalMultipole(rho, theta, -phi, Ynm);
for (integer j = 0; j != FMM_P; ++j) {
for (integer k = 0; k <= j; ++k) {
const integer jkp = j * j + j + k;
const integer jkm = j * j + j - k;
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
M_r[i] = M_i[i] = real(0.0);
}
for (integer n = 0; n <= j; ++n) {
for (integer m = std::max(n - j + k, -n); m <= std::min(j - n + k, +n); ++m) {
const integer nn = n * n + n;
const integer nj = (j - n) * (j - n) + j - n;
const integer jnkm = nj + k - m;
const integer jnpkm = nj + std::abs(k - m);
const integer jnmkm = nj - std::abs(k - m);
const integer nmp = nn + std::abs(m);
const integer nmm = nn - std::abs(m);
const auto Mj_r = CjM.data() + N * jnpkm;
const auto Mj_i = CjM.data() + N * jnmkm;
const real tmp = Anm[nmp] * Anm[jnkm]
/ Anm[jkp]* ODDEVEN((std::abs(k) - std::abs(m) - std::abs(k - m)) / 2 + n);
const real sgn_km = SGN(k-m);
const real Y_r = tmp * Ynm[nmp];
const real Y_i = SGN(m) * tmp * Ynm[nmm];
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
COMPLEX_MULT_ADD(M_r[i], M_i[i], Y_r, Y_i, Mj_r[i], sgn_km * Mj_i[i]);
}
}
}
auto Mi_r = CiM.data() + N * jkp;
auto Mi_i = CiM.data() + N * jkm;
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
Mi_r[i] += M_r[i];
Mi_i[i] += (jkm == jkp) ? 0.0 : M_i[i];
}
}
}
}
/* Returns true (and stores the endpoints in t and u) if the
* epipolar swath e1, e2 intersects this segment */
bool LineSegment2D::IntersectsEpipolarSwath(double e1[3], double e2[3],
double &t, double &u)
{
/* Find the line between the two endpoints */
double p[3] = { m_p1[0], m_p1[1], 1.0 };
double q[3] = { m_p2[0], m_p2[1], 1.0 };
double l[3];
matrix_cross(p, q, l);
/* Intersect the line with the two epipolar lines */
double i1[3], i2[3];
matrix_cross(l, e1, i1);
matrix_cross(l, e2, i2);
double inv_i12 = 1.0 / i1[2];
i1[0] *= inv_i12;
i1[1] *= inv_i12;
double inv_i22 = 1.0 / i2[2];
i2[0] *= inv_i22;
i2[1] *= inv_i22;
double qp[3], i1p[3], i2p[3];
matrix_diff(2, 1, 2, 1, q, p, qp);
matrix_diff(2, 1, 2, 1, i1, p, i1p);
matrix_diff(2, 1, 2, 1, i2, p, i2p);
double dot1, dot2;
matrix_product(1, 2, 2, 1, i1p, qp, &dot1);
matrix_product(1, 2, 2, 1, i2p, qp, &dot2);
int sign1 = SGN(dot1);
int sign2 = SGN(dot2);
double inv_norm = 1.0 / matrix_norm(2, 1, qp);
double mag1 = matrix_norm(2, 1, i1p) * inv_norm; // matrix_norm(2, 1, qp);
double mag2 = matrix_norm(2, 1, i2p) * inv_norm; // matrix_norm(2, 1, qp);
t = sign1 * mag1;
u = sign2 * mag2;
/* Intersection check */
if ((t < 0.0 && u < 0.0) || (t > 1.0 && u > 1.0))
return false;
else
return true;
}
void Vector::Angle(float& u, float& v) const
{
Vector n = Unit();
u = asin(n.y);
if (IsZero(n.z)) {
v = (float)M_PI_2 * SGN(n.x);
} else if (n.z > 0) {
v = atan(n.x / n.z);
} else if (n.z < 0) {
v = IsZero(n.x) ? (float)M_PI : ((float)M_PI * SGN(n.x) + atan(n.x / n.z));
}
}
int p276(problem *p)
{
double alpha,h,y,z,z0,xs,xg,zm_z0,y_ov_zm,coef[3],ans[3];
int i, num_roots;
bool found_solution=false;
double *pp;
pp = p->parmval[p->current_solution];
h = pp[H];
z = pp[Z];
xs = pp[Xs];
xg = pp[Xg];
y = pp[Y];
z0 = pp[Z0];
if (fabs(y) < EPS) {
if (z0<EPS3 || fabs(z-z0)>EPS3)
return EX_DATAFAULT;
pp[Phi] = atan(h/z0);
return p3fhz(p);
}
/* else */
coef[2] = (h*h -y*y)/z - 2*z0;
coef[1] = z0*z0 + y*y - 2*h*h*z0/z;
coef[0] = (h*h*z0*z0) / z;
num_roots = cubic_solve(coef,ans);
for (i=0; i<num_roots; i++) {
/* Reject any answer which gives a negative value for z_m. */
if (ans[i] <= EPS)
continue;
zm_z0 = ans[i] - z0;
y_ov_zm = y / zm_z0;
alpha = SGN(y) * SGN(zm_z0) * asin( 1/sqrt(1+y_ov_zm*y_ov_zm));
if (found_solution) {
pp = p->parmval[p->current_solution];
pp[H] = h;
pp[Z] = z;
pp[Xs] = xs;
pp[Xg] = xg;
pp[Y] = y;
pp[Z0] = z0;
}
pp[Alf] = alpha;
if (ex_check_parms(p,ChAlf+ChH+ChZ+ChZ0)!=0)
continue;
if (p3ahz(p)==0)
found_solution = true;
}
return (found_solution ? 0 : NO_SOLUTION);
} /* END p276() */
// CrossingCount :
// getCount the number of times a Bezier control polygon
// crosses the 0-axis. This number is >= the number of roots.
// Parameters :
// pts: Control pts
// degree: Degree of bezier curve
//
static int CrossingCount(const Point2d* pts, int degree)
{
int i;
int n_crossings = 0; // Number of zero-crossings
int sign, old_sign; // Sign of coefficients
sign = old_sign = SGN(pts[0].y);
for (i = 1; i <= degree; i++) {
sign = SGN(pts[i].y);
if (sign != old_sign) n_crossings++;
old_sign = sign;
}
return n_crossings;
}
void bresenham( const int x1, const int y1, const int x2, const int y2, int t,
const std::function<bool(const point &)> &interact )
{
// The slope components.
const int dx = x2 - x1;
const int dy = y2 - y1;
// Signs of slope values.
const int sx = (dx == 0) ? 0 : SGN(dx);
const int sy = (dy == 0) ? 0 : SGN(dy);
// Absolute values of slopes x2 to avoid rounding errors.
const int ax = abs(dx) * 2;
const int ay = abs(dy) * 2;
point cur(x1, y1);
if( ax == ay ) {
while( cur.x != x2 ) {
cur.y += sy;
cur.x += sx;
if( !interact( cur ) ) {
break;
}
}
} else if( ax > ay ) {
while( cur.x != x2 ) {
if( t > 0 ) {
cur.y += sy;
t -= ax;
}
cur.x += sx;
t += ay;
if( !interact( cur ) ) {
break;
}
}
} else {
while( cur.y != y2 ) {
if( t > 0 ) {
cur.x += sx;
t -= ay;
}
cur.y += sy;
t += ax;
if( !interact( cur ) ) {
break;
}
}
}
}
void exafmm_kernel::L2L(std::vector<real>& CiL, const std::vector<real>& CjL, const std::array<real, NDIM>& dist,
const integer N) {
std::vector<real> Ynm(FMM_P * FMM_P);
real rho, theta, phi;
std::vector<real> L_r(N), L_i(N);
cart2sph(rho, theta, phi, dist);
evalMultipole(rho, theta, phi, Ynm);
for (integer j = 0; j != FMM_P; ++j) {
for (integer k = 0; k <= j; ++k) {
integer jkp = j * j + j + k;
integer jkm = j * j + j - k;
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
L_r[i] = L_i[i] = 0.0;
}
for (integer n = j; n != FMM_P; ++n) {
for (integer m = j - n + k; m <= n - j + k; ++m) {
const integer nn = n * n + n;
const integer nj = (n - j) * ((n - j) + 1);
const integer npm = nn + std::abs(m);
const integer nmm = nn - std::abs(m);
const integer jnpkm = nj + std::abs(m - k);
const integer jnmkm = nj - std::abs(m - k);
const auto Lj_r = CjL.data() + N * npm;
const auto Lj_i = CjL.data() + N * nmm;
const real sgn = SGN(m);
real tmp = std::pow(-real(1.0), real(std::abs(m) - std::abs(k) - std::abs(m - k)) / 2) * Anm[jnpkm] * Anm[jkp]
/ Anm[npm];
const real Y_r = Ynm[jnpkm] * tmp;
const real Y_i = SGN(m-k) * Ynm[jnmkm] * tmp;
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
COMPLEX_MULT_ADD(L_r[i], L_i[i], Y_r, Y_i, Lj_r[i], sgn * Lj_i[i]);
}
}
}
auto Li_r = CiL.data() + N * jkp;
auto Li_i = CiL.data() + N * jkm;
#pragma vector aligned
#pragma simd
for (integer i = 0; i != N; ++i) {
Li_r[i] = L_r[i];
Li_i[i] = (k == 0) ? L_r[i] : L_i[i];
}
}
}
}
/*
* crossing_count:
* Count the number of times a Bezier control polygon
* crosses the 0-axis. This number is >= the number of roots.
*
*/
unsigned
crossing_count(Geom::Point const *V, /* Control pts of Bezier curve */
unsigned degree) /* Degree of Bezier curve */
{
unsigned n_crossings = 0; /* Number of zero-crossings */
int old_sign = SGN(V[0][Geom::Y]);
for (unsigned i = 1; i <= degree; i++) {
int sign = SGN(V[i][Geom::Y]);
if (sign != old_sign)
n_crossings++;
old_sign = sign;
}
return n_crossings;
}
bool classifyFuturePoints(double& a,
double& b,
double& c,
point& c1,
point& c2)
{
point p;
while(readPoint(p,std::cin,false) && !std::cin.eof())
{
double val = a*p.x+b*p.y+c;
if (SGN(val) == SGN(a*c1.x+b*c1.y+c)) std::cout << "1" << std::endl;
else std::cout << "2" << std::endl;
}
return true;
}
const char *
delayb(char c)
{
int xdiff, ydiff;
c -= '0';
if (c >= sp->num_beacons)
return ("Unknown beacon");
xdiff = sp->beacon[(int)c].x - p.xpos;
xdiff = SGN(xdiff);
ydiff = sp->beacon[(int)c].y - p.ypos;
ydiff = SGN(ydiff);
if (xdiff != displacement[p.dir].dx || ydiff != displacement[p.dir].dy)
return ("Beacon is not in flight path");
if (xdiff != 0 && ydiff !=0)
if (abs(sp->beacon[(int)c].x - p.xpos) !=
abs(sp->beacon[(int)c].y - p.ypos))
return ("Beacon is not in flight path");
p.delayd = 1;
p.delayd_no = c;
if (dest_type != T_NODEST) {
switch (dest_type) {
case T_BEACON:
xdiff = sp->beacon[dest_no].x - sp->beacon[(int)c].x;
ydiff = sp->beacon[dest_no].y - sp->beacon[(int)c].y;
break;
case T_EXIT:
xdiff = sp->exit[dest_no].x - sp->beacon[(int)c].x;
ydiff = sp->exit[dest_no].y - sp->beacon[(int)c].y;
break;
case T_AIRPORT:
xdiff = sp->airport[dest_no].x - sp->beacon[(int)c].x;
ydiff = sp->airport[dest_no].y - sp->beacon[(int)c].y;
break;
default:
return ("Bad case in delayb! Get help!");
break;
}
if (xdiff == 0 && ydiff == 0)
return ("Would already be there");
p.new_dir = DIR_FROM_DXDY(xdiff, ydiff);
if (p.new_dir == p.dir)
return ("Already going in that direction");
}
return (NULL);
}
void findNonConduitFlow(int i, double dt)
//
// Input: i = link index
// dt = time step (sec)
// Output: none
// Purpose: finds new flow in a non-conduit-type link
//
{
double qLast; // previous link flow (cfs)
double qNew; // new link flow (cfs)
// --- get link flow from last iteration
qLast = Link[i].newFlow;
Link[i].dqdh = 0.0;
// --- get new inflow to link from its upstream node
// (link_getInflow returns 0 if flap gate closed or pump is offline)
qNew = link_getInflow(i);
if ( Link[i].type == PUMP ) qNew = getModPumpFlow(i, qNew, dt);
// --- find surface area at each end of link
findNonConduitSurfArea(i);
// --- apply under-relaxation with flow from previous iteration;
// --- do not allow flow to change direction without first being 0
if ( Steps > 0 && Link[i].type != PUMP )
{
qNew = (1.0 - Omega) * qLast + Omega * qNew;
if ( qNew * qLast < 0.0 ) qNew = 0.001 * SGN(qNew);
}
Link[i].newFlow = qNew;
}