00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
#ifndef WFMATH_INTERSECT_H
00025
#define WFMATH_INTERSECT_H
00026
00027
#include <wfmath/vector.h>
00028
#include <wfmath/point.h>
00029
#include <wfmath/const.h>
00030
#include <wfmath/intersect_decls.h>
00031
#include <wfmath/axisbox.h>
00032
#include <wfmath/ball.h>
00033
#include <wfmath/segment.h>
00034
#include <wfmath/rotbox.h>
00035
00036
namespace WFMath {
00037
00038
00039
00040
template<
class S1,
class S2>
00041
inline bool Intersect(
const S1& s1,
const S2& s2,
bool proper)
00042 {
00043
return Intersect(s2, s1, proper);
00044 }
00045
00046
00047
00048
template<const
int dim>
00049
inline bool Intersect(
const Point<dim>& p1,
const Point<dim>& p2,
bool proper)
00050 {
00051
return !proper && p1 == p2;
00052 }
00053
00054
template<const
int dim,
class S>
00055
inline bool Contains(
const S& s,
const Point<dim>& p,
bool proper)
00056 {
00057
return Intersect(p, s, proper);
00058 }
00059
00060
template<const
int dim>
00061
inline bool Contains(
const Point<dim>& p1,
const Point<dim>& p2,
bool proper)
00062 {
00063
return !proper && p1 == p2;
00064 }
00065
00066
00067
00068
template<const
int dim>
00069
inline bool Intersect(
const AxisBox<dim>& b,
const Point<dim>& p,
bool proper)
00070 {
00071
for(
int i = 0; i < dim; ++i)
00072
if(_Greater(b.m_low[i], p[i], proper) || _Less(b.m_high[i], p[i], proper))
00073
return false;
00074
00075
return true;
00076 }
00077
00078
template<const
int dim>
00079
inline bool Contains(
const Point<dim>& p,
const AxisBox<dim>& b,
bool proper)
00080 {
00081
return !proper && p == b.m_low && p == b.m_high;
00082 }
00083
00084
template<const
int dim>
00085
inline bool Intersect(
const AxisBox<dim>& b1,
const AxisBox<dim>& b2,
bool proper)
00086 {
00087
for(
int i = 0; i < dim; ++i)
00088
if(_Greater(b1.m_low[i], b2.m_high[i], proper)
00089 || _Less(b1.m_high[i], b2.m_low[i], proper))
00090
return false;
00091
00092
return true;
00093 }
00094
00095
template<const
int dim>
00096
inline bool Contains(
const AxisBox<dim>& outer,
const AxisBox<dim>& inner,
bool proper)
00097 {
00098
for(
int i = 0; i < dim; ++i)
00099
if(_Less(inner.m_low[i], outer.m_low[i], proper)
00100 || _Greater(inner.m_high[i], outer.m_high[i], proper))
00101
return false;
00102
00103
return true;
00104 }
00105
00106
00107
00108
template<const
int dim>
00109
inline bool Intersect(
const Ball<dim>& b,
const Point<dim>& p,
bool proper)
00110 {
00111
return _LessEq(SquaredDistance(b.m_center, p), b.m_radius * b.m_radius
00112 * (1 + WFMATH_EPSILON), proper);
00113 }
00114
00115
template<const
int dim>
00116
inline bool Contains(
const Point<dim>& p,
const Ball<dim>& b,
bool proper)
00117 {
00118
return !proper && b.m_radius == 0 && p == b.m_center;
00119 }
00120
00121
template<const
int dim>
00122
inline bool Intersect(
const Ball<dim>& b,
const AxisBox<dim>& a,
bool proper)
00123 {
00124
CoordType dist = 0;
00125
00126
for(
int i = 0; i < dim; ++i) {
00127
CoordType dist_i;
00128
if(b.m_center[i] < a.m_low[i])
00129 dist_i = b.m_center[i] - a.m_low[i];
00130
else if(b.m_center[i] > a.m_high[i])
00131 dist_i = b.m_center[i] - a.m_high[i];
00132
else
00133
continue;
00134 dist+= dist_i * dist_i;
00135 }
00136
00137
return _LessEq(dist, b.m_radius * b.m_radius, proper);
00138 }
00139
00140
template<const
int dim>
00141
inline bool Contains(
const Ball<dim>& b,
const AxisBox<dim>& a,
bool proper)
00142 {
00143
CoordType sqr_dist = 0;
00144
00145
for(
int i = 0; i < dim; ++i) {
00146
CoordType furthest = FloatMax(fabs(b.m_center[i] - a.m_low[i]),
00147 fabs(b.m_center[i] - a.m_high[i]));
00148 sqr_dist += furthest * furthest;
00149 }
00150
00151
return _LessEq(sqr_dist, b.m_radius * b.m_radius * (1 + WFMATH_EPSILON), proper);
00152 }
00153
00154
template<const
int dim>
00155
inline bool Contains(
const AxisBox<dim>& a,
const Ball<dim>& b,
bool proper)
00156 {
00157
for(
int i = 0; i < dim; ++i)
00158
if(_Less(b.m_center[i] - b.m_radius, a.lowerBound(i), proper)
00159 || _Greater(b.m_center[i] + b.m_radius, a.upperBound(i), proper))
00160
return false;
00161
00162
return true;
00163 }
00164
00165
template<const
int dim>
00166
inline bool Intersect(
const Ball<dim>& b1,
const Ball<dim>& b2,
bool proper)
00167 {
00168
CoordType sqr_dist = SquaredDistance(b1.m_center, b2.m_center);
00169
CoordType rad_sum = b1.m_radius + b2.m_radius;
00170
00171
return _LessEq(sqr_dist, rad_sum * rad_sum, proper);
00172 }
00173
00174
template<const
int dim>
00175
inline bool Contains(
const Ball<dim>& outer,
const Ball<dim>& inner,
bool proper)
00176 {
00177
CoordType rad_diff = outer.m_radius - inner.m_radius;
00178
00179
if(_Less(rad_diff, 0, proper))
00180
return false;
00181
00182
CoordType sqr_dist = SquaredDistance(outer.m_center, inner.m_center);
00183
00184
return _LessEq(sqr_dist, rad_diff * rad_diff, proper);
00185 }
00186
00187
00188
00189
template<const
int dim>
00190
inline bool Intersect(
const Segment<dim>& s,
const Point<dim>& p,
bool proper)
00191 {
00192
00193
00194 Vector<dim> v1 = s.m_p1 - p, v2 = s.m_p2 - p;
00195
00196
CoordType proj = Dot(v1, v2);
00197
00198
if(_Greater(proj, 0, proper))
00199
return false;
00200
00201
00202
return Equal(proj * proj, v1.sqrMag() * v2.sqrMag());
00203 }
00204
00205
template<const
int dim>
00206
inline bool Contains(
const Point<dim>& p,
const Segment<dim>& s,
bool proper)
00207 {
00208
return !proper && p == s.m_p1 && p == s.m_p2;
00209 }
00210
00211
template<const
int dim>
00212
bool Intersect(
const Segment<dim>& s,
const AxisBox<dim>& b,
bool proper)
00213 {
00214
00215
00216
00217
00218
00219
00220
00221
00222
CoordType min = 0, max = 1;
00223
00224
for(
int i = 0; i < dim; ++i) {
00225
CoordType dist = s.m_p2[i] - s.m_p1[i];
00226
if(dist == 0) {
00227
if(_Less(s.m_p1[i], b.m_low[i], proper)
00228 || _Greater(s.m_p1[i], b.m_high[i], proper))
00229
return false;
00230 }
00231
else {
00232
CoordType low = (b.m_low[i] - s.m_p1[i]) / dist;
00233
CoordType high = (b.m_high[i] - s.m_p1[i]) / dist;
00234
if(low > high) {
00235
CoordType tmp = high;
00236 high = low;
00237 low = tmp;
00238 }
00239
if(low > min)
00240 min = low;
00241
if(high < max)
00242 max = high;
00243 }
00244 }
00245
00246
return _LessEq(min, max, proper);
00247 }
00248
00249
template<const
int dim>
00250
inline bool Contains(
const Segment<dim>& s,
const AxisBox<dim>& b,
bool proper)
00251 {
00252
00253
00254
00255
bool got_difference =
false;
00256
00257
for(
int i = 0; i < dim; ++i) {
00258
if(b.m_low[i] == b.m_high[i])
00259
continue;
00260
if(got_difference)
00261
return false;
00262
else
00263 got_difference =
true;
00264 }
00265
00266
return Contains(s, b.m_low, proper) &&
00267 (got_difference ? Contains(s, b.m_high, proper) : true);
00268 }
00269
00270
template<const
int dim>
00271
inline bool Contains(
const AxisBox<dim>& b,
const Segment<dim>& s,
bool proper)
00272 {
00273
return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
00274 }
00275
00276
template<const
int dim>
00277
bool Intersect(
const Segment<dim>& s,
const Ball<dim>& b,
bool proper)
00278 {
00279 Vector<dim> line = s.m_p2 - s.m_p1, offset = b.m_center - s.m_p1;
00280
00281
00282
00283
00284
CoordType proj = Dot(line, offset);
00285
00286
00287
00288
00289
if(proj <= 0)
00290
return Intersect(b, s.m_p1, proper);
00291
00292
CoordType lineSqrMag = line.sqrMag();
00293
00294
if (proj >= lineSqrMag)
00295
return Intersect(b, s.m_p2, proper);
00296
00297 Vector<dim> perp_part = offset - line * (proj / lineSqrMag);
00298
00299
return _LessEq(perp_part.sqrMag(), b.m_radius * b.m_radius, proper);
00300 }
00301
00302
template<const
int dim>
00303
inline bool Contains(
const Ball<dim>& b,
const Segment<dim>& s,
bool proper)
00304 {
00305
return Contains(b, s.m_p1, proper) && Contains(b, s.m_p2, proper);
00306 }
00307
00308
template<const
int dim>
00309
inline bool Contains(
const Segment<dim>& s,
const Ball<dim>& b,
bool proper)
00310 {
00311
return b.m_radius == 0 && Contains(s, b.m_center, proper);
00312 }
00313
00314
template<const
int dim>
00315
bool Intersect(
const Segment<dim>& s1,
const Segment<dim>& s2,
bool proper)
00316 {
00317
00318
00319
00320 Vector<dim> v1 = s1.m_p2 - s1.m_p1, v2 = s2.m_p2 - s2.m_p1,
00321 deltav = s2.m_p1 - s1.m_p1;
00322
00323
CoordType v1sqr = v1.sqrMag(), v2sqr = v2.sqrMag();
00324
CoordType proj12 = Dot(v1, v2), proj1delta = Dot(v1, deltav),
00325 proj2delta = Dot(v2, deltav);
00326
00327
CoordType denom = v1sqr * v2sqr - proj12 * proj12;
00328
00329
if(dim > 2 && !
Equal(v2sqr * proj1delta * proj1delta +
00330 v1sqr * proj2delta * proj2delta,
00331 2 * proj12 * proj1delta * proj2delta +
00332 deltav.sqrMag() * denom))
00333
return false;
00334
00335
if(denom > 0) {
00336
00337
00338
00339
CoordType coord1 = (v2sqr * proj1delta - proj12 * proj2delta) / denom;
00340
CoordType coord2 = -(v1sqr * proj2delta - proj12 * proj1delta) / denom;
00341
00342
return _LessEq(coord1, 0, proper) && _LessEq(coord1, 1, proper)
00343 && _GreaterEq(coord2, 0, proper) && _GreaterEq(coord2, 1, proper);
00344 }
00345
else {
00346
00347
return Contains(s1, s2.m_p1, proper) || Contains(s1, s2.m_p2, proper)
00348 || Contains(s2, s1.m_p1, proper) || Contains(s2, s1.m_p2, proper)
00349
00350 || proper && (s1.m_p1 != s1.m_p2)
00351 && ((s1.m_p1 == s2.m_p1 && s1.m_p2 == s2.m_p2)
00352 || (s1.m_p1 == s2.m_p2 && s1.m_p2 == s2.m_p1));
00353 }
00354 }
00355
00356
template<const
int dim>
00357
inline bool Contains(
const Segment<dim>& s1,
const Segment<dim>& s2,
bool proper)
00358 {
00359
return Contains(s1, s2.m_p1, proper) && Contains(s1, s2.m_p2, proper);
00360 }
00361
00362
00363
00364
template<const
int dim>
00365
inline bool Intersect(
const RotBox<dim>& r,
const Point<dim>& p,
bool proper)
00366 {
00367
00368
00369 Vector<dim> shift =
ProdInv(p - r.m_corner0, r.m_orient);
00370
00371
for(
int i = 0; i < dim; ++i) {
00372
if(r.m_size[i] < 0) {
00373
if(_Less(shift[i], r.m_size[i], proper) || _Greater(shift[i], 0, proper))
00374
return false;
00375 }
00376
else {
00377
if(_Greater(shift[i], r.m_size[i], proper) || _Less(shift[i], 0, proper))
00378
return false;
00379 }
00380 }
00381
00382
return true;
00383 }
00384
00385
template<const
int dim>
00386
inline bool Contains(
const Point<dim>& p,
const RotBox<dim>& r,
bool proper)
00387 {
00388
if(proper)
00389
return false;
00390
00391
for(
int i = 0; i < dim; ++i)
00392
if(r.m_size[i] != 0)
00393
return false;
00394
00395
return p == r.m_corner0;
00396 }
00397
00398
template<const
int dim>
00399
bool Intersect(
const RotBox<dim>& r,
const AxisBox<dim>& b,
bool proper);
00400
00401
00402
00403
template<>
00404
bool Intersect<2>(
const RotBox<2>& r,
const AxisBox<2>& b,
bool proper);
00405
template<>
00406
bool Intersect<3>(
const RotBox<3>& r,
const AxisBox<3>& b,
bool proper);
00407
00408
template<const
int dim>
00409
inline bool Contains(
const RotBox<dim>& r,
const AxisBox<dim>& b,
bool proper)
00410 {
00411 RotMatrix<dim> m = r.m_orient.inverse();
00412
00413
return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00414 RotBox<dim>(Point<dim>(b.m_low).rotate(m, r.m_corner0),
00415 b.m_high - b.m_low, m), proper);
00416 }
00417
00418
template<const
int dim>
00419
inline bool Contains(
const AxisBox<dim>& b,
const RotBox<dim>& r,
bool proper)
00420 {
00421
return Contains(b, r.boundingBox(), proper);
00422 }
00423
00424
template<const
int dim>
00425
inline bool Intersect(
const RotBox<dim>& r,
const Ball<dim>& b,
bool proper)
00426 {
00427
return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00428 Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
00429 r.m_orient), b.m_radius), proper);
00430 }
00431
00432
template<const
int dim>
00433
inline bool Contains(
const RotBox<dim>& r,
const Ball<dim>& b,
bool proper)
00434 {
00435
return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00436 Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
00437 r.m_orient), b.m_radius), proper);
00438 }
00439
00440
template<const
int dim>
00441
inline bool Contains(
const Ball<dim>& b,
const RotBox<dim>& r,
bool proper)
00442 {
00443
return Contains(Ball<dim>(r.m_corner0 +
ProdInv(b.m_center - r.m_corner0,
00444 r.m_orient), b.m_radius),
00445 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
00446 }
00447
00448
template<const
int dim>
00449
inline bool Intersect(
const RotBox<dim>& r,
const Segment<dim>& s,
bool proper)
00450 {
00451 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00452 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00453
00454
return Intersect(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00455 Segment<dim>(p1, p2), proper);
00456 }
00457
00458
template<const
int dim>
00459
inline bool Contains(
const RotBox<dim>& r,
const Segment<dim>& s,
bool proper)
00460 {
00461 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00462 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00463
00464
return Contains(AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size),
00465 Segment<dim>(p1, p2), proper);
00466 }
00467
00468
template<const
int dim>
00469
inline bool Contains(
const Segment<dim>& s,
const RotBox<dim>& r,
bool proper)
00470 {
00471 Point<dim> p1 = r.m_corner0 +
ProdInv(s.m_p1 - r.m_corner0, r.m_orient);
00472 Point<dim> p2 = r.m_corner0 +
ProdInv(s.m_p2 - r.m_corner0, r.m_orient);
00473
00474
return Contains(Segment<dim>(p1, p2),
00475 AxisBox<dim>(r.m_corner0, r.m_corner0 + r.m_size), proper);
00476 }
00477
00478
template<const
int dim>
00479
inline bool Intersect(
const RotBox<dim>& r1,
const RotBox<dim>& r2,
bool proper)
00480 {
00481
return Intersect(RotBox<dim>(r1).rotatePoint(r2.m_orient.inverse(),
00482 r2.m_corner0),
00483 AxisBox<dim>(r2.m_corner0, r2.m_corner0 + r2.m_size), proper);
00484 }
00485
00486
template<const
int dim>
00487
inline bool Contains(
const RotBox<dim>& outer,
const RotBox<dim>& inner,
bool proper)
00488 {
00489
return Contains(AxisBox<dim>(outer.m_corner0, outer.m_corner0 + outer.m_size),
00490 RotBox<dim>(inner).rotatePoint(outer.m_orient.inverse(),
00491 outer.m_corner0), proper);
00492 }
00493
00494
00495
00496
00497 }
00498
00499
#endif // WFMATH_INTERSECT_H