Wm4IntrTriangle2Triangle2.cpp

Go to the documentation of this file.
00001 // Wild Magic Source Code
00002 // David Eberly
00003 // http://www.geometrictools.com
00004 // Copyright (c) 1998-2007
00005 //
00006 // This library is free software; you can redistribute it and/or modify it
00007 // under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation; either version 2.1 of the License, or (at
00009 // your option) any later version.  The license is available for reading at
00010 // either of the locations:
00011 //     http://www.gnu.org/copyleft/lgpl.html
00012 //     http://www.geometrictools.com/License/WildMagicLicense.pdf
00013 // The license applies to versions 0 through 4 of Wild Magic.
00014 //
00015 // Version: 4.0.0 (2006/06/28)
00016 
00017 #include "Wm4FoundationPCH.h"
00018 #include "Wm4IntrTriangle2Triangle2.h"
00019 
00020 namespace Wm4
00021 {
00022 //----------------------------------------------------------------------------
00023 template <class Real>
00024 IntrTriangle2Triangle2<Real>::IntrTriangle2Triangle2 (
00025     const Triangle2<Real>& rkTriangle0, const Triangle2<Real>& rkTriangle1)
00026     :
00027     m_rkTriangle0(rkTriangle0),
00028     m_rkTriangle1(rkTriangle1)
00029 {
00030     m_iQuantity = 0;
00031 }
00032 //----------------------------------------------------------------------------
00033 template <class Real>
00034 const Triangle2<Real>& IntrTriangle2Triangle2<Real>::GetTriangle0 () const
00035 {
00036     return m_rkTriangle0;
00037 }
00038 //----------------------------------------------------------------------------
00039 template <class Real>
00040 const Triangle2<Real>& IntrTriangle2Triangle2<Real>::GetTriangle1 () const
00041 {
00042     return m_rkTriangle1;
00043 }
00044 //----------------------------------------------------------------------------
00045 template <class Real>
00046 bool IntrTriangle2Triangle2<Real>::Test ()
00047 {
00048     int i0, i1;
00049     Vector2<Real> kDir;
00050 
00051     // test edges of triangle0 for separation
00052     for (i0 = 0, i1 = 2; i0 < 3; i1 = i0, i0++)
00053     {
00054         // test axis V0[i1] + t*perp(V0[i0]-V0[i1]), perp(x,y) = (y,-x)
00055         kDir.X() = m_rkTriangle0.V[i0].Y() - m_rkTriangle0.V[i1].Y();
00056         kDir.Y() = m_rkTriangle0.V[i1].X() - m_rkTriangle0.V[i0].X();
00057         if (WhichSide(m_rkTriangle1.V,m_rkTriangle0.V[i1],kDir) > 0)
00058         {
00059             // triangle1 is entirely on positive side of triangle0 edge
00060             return false;
00061         }
00062     }
00063 
00064     // test edges of triangle1 for separation
00065     for (i0 = 0, i1 = 2; i0 < 3; i1 = i0, i0++)
00066     {
00067         // test axis V1[i1] + t*perp(V1[i0]-V1[i1]), perp(x,y) = (y,-x)
00068         kDir.X() = m_rkTriangle1.V[i0].Y() - m_rkTriangle1.V[i1].Y();
00069         kDir.Y() = m_rkTriangle1.V[i1].X() - m_rkTriangle1.V[i0].X();
00070         if (WhichSide(m_rkTriangle0.V,m_rkTriangle1.V[i1],kDir) > 0)
00071         {
00072             // triangle0 is entirely on positive side of triangle1 edge
00073             return false;
00074         }
00075     }
00076 
00077     return true;
00078 }
00079 //----------------------------------------------------------------------------
00080 template <class Real>
00081 bool IntrTriangle2Triangle2<Real>::Find ()
00082 {
00083     // The potential intersection is initialized to triangle1.  The set of
00084     // vertices is refined based on clipping against each edge of triangle0.
00085     m_iQuantity = 3;
00086     for (int i = 0; i < 3; i++)
00087     {
00088         m_akPoint[i] = m_rkTriangle1.V[i];
00089     }
00090 
00091     for (int i1 = 2, i0 = 0; i0 < 3; i1 = i0, i0++)
00092     {
00093         // clip against edge <V0[i1],V0[i0]>
00094         Vector2<Real> kN(
00095             m_rkTriangle0.V[i1].Y() - m_rkTriangle0.V[i0].Y(),
00096             m_rkTriangle0.V[i0].X() - m_rkTriangle0.V[i1].X());
00097         Real fC = kN.Dot(m_rkTriangle0.V[i1]);
00098         ClipConvexPolygonAgainstLine(kN,fC,m_iQuantity,m_akPoint);
00099         if (m_iQuantity == 0)
00100         {
00101             // triangle completely clipped, no intersection occurs
00102             return false;
00103         }
00104     }
00105 
00106     return true;
00107 }
00108 //----------------------------------------------------------------------------
00109 template <class Real>
00110 bool IntrTriangle2Triangle2<Real>::Test (Real fTMax,
00111     const Vector2<Real>& rkVelocity0, const Vector2<Real>& rkVelocity1)
00112 {
00113     // process as if V0-triangle is stationary and V1-triangle is moving
00114     Vector2<Real> kW = rkVelocity1 - rkVelocity0;
00115     int iSide = 0;  // 0 = NONE, -1 = LEFT, +1 = RIGHT
00116     Real fTFirst = (Real)0.0;
00117     Real fTLast = Math<Real>::MAX_REAL;
00118 
00119     Configuration kCfg0, kCfg1, kTCfg0, kTCfg1;
00120     int i0, i1, i2;
00121     Vector2<Real> kD;
00122     Real fSpeed;
00123 
00124     // process edges of V0-triangle
00125     for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2, i2++)
00126     {
00127         // test axis V0[i1] + t*perp(V0[i2]-V0[i1]), perp(x,y) = (y,-x)
00128         kD.X() = m_rkTriangle0.V[i2].Y() - m_rkTriangle0.V[i1].Y();
00129         kD.Y() = m_rkTriangle0.V[i1].X() - m_rkTriangle0.V[i2].X();
00130         fSpeed = kD.Dot(kW);
00131 
00132         ComputeTwo(kCfg0,m_rkTriangle0.V,kD,i0,i1,i2);
00133         ComputeThree(kCfg1,m_rkTriangle1.V,kD,m_rkTriangle0.V[i1]);
00134 
00135         if (NoIntersect(kCfg0,kCfg1,fTMax,fSpeed,iSide,kTCfg0,kTCfg1,
00136             fTFirst,fTLast))
00137         {
00138             return false;
00139         }
00140     }
00141 
00142     // process edges of V1-triangle
00143     for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2, i2++)
00144     {
00145         // test axis V1[i1] + t*perp(V1[i2]-V1[i1]), perp(x,y) = (y,-x)
00146         kD.X() = m_rkTriangle1.V[i2].Y() - m_rkTriangle1.V[i1].Y();
00147         kD.Y() = m_rkTriangle1.V[i1].X() - m_rkTriangle1.V[i2].X();
00148         fSpeed = kD.Dot(kW);
00149 
00150         ComputeTwo(kCfg1,m_rkTriangle1.V,kD,i0,i1,i2);
00151         ComputeThree(kCfg0,m_rkTriangle0.V,kD,m_rkTriangle1.V[i1]);
00152 
00153         if (NoIntersect(kCfg0,kCfg1,fTMax,fSpeed,iSide,kTCfg0,kTCfg1,
00154             fTFirst,fTLast))
00155         {
00156             return false;
00157         }
00158     }
00159 
00160     m_fContactTime = fTFirst;
00161     return true;
00162 }
00163 //----------------------------------------------------------------------------
00164 template <class Real>
00165 bool IntrTriangle2Triangle2<Real>::Find (Real fTMax,
00166     const Vector2<Real>& rkVelocity0, const Vector2<Real>& rkVelocity1)
00167 {
00168     // process as if V0-triangle is stationary and V1-triangle is moving
00169     Vector2<Real> kW = rkVelocity1 - rkVelocity0;
00170     int iSide = 0;  // 0 = NONE, -1 = LEFT, +1 = RIGHT
00171     Real fTFirst = (Real)0.0;
00172     Real fTLast = Math<Real>::MAX_REAL;
00173 
00174     Configuration kCfg0, kCfg1, kTCfg0, kTCfg1;
00175     int i0, i1, i2;
00176     Vector2<Real> kD;
00177     Real fSpeed;
00178 
00179     // process edges of V0-triangle
00180     for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2, i2++)
00181     {
00182         // test axis V0[i1] + t*perp(V0[i2]-V0[i1]), perp(x,y) = (y,-x)
00183         kD.X() = m_rkTriangle0.V[i2].Y() - m_rkTriangle0.V[i1].Y();
00184         kD.Y() = m_rkTriangle0.V[i1].X() - m_rkTriangle0.V[i2].X();
00185         fSpeed = kD.Dot(kW);
00186 
00187         ComputeTwo(kCfg0,m_rkTriangle0.V,kD,i0,i1,i2);
00188         ComputeThree(kCfg1,m_rkTriangle1.V,kD,m_rkTriangle0.V[i1]);
00189 
00190         if (NoIntersect(kCfg0,kCfg1,fTMax,fSpeed,iSide,kTCfg0,kTCfg1,
00191             fTFirst,fTLast))
00192         {
00193             return false;
00194         }
00195     }
00196 
00197     // process edges of V1-triangle
00198     for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2, i2++)
00199     {
00200         // test axis V1[i1] + t*perp(V1[i2]-V1[i1]), perp(x,y) = (y,-x)
00201         kD.X() = m_rkTriangle1.V[i2].Y() - m_rkTriangle1.V[i1].Y();
00202         kD.Y() = m_rkTriangle1.V[i1].X() - m_rkTriangle1.V[i2].X();
00203         fSpeed = kD.Dot(kW);
00204 
00205         ComputeTwo(kCfg1,m_rkTriangle1.V,kD,i0,i1,i2);
00206         ComputeThree(kCfg0,m_rkTriangle0.V,kD,m_rkTriangle1.V[i1]);
00207 
00208         if (NoIntersect(kCfg0,kCfg1,fTMax,fSpeed,iSide,kTCfg0,kTCfg1,
00209             fTFirst,fTLast))
00210         {
00211             return false;
00212         }
00213     }
00214 
00215     // move triangles to first contact
00216     Vector2<Real> akMoveV0[3], akMoveV1[3];
00217     for (int i = 0; i < 3; i++)
00218     {
00219         akMoveV0[i] = m_rkTriangle0.V[i] + fTFirst*rkVelocity0;
00220         akMoveV1[i] = m_rkTriangle1.V[i] + fTFirst*rkVelocity1;
00221     };
00222 
00223     GetIntersection(kTCfg0,kTCfg1,iSide,akMoveV0,akMoveV1,m_iQuantity,
00224         m_akPoint);
00225 
00226     m_fContactTime = fTFirst;
00227     return m_iQuantity > 0;
00228 }
00229 //----------------------------------------------------------------------------
00230 template <class Real>
00231 int IntrTriangle2Triangle2<Real>::GetQuantity () const
00232 {
00233     return m_iQuantity;
00234 }
00235 //----------------------------------------------------------------------------
00236 template <class Real>
00237 const Vector2<Real>& IntrTriangle2Triangle2<Real>::GetPoint (int i) const
00238 {
00239     assert(0 <= i && i < m_iQuantity);
00240     return m_akPoint[i];
00241 }
00242 //----------------------------------------------------------------------------
00243 template <class Real>
00244 int IntrTriangle2Triangle2<Real>::WhichSide (const Vector2<Real> akV[3],
00245     const Vector2<Real>& rkP, const Vector2<Real>& rkD)
00246 {
00247     // Vertices are projected to the form P+t*D.  Return value is +1 if all
00248     // t > 0, -1 if all t < 0, 0 otherwise, in which case the line splits the
00249     // triangle.
00250 
00251     int iPositive = 0, iNegative = 0, iZero = 0;
00252     for (int i = 0; i < 3; i++)
00253     {
00254         Real fT = rkD.Dot(akV[i] - rkP);
00255         if (fT > (Real)0.0)
00256         {
00257             iPositive++;
00258         }
00259         else if (fT < (Real)0.0)
00260         {
00261             iNegative++;
00262         }
00263         else
00264         {
00265             iZero++;
00266         }
00267 
00268         if (iPositive > 0 && iNegative > 0)
00269         {
00270             return 0;
00271         }
00272     }
00273     return (iZero == 0 ? (iPositive > 0 ? 1 : -1) : 0);
00274 }
00275 //----------------------------------------------------------------------------
00276 template <class Real>
00277 void IntrTriangle2Triangle2<Real>::ClipConvexPolygonAgainstLine (
00278     const Vector2<Real>& rkN, Real fC, int& riQuantity,
00279     Vector2<Real> akV[6])
00280 {
00281     // The input vertices are assumed to be in counterclockwise order.  The
00282     // ordering is an invariant of this function.
00283 
00284     // test on which side of line the vertices are
00285     int iPositive = 0, iNegative = 0, iPIndex = -1;
00286     Real afTest[6];
00287     int i;
00288     for (i = 0; i < riQuantity; i++)
00289     {
00290         afTest[i] = rkN.Dot(akV[i]) - fC;
00291         if (afTest[i] > (Real)0.0)
00292         {
00293             iPositive++;
00294             if (iPIndex < 0)
00295             {
00296                 iPIndex = i;
00297             }
00298         }
00299         else if (afTest[i] < (Real)0.0)
00300         {
00301             iNegative++;
00302         }
00303     }
00304 
00305     if (iPositive > 0)
00306     {
00307         if (iNegative > 0)
00308         {
00309             // line transversely intersects polygon
00310             Vector2<Real> akCV[6];
00311             int iCQuantity = 0, iCur, iPrv;
00312             Real fT;
00313 
00314             if (iPIndex > 0)
00315             {
00316                 // first clip vertex on line
00317                 iCur = iPIndex;
00318                 iPrv = iCur-1;
00319                 fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
00320                 akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);
00321 
00322                 // vertices on positive side of line
00323                 while (iCur < riQuantity && afTest[iCur] > (Real)0.0)
00324                 {
00325                     akCV[iCQuantity++] = akV[iCur++];
00326                 }
00327 
00328                 // last clip vertex on line
00329                 if (iCur < riQuantity)
00330                 {
00331                     iPrv = iCur-1;
00332                 }
00333                 else
00334                 {
00335                     iCur = 0;
00336                     iPrv = riQuantity - 1;
00337                 }
00338                 fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
00339                 akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);
00340             }
00341             else  // iPIndex is 0
00342             {
00343                 // vertices on positive side of line
00344                 iCur = 0;
00345                 while (iCur < riQuantity && afTest[iCur] > (Real)0.0)
00346                 {
00347                     akCV[iCQuantity++] = akV[iCur++];
00348                 }
00349 
00350                 // last clip vertex on line
00351                 iPrv = iCur-1;
00352                 fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
00353                 akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);
00354 
00355                 // skip vertices on negative side
00356                 while (iCur < riQuantity && afTest[iCur] <= (Real)0.0)
00357                 {
00358                     iCur++;
00359                 }
00360 
00361                 // first clip vertex on line
00362                 if (iCur < riQuantity)
00363                 {
00364                     iPrv = iCur-1;
00365                     fT = afTest[iCur]/(afTest[iCur] - afTest[iPrv]);
00366                     akCV[iCQuantity++] = akV[iCur]+fT*(akV[iPrv]-akV[iCur]);
00367 
00368                     // vertices on positive side of line
00369                     while (iCur < riQuantity && afTest[iCur] > (Real)0.0)
00370                     {
00371                         akCV[iCQuantity++] = akV[iCur++];
00372                     }
00373                 }
00374                 else
00375                 {
00376                     // iCur = 0
00377                     iPrv = riQuantity - 1;
00378                     fT = afTest[0]/(afTest[0] - afTest[iPrv]);
00379                     akCV[iCQuantity++] = akV[0]+fT*(akV[iPrv]-akV[0]);
00380                 }
00381             }
00382 
00383             riQuantity = iCQuantity;
00384             size_t uiSize = iCQuantity*sizeof(Vector2<Real>);
00385             System::Memcpy(akV,uiSize,akCV,uiSize);
00386         }
00387         // else polygon fully on positive side of line, nothing to do
00388     }
00389     else
00390     {
00391         // polygon does not intersect positive side of line, clip all
00392         riQuantity = 0;
00393     }
00394 }
00395 //----------------------------------------------------------------------------
00396 template <class Real>
00397 void IntrTriangle2Triangle2<Real>::ComputeTwo (Configuration& rkCfg,
00398     const Vector2<Real> akV[3], const Vector2<Real>& rkD, int i0, int i1,
00399     int i2)
00400 {
00401     rkCfg.Map = M12;
00402     rkCfg.Index[0] = i0;
00403     rkCfg.Index[1] = i1;
00404     rkCfg.Index[2] = i2;
00405     rkCfg.Min = rkD.Dot(akV[i0] - akV[i1]);
00406     rkCfg.Max = (Real)0.0;
00407 }
00408 //----------------------------------------------------------------------------
00409 template <class Real>
00410 void IntrTriangle2Triangle2<Real>::ComputeThree (Configuration& rkCfg,
00411     const Vector2<Real> akV[3], const Vector2<Real>& rkD,
00412     const Vector2<Real>& rkP)
00413 {
00414     Real fD0 = rkD.Dot(akV[0] - rkP);
00415     Real fD1 = rkD.Dot(akV[1] - rkP);
00416     Real fD2 = rkD.Dot(akV[2] - rkP);
00417 
00418     // Make sure that m_aiIndex[...] is an even permutation of (0,1,2)
00419     // whenever the map value is M12 or M21.  This is needed to guarantee
00420     // the intersection of overlapping edges is properly computed.
00421 
00422     if (fD0 <= fD1)
00423     {
00424         if (fD1 <= fD2)  // d0 <= d1 <= d2
00425         {
00426             if (fD0 != fD1)
00427             {
00428                 rkCfg.Map = (fD1 != fD2 ? M11 : M12);
00429             }
00430             else
00431             {
00432                 rkCfg.Map = M21;
00433             }
00434 
00435             rkCfg.Index[0] = 0;
00436             rkCfg.Index[1] = 1;
00437             rkCfg.Index[2] = 2;
00438             rkCfg.Min = fD0;
00439             rkCfg.Max = fD2;
00440         }
00441         else if (fD0 <= fD2)  // d0 <= d2 < d1
00442         {
00443             if (fD0 != fD2)
00444             {
00445                 rkCfg.Map = M11;
00446                 rkCfg.Index[0] = 0;
00447                 rkCfg.Index[1] = 2;
00448                 rkCfg.Index[2] = 1;
00449             }
00450             else
00451             {
00452                 rkCfg.Map = M21;
00453                 rkCfg.Index[0] = 2;
00454                 rkCfg.Index[1] = 0;
00455                 rkCfg.Index[2] = 1;
00456             }
00457 
00458             rkCfg.Min = fD0;
00459             rkCfg.Max = fD1;
00460         }
00461         else  // d2 < d0 <= d1
00462         {
00463             rkCfg.Map = (fD0 != fD1 ? M12 : M11);
00464             rkCfg.Index[0] = 2;
00465             rkCfg.Index[1] = 0;
00466             rkCfg.Index[2] = 1;
00467             rkCfg.Min = fD2;
00468             rkCfg.Max = fD1;
00469         }
00470     }
00471     else
00472     {
00473         if (fD2 <= fD1)  // d2 <= d1 < d0
00474         {
00475             if (fD2 != fD1)
00476             {
00477                 rkCfg.Map = M11;
00478                 rkCfg.Index[0] = 2;
00479                 rkCfg.Index[1] = 1;
00480                 rkCfg.Index[2] = 0;
00481             }
00482             else
00483             {
00484                 rkCfg.Map = M21;
00485                 rkCfg.Index[0] = 1;
00486                 rkCfg.Index[1] = 2;
00487                 rkCfg.Index[2] = 0;
00488             }
00489 
00490             rkCfg.Min = fD2;
00491             rkCfg.Max = fD0;
00492         }
00493         else if (fD2 <= fD0)  // d1 < d2 <= d0
00494         {
00495             rkCfg.Map = (fD2 != fD0 ? M11 : M12);
00496             rkCfg.Index[0] = 1;
00497             rkCfg.Index[1] = 2;
00498             rkCfg.Index[2] = 0;
00499             rkCfg.Min = fD1;
00500             rkCfg.Max = fD0;
00501         }
00502         else  // d1 < d0 < d2
00503         {
00504             rkCfg.Map = M11;
00505             rkCfg.Index[0] = 1;
00506             rkCfg.Index[1] = 0;
00507             rkCfg.Index[2] = 2;
00508             rkCfg.Min = fD1;
00509             rkCfg.Max = fD2;
00510         }
00511     }
00512 }
00513 //----------------------------------------------------------------------------
00514 template <class Real>
00515 bool IntrTriangle2Triangle2<Real>::NoIntersect (
00516     const Configuration& rkCfg0, const Configuration& rkCfg1, Real fTMax,
00517     Real fSpeed, int& riSide, Configuration& rkTCfg0, Configuration& rkTCfg1,
00518     Real& rfTFirst, Real& rfTLast)
00519 {
00520     Real fInvSpeed, fT;
00521 
00522     if (rkCfg1.Max < rkCfg0.Min)
00523     {
00524         // V1-interval initially on left of V0-interval
00525         if (fSpeed <= (Real)0.0)
00526         {
00527             return true;  // intervals moving apart
00528         }
00529 
00530         // update first time
00531         fInvSpeed = ((Real)1.0)/fSpeed;
00532         fT = (rkCfg0.Min - rkCfg1.Max)*fInvSpeed;
00533         if (fT > rfTFirst)
00534         {
00535             rfTFirst = fT;
00536             riSide = -1;
00537             rkTCfg0 = rkCfg0;
00538             rkTCfg1 = rkCfg1;
00539         }
00540 
00541         // test for exceedance of time interval
00542         if (rfTFirst > fTMax)
00543         {
00544             return true;
00545         }
00546 
00547         // update last time
00548         fT = (rkCfg0.Max - rkCfg1.Min)*fInvSpeed;
00549         if (fT < rfTLast)
00550         {
00551             rfTLast = fT;
00552         }
00553 
00554         // test for separation
00555         if (rfTFirst > rfTLast)
00556         {
00557             return true;
00558         }
00559     }
00560     else if ( rkCfg0.Max < rkCfg1.Min )
00561     {
00562         // V1-interval initially on right of V0-interval
00563         if (fSpeed >= (Real)0.0)
00564         {
00565             return true;  // intervals moving apart
00566         }
00567 
00568         // update first time
00569         fInvSpeed = ((Real)1.0)/fSpeed;
00570         fT = (rkCfg0.Max - rkCfg1.Min)*fInvSpeed;
00571         if (fT > rfTFirst)
00572         {
00573             rfTFirst = fT;
00574             riSide = 1;
00575             rkTCfg0 = rkCfg0;
00576             rkTCfg1 = rkCfg1;
00577         }
00578 
00579         // test for exceedance of time interval
00580         if (rfTFirst > fTMax)
00581         {
00582             return true;
00583         }
00584 
00585         // update last time
00586         fT = (rkCfg0.Min - rkCfg1.Max)*fInvSpeed;
00587         if (fT < rfTLast)
00588         {
00589             rfTLast = fT;
00590         }
00591 
00592         // test for separation
00593         if (rfTFirst > rfTLast)
00594         {
00595             return true;
00596         }
00597     }
00598     else
00599     {
00600         // V0-interval and V1-interval initially overlap
00601         if (fSpeed > (Real)0.0)
00602         {
00603             // update last time
00604             fInvSpeed = ((Real)1.0)/fSpeed;
00605             fT = (rkCfg0.Max - rkCfg1.Min)*fInvSpeed;
00606             if (fT < rfTLast)
00607             {
00608                 rfTLast = fT;
00609             }
00610 
00611             // test for separation
00612             if (rfTFirst > rfTLast)
00613             {
00614                 return true;
00615             }
00616         }
00617         else if (fSpeed < (Real)0.0)
00618         {
00619             // update last time
00620             fInvSpeed = ((Real)1.0)/fSpeed;
00621             fT = (rkCfg0.Min - rkCfg1.Max)*fInvSpeed;
00622             if (fT < rfTLast)
00623             {
00624                 rfTLast = fT;
00625             }
00626 
00627             // test for separation
00628             if (rfTFirst > rfTLast)
00629             {
00630                 return true;
00631             }
00632         }
00633     }
00634 
00635     return false;
00636 }
00637 //----------------------------------------------------------------------------
00638 template <class Real>
00639 void IntrTriangle2Triangle2<Real>::GetIntersection (
00640     const Configuration& rkCfg0, const Configuration& rkCfg1, int iSide,
00641     const Vector2<Real> akV0[3], const Vector2<Real> akV1[3], int& riQuantity,
00642     Vector2<Real> akVertex[6])
00643 {
00644     Vector2<Real> kEdge, kDiff;
00645     const Vector2<Real>* pkOrigin;
00646     Real fInvEdE, fMin, fMax;
00647     int i;
00648 
00649     if (iSide == 1)  // V1-interval contacts V0-interval on right
00650     {
00651         if (rkCfg0.Map == M21 || rkCfg0.Map == M11)
00652         {
00653             riQuantity = 1;
00654             akVertex[0] = akV0[rkCfg0.Index[2]];
00655         }
00656         else if (rkCfg1.Map == M12 || rkCfg1.Map == M11)
00657         {
00658             riQuantity = 1;
00659             akVertex[0] = akV1[rkCfg1.Index[0]];
00660         }
00661         else  // rkCfg0.Map == M12 && rkCfg1.Map == M21 (edge overlap)
00662         {
00663             pkOrigin = &akV0[rkCfg0.Index[1]];
00664             kEdge = akV0[rkCfg0.Index[2]] - *pkOrigin;
00665             fInvEdE = ((Real)1.0)/kEdge.Dot(kEdge);
00666             kDiff = akV1[rkCfg1.Index[1]] - *pkOrigin;
00667             fMin = kEdge.Dot(kDiff)*fInvEdE;
00668             kDiff = akV1[rkCfg1.Index[0]] - *pkOrigin;
00669             fMax = kEdge.Dot(kDiff)*fInvEdE;
00670             assert(fMin <= fMax);
00671             Intersector1<Real> kIntr((Real)0.0,(Real)1.0,fMin,fMax);
00672             riQuantity = kIntr.GetQuantity();
00673             assert(riQuantity > 0);
00674             for (i = 0; i < riQuantity; i++)
00675             {
00676                 akVertex[i] = *pkOrigin + kIntr.GetOverlap(i)*kEdge;
00677             }
00678         }
00679     }
00680     else if (iSide == -1)  // V1-interval contacts V0-interval on left
00681     {
00682         if (rkCfg1.Map == M21 || rkCfg1.Map == M11)
00683         {
00684             riQuantity = 1;
00685             akVertex[0] = akV1[rkCfg1.Index[2]];
00686         }
00687         else if (rkCfg0.Map == M12 || rkCfg0.Map == M11)
00688         {
00689             riQuantity = 1;
00690             akVertex[0] = akV0[rkCfg0.Index[0]];
00691         }
00692         else  // rkCfg1.Map == M12 && rkCfg0.Map == M21 (edge overlap)
00693         {
00694             pkOrigin = &akV1[rkCfg1.Index[1]];
00695             kEdge = akV1[rkCfg1.Index[2]] - *pkOrigin;
00696             fInvEdE = 1.0f/kEdge.Dot(kEdge);
00697             kDiff = akV0[rkCfg0.Index[1]] - *pkOrigin;
00698             fMin = kEdge.Dot(kDiff)*fInvEdE;
00699             kDiff = akV0[rkCfg0.Index[0]] - *pkOrigin;
00700             fMax = kEdge.Dot(kDiff)*fInvEdE;
00701             assert(fMin <= fMax);
00702             Intersector1<Real> kIntr((Real)0.0,(Real)1.0,fMin,fMax);
00703             riQuantity = kIntr.GetQuantity();
00704             assert(riQuantity > 0);
00705             for (i = 0; i < riQuantity; i++)
00706             {
00707                 akVertex[i] = *pkOrigin + kIntr.GetOverlap(i)*kEdge;
00708             }
00709         }
00710     }
00711     else  // triangles were initially intersecting
00712     {
00713         Triangle2<Real> kTri0(akV0), kTri1(akV1);
00714         IntrTriangle2Triangle2 kIntr(kTri0,kTri1);
00715         kIntr.Find();
00716         riQuantity = kIntr.GetQuantity();
00717         for (i = 0; i < riQuantity; i++)
00718         {
00719             akVertex[i] = kIntr.GetPoint(i);
00720         }
00721     }
00722 }
00723 //----------------------------------------------------------------------------
00724 
00725 //----------------------------------------------------------------------------
00726 // explicit instantiation
00727 //----------------------------------------------------------------------------
00728 template WM4_FOUNDATION_ITEM
00729 class IntrTriangle2Triangle2<float>;
00730 
00731 template WM4_FOUNDATION_ITEM
00732 class IntrTriangle2Triangle2<double>;
00733 //----------------------------------------------------------------------------
00734 }

Generated on Wed Nov 23 19:01:04 2011 for FreeCAD by  doxygen 1.6.1