52 inline static scalar sector
54 scalar dxa, scalar dya,
55 scalar dxb, scalar dyb
75 double xc,
double yc,
double rad,
79 scalar* x_proj_edge_p, scalar* y_proj_edge_p,
80 scalar* x_overlap_p, scalar* y_overlap_p
83 double angle,
area, del;
84 double x_int[6][2], y_int[6][2];
85 double x_arc[6][2], y_arc[6][2];
88 double x_olap_min = GREAT;
89 double x_olap_max = -GREAT;
90 double y_olap_min = GREAT;
91 double y_olap_max = -GREAT;
95 const double dx1 = (x1 - xc);
96 const double dx2 = (x2 - xc);
97 const double dy1 = (
y1 - yc);
98 const double dy2 = (y2 - yc);
100 const double dx1squ = dx1 * dx1;
101 const double dx2squ = dx2 * dx2;
102 const double dy1squ = dy1 * dy1;
103 const double dy2squ = dy2 * dy2;
104 const double radsqu =
rad *
rad;
110 dx[1] = dx1; dy[1] = dy1;
111 dx[2] = dx1; dy[2] = dy2;
112 dx[3] = dx2; dy[3] = dy2;
113 dx[4] = dx2; dy[4] = dy1;
114 dx[0] = dx2; dy[0] = dy1;
115 dx[5] = dx1; dy[5] = dy1;
120 x_arc[2][0] = x_arc[1][1] = -
rad; y_arc[2][0] = y_arc[1][1] = 0.0;
121 x_arc[3][0] = x_arc[2][1] = 0.0; y_arc[3][0] = y_arc[2][1] =
rad;
122 x_arc[4][0] = x_arc[3][1] =
rad; y_arc[4][0] = y_arc[3][1] = 0.0;
123 x_arc[1][0] = x_arc[4][1] = 0.0; y_arc[1][0] = y_arc[4][1] = -
rad;
130 arc_in[1] = (dx1 < -
rad && dy1 < -
rad) ? 1 : 0;
131 arc_in[2] = (dx1 < -
rad && dy2 >
rad) ? 1 : 0;
132 arc_in[3] = (dx2 >
rad && dy2 >
rad) ? 1 : 0;
133 arc_in[4] = (dx2 >
rad && dy1 < -
rad) ? 1 : 0;
138 vert_in[1] = (dx1squ + dy1squ <= radsqu);
139 vert_in[2] = (dx1squ + dy2squ <= radsqu);
140 vert_in[3] = (dx2squ + dy2squ <= radsqu);
141 vert_in[4] = (dx2squ + dy1squ <= radsqu);
142 vert_in[0] = vert_in[4];
143 vert_in[5] = vert_in[1];
146 if (vert_in[1]) ++n_in;
147 if (vert_in[2]) ++n_in;
148 if (vert_in[3]) ++n_in;
149 if (vert_in[4]) ++n_in;
161 if ( dx1squ <= radsqu)
164 if ( ( ( -del ) <= dy2 ) && ( del >= dy1 ) )
166 x_int[1][0] = x_int[1][1] = dx1;
172 if ( dx1 > 0.0 ) { x_arc[4][1] = dx1; y_arc[4][1] = -del; arc_in[4] = 1; }
173 else { x_arc[1][1] = dx1; y_arc[1][1] = -del; arc_in[1] = 1; }
177 y_int[1][n_int[1]] = del;
179 if ( dx1 > 0.0 ) { x_arc[3][0] = dx1; y_arc[3][0] = del; arc_in[3] = 1; }
180 else { x_arc[2][0] = dx1; y_arc[2][0] = del; arc_in[2] = 1; }
186 if ( dy2squ <= radsqu)
189 if ( ( ( -del ) <= dx2 ) && ( del >= dx1 ) )
191 y_int[2][0] = y_int[2][1] = dy2;
196 if ( dy2 > 0.0 ) { x_arc[2][1] = -del; y_arc[2][1] = dy2; arc_in[2] = 1; }
197 else { x_arc[1][1] = -del; y_arc[1][1] = dy2; arc_in[1] = 1; }
201 x_int[2][n_int[2]] = del;
203 if ( dy2 > 0.0 ) { x_arc[3][0] = del; y_arc[3][0] = dy2; arc_in[3] = 1; }
204 else { x_arc[4][0] = del; y_arc[4][0] = dy2; arc_in[4] = 1; }
210 if ( dx2squ <= radsqu)
213 if ( ( ( -del ) <= dy2 ) && ( del >= dy1 ) )
215 x_int[3][0] = x_int[3][1] = dx2;
220 if ( dx2 > 0.0 ) { x_arc[3][1] = dx2; y_arc[3][1] = del; arc_in[3] = 1; }
221 else { x_arc[2][1] = dx2; y_arc[2][1] = del; arc_in[2] = 1; }
225 y_int[3][n_int[3]] = -del;
227 if ( dx2 > 0.0 ) { x_arc[4][0] = dx2; y_arc[4][0] = -del; arc_in[4] = 1; }
228 else { x_arc[1][0] = dx2; y_arc[1][0] = -del; arc_in[1] = 1; }
234 if ( dy1squ <= radsqu)
237 if ( ( ( -del ) <= dx2 ) && ( del >= dx1 ) )
239 y_int[4][0] = y_int[4][1] = dy1;
244 if ( dy1 > 0.0 ) { x_arc[3][1] = del; y_arc[3][1] = dy1; arc_in[3] = 1; }
245 else { x_arc[4][1] = del; y_arc[4][1] = dy1; arc_in[4] = 1; }
249 x_int[4][n_int[4]] = -del;
251 if ( dy1 > 0.0 ) { x_arc[2][0] = -del; y_arc[2][0] = dy1; arc_in[2] = 1; }
252 else { x_arc[1][0] = -del; y_arc[1][0] = dy1; arc_in[1] = 1; }
260 y_int[0][0] = y_int[0][1] = dy1;
261 x_int[0][0] = x_int[4][0];
262 x_int[0][1] = x_int[4][1];
263 x_int[5][0] = x_int[5][1] = dx1;
264 y_int[5][0] = y_int[1][0];
265 y_int[5][1] = y_int[1][1];
275 no_intersection =
true;
276 for (n_vert = 1; n_vert < 5; n_vert++)
278 assert(n_int[n_vert] != 1);
279 if (n_int[n_vert] == 2)
282 no_intersection =
false;
283 angle = sector( x_int[n_vert][1], y_int[n_vert][1], x_int[n_vert][0], y_int[n_vert][0]);
284 area -= angle * radsqu * 0.5;
285 *perim_p -= angle *
rad;
287 area += ( - ( y_int[n_vert][1] - y_int[n_vert][0] ) * x_int[n_vert][0]
288 + ( x_int[n_vert][1] - x_int[n_vert][0] ) * y_int[n_vert][0] ) / 2.0;
293 if ( no_intersection )
295 if ( (dx1>0) ||(dx2<0) || (dy1>0) || (dy2<0) )
297 *perim_p = *x_proj_edge_p = *y_proj_edge_p = 0.0;
298 area = *x_overlap_p = *y_overlap_p = 0.0;
310 while ( !vert_in[n_vert] ) { n_vert++; assert( n_vert < 5 ); }
311 assert( n_int[n_vert-1] == 1 );
312 if ( n_int[n_vert] != 1 )
314 assert( n_int[n_vert] == 1 );
316 angle = sector( x_int[n_vert-1][0], y_int[n_vert-1][0], x_int[n_vert][0], y_int[n_vert][0]);
317 area = angle * radsqu * 0.5;
318 *perim_p = angle *
rad;
320 area -= ( - ( x_int[n_vert][0] - dx[n_vert] ) * dy[n_vert]
321 + ( x_int[n_vert-1][0] - dx[n_vert] ) * dy[n_vert]
322 + ( y_int[n_vert][0] - dy[n_vert] ) * dx[n_vert]
323 - ( y_int[n_vert-1][0] - dy[n_vert] ) * dx[n_vert] ) / 2.0;
332 while ( !(vert_in[n_vert] && vert_in[n_vert+1]) ) { n_vert++; assert( n_vert < 5 ); }
333 assert( n_int[n_vert-1] == 1 );
334 assert( n_int[n_vert+1] == 1 );
335 angle = sector( x_int[n_vert-1][0], y_int[n_vert-1][0], x_int[n_vert+1][0], y_int[n_vert+1][0]);
336 area = angle * radsqu * 0.5;
337 *perim_p = angle *
rad;
339 area += ( ( x_int[n_vert+1][0] - dx[n_vert+1] ) * dy[n_vert+1]
340 - ( x_int[n_vert-1][0] - dx[n_vert] ) * dy[n_vert]
341 - ( y_int[n_vert+1][0] - dy[n_vert+1] ) * dx[n_vert+1]
342 + ( y_int[n_vert-1][0] - dy[n_vert] ) * dx[n_vert]
343 + ( dx[n_vert+1] -dx[n_vert] ) * dy[n_vert]
344 - ( dy[n_vert+1] -dy[n_vert] ) * dx[n_vert] ) / 2.0;
348 case 1: x_olap_min = dx1;
break;
349 case 2: y_olap_max = dy2;
break;
350 case 3: x_olap_max = dx2;
break;
351 case 4: y_olap_min = dy1;
break;
361 while ( vert_in[n_vert] ) { n_vert++; assert( n_vert < 5 ); }
362 assert( n_int[n_vert-1] == 1 );
363 assert( n_int[n_vert] == 1 );
364 n_oppv = (n_vert + 2) % 4;
365 angle = sector( x_int[n_vert][0], y_int[n_vert][0], x_int[n_vert-1][0], y_int[n_vert-1][0]);
366 area = angle * radsqu * 0.5;
367 *perim_p = angle *
rad;
369 area += ( - ( x_int[n_vert][0] - dx[n_vert+1] ) * dy[n_vert+1]
370 + ( x_int[n_vert-1][0] - dx[n_vert-1] ) * dy[n_vert-1]
371 + ( y_int[n_vert][0] - dy[n_vert+1] ) * dx[n_vert+1]
372 - ( y_int[n_vert-1][0] - dy[n_vert-1] ) * dx[n_vert-1]
373 + ( dx[n_oppv] -dx[n_vert+1] ) * dy[n_oppv]
374 - ( dx[n_oppv] -dx[n_vert-1] ) * dy[n_oppv]
375 - ( dy[n_oppv] -dy[n_vert+1] ) * dx[n_oppv]
376 + ( dy[n_oppv] -dy[n_vert-1] ) * dx[n_oppv] ) / 2.0;
389 area = *x_overlap_p = *y_overlap_p = 1.0;
390 *perim_p = *x_proj_edge_p = *y_proj_edge_p = 0.0;
398 assert(
area >=-1.0E-4);
401 area /= ( (x2 - x1 ) * ( y2 -
y1 ) );
406 (y_arc[1][1] - y_arc[1][0]) * arc_in[1]
407 + (y_arc[2][1] - y_arc[2][0]) * arc_in[2]
408 + (y_arc[3][0] - y_arc[3][1]) * arc_in[3]
409 + (y_arc[4][0] - y_arc[4][1]) * arc_in[4]
414 (x_arc[1][0] - x_arc[1][1]) * arc_in[1]
415 + (x_arc[2][1] - x_arc[2][0]) * arc_in[2]
416 + (x_arc[3][1] - x_arc[3][0]) * arc_in[3]
417 + (x_arc[4][0] - x_arc[4][1]) * arc_in[4]
422 x_olap_min =
min(x_olap_min, x_arc[1][1]);
423 x_olap_max =
max(x_olap_max, x_arc[1][0]);
424 y_olap_min =
min(y_olap_min, y_arc[1][0]);
425 y_olap_max =
max(y_olap_max, y_arc[1][1]);
429 x_olap_min =
min(x_olap_min, x_arc[2][0]);
430 x_olap_max =
max(x_olap_max, x_arc[2][1]);
431 y_olap_min =
min(y_olap_min, y_arc[2][0]);
432 y_olap_max =
max(y_olap_max, y_arc[2][1]);
436 x_olap_min =
min(x_olap_min, x_arc[3][0]);
437 x_olap_max =
max(x_olap_max, x_arc[3][1]);
438 y_olap_min =
min(y_olap_min, y_arc[3][1]);
439 y_olap_max =
max(y_olap_max, y_arc[3][0]);
443 x_olap_min =
min(x_olap_min, x_arc[4][1]);
444 x_olap_max =
max(x_olap_max, x_arc[4][0]);
445 y_olap_min =
min(y_olap_min, y_arc[4][1]);
446 y_olap_max =
max(y_olap_max, y_arc[4][0]);
449 *x_overlap_p = ( x_olap_max - x_olap_min ) / ( x2 - x1 );
450 *y_overlap_p = ( y_olap_max - y_olap_min ) / ( y2 -
y1 );
462 double xc,
double yc,
double rad,
463 double x1,
double x2,
464 double y1,
double y2,
465 scalar* count_p, scalar* drag_p, scalar* centre_p
468 double xi = 0.0, lb, lb1, lb2, del;
472 if ( xc < x1 ) { xi = x1; }
473 else if ( xc > x2 ) { xi = x2; }
481 del =
rad*
rad - ( xi - xc ) * ( xi - xc );
482 if ( del < 0.0 ) { del = 0.0; }
486 if ( ( yc + del ) > y2 ) { lb2 = y2; within =
false; }
else { lb2 = yc + del; }
487 if ( ( yc - del ) <
y1 ) { lb1 =
y1; within =
false; }
else { lb1 = yc - del; }
489 lb = (lb2 - lb1) / (y2 -
y1);
490 *centre_p = (lb2 + lb1) * 0.5;
492 if ( lb < 0.0 ) lb = 0.0;
498 if ( within && (lb > 0.0) )
501 if ( ( xc -
rad ) < x1 ) *count_p -= 0.5;
502 if ( ( xc +
rad ) > x2 ) *count_p -= 0.5;
509 if ( lb > 0.99 ) { *count_p = -1000.0; *drag_p = 1000.0; }
519 double xc,
double yc,
double theta,
520 double wa,
double wb,
521 double x1,
double x2,
522 double y1,
double y2,
525 scalar* x_lblk_p, scalar* y_lblk_p,
526 scalar* x_centre_p, scalar* y_centre_p
529 double x_int[6][2], y_int[6][2];
530 double area, lpa, lpb, len;
532 double m =
::tan( theta );
533 double cth =
::cos( theta );
534 double sth =
::sin( theta );
536 double was = wa * sth * 0.5;
537 double wac = wa * cth * 0.5;
538 double wbs = wb * sth * 0.5;
539 double wbc = wb * cth * 0.5;
540 double waos = wa / sth * 0.5;
541 double waoc = wa / cth * 0.5;
542 double wbos = wb / sth * 0.5;
543 double wboc = wb / cth * 0.5;
545 double xb[6], yb[6], xp1, xp2, yp1, yp2;
547 double dx1 = (x1 - xc);
548 double dx2 = (x2 - xc);
549 double dy1 = (
y1 - yc);
550 double dy2 = (y2 - yc);
572 y_int[1][0] =
min(
max(
max(dx1 * m - wboc, -dx1 / m - waos), dy1), dy2);
574 y_int[1][1] =
min(
max(
min(dx1 * m + wboc, -dx1 / m + waos), dy1), dy2);
581 if ( (yb[1] > dy1) && (yb[1] < dy2) )
582 { *count_p += 0.25; }
586 y_int[3][1] =
min(
max(
max(dx2 * m - wboc, -dx2 / m - waos), dy1), dy2);
587 y_int[3][0] =
min(
max(
min(dx2 * m + wboc, -dx2 / m + waos), dy1), dy2);
593 if (yb[3] > dy1 && yb[3] < dy2)
600 x_int[2][0] =
min(
max(
max(dy2 / m - wbos, -dy2 * m - waoc), dx1), dx2);
601 x_int[2][1] =
min(
max(
min(dy2 / m + wbos, -dy2 * m + waoc), dx1), dx2);
607 if ( (xb[2] > dx1) && (xb[2] < dx2) )
608 { *count_p += 0.25; }
612 x_int[4][1] =
min(
max(
max(dy1 / m - wbos, -dy1 * m - waoc ), dx1), dx2);
613 x_int[4][0] =
min(
max(
min(dy1 / m + wbos, -dy1 * m + waoc ), dx1), dx2);
619 if ( (xb[4] > dx1) && (xb[4] < dx2) )
620 { *count_p += 0.25; }
623 y_int[0][0] = y_int[0][1] = dy1;
624 x_int[0][0] = x_int[4][0];
625 x_int[0][1] = x_int[4][1];
626 x_int[5][0] = x_int[5][1] = dx1;
627 y_int[5][0] = y_int[1][0];
628 y_int[5][1] = y_int[1][1];
633 xp1 =
min(x_int[2][0], x_int[4][1]);
634 if ( yb[1] > dy1 && yb[1] < dy2 ) xp1 =
min(xp1, xb[1] );
637 yp2 =
max(y_int[1][1], y_int[3][0] );
638 if ( xb[2] > dx1 && xb[2] < dx2 ) yp2 =
max(yp2, yb[2] );
641 xp2 =
max(x_int[2][1], x_int[4][0] );
642 if ( yb[3] > dy1 && yb[3] < dy2 ) xp2 =
max(xp2, xb[3] );
645 yp1 =
min(y_int[1][0], y_int[3][1]);
646 if ( xb[4] > dx1 && xb[4] < dx2 ) yp1 =
min(yp1, yb[4] );
647 yp1 =
max(yp1, dy1 );
650 *x_lblk_p = (xp2 - xp1 ) / (x2 - x1 );
651 if ( *x_lblk_p < 0.0 ) { *x_lblk_p = 0.0; *count_p = 0.0; };
652 *y_lblk_p = (yp2 - yp1 ) / (y2 -
y1 );
653 if ( *y_lblk_p < 0.0 ) { *y_lblk_p = 0.0; *count_p = 0.0; };
655 *x_centre_p = xc + (xp2 + xp1 ) * 0.5;
656 *y_centre_p = yc + (yp2 + yp1 ) * 0.5;
658 *perim_p = lpa = lpb = 0.0;;
659 area = (xp2 - xp1 ) * ( yp2 - yp1 );
663 dyy =
max(0.0,
min(yb[1], y_int[1][0]) - yp1);
664 dxx =
min(xb[4], x_int[0][1] ) - xp1;
666 if ( ( dxx * dyy) > 0.0 )
668 area -= dxx * dyy * 0.5;
674 dxx =
max(0.0,
min(xb[2], x_int[2][0]) - xp1);
675 dyy = yp2 -
max(yb[1], y_int[1][1] );
676 if ( ( dxx * dyy) > 0.0 )
678 area -= dxx * dyy * 0.5;
684 dyy =
max(0.0, yp2 -
max(yb[3], y_int[3][0]));
685 dxx = xp2 -
max(xb[2], x_int[2][1] );
686 if ( ( dxx * dyy) > 0.0 )
688 area -= dxx * dyy * 0.5;
694 dxx =
max(0.0, xp2 -
max(xb[4], x_int[4][0]));
695 dyy =
min(yb[3], y_int[3][1] ) - yp1;
696 if ( ( dxx * dyy) > 0.0 )
698 area -= dxx * dyy * 0.5;
706 vdrag.xx() = lpa * cth * cth + lpb * sth * sth;
707 vdrag.xy() = lpa * cth * sth - lpb * sth * cth;
708 vdrag.yy() = lpa * sth * sth + lpb * cth * cth;
710 return area / ( (x2 - x1 ) * ( y2 -
y1 ) );