00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "Wm4FoundationPCH.h"
00018 #include "Wm4IntrTriangle3Triangle3.h"
00019 #include "Wm4Intersector1.h"
00020 #include "Wm4IntrTriangle2Triangle2.h"
00021 #include "Wm4Query2.h"
00022
00023 namespace Wm4
00024 {
00025
00026 template <class Real>
00027 IntrTriangle3Triangle3<Real>::IntrTriangle3Triangle3 (
00028 const Triangle3<Real>& rkTriangle0, const Triangle3<Real>& rkTriangle1)
00029 :
00030 m_rkTriangle0(rkTriangle0),
00031 m_rkTriangle1(rkTriangle1)
00032 {
00033 ReportCoplanarIntersections = true;
00034 m_iQuantity = 0;
00035 }
00036
00037 template <class Real>
00038 const Triangle3<Real>& IntrTriangle3Triangle3<Real>::GetTriangle0 () const
00039 {
00040 return m_rkTriangle0;
00041 }
00042
00043 template <class Real>
00044 const Triangle3<Real>& IntrTriangle3Triangle3<Real>::GetTriangle1 () const
00045 {
00046 return m_rkTriangle1;
00047 }
00048
00049 template <class Real>
00050 bool IntrTriangle3Triangle3<Real>::Test ()
00051 {
00052
00053 Vector3<Real> akE0[3] =
00054 {
00055 m_rkTriangle0.V[1] - m_rkTriangle0.V[0],
00056 m_rkTriangle0.V[2] - m_rkTriangle0.V[1],
00057 m_rkTriangle0.V[0] - m_rkTriangle0.V[2]
00058 };
00059
00060
00061 Vector3<Real> kN0 = akE0[0].UnitCross(akE0[1]);
00062
00063
00064 Real fN0dT0V0 = kN0.Dot(m_rkTriangle0.V[0]);
00065 Real fMin1, fMax1;
00066 ProjectOntoAxis(m_rkTriangle1,kN0,fMin1,fMax1);
00067 if (fN0dT0V0 < fMin1 || fN0dT0V0 > fMax1)
00068 {
00069 return false;
00070 }
00071
00072
00073 Vector3<Real> akE1[3] =
00074 {
00075 m_rkTriangle1.V[1] - m_rkTriangle1.V[0],
00076 m_rkTriangle1.V[2] - m_rkTriangle1.V[1],
00077 m_rkTriangle1.V[0] - m_rkTriangle1.V[2]
00078 };
00079
00080
00081 Vector3<Real> kN1 = akE1[0].UnitCross(akE1[1]);
00082
00083 Vector3<Real> kDir;
00084 Real fMin0, fMax0;
00085 int i0, i1;
00086
00087 Vector3<Real> kN0xN1 = kN0.UnitCross(kN1);
00088 if (kN0xN1.Dot(kN0xN1) >= Math<Real>::ZERO_TOLERANCE)
00089 {
00090
00091
00092
00093
00094 Real fN1dT1V0 = kN1.Dot(m_rkTriangle1.V[0]);
00095 ProjectOntoAxis(m_rkTriangle0,kN1,fMin0,fMax0);
00096 if (fN1dT1V0 < fMin0 || fN1dT1V0 > fMax0)
00097 {
00098 return false;
00099 }
00100
00101
00102 for (i1 = 0; i1 < 3; i1++)
00103 {
00104 for (i0 = 0; i0 < 3; i0++)
00105 {
00106 kDir = akE0[i0].UnitCross(akE1[i1]);
00107 ProjectOntoAxis(m_rkTriangle0,kDir,fMin0,fMax0);
00108 ProjectOntoAxis(m_rkTriangle1,kDir,fMin1,fMax1);
00109 if (fMax0 < fMin1 || fMax1 < fMin0)
00110 {
00111 return false;
00112 }
00113 }
00114 }
00115 }
00116 else
00117 {
00118
00119 for (i0 = 0; i0 < 3; i0++)
00120 {
00121 kDir = kN0.UnitCross(akE0[i0]);
00122 ProjectOntoAxis(m_rkTriangle0,kDir,fMin0,fMax0);
00123 ProjectOntoAxis(m_rkTriangle1,kDir,fMin1,fMax1);
00124 if (fMax0 < fMin1 || fMax1 < fMin0)
00125 {
00126 return false;
00127 }
00128 }
00129
00130
00131 for (i1 = 0; i1 < 3; i1++)
00132 {
00133 kDir = kN1.UnitCross(akE1[i1]);
00134 ProjectOntoAxis(m_rkTriangle0,kDir,fMin0,fMax0);
00135 ProjectOntoAxis(m_rkTriangle1,kDir,fMin1,fMax1);
00136 if (fMax0 < fMin1 || fMax1 < fMin0)
00137 {
00138 return false;
00139 }
00140 }
00141 }
00142
00143 return true;
00144 }
00145
00146 template <class Real>
00147 bool IntrTriangle3Triangle3<Real>::Find ()
00148 {
00149 int i, iM, iP;
00150
00151
00152 Plane3<Real> kPlane0(m_rkTriangle0.V[0],m_rkTriangle0.V[1],
00153 m_rkTriangle0.V[2]);
00154
00155
00156
00157 int iPos1, iNeg1, iZero1, aiSign1[3];
00158 Real afDist1[3];
00159 TrianglePlaneRelations(m_rkTriangle1,kPlane0,afDist1,aiSign1,iPos1,iNeg1,
00160 iZero1);
00161
00162 if (iPos1 == 3 || iNeg1 == 3)
00163 {
00164
00165 return false;
00166 }
00167
00168 if (iZero1 == 3)
00169 {
00170
00171 if (ReportCoplanarIntersections)
00172 {
00173 return GetCoplanarIntersection(kPlane0,m_rkTriangle0,
00174 m_rkTriangle1);
00175 }
00176 return false;
00177 }
00178
00179
00180 if (iPos1 == 0 || iNeg1 == 0)
00181 {
00182 if (iZero1 == 2)
00183 {
00184
00185 for (i = 0; i < 3; i++)
00186 {
00187 if (aiSign1[i] != 0)
00188 {
00189 iM = (i + 2) % 3;
00190 iP = (i + 1) % 3;
00191 return IntersectsSegment(m_rkTriangle0,
00192 m_rkTriangle1.V[iM],m_rkTriangle1.V[iP]);
00193 }
00194 }
00195 }
00196 else
00197 {
00198
00199 for (i = 0; i < 3; i++)
00200 {
00201 if (aiSign1[i] == 0)
00202 {
00203 return ContainsPoint(m_rkTriangle0,kPlane0,
00204 m_rkTriangle1.V[i]);
00205 }
00206 }
00207 }
00208 }
00209
00210
00211
00212
00213 Real fT;
00214 Vector3<Real> kIntr0, kIntr1;
00215 if (iZero1 == 0)
00216 {
00217 int iSign = (iPos1 == 1 ? +1 : -1);
00218 for (i = 0; i < 3; i++)
00219 {
00220 if (aiSign1[i] == iSign)
00221 {
00222 iM = (i + 2) % 3;
00223 iP = (i + 1) % 3;
00224 fT = afDist1[i]/(afDist1[i] - afDist1[iM]);
00225 kIntr0 = m_rkTriangle1.V[i] + fT*(m_rkTriangle1.V[iM] -
00226 m_rkTriangle1.V[i]);
00227 fT = afDist1[i]/(afDist1[i] - afDist1[iP]);
00228 kIntr1 = m_rkTriangle1.V[i] + fT*(m_rkTriangle1.V[iP] -
00229 m_rkTriangle1.V[i]);
00230 return IntersectsSegment(m_rkTriangle0,kIntr0,kIntr1);
00231 }
00232 }
00233 }
00234
00235
00236 for (i = 0; i < 3; i++)
00237 {
00238 if (aiSign1[i] == 0)
00239 {
00240 iM = (i + 2) % 3;
00241 iP = (i + 1) % 3;
00242 fT = afDist1[iM]/(afDist1[iM] - afDist1[iP]);
00243 kIntr0 = m_rkTriangle1.V[iM] + fT*(m_rkTriangle1.V[iP] -
00244 m_rkTriangle1.V[iM]);
00245 return IntersectsSegment(m_rkTriangle0,m_rkTriangle1.V[i],kIntr0);
00246 }
00247 }
00248
00249 assert(false);
00250 return false;
00251 }
00252
00253 template <class Real>
00254 bool IntrTriangle3Triangle3<Real>::Test (Real fTMax,
00255 const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
00256 {
00257 Real fTFirst = (Real)0.0;
00258 Real fTLast = Math<Real>::MAX_REAL;
00259
00260
00261 Vector3<Real> kVel = rkVelocity1 - rkVelocity0;
00262
00263
00264 Vector3<Real> akE[3] =
00265 {
00266 m_rkTriangle0.V[1] - m_rkTriangle0.V[0],
00267 m_rkTriangle0.V[2] - m_rkTriangle0.V[1],
00268 m_rkTriangle0.V[0] - m_rkTriangle0.V[2]
00269 };
00270 Vector3<Real> kN = akE[0].UnitCross(akE[1]);
00271 if (!TestOverlap(kN,fTMax,kVel,fTFirst,fTLast))
00272 {
00273 return false;
00274 }
00275
00276
00277 Vector3<Real> akF[3] =
00278 {
00279 m_rkTriangle1.V[1] - m_rkTriangle1.V[0],
00280 m_rkTriangle1.V[2] - m_rkTriangle1.V[1],
00281 m_rkTriangle1.V[0] - m_rkTriangle1.V[2]
00282 };
00283 Vector3<Real> kM = akF[0].UnitCross(akF[1]);
00284
00285 Vector3<Real> kDir;
00286 int i0, i1;
00287
00288 if (Math<Real>::FAbs(kN.Dot(kM)) < 1.0f - Math<Real>::ZERO_TOLERANCE)
00289 {
00290
00291
00292
00293 if (!TestOverlap(kM,fTMax,kVel,fTFirst,fTLast))
00294 {
00295 return false;
00296 }
00297
00298
00299 for (i1 = 0; i1 < 3; i1++)
00300 {
00301 for (i0 = 0; i0 < 3; i0++)
00302 {
00303 kDir = akE[i0].UnitCross(akF[i1]);
00304 if (!TestOverlap(kDir,fTMax,kVel,fTFirst,fTLast))
00305 {
00306 return false;
00307 }
00308 }
00309 }
00310 }
00311 else
00312 {
00313
00314 for (i0 = 0; i0 < 3; i0++)
00315 {
00316 kDir = kN.UnitCross(akE[i0]);
00317 if (!TestOverlap(kDir,fTMax,kVel,fTFirst,fTLast))
00318 {
00319 return false;
00320 }
00321 }
00322
00323
00324 for (i1 = 0; i1 < 3; i1++)
00325 {
00326 kDir = kM.UnitCross(akF[i1]);
00327 if (!TestOverlap(kDir,fTMax,kVel,fTFirst,fTLast))
00328 {
00329 return false;
00330 }
00331 }
00332 }
00333
00334 m_fContactTime = fTFirst;
00335 return true;
00336 }
00337
00338 template <class Real>
00339 bool IntrTriangle3Triangle3<Real>::Find (Real fTMax,
00340 const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
00341 {
00342 Real fTFirst = (Real)0.0;
00343 Real fTLast = Math<Real>::MAX_REAL;
00344
00345
00346 Vector3<Real> kVel = rkVelocity1 - rkVelocity0;
00347
00348 ContactSide eSide = CS_NONE;
00349 Configuration kTCfg0, kTCfg1;
00350
00351
00352 Vector3<Real> akE[3] =
00353 {
00354 m_rkTriangle0.V[1] - m_rkTriangle0.V[0],
00355 m_rkTriangle0.V[2] - m_rkTriangle0.V[1],
00356 m_rkTriangle0.V[0] - m_rkTriangle0.V[2]
00357 };
00358 Vector3<Real> kN = akE[0].UnitCross(akE[1]);
00359 if (!FindOverlap(kN,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,fTLast))
00360 {
00361 return false;
00362 }
00363
00364
00365 Vector3<Real> akF[3] =
00366 {
00367 m_rkTriangle1.V[1] - m_rkTriangle1.V[0],
00368 m_rkTriangle1.V[2] - m_rkTriangle1.V[1],
00369 m_rkTriangle1.V[0] - m_rkTriangle1.V[2]
00370 };
00371 Vector3<Real> kM = akF[0].UnitCross(akF[1]);
00372
00373 Vector3<Real> kDir;
00374 int i0, i1;
00375
00376 if (Math<Real>::FAbs(kN.Dot(kM)) < 1.0f - Math<Real>::ZERO_TOLERANCE)
00377 {
00378
00379
00380
00381 if (!FindOverlap(kM,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,fTLast))
00382 {
00383 return false;
00384 }
00385
00386
00387 for (i1 = 0; i1 < 3; i1++)
00388 {
00389 for (i0 = 0; i0 < 3; i0++)
00390 {
00391 kDir = akE[i0].UnitCross(akF[i1]);
00392 if (!FindOverlap(kDir,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,
00393 fTLast))
00394 {
00395 return false;
00396 }
00397 }
00398 }
00399 }
00400 else
00401 {
00402
00403 for (i0 = 0; i0 < 3; i0++)
00404 {
00405 kDir = kN.UnitCross(akE[i0]);
00406 if (!FindOverlap(kDir,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,
00407 fTLast))
00408 {
00409 return false;
00410 }
00411 }
00412
00413
00414 for (i1 = 0; i1 < 3; i1++)
00415 {
00416 kDir = kM.UnitCross(akF[i1]);
00417 if (!FindOverlap(kDir,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,
00418 fTLast))
00419 {
00420 return false;
00421 }
00422 }
00423 }
00424
00425 if (fTFirst <= (Real)0.0)
00426 {
00427 return false;
00428 }
00429
00430 m_fContactTime = fTFirst;
00431
00432
00433 Triangle3<Real> akMTri0, akMTri1;
00434 for (i0 = 0; i0 < 3; i0++)
00435 {
00436 akMTri0.V[i0] = m_rkTriangle0.V[i0] + fTFirst*rkVelocity0;
00437 akMTri1.V[i0] = m_rkTriangle1.V[i0] + fTFirst*rkVelocity1;
00438 }
00439
00440 FindContactSet(akMTri0,akMTri1,eSide,kTCfg0,kTCfg1);
00441 return true;
00442 }
00443
00444 template <class Real>
00445 int IntrTriangle3Triangle3<Real>::GetQuantity () const
00446 {
00447 return m_iQuantity;
00448 }
00449
00450 template <class Real>
00451 const Vector3<Real>& IntrTriangle3Triangle3<Real>::GetPoint (int i) const
00452 {
00453 assert(0 <= i && i < m_iQuantity);
00454 return m_akPoint[i];
00455 }
00456
00457 template <class Real>
00458 void IntrTriangle3Triangle3<Real>::ProjectOntoAxis (
00459 const Triangle3<Real>& rkTri, const Vector3<Real>& rkAxis, Real& rfMin,
00460 Real& rfMax)
00461 {
00462 Real fDot0 = rkAxis.Dot(rkTri.V[0]);
00463 Real fDot1 = rkAxis.Dot(rkTri.V[1]);
00464 Real fDot2 = rkAxis.Dot(rkTri.V[2]);
00465
00466 rfMin = fDot0;
00467 rfMax = rfMin;
00468
00469 if (fDot1 < rfMin)
00470 {
00471 rfMin = fDot1;
00472 }
00473 else if (fDot1 > rfMax)
00474 {
00475 rfMax = fDot1;
00476 }
00477
00478 if (fDot2 < rfMin)
00479 {
00480 rfMin = fDot2;
00481 }
00482 else if (fDot2 > rfMax)
00483 {
00484 rfMax = fDot2;
00485 }
00486 }
00487
00488 template <class Real>
00489 void IntrTriangle3Triangle3<Real>::TrianglePlaneRelations (
00490 const Triangle3<Real>& rkTriangle, const Plane3<Real>& rkPlane,
00491 Real afDistance[3], int aiSign[3], int& riPositive, int& riNegative,
00492 int& riZero)
00493 {
00494
00495
00496 riPositive = 0;
00497 riNegative = 0;
00498 riZero = 0;
00499 for (int i = 0; i < 3; i++)
00500 {
00501 afDistance[i] = rkPlane.DistanceTo(rkTriangle.V[i]);
00502 if (afDistance[i] > Math<Real>::ZERO_TOLERANCE)
00503 {
00504 aiSign[i] = 1;
00505 riPositive++;
00506 }
00507 else if (afDistance[i] < -Math<Real>::ZERO_TOLERANCE)
00508 {
00509 aiSign[i] = -1;
00510 riNegative++;
00511 }
00512 else
00513 {
00514 afDistance[i] = (Real)0.0;
00515 aiSign[i] = 0;
00516 riZero++;
00517 }
00518 }
00519 }
00520
00521 template <class Real>
00522 void IntrTriangle3Triangle3<Real>::GetInterval (
00523 const Triangle3<Real>& rkTriangle, const Line3<Real>& rkLine,
00524 const Real afDistance[3], const int aiSign[3], Real afParam[2])
00525 {
00526
00527 Real afProj[3];
00528 int i;
00529 for (i = 0; i < 3; i++)
00530 {
00531 Vector3<Real> kDiff = rkTriangle.V[i] - rkLine.Origin;
00532 afProj[i] = rkLine.Direction.Dot(kDiff);
00533 }
00534
00535
00536 Real fNumer, fDenom;
00537 int i0, i1, i2;
00538 int iQuantity = 0;
00539 for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
00540 {
00541 if (aiSign[i0]*aiSign[i1] < 0)
00542 {
00543 assert( iQuantity < 2 );
00544 fNumer = afDistance[i0]*afProj[i1] - afDistance[i1]*afProj[i0];
00545 fDenom = afDistance[i0] - afDistance[i1];
00546 afParam[iQuantity++] = fNumer/fDenom;
00547 }
00548 }
00549
00550
00551 if (iQuantity < 2)
00552 {
00553 for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2, i2++)
00554 {
00555 if (aiSign[i2] == 0)
00556 {
00557 assert(iQuantity < 2);
00558 afParam[iQuantity++] = afProj[i2];
00559 }
00560 }
00561 }
00562
00563
00564 assert(iQuantity == 1 || iQuantity == 2);
00565 if (iQuantity == 2)
00566 {
00567 if (afParam[0] > afParam[1])
00568 {
00569 Real fSave = afParam[0];
00570 afParam[0] = afParam[1];
00571 afParam[1] = fSave;
00572 }
00573 }
00574 else
00575 {
00576 afParam[1] = afParam[0];
00577 }
00578 }
00579
00580 template <class Real>
00581 bool IntrTriangle3Triangle3<Real>::ContainsPoint (
00582 const Triangle3<Real>& rkTriangle, const Plane3<Real>& rkPlane,
00583 const Vector3<Real>& rkPoint)
00584 {
00585 Vector3<Real> kU0, kU1;
00586 Vector3<Real>::GenerateComplementBasis(kU0,kU1,rkPlane.Normal);
00587
00588 Vector3<Real> kPmV0 = rkPoint - rkTriangle.V[0];
00589 Vector3<Real> kV1mV0 = rkTriangle.V[1] - rkTriangle.V[0];
00590 Vector3<Real> kV2mV0 = rkTriangle.V[2] - rkTriangle.V[0];
00591
00592 Vector2<Real> kProjP(kU0.Dot(kPmV0),kU1.Dot(kPmV0));
00593 Vector2<Real> akProjV[3] =
00594 {
00595 Vector2<Real>::ZERO,
00596 Vector2<Real>(kU0.Dot(kV1mV0),kU1.Dot(kV1mV0)),
00597 Vector2<Real>(kU0.Dot(kV2mV0),kU1.Dot(kV2mV0))
00598 };
00599
00600 return Query2<Real>(3,akProjV).ToTriangle(kProjP,0,1,2) <= 0;
00601 }
00602
00603 template <class Real>
00604 bool IntrTriangle3Triangle3<Real>::IntersectsSegment (
00605 const Triangle3<Real>& rkTriangle, const Vector3<Real>& rkEnd0,
00606 const Vector3<Real>& rkEnd1)
00607 {
00608
00609 return false;
00610 }
00611
00612 template <class Real>
00613 bool IntrTriangle3Triangle3<Real>::GetCoplanarIntersection (
00614 const Plane3<Real>& rkPlane, const Triangle3<Real>& rkTri0,
00615 const Triangle3<Real>& rkTri1)
00616 {
00617
00618
00619 int iMaxNormal = 0;
00620 Real fMax = Math<Real>::FAbs(rkPlane.Normal.X());
00621 Real fAbs = Math<Real>::FAbs(rkPlane.Normal.Y());
00622 if (fAbs > fMax)
00623 {
00624 iMaxNormal = 1;
00625 fMax = fAbs;
00626 }
00627 fAbs = Math<Real>::FAbs(rkPlane.Normal.Z());
00628 if (fAbs > fMax)
00629 {
00630 iMaxNormal = 2;
00631 }
00632
00633 Triangle2<Real> kProjTri0, kProjTri1;
00634 int i;
00635
00636 if (iMaxNormal == 0)
00637 {
00638
00639 for (i = 0; i < 3; i++)
00640 {
00641 kProjTri0.V[i].X() = m_rkTriangle0.V[i].Y();
00642 kProjTri0.V[i].Y() = m_rkTriangle0.V[i].Z();
00643 kProjTri1.V[i].X() = m_rkTriangle1.V[i].Y();
00644 kProjTri1.V[i].Y() = m_rkTriangle1.V[i].Z();
00645 }
00646 }
00647 else if (iMaxNormal == 1)
00648 {
00649
00650 for (i = 0; i < 3; i++)
00651 {
00652 kProjTri0.V[i].X() = m_rkTriangle0.V[i].X();
00653 kProjTri0.V[i].Y() = m_rkTriangle0.V[i].Z();
00654 kProjTri1.V[i].X() = m_rkTriangle1.V[i].X();
00655 kProjTri1.V[i].Y() = m_rkTriangle1.V[i].Z();
00656 }
00657 }
00658 else
00659 {
00660
00661 for (i = 0; i < 3; i++)
00662 {
00663 kProjTri0.V[i].X() = m_rkTriangle0.V[i].X();
00664 kProjTri0.V[i].Y() = m_rkTriangle0.V[i].Y();
00665 kProjTri1.V[i].X() = m_rkTriangle1.V[i].X();
00666 kProjTri1.V[i].Y() = m_rkTriangle1.V[i].Y();
00667 }
00668 }
00669
00670
00671 Vector2<Real> kSave;
00672 Vector2<Real> kEdge0 = kProjTri0.V[1] - kProjTri0.V[0];
00673 Vector2<Real> kEdge1 = kProjTri0.V[2] - kProjTri0.V[0];
00674 if (kEdge0.DotPerp(kEdge1) < (Real)0.0)
00675 {
00676
00677 kSave = kProjTri0.V[1];
00678 kProjTri0.V[1] = kProjTri0.V[2];
00679 kProjTri0.V[2] = kSave;
00680 }
00681
00682 kEdge0 = kProjTri1.V[1] - kProjTri1.V[0];
00683 kEdge1 = kProjTri1.V[2] - kProjTri1.V[0];
00684 if (kEdge0.DotPerp(kEdge1) < (Real)0.0)
00685 {
00686
00687 kSave = kProjTri1.V[1];
00688 kProjTri1.V[1] = kProjTri1.V[2];
00689 kProjTri1.V[2] = kSave;
00690 }
00691
00692 IntrTriangle2Triangle2<Real> kIntr(kProjTri0,kProjTri1);
00693 if (!kIntr.Find())
00694 {
00695 return false;
00696 }
00697
00698
00699 m_iQuantity = kIntr.GetQuantity();
00700 if (iMaxNormal == 0)
00701 {
00702 Real fInvNX = ((Real)1.0)/rkPlane.Normal.X();
00703 for (i = 0; i < m_iQuantity; i++)
00704 {
00705 m_akPoint[i].Y() = kIntr.GetPoint(i).X();
00706 m_akPoint[i].Z() = kIntr.GetPoint(i).Y();
00707 m_akPoint[i].X() = fInvNX*(rkPlane.Constant -
00708 rkPlane.Normal.Y()*m_akPoint[i].Y() -
00709 rkPlane.Normal.Z()*m_akPoint[i].Z());
00710 }
00711 }
00712 else if (iMaxNormal == 1)
00713 {
00714 Real fInvNY = ((Real)1.0)/rkPlane.Normal.Y();
00715 for (i = 0; i < m_iQuantity; i++)
00716 {
00717 m_akPoint[i].X() = kIntr.GetPoint(i).X();
00718 m_akPoint[i].Z() = kIntr.GetPoint(i).Y();
00719 m_akPoint[i].Y() = fInvNY*(rkPlane.Constant -
00720 rkPlane.Normal.X()*m_akPoint[i].X() -
00721 rkPlane.Normal.Z()*m_akPoint[i].Z());
00722 }
00723 }
00724 else
00725 {
00726 Real fInvNZ = ((Real)1.0)/rkPlane.Normal.Z();
00727 for (i = 0; i < m_iQuantity; i++)
00728 {
00729 m_akPoint[i].X() = kIntr.GetPoint(i).X();
00730 m_akPoint[i].Y() = kIntr.GetPoint(i).Y();
00731 m_akPoint[i].Z() = fInvNZ*(rkPlane.Constant -
00732 rkPlane.Normal.X()*m_akPoint[i].X() -
00733 rkPlane.Normal.Y()*m_akPoint[i].Y());
00734 }
00735 }
00736
00737 return true;
00738 }
00739
00740 template <class Real>
00741 bool IntrTriangle3Triangle3<Real>::TestOverlap (const Vector3<Real>& rkAxis,
00742 Real fTMax, Real fSpeed, Real fUMin, Real fUMax, Real fVMin, Real fVMax,
00743 Real& rfTFirst, Real& rfTLast)
00744 {
00745
00746
00747 Real fT;
00748
00749 if (fVMax < fUMin)
00750 {
00751 if (fSpeed <= (Real)0.0)
00752 {
00753 return false;
00754 }
00755
00756
00757 fT = (fUMin - fVMax)/fSpeed;
00758 if (fT > rfTFirst)
00759 {
00760 rfTFirst = fT;
00761 }
00762
00763
00764 if (rfTFirst > fTMax)
00765 {
00766 return false;
00767 }
00768
00769
00770 fT = (fUMax - fVMin)/fSpeed;
00771 if (fT < rfTLast)
00772 {
00773 rfTLast = fT;
00774 }
00775
00776
00777 if (rfTFirst > rfTLast)
00778 {
00779 return false;
00780 }
00781 }
00782 else if ( fUMax < fVMin )
00783 {
00784 if (fSpeed >= (Real)0.0)
00785 {
00786 return false;
00787 }
00788
00789
00790 fT = (fUMax - fVMin)/fSpeed;
00791 if (fT > rfTFirst)
00792 {
00793 rfTFirst = fT;
00794 }
00795
00796
00797 if (rfTFirst > fTMax)
00798 {
00799 return false;
00800 }
00801
00802
00803 fT = (fUMin - fVMax)/fSpeed;
00804 if (fT < rfTLast)
00805 {
00806 rfTLast = fT;
00807 }
00808
00809
00810 if (rfTFirst > rfTLast)
00811 {
00812 return false;
00813 }
00814
00815 }
00816 else
00817 {
00818 if (fSpeed > (Real)0.0)
00819 {
00820
00821 fT = (fUMax - fVMin)/fSpeed;
00822 if (fT < rfTLast)
00823 {
00824 rfTLast = fT;
00825 }
00826
00827
00828 if (rfTFirst > rfTLast)
00829 {
00830 return false;
00831 }
00832 }
00833 else if (fSpeed < (Real)0.0)
00834 {
00835
00836 fT = (fUMin - fVMax)/fSpeed;
00837 if (fT < rfTLast)
00838 {
00839 rfTLast = fT;
00840 }
00841
00842
00843 if (rfTFirst > rfTLast)
00844 {
00845 return false;
00846 }
00847 }
00848 }
00849 return true;
00850 }
00851
00852 template <class Real>
00853 bool IntrTriangle3Triangle3<Real>::FindOverlap (const Vector3<Real>& rkAxis,
00854 Real fTMax, Real fSpeed, const Configuration& rkUC,
00855 const Configuration& rkVC, ContactSide& rkSide, Configuration& rkTUC,
00856 Configuration& rkTVC, Real& rfTFirst, Real& rfTLast)
00857 {
00858
00859
00860
00861
00862 Real fT;
00863
00864 if (rkVC.Max < rkUC.Min)
00865 {
00866 if (fSpeed <= (Real)0.0)
00867 {
00868 return false;
00869 }
00870
00871
00872 fT = (rkUC.Min - rkVC.Max)/fSpeed;
00873
00874
00875
00876 if (fT > rfTFirst)
00877 {
00878 rfTFirst = fT;
00879 rkSide = CS_LEFT;
00880 rkTUC = rkUC;
00881 rkTVC = rkVC;
00882 }
00883
00884
00885 if (rfTFirst > fTMax)
00886 {
00887 return false;
00888 }
00889
00890
00891 fT = (rkUC.Max - rkVC.Min)/fSpeed;
00892 if (fT < rfTLast)
00893 {
00894 rfTLast = fT;
00895 }
00896
00897
00898 if (rfTFirst > rfTLast)
00899 {
00900 return false;
00901 }
00902 }
00903 else if (rkUC.Max < rkVC.Min)
00904 {
00905 if (fSpeed >= (Real)0.0)
00906 {
00907 return false;
00908 }
00909
00910
00911 fT = (rkUC.Max - rkVC.Min)/fSpeed;
00912
00913
00914
00915 if (fT > rfTFirst)
00916 {
00917 rfTFirst = fT;
00918 rkSide = CS_RIGHT;
00919 rkTUC = rkUC;
00920 rkTVC = rkVC;
00921 }
00922
00923
00924 if (rfTFirst > fTMax)
00925 {
00926 return false;
00927 }
00928
00929
00930 fT = (rkUC.Min - rkVC.Max)/fSpeed;
00931 if (fT < rfTLast)
00932 {
00933 rfTLast = fT;
00934 }
00935
00936
00937 if (rfTFirst > rfTLast)
00938 {
00939 return false;
00940 }
00941 }
00942 else
00943 {
00944 if (fSpeed > (Real)0.0)
00945 {
00946
00947 fT = (rkUC.Max - rkVC.Min)/fSpeed;
00948 if (fT < rfTLast)
00949 {
00950 rfTLast = fT;
00951 }
00952
00953
00954 if (rfTFirst > rfTLast)
00955 {
00956 return false;
00957 }
00958 }
00959 else if ( fSpeed < (Real)0.0 )
00960 {
00961
00962 fT = (rkUC.Min - rkVC.Max)/fSpeed;
00963 if (fT < rfTLast)
00964 {
00965 rfTLast = fT;
00966 }
00967
00968
00969 if (rfTFirst > rfTLast)
00970 {
00971 return false;
00972 }
00973 }
00974 }
00975 return true;
00976 }
00977
00978 template <class Real>
00979 bool IntrTriangle3Triangle3<Real>::TestOverlap (const Vector3<Real>& rkAxis,
00980 Real fTMax, const Vector3<Real>& rkVelocity, Real& rfTFirst,
00981 Real& rfTLast)
00982 {
00983 Real fMin0, fMax0, fMin1, fMax1;
00984 ProjectOntoAxis(m_rkTriangle0,rkAxis,fMin0,fMax0);
00985 ProjectOntoAxis(m_rkTriangle1,rkAxis,fMin1,fMax1);
00986 Real fSpeed = rkVelocity.Dot(rkAxis);
00987 return TestOverlap(rkAxis,fTMax,fSpeed,fMin0,fMax0,fMin1,fMax1,rfTFirst,
00988 rfTLast);
00989 }
00990
00991 template <class Real>
00992 bool IntrTriangle3Triangle3<Real>::FindOverlap (const Vector3<Real>& rkAxis,
00993 Real fTMax, const Vector3<Real>& rkVelocity, ContactSide& reSide,
00994 Configuration& rkTCfg0, Configuration& rkTCfg1, Real& rfTFirst,
00995 Real& rfTLast)
00996 {
00997 Configuration kCfg0, kCfg1;
00998 ProjectOntoAxis(m_rkTriangle0,rkAxis,kCfg0);
00999 ProjectOntoAxis(m_rkTriangle1,rkAxis,kCfg1);
01000 Real fSpeed = rkVelocity.Dot(rkAxis);
01001 return FindOverlap(rkAxis,fTMax,fSpeed,kCfg0,kCfg1,reSide,rkTCfg0,rkTCfg1,
01002 rfTFirst,rfTLast);
01003 }
01004
01005 template <class Real>
01006 void IntrTriangle3Triangle3<Real>::ProjectOntoAxis (
01007 const Triangle3<Real>& rkTri, const Vector3<Real>& rkAxis,
01008 Configuration& rkCfg)
01009 {
01010
01011 Real fD0 = rkAxis.Dot(rkTri.V[0]);
01012 Real fD1 = rkAxis.Dot(rkTri.V[1]);
01013 Real fD2 = rkAxis.Dot(rkTri.V[2]);
01014
01015
01016 if (fD0 <= fD1)
01017 {
01018 if (fD1 <= fD2)
01019 {
01020 if (fD0 != fD1)
01021 {
01022 if (fD1 != fD2)
01023 {
01024 rkCfg.Map = M111;
01025 }
01026 else
01027 {
01028 rkCfg.Map = M12;
01029 }
01030 }
01031 else
01032 {
01033 if (fD1 != fD2)
01034 {
01035 rkCfg.Map = M21;
01036 }
01037 else
01038 {
01039 rkCfg.Map = M3;
01040 }
01041 }
01042 rkCfg.Index[0] = 0;
01043 rkCfg.Index[1] = 1;
01044 rkCfg.Index[2] = 2;
01045 rkCfg.Min = fD0;
01046 rkCfg.Max = fD2;
01047 }
01048 else if (fD0 <= fD2)
01049 {
01050 if (fD0 != fD2)
01051 {
01052 rkCfg.Map = M111;
01053 rkCfg.Index[0] = 0;
01054 rkCfg.Index[1] = 2;
01055 rkCfg.Index[2] = 1;
01056 }
01057 else
01058 {
01059 rkCfg.Map = M21;
01060 rkCfg.Index[0] = 2;
01061 rkCfg.Index[1] = 0;
01062 rkCfg.Index[2] = 1;
01063 }
01064 rkCfg.Min = fD0;
01065 rkCfg.Max = fD1;
01066 }
01067 else
01068 {
01069 if (fD0 != fD1)
01070 {
01071 rkCfg.Map = M111;
01072 }
01073 else
01074 {
01075 rkCfg.Map = M12;
01076 }
01077
01078 rkCfg.Index[0] = 2;
01079 rkCfg.Index[1] = 0;
01080 rkCfg.Index[2] = 1;
01081 rkCfg.Min = fD2;
01082 rkCfg.Max = fD1;
01083 }
01084 }
01085 else if (fD2 <= fD1)
01086 {
01087 if (fD2 != fD1)
01088 {
01089 rkCfg.Map = M111;
01090 rkCfg.Index[0] = 2;
01091 rkCfg.Index[1] = 1;
01092 rkCfg.Index[2] = 0;
01093 }
01094 else
01095 {
01096 rkCfg.Map = M21;
01097 rkCfg.Index[0] = 1;
01098 rkCfg.Index[1] = 2;
01099 rkCfg.Index[2] = 0;
01100
01101 }
01102 rkCfg.Min = fD2;
01103 rkCfg.Max = fD0;
01104 }
01105 else if (fD2 <= fD0)
01106 {
01107 if (fD2 != fD0)
01108 {
01109 rkCfg.Map = M111;
01110 }
01111 else
01112 {
01113 rkCfg.Map = M12;
01114 }
01115
01116 rkCfg.Index[0] = 1;
01117 rkCfg.Index[1] = 2;
01118 rkCfg.Index[2] = 0;
01119 rkCfg.Min = fD1;
01120 rkCfg.Max = fD0;
01121 }
01122 else
01123 {
01124 rkCfg.Map = M111;
01125 rkCfg.Index[0] = 1;
01126 rkCfg.Index[1] = 0;
01127 rkCfg.Index[2] = 2;
01128 rkCfg.Min = fD1;
01129 rkCfg.Max = fD2;
01130 }
01131 }
01132
01133 template <class Real>
01134 void IntrTriangle3Triangle3<Real>::FindContactSet (
01135 const Triangle3<Real>& rkTri0, const Triangle3<Real>& rkTri1,
01136 ContactSide& reSide, Configuration& rkCfg0, Configuration& rkCfg1)
01137 {
01138 if (reSide == CS_RIGHT)
01139 {
01140 if (rkCfg0.Map == M21 || rkCfg0.Map == M111)
01141 {
01142
01143 m_iQuantity = 1;
01144 m_akPoint[0] = rkTri0.V[2];
01145 }
01146 else if (rkCfg1.Map == M12 || rkCfg1.Map == M111)
01147 {
01148
01149 m_iQuantity = 1;
01150 m_akPoint[0] = rkTri1.V[0];
01151 }
01152 else if (rkCfg0.Map == M12)
01153 {
01154 if (rkCfg1.Map == M21)
01155 {
01156
01157 GetEdgeEdgeIntersection(rkTri0.V[1],rkTri0.V[2],rkTri1.V[0],
01158 rkTri1.V[1]);
01159 }
01160 else
01161 {
01162
01163
01164
01165
01166 }
01167 }
01168 else
01169 {
01170 if (rkCfg1.Map == M21)
01171 {
01172
01173
01174
01175
01176 }
01177 else
01178 {
01179
01180 Plane3<Real> kPlane0(rkTri0.V[0],rkTri0.V[1],rkTri0.V[2]);
01181 GetCoplanarIntersection(kPlane0,rkTri0,rkTri1);
01182 }
01183 }
01184 }
01185 else if (reSide == CS_LEFT)
01186 {
01187 if (rkCfg1.Map == M21 || rkCfg1.Map == M111)
01188 {
01189
01190 m_iQuantity = 1;
01191 m_akPoint[0] = rkTri1.V[2];
01192 }
01193 else if (rkCfg0.Map == M12 || rkCfg0.Map == M111)
01194 {
01195
01196 m_iQuantity = 1;
01197 m_akPoint[0] = rkTri0.V[0];
01198 }
01199 else if (rkCfg1.Map == M12)
01200 {
01201 if (rkCfg0.Map == M21)
01202 {
01203
01204 GetEdgeEdgeIntersection(rkTri0.V[0],rkTri0.V[1],rkTri1.V[1],
01205 rkTri1.V[2]);
01206 }
01207 else
01208 {
01209
01210
01211
01212
01213 }
01214 }
01215 else
01216 {
01217 if (rkCfg0.Map == M21)
01218 {
01219
01220
01221
01222
01223 }
01224 else
01225 {
01226
01227 Plane3<Real> kPlane0(rkTri0.V[0],rkTri0.V[1],rkTri0.V[2]);
01228 GetCoplanarIntersection(kPlane0,rkTri0,rkTri1);
01229 }
01230 }
01231 }
01232 else
01233 {
01234
01235 IntrTriangle3Triangle3<Real>(rkTri0,rkTri1).Find();
01236 }
01237 }
01238
01239 template <class Real>
01240 void IntrTriangle3Triangle3<Real>::GetEdgeEdgeIntersection (
01241 const Vector3<Real>& rkU0, const Vector3<Real>& rkU1,
01242 const Vector3<Real>& rkV0, const Vector3<Real>& rkV1)
01243 {
01244
01245 }
01246
01247
01248
01249
01250
01251 template WM4_FOUNDATION_ITEM
01252 class IntrTriangle3Triangle3<float>;
01253
01254 template WM4_FOUNDATION_ITEM
01255 class IntrTriangle3Triangle3<double>;
01256
01257 }