00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 #include <algorithm>
00074 #include <vector>
00075
00076 #include "TBuffer3D.h"
00077 #include "Rtypes.h"
00078 #include "TMath.h"
00079 #include "CsgOps.h"
00080
00081 namespace RootCsg {
00082
00083 const Double_t epsilon = 1e-10;
00084 const Double_t epsilon2 = 1e-20;
00085 const Double_t infinity = 1e50;
00086
00087
00088 Int_t sign(Double_t x)
00089 {
00090 return x < 0. ? -1 : x > 0. ? 1 : 0;
00091 }
00092
00093
00094 Bool_t fuzzy_zero(Double_t x)
00095 {
00096 return TMath::Abs(x) < epsilon;
00097 }
00098
00099
00100 Bool_t fuzzy_zero2(Double_t x)
00101 {
00102 return TMath::Abs(x) < epsilon2;
00103 }
00104
00105 class Tuple2 {
00106 protected:
00107 Double_t fCo[2];
00108
00109 public:
00110 Tuple2(){SetValue(0, 0);}
00111 Tuple2(const Double_t *vv){SetValue(vv);}
00112 Tuple2(Double_t xx, Double_t yy){SetValue(xx, yy);}
00113
00114 Double_t &operator [] (Int_t i){return fCo[i];}
00115 const Double_t &operator [] (Int_t i)const{return fCo[i];}
00116
00117 Double_t &X(){return fCo[0];}
00118 const Double_t &X()const{return fCo[0];}
00119 Double_t &Y(){return fCo[1];}
00120 const Double_t &Y()const{return fCo[1];}
00121 Double_t &U(){return fCo[0];}
00122 const Double_t &U()const{return fCo[0];}
00123 Double_t &V(){return fCo[1];}
00124 const Double_t &V()const{return fCo[1];}
00125
00126 Double_t *GetValue(){return fCo;}
00127 const Double_t *GetValue()const{return fCo;}
00128 void GetValue(Double_t *vv)const{vv[0] = fCo[0]; vv[1] = fCo[1];}
00129
00130 void SetValue(const Double_t *vv){fCo[0] = vv[0]; fCo[1] = vv[1];}
00131 void SetValue(Double_t xx, Double_t yy){fCo[0] = xx; fCo[1] = yy;}
00132 };
00133
00134 Bool_t operator == (const Tuple2 &t1, const Tuple2 &t2)
00135 {
00136 return t1[0] == t2[0] && t1[1] == t2[1];
00137 }
00138
00139 class TVector2 : public Tuple2 {
00140 public:
00141 TVector2(){}
00142 TVector2(const Double_t *v) : Tuple2(v) {}
00143 TVector2(Double_t xx, Double_t yy) : Tuple2(xx, yy) {}
00144
00145 TVector2 &operator += (const TVector2 &v);
00146 TVector2 &operator -= (const TVector2 &v);
00147 TVector2 &operator *= (Double_t s);
00148 TVector2 &operator /= (Double_t s);
00149
00150 Double_t Dot(const TVector2 &v)const;
00151 Double_t Length2()const;
00152 Double_t Length()const;
00153 TVector2 Absolute()const;
00154 void Normalize();
00155 TVector2 Normalized()const;
00156 void Scale(Double_t x, Double_t y);
00157 TVector2 Scaled(Double_t x, Double_t y)const;
00158 Bool_t FuzzyZero()const;
00159 Double_t Angle(const TVector2 &v)const;
00160 TVector2 Cross(const TVector2 &v)const;
00161 Double_t Triple(const TVector2 &v1, const TVector2 &v2)const;
00162 };
00163
00164
00165 TVector2 &TVector2::operator+=(const TVector2 &vv)
00166 {
00167
00168 fCo[0] += vv[0]; fCo[1] += vv[1];
00169 return *this;
00170 }
00171
00172
00173 TVector2 &TVector2::operator-=(const TVector2 &vv)
00174 {
00175
00176 fCo[0] -= vv[0]; fCo[1] -= vv[1];
00177 return *this;
00178 }
00179
00180
00181 TVector2 &TVector2::operator *= (Double_t s)
00182 {
00183
00184 fCo[0] *= s; fCo[1] *= s; return *this;
00185 }
00186
00187
00188 TVector2 &TVector2::operator /= (Double_t s)
00189 {
00190
00191 return *this *= 1. / s;
00192 }
00193
00194
00195 TVector2 operator + (const TVector2 &v1, const TVector2 &v2)
00196 {
00197
00198 return TVector2(v1[0] + v2[0], v1[1] + v2[1]);
00199 }
00200
00201
00202 TVector2 operator - (const TVector2 &v1, const TVector2 &v2)
00203 {
00204
00205 return TVector2(v1[0] - v2[0], v1[1] - v2[1]);
00206 }
00207
00208
00209 TVector2 operator - (const TVector2 &v)
00210 {
00211
00212 return TVector2(-v[0], -v[1]);
00213 }
00214
00215
00216 TVector2 operator * (const TVector2 &v, Double_t s)
00217 {
00218
00219 return TVector2(v[0] * s, v[1] * s);
00220 }
00221
00222
00223 TVector2 operator * (Double_t s, const TVector2 &v)
00224 {
00225
00226 return v * s;
00227 }
00228
00229
00230 TVector2 operator / (const TVector2 & v, Double_t s)
00231 {
00232
00233 return v * (1.0 / s);
00234 }
00235
00236
00237 Double_t TVector2::Dot(const TVector2 &vv)const
00238 {
00239
00240 return fCo[0] * vv[0] + fCo[1] * vv[1];
00241 }
00242
00243
00244 Double_t TVector2::Length2()const
00245 {
00246
00247 return Dot(*this);
00248 }
00249
00250
00251 Double_t TVector2::Length()const
00252 {
00253
00254 return TMath::Sqrt(Length2());
00255 }
00256
00257
00258 TVector2 TVector2::Absolute()const
00259 {
00260
00261 return TVector2(TMath::Abs(fCo[0]), TMath::Abs(fCo[1]));
00262 }
00263
00264
00265 Bool_t TVector2::FuzzyZero()const
00266 {
00267
00268 return fuzzy_zero2(Length2());
00269 }
00270
00271
00272 void TVector2::Normalize()
00273 {
00274
00275 *this /= Length();
00276 }
00277
00278
00279 TVector2 TVector2::Normalized()const
00280 {
00281
00282 return *this / Length();
00283 }
00284
00285
00286 void TVector2::Scale(Double_t xx, Double_t yy)
00287 {
00288
00289 fCo[0] *= xx; fCo[1] *= yy;
00290 }
00291
00292
00293 TVector2 TVector2::Scaled(Double_t xx, Double_t yy)const
00294 {
00295
00296 return TVector2(fCo[0] * xx, fCo[1] * yy);
00297 }
00298
00299
00300 Double_t TVector2::Angle(const TVector2 &vv)const
00301 {
00302
00303 Double_t s = TMath::Sqrt(Length2() * vv.Length2());
00304 return TMath::ACos(Dot(vv) / s);
00305 }
00306
00307
00308 Double_t dot(const TVector2 &v1, const TVector2 &v2)
00309 {
00310
00311 return v1.Dot(v2);
00312 }
00313
00314
00315 Double_t length2(const TVector2 &v)
00316 {
00317 return v.Length2();
00318 }
00319
00320
00321 Double_t length(const TVector2 &v)
00322 {
00323 return v.Length();
00324 }
00325
00326
00327 Bool_t fuzzy_zero(const TVector2 &v)
00328 {
00329 return v.FuzzyZero();
00330 }
00331
00332
00333 Bool_t fuzzy_equal(const TVector2 &v1, const TVector2 &v2)
00334 {
00335 return fuzzy_zero(v1 - v2);
00336 }
00337
00338
00339 Double_t Angle(const TVector2 &v1, const TVector2 &v2)
00340 {
00341 return v1.Angle(v2);
00342 }
00343
00344 class TPoint2 : public TVector2 {
00345 public:
00346 TPoint2(){}
00347 TPoint2(const Double_t *v) : TVector2(v){}
00348 TPoint2(Double_t x, Double_t y) : TVector2(x, y) {}
00349
00350 TPoint2 &operator += (const TVector2 &v);
00351 TPoint2 &operator -= (const TVector2 &v);
00352 TPoint2 &operator = (const TVector2 &v);
00353
00354 Double_t Distance(const TPoint2 &p)const;
00355 Double_t Distance2(const TPoint2 &p)const;
00356 TPoint2 Lerp(const TPoint2 &p, Double_t t)const;
00357 };
00358
00359
00360 TPoint2 &TPoint2::operator += (const TVector2 &v)
00361 {
00362
00363 fCo[0] += v[0]; fCo[1] += v[1];
00364 return *this;
00365 }
00366
00367
00368 TPoint2 &TPoint2::operator-=(const TVector2& v)
00369 {
00370
00371 fCo[0] -= v[0]; fCo[1] -= v[1];
00372 return *this;
00373 }
00374
00375
00376 TPoint2 &TPoint2::operator=(const TVector2& v)
00377 {
00378
00379 fCo[0] = v[0]; fCo[1] = v[1];
00380 return *this;
00381 }
00382
00383
00384 Double_t TPoint2::Distance(const TPoint2& p)const
00385 {
00386
00387 return (p - *this).Length();
00388 }
00389
00390
00391 Double_t TPoint2::Distance2(const TPoint2& p)const
00392 {
00393
00394 return (p - *this).Length2();
00395 }
00396
00397
00398 TPoint2 TPoint2::Lerp(const TPoint2 &p, Double_t t)const
00399 {
00400
00401 return TPoint2(fCo[0] + (p[0] - fCo[0]) * t,
00402 fCo[1] + (p[1] - fCo[1]) * t);
00403 }
00404
00405
00406 TPoint2 operator + (const TPoint2 &p, const TVector2 &v)
00407 {
00408
00409 return TPoint2(p[0] + v[0], p[1] + v[1]);
00410 }
00411
00412
00413 TPoint2 operator - (const TPoint2 &p, const TVector2 &v)
00414 {
00415
00416 return TPoint2(p[0] - v[0], p[1] - v[1]);
00417 }
00418
00419
00420 TVector2 operator - (const TPoint2 &p1, const TPoint2 &p2)
00421 {
00422
00423 return TVector2(p1[0] - p2[0], p1[1] - p2[1]);
00424 }
00425
00426
00427 Double_t distance(const TPoint2 &p1, const TPoint2 &p2)
00428 {
00429
00430 return p1.Distance(p2);
00431 }
00432
00433
00434 Double_t distance2(const TPoint2 &p1, const TPoint2 &p2)
00435 {
00436
00437 return p1.Distance2(p2);
00438 }
00439
00440
00441 TPoint2 lerp(const TPoint2 &p1, const TPoint2 &p2, Double_t t)
00442 {
00443
00444 return p1.Lerp(p2, t);
00445 }
00446
00447 class Tuple3 {
00448 protected:
00449 Double_t fCo[3];
00450 public:
00451 Tuple3(){SetValue(0, 0, 0);}
00452 Tuple3(const Double_t *v){SetValue(v);}
00453 Tuple3(Double_t xx, Double_t yy, Double_t zz){SetValue(xx, yy, zz);}
00454
00455 Double_t &operator [] (Int_t i){return fCo[i];}
00456 const Double_t &operator [] (Int_t i)const{return fCo[i];}
00457
00458 Double_t &X(){return fCo[0];}
00459 const Double_t &X()const{return fCo[0];}
00460 Double_t &Y(){return fCo[1];}
00461 const Double_t &Y()const{return fCo[1];}
00462 Double_t &Z(){return fCo[2];}
00463 const Double_t &Z()const{return fCo[2];}
00464
00465 Double_t *GetValue(){return fCo;}
00466 const Double_t *GetValue()const{ return fCo; }
00467
00468 void GetValue(Double_t *v)const
00469 {
00470 v[0] = Double_t(fCo[0]), v[1] = Double_t(fCo[1]), v[2] = Double_t(fCo[2]);
00471 }
00472
00473 void SetValue(const Double_t *v)
00474 {
00475 fCo[0] = Double_t(v[0]), fCo[1] = Double_t(v[1]), fCo[2] = Double_t(v[2]);
00476 }
00477 void SetValue(Double_t xx, Double_t yy, Double_t zz)
00478 {
00479 fCo[0] = xx; fCo[1] = yy; fCo[2] = zz;
00480 }
00481 };
00482
00483
00484 Bool_t operator==(const Tuple3& t1, const Tuple3& t2)
00485 {
00486 return t1[0] == t2[0] && t1[1] == t2[1] && t1[2] == t2[2];
00487 }
00488
00489 class TVector3 : public Tuple3 {
00490 public:
00491 TVector3(){}
00492 TVector3(const Double_t *v) : Tuple3(v){}
00493 TVector3(Double_t xx, Double_t yy, Double_t zz) : Tuple3(xx, yy, zz){}
00494
00495 TVector3 &operator += (const TVector3& v);
00496 TVector3 &operator -= (const TVector3& v);
00497 TVector3 &operator *= (Double_t s);
00498 TVector3 &operator /= (Double_t s);
00499
00500 Double_t Dot(const TVector3& v)const;
00501 Double_t Length2()const;
00502 Double_t Length()const;
00503 TVector3 Absolute()const;
00504 void NoiseGate(Double_t threshold);
00505 void Normalize();
00506 TVector3 Normalized()const;
00507 TVector3 SafeNormalized()const;
00508 void Scale(Double_t x, Double_t y, Double_t z);
00509 TVector3 Scaled(Double_t x, Double_t y, Double_t z)const;
00510 Bool_t FuzzyZero()const;
00511 Double_t Angle(const TVector3 &v)const;
00512 TVector3 Cross(const TVector3 &v)const;
00513 Double_t Triple(const TVector3 &v1, const TVector3 &v2)const;
00514 Int_t ClosestAxis()const;
00515
00516 static TVector3 Random();
00517 };
00518
00519
00520 TVector3 &TVector3::operator += (const TVector3 &v)
00521 {
00522
00523 fCo[0] += v[0]; fCo[1] += v[1]; fCo[2] += v[2];
00524 return *this;
00525 }
00526
00527
00528 TVector3 &TVector3::operator -= (const TVector3 &v)
00529 {
00530
00531 fCo[0] -= v[0]; fCo[1] -= v[1]; fCo[2] -= v[2];
00532 return *this;
00533 }
00534
00535
00536 TVector3 &TVector3::operator *= (Double_t s)
00537 {
00538
00539 fCo[0] *= s; fCo[1] *= s; fCo[2] *= s;
00540 return *this;
00541 }
00542
00543
00544 TVector3 &TVector3::operator /= (Double_t s)
00545 {
00546
00547 return *this *= Double_t(1.0) / s;
00548 }
00549
00550
00551 Double_t TVector3::Dot(const TVector3 &v)const
00552 {
00553
00554 return fCo[0] * v[0] + fCo[1] * v[1] + fCo[2] * v[2];
00555 }
00556
00557
00558 Double_t TVector3::Length2()const
00559 {
00560
00561 return Dot(*this);
00562 }
00563
00564
00565 Double_t TVector3::Length()const
00566 {
00567
00568 return TMath::Sqrt(Length2());
00569 }
00570
00571
00572 TVector3 TVector3::Absolute()const
00573 {
00574
00575 return TVector3(TMath::Abs(fCo[0]), TMath::Abs(fCo[1]), TMath::Abs(fCo[2]));
00576 }
00577
00578
00579 Bool_t TVector3::FuzzyZero()const
00580 {
00581
00582 return fuzzy_zero(Length2());
00583 }
00584
00585
00586 void TVector3::NoiseGate(Double_t threshold)
00587 {
00588
00589 if (Length2() < threshold) SetValue(0., 0., 0.);
00590 }
00591
00592
00593 void TVector3::Normalize()
00594 {
00595
00596 *this /= Length();
00597 }
00598
00599
00600 TVector3 operator * (const TVector3 &v, Double_t s)
00601 {
00602 return TVector3(v[0] * s, v[1] * s, v[2] * s);
00603 }
00604
00605
00606 TVector3 operator / (const TVector3 &v, Double_t s)
00607 {
00608 return v * (1. / s);
00609 }
00610
00611
00612 TVector3 TVector3::Normalized()const
00613 {
00614
00615 return *this / Length();
00616 }
00617
00618
00619 TVector3 TVector3::SafeNormalized()const
00620 {
00621
00622 Double_t len = Length();
00623 return fuzzy_zero(len) ? TVector3(1., 0., 0.):*this / len;
00624 }
00625
00626
00627 void TVector3::Scale(Double_t xx, Double_t yy, Double_t zz)
00628 {
00629
00630 fCo[0] *= xx; fCo[1] *= yy; fCo[2] *= zz;
00631 }
00632
00633
00634 TVector3 TVector3::Scaled(Double_t xx, Double_t yy, Double_t zz)const
00635 {
00636
00637 return TVector3(fCo[0] * xx, fCo[1] * yy, fCo[2] * zz);
00638 }
00639
00640
00641 Double_t TVector3::Angle(const TVector3 &v)const
00642 {
00643
00644 Double_t s = TMath::Sqrt(Length2() * v.Length2());
00645 return TMath::ACos(Dot(v) / s);
00646 }
00647
00648
00649 TVector3 TVector3::Cross(const TVector3 &v)const
00650 {
00651
00652 return TVector3(fCo[1] * v[2] - fCo[2] * v[1],
00653 fCo[2] * v[0] - fCo[0] * v[2],
00654 fCo[0] * v[1] - fCo[1] * v[0]);
00655 }
00656
00657
00658 Double_t TVector3::Triple(const TVector3 &v1, const TVector3 &v2)const
00659 {
00660
00661 return fCo[0] * (v1[1] * v2[2] - v1[2] * v2[1]) +
00662 fCo[1] * (v1[2] * v2[0] - v1[0] * v2[2]) +
00663 fCo[2] * (v1[0] * v2[1] - v1[1] * v2[0]);
00664 }
00665
00666
00667 Int_t TVector3::ClosestAxis()const
00668 {
00669
00670 TVector3 a = Absolute();
00671 return a[0] < a[1] ? (a[1] < a[2] ? 2 : 1) : (a[0] < a[2] ? 2 : 0);
00672 }
00673
00674
00675 TVector3 operator + (const TVector3 &v1, const TVector3 &v2)
00676 {
00677 return TVector3(v1[0] + v2[0], v1[1] + v2[1], v1[2] + v2[2]);
00678 }
00679
00680
00681 TVector3 operator - (const TVector3 &v1, const TVector3 &v2)
00682 {
00683 return TVector3(v1[0] - v2[0], v1[1] - v2[1], v1[2] - v2[2]);
00684 }
00685
00686
00687 TVector3 operator - (const TVector3 &v)
00688 {
00689 return TVector3(-v[0], -v[1], -v[2]);
00690 }
00691
00692
00693 TVector3 operator * (Double_t s, const TVector3 &v)
00694 {
00695 return v * s;
00696 }
00697
00698
00699 TVector3 operator * (const TVector3 &v1, const TVector3 &v2)
00700 {
00701 return TVector3(v1[0] * v2[0], v1[1] * v2[1], v1[2] * v2[2]);
00702 }
00703
00704
00705 Double_t dot(const TVector3 &v1, const TVector3 &v2)
00706 {
00707 return v1.Dot(v2);
00708 }
00709
00710
00711 Double_t length2(const TVector3 &v)
00712 {
00713 return v.Length2();
00714 }
00715
00716
00717 Double_t length(const TVector3 &v)
00718 {
00719 return v.Length();
00720 }
00721
00722
00723 Bool_t fuzzy_zero(const TVector3 &v)
00724 {
00725 return v.FuzzyZero();
00726 }
00727
00728
00729 Bool_t fuzzy_equal(const TVector3 &v1, const TVector3 &v2)
00730 {
00731 return fuzzy_zero(v1 - v2);
00732 }
00733
00734
00735 Double_t Angle(const TVector3 &v1, const TVector3 &v2)
00736 {
00737 return v1.Angle(v2);
00738 }
00739
00740
00741 TVector3 cross(const TVector3 &v1, const TVector3 &v2)
00742 {
00743 return v1.Cross(v2);
00744 }
00745
00746
00747 Double_t triple(const TVector3 &v1, const TVector3 &v2, const TVector3 &v3)
00748 {
00749 return v1.Triple(v2, v3);
00750 }
00751
00752 class TPoint3 : public TVector3 {
00753 public:
00754 TPoint3(){}
00755 TPoint3(const Double_t *v) : TVector3(v) {}
00756 TPoint3(Double_t xx, Double_t yy, Double_t zz) : TVector3(xx, yy, zz) {}
00757
00758 TPoint3 &operator += (const TVector3 &v);
00759 TPoint3 &operator -= (const TVector3 &v);
00760 TPoint3 &operator = (const TVector3 &v);
00761
00762 Double_t Distance(const TPoint3 &p)const;
00763 Double_t Distance2(const TPoint3 &p)const;
00764 TPoint3 Lerp(const TPoint3 &p, Double_t t)const;
00765 };
00766
00767
00768 TPoint3 &TPoint3::operator += (const TVector3 &v)
00769 {
00770
00771 fCo[0] += v[0]; fCo[1] += v[1]; fCo[2] += v[2];
00772 return *this;
00773 }
00774
00775
00776 TPoint3 &TPoint3::operator-=(const TVector3 &v)
00777 {
00778
00779 fCo[0] -= v[0]; fCo[1] -= v[1]; fCo[2] -= v[2];
00780 return *this;
00781 }
00782
00783
00784 TPoint3 &TPoint3::operator=(const TVector3 &v)
00785 {
00786
00787 fCo[0] = v[0]; fCo[1] = v[1]; fCo[2] = v[2];
00788 return *this;
00789 }
00790
00791
00792 Double_t TPoint3::Distance(const TPoint3 &p)const
00793 {
00794
00795 return (p - *this).Length();
00796 }
00797
00798
00799 Double_t TPoint3::Distance2(const TPoint3 &p)const
00800 {
00801
00802 return (p - *this).Length2();
00803 }
00804
00805
00806 TPoint3 TPoint3::Lerp(const TPoint3 &p, Double_t t)const
00807 {
00808
00809 return TPoint3(fCo[0] + (p[0] - fCo[0]) * t,
00810 fCo[1] + (p[1] - fCo[1]) * t,
00811 fCo[2] + (p[2] - fCo[2]) * t);
00812 }
00813
00814
00815 TPoint3 operator + (const TPoint3 &p, const TVector3 &v)
00816 {
00817 return TPoint3(p[0] + v[0], p[1] + v[1], p[2] + v[2]);
00818 }
00819
00820
00821 TPoint3 operator - (const TPoint3 &p, const TVector3 &v)
00822 {
00823 return TPoint3(p[0] - v[0], p[1] - v[1], p[2] - v[2]);
00824 }
00825
00826
00827 TVector3 operator - (const TPoint3 &p1, const TPoint3 &p2)
00828 {
00829 return TVector3(p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]);
00830 }
00831
00832
00833 Double_t distance(const TPoint3 &p1, const TPoint3 &p2)
00834 {
00835 return p1.Distance(p2);
00836 }
00837
00838
00839 Double_t distance2(const TPoint3 &p1, const TPoint3 &p2)
00840 {
00841 return p1.Distance2(p2);
00842 }
00843
00844
00845 TPoint3 lerp(const TPoint3 &p1, const TPoint3 &p2, Double_t t)
00846 {
00847 return p1.Lerp(p2, t);
00848 }
00849
00850 class Tuple4 {
00851 protected:
00852 Double_t fCo[4];
00853
00854 public:
00855 Tuple4(){SetValue(0, 0, 0, 0);}
00856 Tuple4(const Double_t *v){SetValue(v);}
00857 Tuple4(Double_t xx, Double_t yy, Double_t zz, Double_t ww)
00858 {
00859 SetValue(xx, yy, zz, ww);
00860 }
00861
00862 Double_t &operator [] (Int_t i){return fCo[i];}
00863 const Double_t &operator [] (Int_t i)const{return fCo[i];}
00864
00865 Double_t &X(){return fCo[0];}
00866 const Double_t &X()const{return fCo[0];}
00867 Double_t &Y(){return fCo[1];}
00868 const Double_t &Y()const{return fCo[1];}
00869 Double_t &Z(){return fCo[2];}
00870 const Double_t &Z()const{return fCo[2];}
00871 Double_t &W(){return fCo[3];}
00872 const Double_t &W()const{return fCo[3];}
00873
00874 Double_t *GetValue(){return fCo;}
00875 const Double_t *GetValue()const{return fCo;}
00876
00877 void GetValue(Double_t *v)const
00878 {
00879 v[0] = fCo[0]; v[1] = fCo[1]; v[2] = fCo[2]; v[3] = fCo[3];
00880 }
00881
00882 void SetValue(const Double_t *v)
00883 {
00884 fCo[0] = v[0]; fCo[1] = v[1]; fCo[2] = v[2]; fCo[3] = v[3];
00885 }
00886 void SetValue(Double_t xx, Double_t yy, Double_t zz, Double_t ww)
00887 {
00888 fCo[0] = xx; fCo[1] = yy; fCo[2] = zz; fCo[3] = ww;
00889 }
00890 };
00891
00892
00893 Bool_t operator == (const Tuple4 &t1, const Tuple4 &t2)
00894 {
00895 return t1[0] == t2[0] && t1[1] == t2[1] && t1[2] == t2[2] && t1[3] == t2[3];
00896 }
00897
00898 class TMatrix3x3 {
00899 protected:
00900 TVector3 fEl[3];
00901
00902 public:
00903 TMatrix3x3(){}
00904 TMatrix3x3(const Double_t *m){SetValue(m);}
00905 TMatrix3x3(const TVector3 &euler){SetEuler(euler);}
00906 TMatrix3x3(const TVector3 &euler, const TVector3 &s)
00907 {
00908 SetEuler(euler); Scale(s[0], s[1], s[2]);
00909 }
00910 TMatrix3x3(Double_t xx, Double_t xy, Double_t xz,
00911 Double_t yx, Double_t yy, Double_t yz,
00912 Double_t zx, Double_t zy, Double_t zz)
00913 {
00914 SetValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
00915 }
00916
00917 TVector3 &operator [] (Int_t i){return fEl[i];}
00918 const TVector3 &operator [] (Int_t i)const{return fEl[i];}
00919
00920 void SetValue(const Double_t *m)
00921 {
00922 fEl[0][0] = *m++; fEl[1][0] = *m++; fEl[2][0] = *m++; m++;
00923 fEl[0][1] = *m++; fEl[1][1] = *m++; fEl[2][1] = *m++; m++;
00924 fEl[0][2] = *m++; fEl[1][2] = *m++; fEl[2][2] = *m;
00925 }
00926 void SetValue(Double_t xx, Double_t xy, Double_t xz,
00927 Double_t yx, Double_t yy, Double_t yz,
00928 Double_t zx, Double_t zy, Double_t zz)
00929 {
00930 fEl[0][0] = xx; fEl[0][1] = xy; fEl[0][2] = xz;
00931 fEl[1][0] = yx; fEl[1][1] = yy; fEl[1][2] = yz;
00932 fEl[2][0] = zx; fEl[2][1] = zy; fEl[2][2] = zz;
00933 }
00934 void SetEuler(const TVector3 &euler)
00935 {
00936 Double_t ci = TMath::Cos(euler[0]);
00937 Double_t cj = TMath::Cos(euler[1]);
00938 Double_t ch = TMath::Cos(euler[2]);
00939 Double_t si = TMath::Sin(euler[0]);
00940 Double_t sj = TMath::Sin(euler[1]);
00941 Double_t sh = TMath::Sin(euler[2]);
00942 Double_t cc = ci * ch;
00943 Double_t cs = ci * sh;
00944 Double_t sc = si * ch;
00945 Double_t ss = si * sh;
00946 SetValue(cj * ch, sj * sc - cs, sj * cc + ss,
00947 cj * sh, sj * ss + cc, sj * cs - sc,
00948 -sj, cj * si, cj * ci);
00949 }
00950
00951 void Scale(Double_t x, Double_t y, Double_t z)
00952 {
00953 fEl[0][0] *= x; fEl[0][1] *= y; fEl[0][2] *= z;
00954 fEl[1][0] *= x; fEl[1][1] *= y; fEl[1][2] *= z;
00955 fEl[2][0] *= x; fEl[2][1] *= y; fEl[2][2] *= z;
00956 }
00957 TMatrix3x3 Scaled(Double_t x, Double_t y, Double_t z)const
00958 {
00959 return TMatrix3x3(fEl[0][0] * x, fEl[0][1] * y, fEl[0][2] * z,
00960 fEl[1][0] * x, fEl[1][1] * y, fEl[1][2] * z,
00961 fEl[2][0] * x, fEl[2][1] * y, fEl[2][2] * z);
00962 }
00963
00964 void SetIdentity()
00965 {
00966 SetValue(1., 0., 0., 0., 1., 0., 0., 0., 1.);
00967 }
00968 void GetValue(Double_t *m)const
00969 {
00970 *m++ = fEl[0][0]; *m++ = fEl[1][0]; *m++ = fEl[2][0]; *m++ = 0.0;
00971 *m++ = fEl[0][1]; *m++ = fEl[1][1]; *m++ = fEl[2][1]; *m++ = 0.0;
00972 *m++ = fEl[0][2]; *m++ = fEl[1][2]; *m++ = fEl[2][2]; *m = 0.0;
00973 }
00974
00975 TMatrix3x3 &operator *= (const TMatrix3x3 &m);
00976 Double_t Tdot(Int_t c, const TVector3 &v)const
00977 {
00978 return fEl[0][c] * v[0] + fEl[1][c] * v[1] + fEl[2][c] * v[2];
00979 }
00980 Double_t Cofac(Int_t r1, Int_t c1, Int_t r2, Int_t c2)const
00981 {
00982 return fEl[r1][c1] * fEl[r2][c2] - fEl[r1][c2] * fEl[r2][c1];
00983 }
00984
00985 Double_t Determinant()const;
00986 TMatrix3x3 Adjoint()const;
00987 TMatrix3x3 Absolute()const;
00988 TMatrix3x3 Transposed()const;
00989 void Transpose();
00990 TMatrix3x3 Inverse()const;
00991 void Invert();
00992 };
00993
00994
00995 TMatrix3x3 &TMatrix3x3::operator *= (const TMatrix3x3 &m)
00996 {
00997
00998 SetValue(m.Tdot(0, fEl[0]), m.Tdot(1, fEl[0]), m.Tdot(2, fEl[0]),
00999 m.Tdot(0, fEl[1]), m.Tdot(1, fEl[1]), m.Tdot(2, fEl[1]),
01000 m.Tdot(0, fEl[2]), m.Tdot(1, fEl[2]), m.Tdot(2, fEl[2]));
01001 return *this;
01002 }
01003
01004
01005 Double_t TMatrix3x3::Determinant()const
01006 {
01007
01008 return triple((*this)[0], (*this)[1], (*this)[2]);
01009 }
01010
01011
01012 TMatrix3x3 TMatrix3x3::Absolute()const
01013 {
01014
01015 return TMatrix3x3(TMath::Abs(fEl[0][0]), TMath::Abs(fEl[0][1]), TMath::Abs(fEl[0][2]),
01016 TMath::Abs(fEl[1][0]), TMath::Abs(fEl[1][1]), TMath::Abs(fEl[1][2]),
01017 TMath::Abs(fEl[2][0]), TMath::Abs(fEl[2][1]), TMath::Abs(fEl[2][2]));
01018 }
01019
01020
01021 TMatrix3x3 TMatrix3x3::Transposed()const
01022 {
01023
01024 return TMatrix3x3(fEl[0][0], fEl[1][0], fEl[2][0],
01025 fEl[0][1], fEl[1][1], fEl[2][1],
01026 fEl[0][2], fEl[1][2], fEl[2][2]);
01027 }
01028
01029
01030 void TMatrix3x3::Transpose()
01031 {
01032
01033 *this = Transposed();
01034 }
01035
01036
01037 TMatrix3x3 TMatrix3x3::Adjoint()const
01038 {
01039
01040 return TMatrix3x3(Cofac(1, 1, 2, 2), Cofac(0, 2, 2, 1), Cofac(0, 1, 1, 2),
01041 Cofac(1, 2, 2, 0), Cofac(0, 0, 2, 2), Cofac(0, 2, 1, 0),
01042 Cofac(1, 0, 2, 1), Cofac(0, 1, 2, 0), Cofac(0, 0, 1, 1));
01043 }
01044
01045
01046 TMatrix3x3 TMatrix3x3::Inverse()const
01047 {
01048
01049 TVector3 co(Cofac(1, 1, 2, 2), Cofac(1, 2, 2, 0), Cofac(1, 0, 2, 1));
01050 Double_t det = dot((*this)[0], co);
01051 Double_t s = 1. / det;
01052 return TMatrix3x3(co[0] * s, Cofac(0, 2, 2, 1) * s, Cofac(0, 1, 1, 2) * s,
01053 co[1] * s, Cofac(0, 0, 2, 2) * s, Cofac(0, 2, 1, 0) * s,
01054 co[2] * s, Cofac(0, 1, 2, 0) * s, Cofac(0, 0, 1, 1) * s);
01055 }
01056
01057
01058 void TMatrix3x3::Invert()
01059 {
01060
01061 *this = Inverse();
01062 }
01063
01064
01065 TVector3 operator * (const TMatrix3x3& m, const TVector3& v)
01066 {
01067 return TVector3(dot(m[0], v), dot(m[1], v), dot(m[2], v));
01068 }
01069
01070
01071 TVector3 operator * (const TVector3& v, const TMatrix3x3& m)
01072 {
01073 return TVector3(m.Tdot(0, v), m.Tdot(1, v), m.Tdot(2, v));
01074 }
01075
01076
01077 TMatrix3x3 operator * (const TMatrix3x3 &m1, const TMatrix3x3 &m2)
01078 {
01079 return TMatrix3x3(m2.Tdot(0, m1[0]), m2.Tdot(1, m1[0]), m2.Tdot(2, m1[0]),
01080 m2.Tdot(0, m1[1]), m2.Tdot(1, m1[1]), m2.Tdot(2, m1[1]),
01081 m2.Tdot(0, m1[2]), m2.Tdot(1, m1[2]), m2.Tdot(2, m1[2]));
01082 }
01083
01084
01085 TMatrix3x3 mmult_transpose_left(const TMatrix3x3 &m1, const TMatrix3x3 &m2)
01086 {
01087 return TMatrix3x3(m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
01088 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
01089 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
01090 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
01091 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
01092 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
01093 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
01094 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
01095 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
01096 }
01097
01098
01099 TMatrix3x3 mmult_transpose_right(const TMatrix3x3& m1, const TMatrix3x3& m2)
01100 {
01101 return TMatrix3x3(m1[0].Dot(m2[0]), m1[0].Dot(m2[1]), m1[0].Dot(m2[2]),
01102 m1[1].Dot(m2[0]), m1[1].Dot(m2[1]), m1[1].Dot(m2[2]),
01103 m1[2].Dot(m2[0]), m1[2].Dot(m2[1]), m1[2].Dot(m2[2]));
01104 }
01105
01106 class TLine3 {
01107 private :
01108 Bool_t fBounds[2];
01109 Double_t fParams[2];
01110 TPoint3 fOrigin;
01111 TVector3 fDir;
01112
01113 public :
01114 TLine3();
01115 TLine3(const TPoint3 &p1, const TPoint3 &p2);
01116 TLine3(const TPoint3 &p1, const TVector3 &v);
01117 TLine3(const TPoint3 &p1, const TVector3 &v, Bool_t bound1, Bool_t bound2);
01118
01119 static TLine3 InfiniteRay(const TPoint3 &p1, const TVector3 &v);
01120 const TVector3 &Direction()const {return fDir;}
01121 const TPoint3 &Origin()const{ return fOrigin;}
01122
01123 Bool_t Bounds(Int_t i)const
01124 {
01125 return (i == 0 ? fBounds[0] : fBounds[1]);
01126 }
01127 Bool_t &Bounds(Int_t i)
01128 {
01129 return (i == 0 ? fBounds[0] : fBounds[1]);
01130 }
01131 const Double_t &Param(Int_t i)const
01132 {
01133 return (i == 0 ? fParams[0] : fParams[1]);
01134 }
01135 Double_t &Param(Int_t i)
01136 {
01137 return (i == 0 ? fParams[0] : fParams[1]);
01138 }
01139 TVector3 UnboundSmallestVector(const TPoint3 &point)const
01140 {
01141 TVector3 diff(fOrigin - point);
01142 return diff - fDir * diff.Dot(fDir);
01143 }
01144 Double_t UnboundClosestParameter(const TPoint3 &point)const
01145 {
01146 TVector3 diff(fOrigin-point);
01147 return diff.Dot(fDir);
01148 }
01149 Double_t UnboundDistance(const TPoint3& point)const
01150 {
01151 return UnboundSmallestVector(point).Length();
01152 }
01153 Bool_t IsParameterOnLine(const Double_t &t) const
01154 {
01155 return ((fParams[0] - epsilon < t) || (!fBounds[0])) && ((fParams[1] > t + epsilon) || (!fBounds[1]));
01156 }
01157 };
01158
01159
01160 TLine3::TLine3() : fOrigin(0,0,0), fDir(1,0,0)
01161 {
01162
01163 fBounds[0] = kFALSE; fBounds[1] = kFALSE;
01164 fParams[0] = 0; fParams[1] = 1;
01165 }
01166
01167
01168 TLine3::TLine3(const TPoint3 &p1, const TPoint3 &p2) : fOrigin(p1), fDir(p2-p1)
01169 {
01170
01171 fBounds[0] = kTRUE; fBounds[1] = kTRUE;
01172 fParams[0] = 0; fParams[1] = 1;
01173 }
01174
01175
01176 TLine3::TLine3(const TPoint3 &p1, const TVector3 &v): fOrigin(p1), fDir(v)
01177 {
01178
01179 fBounds[0] = kFALSE; fBounds[1] = kFALSE;
01180 fParams[0] = 0; fParams[1] = 1;
01181 }
01182
01183
01184 TLine3::TLine3(const TPoint3 &p1, const TVector3 &v, Bool_t bound1, Bool_t bound2)
01185 : fOrigin(p1), fDir(v)
01186 {
01187
01188 fBounds[0] = bound1; fBounds[1] = bound2;
01189 fParams[0] = 0; fParams[1] = 1;
01190 }
01191
01192 class TPlane3 : public Tuple4 {
01193 public :
01194 TPlane3(const TVector3 &a, const TVector3 &b, const TVector3 &c);
01195 TPlane3(const TVector3 &n, const TVector3 &p);
01196 TPlane3();
01197 TPlane3(const TPlane3 & p):Tuple4(p){}
01198
01199 TVector3 Normal()const;
01200 Double_t Scalar()const;
01201 void Invert();
01202 Double_t SignedDistance(const TVector3 &)const;
01203
01204 TPlane3 &operator = (const TPlane3 & rhs);
01205 };
01206
01207
01208 TPlane3::TPlane3(const TVector3 &a, const TVector3 &b, const TVector3 &c)
01209 {
01210
01211 TVector3 l1 = b-a;
01212 TVector3 l2 = c-b;
01213 TVector3 n = l1.Cross(l2);
01214 n = n.SafeNormalized();
01215 Double_t d = n.Dot(a);
01216 fCo[0] = n.X(); fCo[1] = n.Y(); fCo[2] = n.Z(); fCo[3] = -d;
01217 }
01218
01219
01220 TPlane3::TPlane3(const TVector3 &n, const TVector3 &p)
01221 {
01222
01223 TVector3 mn = n.SafeNormalized();
01224 Double_t md = mn.Dot(p);
01225 fCo[0] = mn.X(); fCo[1] = mn.Y(); fCo[2] = mn.Z(); fCo[3] = -md;
01226 }
01227
01228
01229 TPlane3::TPlane3() : Tuple4()
01230 {
01231
01232 fCo[0] = 1.; fCo[1] = 0.;
01233 fCo[2] = 0.; fCo[3] = 0.;
01234 }
01235
01236
01237 TVector3 TPlane3::Normal()const
01238 {
01239
01240 return TVector3(fCo[0],fCo[1],fCo[2]);
01241 }
01242
01243
01244 Double_t TPlane3::Scalar()const
01245 {
01246
01247 return fCo[3];
01248 }
01249
01250
01251 void TPlane3::Invert()
01252 {
01253
01254 fCo[0] = -fCo[0]; fCo[1] = -fCo[1]; fCo[2] = -fCo[2]; fCo[3] = -fCo[3];
01255 }
01256
01257
01258 TPlane3 &TPlane3::operator = (const TPlane3 &rhs)
01259 {
01260
01261 fCo[0] = rhs.fCo[0]; fCo[1] = rhs.fCo[1]; fCo[2] = rhs.fCo[2]; fCo[3] = rhs.fCo[3];
01262 return *this;
01263 }
01264
01265
01266 Double_t TPlane3::SignedDistance(const TVector3 &v)const
01267 {
01268
01269 return Normal().Dot(v) + fCo[3];
01270 }
01271
01272 class TBBox {
01273 friend Bool_t intersect(const TBBox &a, const TBBox &b);
01274
01275 private:
01276 TPoint3 fCenter;
01277 TVector3 fExtent;
01278
01279 public:
01280 TBBox(){}
01281 TBBox(const TPoint3 &mini, const TPoint3 &maxi){SetValue(mini,maxi);}
01282
01283 const TPoint3 &Center()const{return fCenter;}
01284 const TVector3 &Extent()const{return fExtent;}
01285 TPoint3 &Center(){return fCenter;}
01286 TVector3 &Extent(){return fExtent;}
01287
01288 void SetValue(const TPoint3 &mini,const TPoint3 &maxi)
01289 {
01290 fExtent = (maxi - mini) / 2;
01291 fCenter = mini + fExtent;
01292 }
01293 void Enclose(const TBBox &a, const TBBox &b)
01294 {
01295 TPoint3 lower(
01296 TMath::Min(a.Lower(0), b.Lower(0)),
01297 TMath::Min(a.Lower(1), b.Lower(1)),
01298 TMath::Min(a.Lower(2), b.Lower(2))
01299 );
01300 TPoint3 upper(
01301 TMath::Max(a.Upper(0), b.Upper(0)),
01302 TMath::Max(a.Upper(1), b.Upper(1)),
01303 TMath::Max(a.Upper(2), b.Upper(2))
01304 );
01305 SetValue(lower, upper);
01306 }
01307
01308 void SetEmpty()
01309 {
01310 fCenter.SetValue(0., 0., 0.);
01311 fExtent.SetValue(-infinity, -infinity, -infinity);
01312 }
01313 void Include(const TPoint3 &p)
01314 {
01315 TPoint3 lower(
01316 TMath::Min(Lower(0), p[0]),
01317 TMath::Min(Lower(1), p[1]),
01318 TMath::Min(Lower(2), p[2])
01319 );
01320 TPoint3 upper(
01321 TMath::Max(Upper(0), p[0]),
01322 TMath::Max(Upper(1), p[1]),
01323 TMath::Max(Upper(2), p[2])
01324 );
01325 SetValue(lower, upper);
01326 }
01327
01328 void Include(const TBBox &b)
01329 {
01330 Enclose(*this, b);
01331 }
01332 Double_t Lower(Int_t i)const
01333 {
01334 return fCenter[i] - fExtent[i];
01335 }
01336 Double_t Upper(Int_t i)const
01337 {
01338 return fCenter[i] + fExtent[i];
01339 }
01340 TPoint3 Lower()const
01341 {
01342 return fCenter - fExtent;
01343 }
01344 TPoint3 Upper()const
01345 {
01346 return fCenter + fExtent;
01347 }
01348 Double_t Size()const
01349 {
01350 return TMath::Max(TMath::Max(fExtent[0], fExtent[1]), fExtent[2]);
01351 }
01352 Int_t LongestAxis()const
01353 {
01354 return fExtent.ClosestAxis();
01355 }
01356 Bool_t IntersectXRay(const TPoint3 &xBase)const
01357 {
01358 if (xBase[0] <= Upper(0)) {
01359 if (xBase[1] <= Upper(1) && xBase[1] >= Lower(1)) {
01360 if (xBase[2] <= Upper(2) && xBase[2] >= Lower(2)) {return kTRUE;}
01361 }
01362 }
01363 return kFALSE;
01364 }
01365 };
01366
01367
01368 Bool_t intersect(const TBBox &a, const TBBox &b)
01369 {
01370 return TMath::Abs(a.fCenter[0] - b.fCenter[0]) <= a.fExtent[0] + b.fExtent[0] &&
01371 TMath::Abs(a.fCenter[1] - b.fCenter[1]) <= a.fExtent[1] + b.fExtent[1] &&
01372 TMath::Abs(a.fCenter[2] - b.fCenter[2]) <= a.fExtent[2] + b.fExtent[2];
01373 }
01374
01375 class TBBoxNode {
01376 public:
01377 enum ETagType {kLeaf, kInternal};
01378 TBBox fBBox;
01379 ETagType fTag;
01380 };
01381
01382 class TBBoxLeaf : public TBBoxNode {
01383 public:
01384 Int_t fPolyIndex;
01385
01386 TBBoxLeaf() : fPolyIndex(0) {}
01387 TBBoxLeaf(Int_t polyIndex, const TBBox &bbox) : fPolyIndex(polyIndex)
01388 {
01389 fBBox = bbox;
01390 fTag = kLeaf;
01391 }
01392 };
01393
01394 typedef TBBoxLeaf *LeafPtr_t;
01395 typedef TBBoxNode *NodePtr_t;
01396
01397 class TBBoxInternal : public TBBoxNode {
01398 public:
01399 NodePtr_t fLeftSon;
01400 NodePtr_t fRightSon;
01401 TBBoxInternal() : fLeftSon(0) ,fRightSon(0) {}
01402 TBBoxInternal(Int_t n, LeafPtr_t leafIt);
01403 };
01404
01405 typedef TBBoxInternal* InternalPtr_t;
01406
01407 class TBBoxTree {
01408 private:
01409 Int_t fBranch;
01410 LeafPtr_t fLeaves;
01411 InternalPtr_t fInternals;
01412 Int_t fNumLeaves;
01413
01414 public :
01415 TBBoxTree() : fBranch(0), fLeaves(0), fInternals(0), fNumLeaves(0) {}
01416 NodePtr_t RootNode()const{return fInternals;}
01417 ~TBBoxTree()
01418 {
01419 delete[] fLeaves;
01420 delete[] fInternals;
01421 }
01422 void BuildTree(LeafPtr_t leaves, Int_t numLeaves);
01423
01424 private :
01425 void RecursiveTreeBuild(Int_t n, LeafPtr_t leafIt);
01426 };
01427
01428
01429 TBBoxInternal::TBBoxInternal(Int_t n, LeafPtr_t leafIt) :
01430 fLeftSon(0) ,fRightSon(0)
01431 {
01432
01433 fTag = kInternal;
01434 fBBox.SetEmpty();
01435 for (Int_t i=0;i<n;i++)
01436 fBBox.Include(leafIt[i].fBBox);
01437 }
01438
01439
01440 void TBBoxTree::BuildTree(LeafPtr_t leaves, Int_t numLeaves)
01441 {
01442
01443 fBranch = 0;
01444 fLeaves = leaves;
01445 fNumLeaves = numLeaves;
01446 fInternals = new TBBoxInternal[numLeaves];
01447 RecursiveTreeBuild(fNumLeaves,fLeaves);
01448 }
01449
01450
01451 void TBBoxTree::RecursiveTreeBuild(Int_t n, LeafPtr_t leafIt)
01452 {
01453
01454 fInternals[fBranch] = TBBoxInternal(n,leafIt);
01455 TBBoxInternal &aBBox = fInternals[fBranch];
01456 fBranch++;
01457
01458 Int_t axis = aBBox.fBBox.LongestAxis();
01459 Int_t i = 0, mid = n;
01460
01461 while (i < mid) {
01462 if (leafIt[i].fBBox.Center()[axis] < aBBox.fBBox.Center()[axis]) {
01463 ++i;
01464 } else {
01465 --mid;
01466 std::swap(leafIt[i], leafIt[mid]);
01467 }
01468 }
01469
01470 if (mid == 0 || mid == n) {
01471 mid = n / 2;
01472 }
01473 if (mid >= 2) {
01474 aBBox.fRightSon = fInternals + fBranch;
01475 RecursiveTreeBuild(mid,leafIt);
01476 } else {
01477 aBBox.fRightSon = leafIt;
01478 }
01479 if (n - mid >= 2) {
01480 aBBox.fLeftSon = fInternals + fBranch;
01481 RecursiveTreeBuild(n - mid, leafIt + mid);
01482 } else {
01483 aBBox.fLeftSon = leafIt + mid;
01484 }
01485 }
01486
01487 class TBlenderVProp {
01488 private:
01489 Int_t fVertexIndex;
01490
01491 public:
01492 TBlenderVProp(Int_t vIndex) : fVertexIndex(vIndex){}
01493 TBlenderVProp(Int_t vIndex, const TBlenderVProp &,
01494 const TBlenderVProp &, const Double_t &)
01495 {
01496 fVertexIndex = vIndex;
01497 }
01498 TBlenderVProp() : fVertexIndex(-1){}
01499 operator Int_t()const
01500 {
01501 return fVertexIndex;
01502 }
01503 TBlenderVProp &operator = (Int_t i)
01504 {
01505 fVertexIndex = i; return *this;
01506 }
01507 };
01508
01509 template <class TMesh>
01510 class TPolygonGeometry {
01511 public:
01512 typedef typename TMesh::Polygon TPolygon;
01513
01514 private:
01515 const TMesh &fMesh;
01516 const TPolygon &fPoly;
01517 public:
01518 TPolygonGeometry(const TMesh &mesh, Int_t pIndex)
01519 : fMesh(mesh), fPoly(mesh.Polys()[pIndex])
01520 {}
01521 TPolygonGeometry(const TMesh &mesh, const TPolygon &poly)
01522 : fMesh(mesh), fPoly(poly)
01523 {}
01524 const TPoint3 &operator [] (Int_t i)const
01525 {
01526 return fMesh.Verts()[fPoly[i]].Pos();
01527 }
01528 Int_t Size()const
01529 {
01530 return fPoly.Size();
01531 }
01532
01533 private:
01534 TPolygonGeometry(const TPolygonGeometry &);
01535 TPolygonGeometry& operator = (TPolygonGeometry &);
01536 };
01537
01538 template <class TPolygon, class TVertex>
01539 class TMesh : public TBaseMesh {
01540 public:
01541 typedef std::vector<TVertex> VLIST;
01542 typedef std::vector<TPolygon> PLIST;
01543 typedef TPolygon Polygon;
01544 typedef TVertex Vertex;
01545 typedef TPolygonGeometry<TMesh> TGBinder;
01546
01547 private:
01548 VLIST fVerts;
01549 PLIST fPolys;
01550
01551 public:
01552 VLIST &Verts(){return fVerts;}
01553 const VLIST &Verts()const{return fVerts;}
01554 PLIST &Polys(){return fPolys;}
01555 const PLIST &Polys()const{return fPolys;}
01556
01557
01558 UInt_t NumberOfPolys()const{return fPolys.size();}
01559 UInt_t NumberOfVertices()const{return fVerts.size();}
01560 UInt_t SizeOfPoly(UInt_t polyIndex)const{return fPolys[polyIndex].Size();}
01561 const Double_t *GetVertex(UInt_t vertexNum)const{return fVerts[vertexNum].GetValue();}
01562
01563 Int_t GetVertexIndex(UInt_t polyNum, UInt_t vertexNum)const
01564 {
01565 return fPolys[polyNum][vertexNum];
01566 }
01567 };
01568
01569 const Int_t cofacTable[3][2] = {{1,2}, {0,2}, {0,1}};
01570
01571
01572 Bool_t intersect(const TPlane3 &p1, const TPlane3 &p2, TLine3 &output)
01573 {
01574 TMatrix3x3 mat;
01575 mat[0] = p1.Normal();
01576 mat[1] = p2.Normal();
01577 mat[2] = mat[0].Cross(mat[1]);
01578 if (mat[2].FuzzyZero()) return kFALSE;
01579 TVector3 aPoint(-p1.Scalar(),-p2.Scalar(),0);
01580 output = TLine3(TPoint3(0., 0., 0.) + mat.Inverse() * aPoint ,mat[2]);
01581 return kTRUE;
01582 }
01583
01584
01585 Bool_t intersect_2d_no_bounds_check(const TLine3 &l1, const TLine3 &l2, Int_t majAxis,
01586 Double_t &l1Param, Double_t &l2Param)
01587 {
01588 Int_t ind1 = cofacTable[majAxis][0];
01589 Int_t ind2 = cofacTable[majAxis][1];
01590 Double_t zX = l2.Origin()[ind1] - l1.Origin()[ind1];
01591 Double_t zY = l2.Origin()[ind2] - l1.Origin()[ind2];
01592 Double_t det = l1.Direction()[ind1]*l2.Direction()[ind2] -
01593 l2.Direction()[ind1]*l1.Direction()[ind2];
01594 if (fuzzy_zero(det)) return kFALSE;
01595 l1Param = (l2.Direction()[ind2] * zX - l2.Direction()[ind1] * zY)/det;
01596 l2Param = -(-l1.Direction()[ind2] * zX + l1.Direction()[ind1] * zY)/det;
01597 return kTRUE;
01598 }
01599
01600
01601 Bool_t intersect_2d_bounds_check(const TLine3 &l1, const TLine3 &l2, Int_t majAxis,
01602 Double_t &l1Param, Double_t &l2Param)
01603 {
01604 Bool_t isect = intersect_2d_no_bounds_check(l1, l2, majAxis, l1Param, l2Param);
01605 if (!isect) return kFALSE;
01606 return l1.IsParameterOnLine(l1Param) && l2.IsParameterOnLine(l2Param);
01607 }
01608
01609
01610 Int_t compute_classification(const Double_t &distance, const Double_t &epsil)
01611 {
01612 if (TMath::Abs(distance) < epsil) return 0;
01613 else return distance < 0 ? 1 : 2;
01614 }
01615
01616
01617 template<typename TGBinder>
01618 Bool_t intersect_poly_with_line_2d(const TLine3 &l, const TGBinder &p1, const TPlane3 &plane,
01619 Double_t &a, Double_t &b)
01620 {
01621 Int_t majAxis = plane.Normal().ClosestAxis();
01622 Int_t lastInd = p1.Size()-1;
01623 b = (-infinity); a = (infinity);
01624 Double_t isectParam(0.), isectParam2(0.);
01625 Int_t i;
01626 Int_t j = lastInd;
01627 Int_t isectsFound(0);
01628 for (i=0;i<=lastInd; j=i,i++ ) {
01629 TLine3 testLine(p1[j],p1[i]);
01630 if (intersect_2d_bounds_check(l, testLine, majAxis, isectParam, isectParam2)) {
01631 ++isectsFound;
01632 b = TMath::Max(isectParam, b);
01633 a = TMath::Min(isectParam, a);
01634 }
01635 }
01636
01637 return (isectsFound > 0);
01638 }
01639
01640
01641 template<typename TGBinder>
01642 Bool_t instersect_poly_with_line_3d(const TLine3 &l, const TGBinder &p1,
01643 const TPlane3 &plane, Double_t &a)
01644 {
01645 Double_t determinant = l.Direction().Dot(plane.Normal());
01646 if (fuzzy_zero(determinant)) return kFALSE;
01647 a = -plane.Scalar() - plane.Normal().Dot(l.Origin());
01648 a /= determinant;
01649 if (a <= 0 ) return kFALSE;
01650 if (!l.IsParameterOnLine(a)) return kFALSE;
01651 TPoint3 pointOnPlane = l.Origin() + l.Direction() * a;
01652 return point_in_polygon_test_3d(p1, plane, l.Origin(), pointOnPlane);
01653 }
01654
01655
01656 template<typename TGBinder>
01657 Bool_t point_in_polygon_test_3d(const TGBinder& p1, const TPlane3& plane, const TPoint3& origin,
01658 const TPoint3 &pointOnPlane)
01659 {
01660 Bool_t discardSign = plane.SignedDistance(origin) < 0 ? kTRUE : kFALSE;
01661 const Int_t polySize = p1.Size();
01662 TPoint3 lastPoint = p1[polySize-1];
01663 for (Int_t i=0;i<polySize; ++i) {
01664 const TPoint3& aPoint = p1[i];
01665 TPlane3 testPlane(origin, lastPoint, aPoint);
01666 if ((testPlane.SignedDistance(pointOnPlane) <= 0) == discardSign) return kFALSE;
01667 lastPoint = aPoint;
01668 }
01669
01670 return kTRUE;
01671 }
01672
01673
01674 template <typename TGBinder>
01675 TPoint3 polygon_mid_point(const TGBinder &p1)
01676 {
01677 TPoint3 midPoint(0., 0., 0.);
01678 Int_t i;
01679 for (i=0; i < p1.Size(); i++)
01680 midPoint += p1[i];
01681 return TPoint3(midPoint[0] / i, midPoint[1] / i, midPoint[2] / i);
01682 }
01683
01684
01685 template <typename TGBinder>
01686 Int_t which_side(const TGBinder &p1, const TPlane3 &plane1)
01687 {
01688 Int_t output = 0;
01689 Int_t i;
01690 for (i=0; i<p1.Size(); i++) {
01691 Double_t signedDistance = plane1.SignedDistance(p1[i]);
01692 if (!fuzzy_zero(signedDistance))
01693 signedDistance < 0 ? (output |= 1) : (output |=2);
01694 }
01695
01696 return output;
01697 }
01698
01699
01700 template <typename TGBinder>
01701 TLine3 polygon_mid_point_ray(const TGBinder &p1, const TPlane3 &plane)
01702 {
01703 return TLine3(polygon_mid_point(p1),plane.Normal(),kTRUE,kFALSE);
01704 }
01705
01706
01707 template <typename TGBinder>
01708 TPlane3 compute_plane(const TGBinder &poly)
01709 {
01710 TPoint3 plast(poly[poly.Size()-1]);
01711 TPoint3 pivot;
01712 TVector3 edge;
01713 Int_t j;
01714 for (j = 0; j < poly.Size(); j++) {
01715 pivot = poly[j];
01716 edge = pivot - plast;
01717 if (!edge.FuzzyZero()) break;
01718 }
01719 for (; j < poly.Size(); j++) {
01720 TVector3 v2 = poly[j] - pivot;
01721 TVector3 v3 = edge.Cross(v2);
01722 if (!v3.FuzzyZero())
01723 return TPlane3(v3,pivot);
01724 }
01725
01726 return TPlane3();
01727 }
01728
01729
01730 template <typename TGBinder>
01731 TBBox fit_bbox(const TGBinder &p1)
01732 {
01733 TBBox bbox; bbox.SetEmpty();
01734 for (Int_t i = 0; i < p1.Size(); ++i)
01735 bbox.Include(p1[i]);
01736 return bbox;
01737 }
01738
01739
01740 template<typename TGBinderA, typename TGBinderB>
01741 Bool_t intersect_polygons (const TGBinderA &p1, const TGBinderB &p2,
01742 const TPlane3 &plane1, const TPlane3 &plane2)
01743 {
01744 TLine3 intersectLine;
01745 if (!intersect(plane1, plane2, intersectLine))
01746 return kFALSE;
01747 Double_t p1A, p1B;
01748 Double_t p2A, p2B;
01749 if (
01750 !intersect_poly_with_line_2d(intersectLine,p1,plane1,p1A,p1B) ||
01751 !intersect_poly_with_line_2d(intersectLine,p2,plane2,p2A,p2B))
01752 {
01753 return kFALSE;
01754 }
01755 Double_t maxOMin = TMath::Max(p1A,p2A);
01756 Double_t minOMax = TMath::Min(p1B,p2B);
01757 return (maxOMin <= minOMax);
01758 }
01759
01760 template <class TMesh, class TSplitFunctionBinder>
01761 class TSplitFunction {
01762 private:
01763 TMesh &fMesh;
01764 TSplitFunctionBinder &fFunctionBinder;
01765
01766 public:
01767 TSplitFunction(TMesh &mesh, TSplitFunctionBinder &functionBindor)
01768 : fMesh(mesh), fFunctionBinder(functionBindor)
01769 {}
01770 void SplitPolygon(const Int_t p1Index, const TPlane3 &plane,
01771 Int_t &inPiece, Int_t &outPiece,
01772 const Double_t onEpsilon)
01773 {
01774 const typename TMesh::Polygon &p = fMesh.Polys()[p1Index];
01775 typename TMesh::Polygon inP(p),outP(p);
01776 inP.Verts().clear();
01777 outP.Verts().clear();
01778 fFunctionBinder.DisconnectPolygon(p1Index);
01779 Int_t lastIndex = p.Verts().back();
01780 TPoint3 lastVertex = fMesh.Verts()[lastIndex].Pos();
01781 Int_t lastClassification = compute_classification(plane.SignedDistance(lastVertex),onEpsilon);
01782 Int_t totalClassification(lastClassification);
01783 Int_t i;
01784 Int_t j=p.Size()-1;
01785 for (i = 0; i < p.Size(); j = i, ++i)
01786 {
01787 Int_t newIndex = p[i];
01788 TPoint3 aVertex = fMesh.Verts()[newIndex].Pos();
01789 Int_t newClassification = compute_classification(plane.SignedDistance(aVertex),onEpsilon);
01790 if ((newClassification != lastClassification) && newClassification && lastClassification)
01791 {
01792 Int_t newVertexIndex = fMesh.Verts().size();
01793 typedef typename TMesh::Vertex VERTEX_t;
01794 fMesh.Verts().push_back(VERTEX_t());
01795 TVector3 v = aVertex - lastVertex;
01796 Double_t sideA = plane.SignedDistance(lastVertex);
01797 Double_t epsil = -sideA / plane.Normal().Dot(v);
01798 fMesh.Verts().back().Pos() = lastVertex + (v * epsil);
01799 typename TMesh::Polygon::TVProp splitProp(newVertexIndex,p.VertexProps(j),p.VertexProps(i),epsil);
01800 inP.Verts().push_back( splitProp );
01801 outP.Verts().push_back( splitProp );
01802 fFunctionBinder.InsertVertexAlongEdge(lastIndex,newIndex,splitProp);
01803 }
01804 Classify(inP.Verts(),outP.Verts(),newClassification, p.VertexProps(i));
01805 lastClassification = newClassification;
01806 totalClassification |= newClassification;
01807 lastVertex = aVertex;
01808 lastIndex = newIndex;
01809 }
01810 if (totalClassification == 3) {
01811 inPiece = p1Index;
01812 outPiece = fMesh.Polys().size();
01813 fMesh.Polys()[p1Index] = inP;
01814 fMesh.Polys().push_back(outP);
01815 fFunctionBinder.ConnectPolygon(inPiece);
01816 fFunctionBinder.ConnectPolygon(outPiece);
01817 } else {
01818 fFunctionBinder.ConnectPolygon(p1Index);
01819 if (totalClassification == 1) {
01820 inPiece = p1Index;
01821 outPiece = -1;
01822 } else {
01823 outPiece = p1Index;
01824 inPiece = -1;
01825 }
01826 }
01827 }
01828
01829 void Classify(typename TMesh::Polygon::TVPropList &inGroup,
01830 typename TMesh::Polygon::TVPropList &outGroup,
01831 Int_t classification,
01832 typename TMesh::Polygon::TVProp prop)
01833 {
01834 switch (classification) {
01835 case 0 :
01836 inGroup.push_back(prop);
01837 outGroup.push_back(prop);
01838 break;
01839 case 1 :
01840 inGroup.push_back(prop);
01841 break;
01842 case 2 :
01843 outGroup.push_back(prop);
01844 break;
01845 default :
01846 break;
01847 }
01848 }
01849
01850 };
01851
01852 template <typename PROP>
01853 class TDefaultSplitFunctionBinder {
01854 public :
01855 void DisconnectPolygon(Int_t){}
01856 void ConnectPolygon(Int_t){}
01857 void InsertVertexAlongEdge(Int_t, Int_t, const PROP &){}
01858 };
01859
01860 template <typename TMesh>
01861 class TMeshWrapper {
01862 private :
01863 TMesh &fMesh;
01864
01865 public:
01866 typedef typename TMesh::Polygon Polygon;
01867 typedef typename TMesh::Vertex Vertex;
01868 typedef typename TMesh::VLIST VLIST;
01869 typedef typename TMesh::PLIST PLIST;
01870 typedef TPolygonGeometry<TMeshWrapper> TGBinder;
01871 typedef TMeshWrapper<TMesh> MyType;
01872
01873 public:
01874 TMeshWrapper(TMesh &mesh):fMesh(mesh){}
01875
01876 VLIST &Verts(){return fMesh.Verts();}
01877 const VLIST &Verts()const{return fMesh.Verts();}
01878 PLIST &Polys(){return fMesh.Polys();}
01879 const PLIST &Polys() const {return fMesh.Polys();}
01880
01881 void ComputePlanes();
01882 TBBox ComputeBBox()const;
01883 void SplitPolygon(Int_t p1Index, const TPlane3 &plane,
01884 Int_t &inPiece, Int_t &outPiece, Double_t onEpsilon);
01885 };
01886
01887
01888 template <typename TMesh>
01889 void TMeshWrapper<TMesh>::ComputePlanes()
01890 {
01891
01892 PLIST& polyList = Polys();
01893 UInt_t i;
01894 for (i=0;i < polyList.size(); i++) {
01895 TGBinder binder(fMesh,i);
01896 polyList[i].SetPlane(compute_plane(binder));
01897 }
01898 }
01899
01900
01901 template <typename TMesh>
01902 TBBox TMeshWrapper<TMesh>::ComputeBBox()const
01903 {
01904
01905 const VLIST &vertexList = Verts();
01906 TBBox bbox;
01907 bbox.SetEmpty();
01908 Int_t i;
01909 for (i=0;i<vertexList.size(); i++)
01910 bbox.Include(vertexList[i].Pos());
01911 return bbox;
01912 }
01913
01914
01915 template<typename TMesh>
01916 void TMeshWrapper<TMesh>::SplitPolygon(Int_t p1Index, const TPlane3 &plane,
01917 Int_t &inPiece, Int_t &outPiece,
01918 Double_t onEpsilon)
01919 {
01920 typedef typename TMesh::Polygon::TVProp mesh;
01921 TDefaultSplitFunctionBinder<mesh> defaultSplitFunction;
01922 TSplitFunction<MyType,TDefaultSplitFunctionBinder<mesh> >
01923 splitFunction(*this,defaultSplitFunction);
01924 splitFunction.SplitPolygon(p1Index,plane,inPiece,outPiece,onEpsilon);
01925 }
01926
01927 template <typename AVProp, typename AFProp>
01928 class TPolygonBase {
01929 public:
01930 typedef AVProp TVProp;
01931 typedef AFProp TFProp;
01932 typedef std::vector<TVProp> TVPropList;
01933 typedef typename TVPropList::iterator TVPropIt;
01934
01935 private :
01936 TVPropList fVerts;
01937 TPlane3 fPlane;
01938 TFProp fFaceProp;
01939 Int_t fClassification;
01940
01941 public:
01942 const TVPropList &Verts()const{return fVerts;}
01943 TVPropList &Verts(){return fVerts;}
01944 Int_t Size()const{return Int_t(fVerts.size());}
01945
01946 Int_t operator[](Int_t i) const {return fVerts[i];}
01947
01948 const TVProp &VertexProps(Int_t i)const{return fVerts[i];}
01949 TVProp &VertexProps(Int_t i){return fVerts[i];}
01950 void SetPlane(const TPlane3 &plane){fPlane = plane;}
01951 const TPlane3 &Plane()const{return fPlane;}
01952 TVector3 Normal()const{return fPlane.Normal();}
01953 Int_t &Classification(){ return fClassification;}
01954 const Int_t &Classification()const{return fClassification;}
01955
01956 void Reverse()
01957 {
01958 std::reverse(fVerts.begin(),fVerts.end());
01959 fPlane.Invert();
01960 }
01961
01962 TFProp &FProp(){return fFaceProp;}
01963 const TFProp &FProp()const{return fFaceProp;}
01964 void AddProp(const TVProp &prop){fVerts.push_back(prop);}
01965 };
01966
01967 typedef std::vector<Int_t> PIndexList_t;
01968 typedef PIndexList_t::iterator PIndexIt_t;
01969 typedef std::vector< PIndexList_t > OverlapTable_t;
01970
01971 template <typename TMesh>
01972 class TreeIntersector {
01973 private:
01974 OverlapTable_t *fAoverlapsB;
01975 OverlapTable_t *fBoverlapsA;
01976 const TMesh *fMeshA;
01977 const TMesh *fMeshB;
01978
01979 public :
01980 TreeIntersector(const TBBoxTree &a, const TBBoxTree &b,
01981 OverlapTable_t *aOverlapsB, OverlapTable_t *bOverlapsA,
01982 const TMesh *meshA, const TMesh *meshB)
01983 {
01984 fAoverlapsB = aOverlapsB;
01985 fBoverlapsA = bOverlapsA;
01986 fMeshA = meshA;
01987 fMeshB = meshB;
01988 MarkIntersectingPolygons(a.RootNode(),b.RootNode());
01989 }
01990
01991 private :
01992 void MarkIntersectingPolygons(const TBBoxNode *a, const TBBoxNode *b)
01993 {
01994 if (!intersect(a->fBBox, b->fBBox)) return;
01995 if (a->fTag == TBBoxNode::kLeaf && b->fTag == TBBoxNode::kLeaf) {
01996 const TBBoxLeaf *la = (const TBBoxLeaf *)a;
01997 const TBBoxLeaf *lb = (const TBBoxLeaf *)b;
01998
01999 TPolygonGeometry<TMesh> pg1(*fMeshA,la->fPolyIndex);
02000 TPolygonGeometry<TMesh> pg2(*fMeshB,lb->fPolyIndex);
02001
02002 if (intersect_polygons(pg1, pg2, fMeshA->Polys()[la->fPolyIndex].Plane(),
02003 fMeshB->Polys()[lb->fPolyIndex].Plane())) {
02004 (*fAoverlapsB)[lb->fPolyIndex].push_back(la->fPolyIndex);
02005 (*fBoverlapsA)[la->fPolyIndex].push_back(lb->fPolyIndex);
02006 }
02007 } else if ( a->fTag == TBBoxNode::kLeaf || (b->fTag != TBBoxNode::kLeaf && a->fBBox.Size() < b->fBBox.Size()))
02008 {
02009 MarkIntersectingPolygons(a, ((const TBBoxInternal *)b)->fLeftSon);
02010 MarkIntersectingPolygons(a, ((const TBBoxInternal *)b)->fRightSon);
02011 } else {
02012 MarkIntersectingPolygons(((const TBBoxInternal *)a)->fLeftSon, b);
02013 MarkIntersectingPolygons(((const TBBoxInternal *)a)->fRightSon, b);
02014 }
02015 }
02016 };
02017
02018 template<typename TMesh>
02019 class TRayTreeIntersector {
02020 private:
02021 const TMesh *fMeshA;
02022 Double_t fLastIntersectValue;
02023 Int_t fPolyIndex;
02024
02025 public:
02026 TRayTreeIntersector(const TBBoxTree &a, const TMesh *meshA, const TLine3 &xRay, Int_t &polyIndex)
02027 : fMeshA(meshA), fLastIntersectValue(infinity), fPolyIndex(-1)
02028 {
02029 FindIntersectingPolygons(a.RootNode(),xRay);
02030 polyIndex = fPolyIndex;
02031 }
02032
02033 private :
02034 void FindIntersectingPolygons(const TBBoxNode*a,const TLine3& xRay)
02035 {
02036 if ((xRay.Origin().X() + fLastIntersectValue < a->fBBox.Lower(0)) ||!a->fBBox.IntersectXRay(xRay.Origin()))
02037 return;
02038 if (a->fTag == TBBoxNode::kLeaf) {
02039 const TBBoxLeaf *la = (const TBBoxLeaf *)a;
02040 Double_t testParameter(0.);
02041 TPolygonGeometry<TMesh> pg(*fMeshA, la->fPolyIndex);
02042 if (instersect_poly_with_line_3d(xRay,pg,fMeshA->Polys()[la->fPolyIndex].Plane(),testParameter))
02043 {
02044 if (testParameter < fLastIntersectValue) {
02045 fLastIntersectValue = testParameter;
02046 fPolyIndex = la->fPolyIndex;
02047 }
02048 }
02049 } else {
02050 FindIntersectingPolygons(((const TBBoxInternal*)a)->fLeftSon, xRay);
02051 FindIntersectingPolygons(((const TBBoxInternal*)a)->fRightSon, xRay);
02052 }
02053 }
02054 };
02055
02056 class TVertexBase {
02057 protected:
02058 Int_t fVertexMap;
02059 TPoint3 fPos;
02060
02061 public:
02062 TVertexBase(Double_t x, Double_t y, Double_t z) : fVertexMap(-1), fPos(x, y, z){}
02063 TVertexBase():fVertexMap(-1) {}
02064
02065 const TPoint3 &Pos()const{return fPos;}
02066 TPoint3 &Pos(){return fPos;}
02067 Int_t &VertexMap(){return fVertexMap;}
02068 const Int_t &VertexMap()const{return fVertexMap;}
02069 const Double_t * GetValue()const{return fPos.GetValue();}
02070
02071 Double_t operator [] (Int_t ind)const{return fPos[ind];}
02072 };
02073
02074 class TCVertex : public TVertexBase {
02075 private:
02076 PIndexList_t fPolygons;
02077 public:
02078 TCVertex() : TVertexBase(){}
02079 TCVertex(const TVertexBase& vertex) : TVertexBase(vertex){}
02080
02081 TCVertex &operator = (const TVertexBase &other)
02082 {
02083 fPos= other.Pos();
02084 return *this;
02085 }
02086 const PIndexList_t &Polys()const{return fPolygons;}
02087 PIndexList_t &Polys(){return fPolygons;}
02088
02089 Int_t &operator [] (Int_t i) { return fPolygons[i];}
02090 const Int_t &operator [] (Int_t i)const{return fPolygons[i];}
02091
02092 void AddPoly(Int_t polyIndex){fPolygons.push_back(polyIndex);}
02093 void RemovePolygon(Int_t polyIndex)
02094 {
02095 PIndexIt_t foundIt = std::find(fPolygons.begin(), fPolygons.end(), polyIndex);
02096 if (foundIt != fPolygons.end()) {
02097 std::swap(fPolygons.back(), *foundIt);
02098 fPolygons.pop_back();
02099 }
02100 }
02101 };
02102
02103 template <typename TMesh>
02104 class TConnectedMeshWrapper {
02105 private:
02106 TMesh &fMesh;
02107 UInt_t fUniqueEdgeTestId;
02108 public:
02109 typedef typename TMesh::Polygon Polygon;
02110 typedef typename TMesh::Vertex Vertex;
02111 typedef typename TMesh::Polygon::TVProp VProp;
02112 typedef typename TMesh::VLIST VLIST;
02113 typedef typename TMesh::PLIST PLIST;
02114 typedef TPolygonGeometry<TConnectedMeshWrapper> TGBinder;
02115 typedef TConnectedMeshWrapper<TMesh> MyType;
02116
02117 TConnectedMeshWrapper(TMesh &mesh) : fMesh(mesh), fUniqueEdgeTestId(0){}
02118
02119 VLIST &Verts(){return fMesh.Verts();}
02120 const VLIST &Verts()const{return fMesh.Verts();}
02121 PLIST &Polys() {return fMesh.Polys();}
02122 const PLIST &Polys() const {return fMesh.Polys();}
02123 void BuildVertexPolyLists();
02124 void DisconnectPolygon(Int_t polyIndex);
02125 void ConnectPolygon(Int_t polyIndex);
02126
02127 void EdgePolygons(Int_t v1, Int_t v2, PIndexList_t &polys);
02128 void InsertVertexAlongEdge(Int_t v1,Int_t v2, const VProp &prop);
02129 void SplitPolygon(Int_t p1Index, const TPlane3 &plane, Int_t &inPiece,
02130 Int_t &outPiece, Double_t onEpsilon);
02131 };
02132
02133 template <class CMesh> class TSplitFunctionBinder {
02134 private:
02135 CMesh &fMesh;
02136
02137 public:
02138 TSplitFunctionBinder(CMesh &mesh):fMesh(mesh){}
02139 void DisconnectPolygon(Int_t polyIndex){fMesh.DisconnectPolygon(polyIndex);}
02140 void ConnectPolygon(Int_t polygonIndex){fMesh.ConnectPolygon(polygonIndex);}
02141 void InsertVertexAlongEdge(Int_t lastIndex, Int_t newIndex, const typename CMesh::VProp &prop)
02142 {
02143 fMesh.InsertVertexAlongEdge(lastIndex, newIndex,prop);
02144 }
02145 };
02146
02147
02148 template <typename TMesh>
02149 void TConnectedMeshWrapper<TMesh>::BuildVertexPolyLists()
02150 {
02151
02152 UInt_t i;
02153 for (i=0; i < Polys().size(); i++)
02154 ConnectPolygon(i);
02155 }
02156
02157
02158 template <typename TMesh>
02159 void TConnectedMeshWrapper<TMesh>::DisconnectPolygon(Int_t polyIndex)
02160 {
02161
02162 const Polygon &poly = Polys()[polyIndex];
02163 UInt_t j;
02164 for (j=0;j<poly.Verts().size(); j++) {
02165 Verts()[poly[j]].RemovePolygon(polyIndex);
02166 }
02167 }
02168
02169
02170 template <typename TMesh>
02171 void TConnectedMeshWrapper<TMesh>::ConnectPolygon(Int_t polyIndex)
02172 {
02173
02174 const Polygon &poly = Polys()[polyIndex];
02175 UInt_t j;
02176 for (j=0;j<poly.Verts().size(); j++) {
02177 Verts()[poly[j]].AddPoly(polyIndex);
02178 }
02179 }
02180
02181
02182 template <typename TMesh>
02183 void TConnectedMeshWrapper<TMesh>::EdgePolygons(Int_t v1, Int_t v2, PIndexList_t &polys)
02184 {
02185
02186 ++fUniqueEdgeTestId;
02187 Vertex &vb1 = Verts()[v1];
02188 UInt_t i;
02189 for (i=0;i < vb1.Polys().size(); ++i){Polys()[vb1[i]].Classification() = fUniqueEdgeTestId;}
02190 Vertex &vb2 = Verts()[v2];
02191 UInt_t j;
02192 for (j=0;j < vb2.Polys().size(); ++j) {
02193 if ((UInt_t)Polys()[vb2[j]].Classification() == fUniqueEdgeTestId) {
02194 polys.push_back(vb2[j]);
02195 }
02196 }
02197 }
02198
02199
02200 template <typename TMesh>
02201 void TConnectedMeshWrapper<TMesh>::InsertVertexAlongEdge(Int_t v1, Int_t v2, const VProp &prop)
02202 {
02203
02204 PIndexList_t npolys;
02205 EdgePolygons(v1,v2,npolys);
02206 Int_t newVertex = Int_t(prop);
02207 UInt_t i;
02208 for (i=0;i < npolys.size(); i++) {
02209 typename Polygon::TVPropList& polyVerts = Polys()[npolys[i]].Verts();
02210 typename Polygon::TVPropIt v1pos = std::find(polyVerts.begin(),polyVerts.end(),v1);
02211 if (v1pos != polyVerts.end()) {
02212 typename Polygon::TVPropIt prevPos = (v1pos == polyVerts.begin()) ? polyVerts.end()-1 : v1pos-1;
02213 typename Polygon::TVPropIt nextPos = (v1pos == polyVerts.end()-1) ? polyVerts.begin() : v1pos+1;
02214 if (*prevPos == v2) {
02215 polyVerts.insert(v1pos,prop);
02216 } else if (*nextPos == v2) {
02217 polyVerts.insert(nextPos, prop);
02218 } else {
02219
02220 }
02221 Verts()[newVertex].AddPoly(npolys[i]);
02222 } else {
02223
02224 }
02225 }
02226 }
02227
02228
02229 template <typename TMesh>
02230 void TConnectedMeshWrapper<TMesh>::SplitPolygon(Int_t p1Index, const TPlane3 &plane,
02231 Int_t &inPiece, Int_t &outPiece,
02232 Double_t onEpsilon)
02233 {
02234 TSplitFunctionBinder<MyType> functionBindor(*this);
02235 TSplitFunction<MyType,TSplitFunctionBinder<MyType> > splitFunction(*this,functionBindor);
02236 splitFunction.SplitPolygon(p1Index, plane, inPiece, outPiece, onEpsilon);
02237 }
02238
02239 struct NullType_t{};
02240
02241
02242 typedef TPolygonBase<TBlenderVProp, NullType_t> TestPolygon_t;
02243 typedef TMesh<TestPolygon_t,TVertexBase> AMesh_t;
02244 typedef TMesh<TestPolygon_t,TCVertex > AConnectedMesh_t;
02245 typedef TMeshWrapper<AMesh_t> AMeshWrapper_t;
02246 typedef TConnectedMeshWrapper<AConnectedMesh_t> AConnectedMeshWrapper_t;
02247
02248
02249 template <class TMesh>
02250 void build_split_group(const TMesh &meshA, const TMesh &meshB,
02251 const TBBoxTree &treeA, const TBBoxTree &treeB,
02252 OverlapTable_t &aOverlapsB, OverlapTable_t &bOverlapsA)
02253 {
02254 aOverlapsB = OverlapTable_t(meshB.Polys().size());
02255 bOverlapsA = OverlapTable_t(meshA.Polys().size());
02256 TreeIntersector<TMesh>(treeA, treeB, &aOverlapsB, &bOverlapsA, &meshA, &meshB);
02257 }
02258
02259
02260 template <class CMesh, class TMesh>
02261 void partition_mesh(CMesh &mesh, const TMesh &mesh2, const OverlapTable_t &table)
02262 {
02263 UInt_t i;
02264 Double_t onEpsilon(1e-4);
02265 for (i = 0; i < table.size(); i++) {
02266 if (table[i].size()) {
02267 PIndexList_t fragments;
02268 fragments.push_back(i);
02269 UInt_t j;
02270 for (j =0 ; j <table[i].size(); ++j) {
02271 PIndexList_t newFragments;
02272 TPlane3 splitPlane = mesh2.Polys()[table[i][j]].Plane();
02273 UInt_t k;
02274 for (k = 0; k < fragments.size(); ++k) {
02275 Int_t newInFragment;
02276 Int_t newOutFragment;
02277 typename CMesh::TGBinder pg1(mesh,fragments[k]);
02278 typename TMesh::TGBinder pg2(mesh2,table[i][j]);
02279 const TPlane3 &fragPlane = mesh.Polys()[fragments[k]].Plane();
02280
02281 if (intersect_polygons(pg1,pg2,fragPlane,splitPlane)) {
02282 mesh.SplitPolygon(fragments[k], splitPlane, newInFragment, newOutFragment, onEpsilon);
02283 if (-1 != newInFragment) newFragments.push_back(newInFragment);
02284 if (-1 != newOutFragment) newFragments.push_back(newOutFragment);
02285 } else {
02286 newFragments.push_back(fragments[k]);
02287 }
02288 }
02289 fragments = newFragments;
02290 }
02291 }
02292 }
02293 }
02294
02295
02296 template <typename CMesh, typename TMesh>
02297 void classify_mesh(const TMesh &meshA, const TBBoxTree &aTree, CMesh &meshB)
02298 {
02299 UInt_t i;
02300 for (i = 0; i < meshB.Polys().size(); i++) {
02301 typename CMesh::TGBinder pg(meshB,i);
02302 TLine3 midPointRay = polygon_mid_point_ray(pg,meshB.Polys()[i].Plane());
02303 TLine3 midPointXRay(midPointRay.Origin(),TVector3(1,0,0));
02304 Int_t aPolyIndex(-1);
02305 TRayTreeIntersector<TMesh>(aTree,&meshA,midPointXRay,aPolyIndex);
02306 if (-1 != aPolyIndex) {
02307 if (meshA.Polys()[aPolyIndex].Plane().SignedDistance(midPointXRay.Origin()) < 0) {
02308 meshB.Polys()[i].Classification()= 1;
02309 } else {
02310 meshB.Polys()[i].Classification()= 2;
02311 }
02312 } else {
02313 meshB.Polys()[i].Classification()= 2;
02314 }
02315 }
02316 }
02317
02318
02319 template <typename CMesh, typename TMesh>
02320 void extract_classification(CMesh &meshA, TMesh &newMesh, Int_t classification, Bool_t reverse)
02321 {
02322 UInt_t i;
02323 for (i = 0; i < meshA.Polys().size(); ++i) {
02324 typename CMesh::Polygon &meshAPolygon = meshA.Polys()[i];
02325 if (meshAPolygon.Classification() == classification) {
02326 newMesh.Polys().push_back(meshAPolygon);
02327 typename TMesh::Polygon &newPolygon = newMesh.Polys().back();
02328 if (reverse) newPolygon.Reverse();
02329 Int_t j;
02330 for (j=0; j< newPolygon.Size(); j++) {
02331 if (meshA.Verts()[newPolygon[j]].VertexMap() == -1) {
02332 newMesh.Verts().push_back(meshA.Verts()[newPolygon[j]]);
02333 meshA.Verts()[newPolygon[j]].VertexMap() = newMesh.Verts().size() -1;
02334 }
02335 newPolygon.VertexProps(j) = meshA.Verts()[newPolygon[j]].VertexMap();
02336 }
02337 }
02338 }
02339 }
02340
02341
02342 template <typename MeshA, typename MeshB>
02343 void copy_mesh(const MeshA &source, MeshB &output)
02344 {
02345 Int_t vertexNum = source.Verts().size();
02346 Int_t polyNum = source.Polys().size();
02347
02348 typedef typename MeshB::VLIST VLIST_t;
02349 typedef typename MeshB::PLIST PLIST_t;
02350
02351 output.Verts() = VLIST_t(vertexNum);
02352 output.Polys() = PLIST_t(polyNum);
02353
02354 std::copy(source.Verts().begin(), source.Verts().end(), output.Verts().begin());
02355 std::copy(source.Polys().begin(), source.Polys().end(), output.Polys().begin());
02356 }
02357
02358
02359 void build_tree(const AMesh_t &mesh, TBBoxTree &tree)
02360 {
02361 Int_t numLeaves = mesh.Polys().size();
02362 TBBoxLeaf *aLeaves = new TBBoxLeaf[numLeaves];
02363 UInt_t i;
02364 for (i=0;i< mesh.Polys().size(); i++) {
02365 TPolygonGeometry<AMesh_t> pg(mesh,i);
02366 aLeaves[i] = TBBoxLeaf(i, fit_bbox(pg));
02367 }
02368 tree.BuildTree(aLeaves,numLeaves);
02369 }
02370
02371
02372 void extract_classification_preserve(const AMesh_t &meshA,
02373 const AMesh_t &meshB,
02374 const TBBoxTree &aTree,
02375 const TBBoxTree &bTree,
02376 const OverlapTable_t &aOverlapsB,
02377 const OverlapTable_t &bOverlapsA,
02378 Int_t aClassification,
02379 Int_t bClassification,
02380 Bool_t reverseA,
02381 Bool_t reverseB,
02382 AMesh_t &output)
02383 {
02384 AConnectedMesh_t meshAPartitioned;
02385 AConnectedMesh_t meshBPartitioned;
02386 copy_mesh(meshA,meshAPartitioned);
02387 copy_mesh(meshB,meshBPartitioned);
02388 AConnectedMeshWrapper_t meshAWrapper(meshAPartitioned);
02389 AConnectedMeshWrapper_t meshBWrapper(meshBPartitioned);
02390 meshAWrapper.BuildVertexPolyLists();
02391 meshBWrapper.BuildVertexPolyLists();
02392 partition_mesh(meshAWrapper, meshB, bOverlapsA);
02393 partition_mesh(meshBWrapper, meshA, aOverlapsB);
02394 classify_mesh(meshB, bTree, meshAPartitioned);
02395 classify_mesh(meshA, aTree, meshBPartitioned);
02396 extract_classification(meshAPartitioned, output, aClassification, reverseA);
02397 extract_classification(meshBPartitioned, output, bClassification, reverseB);
02398 }
02399
02400
02401 void extract_classification(const AMesh_t &meshA,
02402 const AMesh_t &meshB,
02403 const TBBoxTree &aTree,
02404 const TBBoxTree &bTree,
02405 const OverlapTable_t &aOverlapsB,
02406 const OverlapTable_t &bOverlapsA,
02407 Int_t aClassification,
02408 Int_t bClassification,
02409 Bool_t reverseA,
02410 Bool_t reverseB,
02411 AMesh_t &output)
02412 {
02413 AMesh_t meshAPartitioned(meshA);
02414 AMesh_t meshBPartitioned(meshB);
02415 AMeshWrapper_t meshAWrapper(meshAPartitioned);
02416 AMeshWrapper_t meshBWrapper(meshBPartitioned);
02417 partition_mesh(meshAWrapper, meshB, bOverlapsA);
02418 partition_mesh(meshBWrapper, meshA, aOverlapsB);
02419 classify_mesh(meshB, bTree, meshAPartitioned);
02420 classify_mesh(meshA, aTree, meshBPartitioned);
02421 extract_classification(meshAPartitioned, output, aClassification, reverseA);
02422 extract_classification(meshBPartitioned, output, bClassification, reverseB);
02423 }
02424
02425
02426 AMesh_t *build_intersection(const AMesh_t &meshA, const AMesh_t &meshB, Bool_t preserve)
02427 {
02428 TBBoxTree aTree, bTree;
02429 build_tree(meshA, aTree);
02430 build_tree(meshB, bTree);
02431 OverlapTable_t bOverlapsA(meshA.Polys().size());
02432 OverlapTable_t aOverlapsB(meshB.Polys().size());
02433 build_split_group(meshA, meshB, aTree, bTree, aOverlapsB, bOverlapsA);
02434 AMesh_t *output = new AMesh_t;
02435 if (preserve) {
02436 extract_classification_preserve(
02437 meshA, meshB, aTree, bTree,
02438 aOverlapsB, bOverlapsA,
02439 1, 1, kFALSE, kFALSE, *output
02440 );
02441 } else {
02442 extract_classification(
02443 meshA, meshB, aTree, bTree,
02444 aOverlapsB, bOverlapsA,
02445 1, 1, kFALSE, kFALSE, *output
02446 );
02447 }
02448 return output;
02449 }
02450
02451
02452 AMesh_t *build_union(const AMesh_t &meshA, const AMesh_t &meshB, Bool_t preserve)
02453 {
02454 TBBoxTree aTree, bTree;
02455 build_tree(meshA, aTree);
02456 build_tree(meshB, bTree);
02457 OverlapTable_t bOverlapsA(meshA.Polys().size());
02458 OverlapTable_t aOverlapsB(meshB.Polys().size());
02459 build_split_group(meshA, meshB, aTree, bTree, aOverlapsB, bOverlapsA);
02460 AMesh_t *output = new AMesh_t;
02461 if (preserve) {
02462 extract_classification_preserve(
02463 meshA, meshB, aTree, bTree,
02464 aOverlapsB, bOverlapsA,
02465 2, 2, kFALSE, kFALSE, *output
02466 );
02467 } else {
02468 extract_classification(
02469 meshA, meshB, aTree, bTree,
02470 aOverlapsB, bOverlapsA,
02471 2, 2, kFALSE, kFALSE, *output
02472 );
02473 }
02474 return output;
02475 }
02476
02477
02478 AMesh_t *build_difference(const AMesh_t &meshA, const AMesh_t &meshB, Bool_t preserve)
02479 {
02480 TBBoxTree aTree, bTree;
02481 build_tree(meshA, aTree);
02482 build_tree(meshB, bTree);
02483 OverlapTable_t bOverlapsA(meshA.Polys().size());
02484 OverlapTable_t aOverlapsB(meshB.Polys().size());
02485 build_split_group(meshA, meshB, aTree, bTree, aOverlapsB, bOverlapsA);
02486 AMesh_t *output = new AMesh_t;
02487 if (preserve) {
02488 extract_classification_preserve(
02489 meshA, meshB, aTree, bTree,
02490 aOverlapsB, bOverlapsA,
02491 2, 1, kFALSE, kTRUE, *output
02492 );
02493 } else {
02494 extract_classification(
02495 meshA, meshB, aTree, bTree,
02496 aOverlapsB, bOverlapsA,
02497 2, 1, kFALSE, kTRUE, *output
02498 );
02499 }
02500 return output;
02501 }
02502
02503
02504 TBaseMesh *ConvertToMesh(const TBuffer3D &buff)
02505 {
02506 AMesh_t *newMesh = new AMesh_t;
02507 const Double_t *v = buff.fPnts;
02508
02509 newMesh->Verts().resize(buff.NbPnts());
02510
02511 for (UInt_t i = 0; i < buff.NbPnts(); ++i)
02512 newMesh->Verts()[i] = TVertexBase(v[i * 3], v[i * 3 + 1], v[i * 3 + 2]);
02513
02514 const Int_t *segs = buff.fSegs;
02515 const Int_t *pols = buff.fPols;
02516
02517 newMesh->Polys().resize(buff.NbPols());
02518
02519 for (UInt_t numPol = 0, j = 1; numPol < buff.NbPols(); ++numPol) {
02520 TestPolygon_t &currPoly = newMesh->Polys()[numPol];
02521 Int_t segmentInd = pols[j] + j;
02522 Int_t segmentCol = pols[j];
02523 Int_t s1 = pols[segmentInd];
02524 segmentInd--;
02525 Int_t s2 = pols[segmentInd];
02526 segmentInd--;
02527 Int_t segEnds[] = {segs[s1 * 3 + 1], segs[s1 * 3 + 2],
02528 segs[s2 * 3 + 1], segs[s2 * 3 + 2]};
02529 Int_t numPnts[3] = {0};
02530
02531 if (segEnds[0] == segEnds[2]) {
02532 numPnts[0] = segEnds[1], numPnts[1] = segEnds[0], numPnts[2] = segEnds[3];
02533 } else if (segEnds[0] == segEnds[3]) {
02534 numPnts[0] = segEnds[1], numPnts[1] = segEnds[0], numPnts[2] = segEnds[2];
02535 } else if (segEnds[1] == segEnds[2]) {
02536 numPnts[0] = segEnds[0], numPnts[1] = segEnds[1], numPnts[2] = segEnds[3];
02537 } else {
02538 numPnts[0] = segEnds[0], numPnts[1] = segEnds[1], numPnts[2] = segEnds[2];
02539 }
02540
02541 currPoly.AddProp(TBlenderVProp(numPnts[0]));
02542 currPoly.AddProp(TBlenderVProp(numPnts[1]));
02543 currPoly.AddProp(TBlenderVProp(numPnts[2]));
02544
02545 Int_t lastAdded = numPnts[2];
02546
02547 Int_t end = j + 1;
02548 for (; segmentInd != end; segmentInd--) {
02549 segEnds[0] = segs[pols[segmentInd] * 3 + 1];
02550 segEnds[1] = segs[pols[segmentInd] * 3 + 2];
02551 if (segEnds[0] == lastAdded) {
02552 currPoly.AddProp(TBlenderVProp(segEnds[1]));
02553 lastAdded = segEnds[1];
02554 } else {
02555 currPoly.AddProp(TBlenderVProp(segEnds[0]));
02556 lastAdded = segEnds[0];
02557 }
02558 }
02559 j += segmentCol + 2;
02560 }
02561
02562 AMeshWrapper_t wrap(*newMesh);
02563
02564 wrap.ComputePlanes();
02565
02566 return newMesh;
02567 }
02568
02569
02570 TBaseMesh *BuildUnion(const TBaseMesh *l, const TBaseMesh *r)
02571 {
02572 return build_union(*static_cast<const AMesh_t *>(l), *static_cast<const AMesh_t *>(r), kFALSE);
02573 }
02574
02575
02576 TBaseMesh *BuildIntersection(const TBaseMesh *l, const TBaseMesh *r)
02577 {
02578 return build_intersection(*static_cast<const AMesh_t *>(l), *static_cast<const AMesh_t *>(r), kFALSE);
02579 }
02580
02581
02582 TBaseMesh *BuildDifference(const TBaseMesh *l, const TBaseMesh *r)
02583 {
02584 return build_difference(*static_cast<const AMesh_t *>(l), *static_cast<const AMesh_t *>(r), kFALSE);
02585 }
02586
02587 }
