Wm4IntrTriangle3Triangle3.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.2 (2006/07/25)
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     // get edge vectors for triangle0
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     // get normal vector of triangle0
00061     Vector3<Real> kN0 = akE0[0].UnitCross(akE0[1]);
00062 
00063     // project triangle1 onto normal line of triangle0, test for separation
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     // get edge vectors for triangle1
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     // get normal vector of triangle1
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         // triangles are not parallel
00091 
00092         // Project triangle0 onto normal line of triangle1, test for
00093         // separation.
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         // directions E0[i0]xE1[i1]
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  // triangles are parallel (and, in fact, coplanar)
00117     {
00118         // directions N0xE0[i0]
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         // directions N1xE1[i1]
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     // Get the plane of triangle0.
00152     Plane3<Real> kPlane0(m_rkTriangle0.V[0],m_rkTriangle0.V[1],
00153         m_rkTriangle0.V[2]);
00154 
00155     // Compute the signed distances of triangle1 vertices to plane0.  Use
00156     // an epsilon-thick plane test.
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         // Triangle1 is fully on one side of plane0.
00165         return false;
00166     }
00167 
00168     if (iZero1 == 3)
00169     {
00170         // Triangle1 is contained by plane0.
00171         if (ReportCoplanarIntersections)
00172         {
00173             return GetCoplanarIntersection(kPlane0,m_rkTriangle0,
00174                 m_rkTriangle1);
00175         }
00176         return false;
00177     }
00178 
00179     // Check for grazing contact between triangle1 and plane0.
00180     if (iPos1 == 0 || iNeg1 == 0)
00181     {
00182         if (iZero1 == 2)
00183         {
00184             // An edge of triangle1 is in plane0.
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 // iZero1 == 1
00197         {
00198             // A vertex of triangle1 is in plane0.
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     // At this point, triangle1 tranversely intersects plane 0.  Compute the
00211     // line segment of intersection.  Then test for intersection between this
00212     // segment and triangle 0.
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     // iZero1 == 1
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     // velocity relative to triangle0
00261     Vector3<Real> kVel = rkVelocity1 - rkVelocity0;
00262 
00263     // compute edge and normal directions for triangle0
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     // compute edge and normal directions for triangle1
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         // triangles are not parallel
00291 
00292         // direction M
00293         if (!TestOverlap(kM,fTMax,kVel,fTFirst,fTLast))
00294         {
00295             return false;
00296         }
00297 
00298         // directions E[i0]xF[i1]
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  // triangles are parallel (and, in fact, coplanar)
00312     {
00313         // directions NxE[i0]
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         // directions NxF[i1]
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     // velocity relative to triangle0
00346     Vector3<Real> kVel = rkVelocity1 - rkVelocity0;
00347 
00348     ContactSide eSide = CS_NONE;
00349     Configuration kTCfg0, kTCfg1;
00350 
00351     // compute edge and normal directions for triangle0
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     // compute edge and normal directions for triangle1
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         // triangles are not parallel
00379 
00380         // direction M
00381         if (!FindOverlap(kM,fTMax,kVel,eSide,kTCfg0,kTCfg1,fTFirst,fTLast))
00382         {
00383             return false;
00384         }
00385 
00386         // directions E[i0]xF[i1]
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  // triangles are parallel (and, in fact, coplanar)
00401     {
00402         // directions NxE[i0]
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         // directions NxF[i1]
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     // adjust U and V for first time of contact before finding contact set
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     // Compute the signed distances of triangle vertices to the plane.  Use
00495     // an epsilon-thick plane test.
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     // project triangle onto line
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     // compute transverse intersections of triangle edges with line
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     // check for grazing contact
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     // sort
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     // TO DO.
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     // Project triangles onto coordinate plane most aligned with plane
00618     // normal.
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         // project onto yz-plane
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         // project onto xz-plane
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         // project onto xy-plane
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     // 2D triangle intersection routines require counterclockwise ordering
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         // triangle is clockwise, reorder it
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         // triangle is clockwise, reorder it
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     // map 2D intersections back to the 3D triangle space
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     // Constant velocity separating axis test.
00746 
00747     Real fT;
00748 
00749     if (fVMax < fUMin) // V on left of U
00750     {
00751         if (fSpeed <= (Real)0.0) // V moving away from U
00752         {
00753             return false;
00754         }
00755 
00756         // find first time of contact on this axis
00757         fT = (fUMin - fVMax)/fSpeed;
00758         if (fT > rfTFirst)
00759         {
00760             rfTFirst = fT;
00761         }
00762 
00763         // quick out: intersection after desired time interval
00764         if (rfTFirst > fTMax)
00765         {
00766             return false;   
00767         }
00768 
00769         // find last time of contact on this axis
00770         fT = (fUMax - fVMin)/fSpeed;
00771         if (fT < rfTLast)
00772         {
00773             rfTLast = fT;
00774         }
00775 
00776         // quick out: intersection before desired time interval
00777         if (rfTFirst > rfTLast)
00778         {
00779             return false; 
00780         }
00781     }
00782     else if ( fUMax < fVMin )   // V on right of U
00783     {
00784         if (fSpeed >= (Real)0.0) // V moving away from U
00785         {
00786             return false;
00787         }
00788 
00789         // find first time of contact on this axis
00790         fT = (fUMax - fVMin)/fSpeed;
00791         if (fT > rfTFirst)
00792         {
00793             rfTFirst = fT;
00794         }
00795 
00796         // quick out: intersection after desired time interval
00797         if (rfTFirst > fTMax)
00798         {
00799             return false;   
00800         }
00801 
00802         // find last time of contact on this axis
00803         fT = (fUMin - fVMax)/fSpeed;
00804         if (fT < rfTLast)
00805         {
00806             rfTLast = fT;
00807         }
00808 
00809         // quick out: intersection before desired time interval
00810         if (rfTFirst > rfTLast)
00811         {
00812             return false; 
00813         }
00814 
00815     }
00816     else // V and U on overlapping interval
00817     {
00818         if (fSpeed > (Real)0.0)
00819         {
00820             // find last time of contact on this axis
00821             fT = (fUMax - fVMin)/fSpeed;
00822             if (fT < rfTLast)
00823             {
00824                 rfTLast = fT;
00825             }
00826 
00827             // quick out: intersection before desired interval
00828             if (rfTFirst > rfTLast)
00829             {
00830                 return false; 
00831             }
00832         }
00833         else if (fSpeed < (Real)0.0)
00834         {
00835             // find last time of contact on this axis
00836             fT = (fUMin - fVMax)/fSpeed;
00837             if (fT < rfTLast)
00838             {
00839                 rfTLast = fT;
00840             }
00841 
00842             // quick out: intersection before desired interval
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     // Constant velocity separating axis test.  UC and VC are the new
00859     // potential configurations, and TUC and TVC are the best known
00860     // configurations.
00861 
00862     Real fT;
00863 
00864     if (rkVC.Max < rkUC.Min) // V on left of U
00865     {
00866         if (fSpeed <= (Real)0.0) // V moving away from U
00867         {
00868             return false;
00869         }
00870 
00871         // find first time of contact on this axis
00872         fT = (rkUC.Min - rkVC.Max)/fSpeed;
00873 
00874         // If this is the new maximum first time of contact, set side and
00875         // configuration.
00876         if (fT > rfTFirst)
00877         {
00878             rfTFirst = fT;
00879             rkSide = CS_LEFT;
00880             rkTUC = rkUC;
00881             rkTVC = rkVC;
00882         }
00883 
00884         // quick out: intersection after desired interval
00885         if (rfTFirst > fTMax)
00886         {
00887             return false;
00888         }
00889 
00890         // find last time of contact on this axis
00891         fT = (rkUC.Max - rkVC.Min)/fSpeed;
00892         if (fT < rfTLast)
00893         {
00894             rfTLast = fT;
00895         }
00896 
00897         // quick out: intersection before desired interval
00898         if (rfTFirst > rfTLast)
00899         {
00900             return false;
00901         }
00902     }
00903     else if (rkUC.Max < rkVC.Min)   // V on right of U
00904     {
00905         if (fSpeed >= (Real)0.0) // V moving away from U
00906         {
00907             return false;
00908         }
00909 
00910         // find first time of contact on this axis
00911         fT = (rkUC.Max - rkVC.Min)/fSpeed;
00912 
00913         // If this is the new maximum first time of contact, set side and
00914         // configuration.
00915         if (fT > rfTFirst)
00916         {
00917             rfTFirst = fT;
00918             rkSide = CS_RIGHT;
00919             rkTUC = rkUC;
00920             rkTVC = rkVC;
00921         }
00922 
00923         // quick out: intersection after desired interval
00924         if (rfTFirst > fTMax)
00925         {
00926             return false;   
00927         }
00928 
00929         // find last time of contact on this axis
00930         fT = (rkUC.Min - rkVC.Max)/fSpeed;
00931         if (fT < rfTLast)
00932         {
00933             rfTLast = fT;
00934         }
00935 
00936         // quick out: intersection before desired interval
00937         if (rfTFirst > rfTLast)
00938         {
00939             return false;
00940         }
00941     }
00942     else // V and U on overlapping interval
00943     {
00944         if (fSpeed > (Real)0.0)
00945         {
00946             // find last time of contact on this axis
00947             fT = (rkUC.Max - rkVC.Min)/fSpeed;
00948             if (fT < rfTLast)
00949             {
00950                 rfTLast = fT;
00951             }
00952 
00953             // quick out: intersection before desired interval
00954             if (rfTFirst > rfTLast)
00955             {
00956                 return false;
00957             }
00958         }
00959         else if ( fSpeed < (Real)0.0 )
00960         {
00961             // find last time of contact on this axis
00962             fT = (rkUC.Min - rkVC.Max)/fSpeed;
00963             if (fT < rfTLast)
00964             {
00965                 rfTLast = fT;
00966             }
00967 
00968             // quick out: intersection before desired interval
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     // find projections of vertices onto potential separating axis
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     // explicit sort of vertices to construct a Configuration object
01016     if (fD0 <= fD1)
01017     {
01018         if (fD1 <= fD2) // D0 <= D1 <= D2
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 // ( D0 == D1 )
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) // D0 <= D2 < D1
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 // D2 < D0 <= D1
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) // D2 <= D1 < D0
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) // D1 < D2 <= D0
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 // D1 < D0 < D2
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) // tri1 to the right of tri0
01139     {
01140         if (rkCfg0.Map == M21 || rkCfg0.Map == M111)
01141         {
01142             // tri0 touching tri1 at a single point
01143             m_iQuantity = 1;
01144             m_akPoint[0] = rkTri0.V[2];
01145         }
01146         else if (rkCfg1.Map == M12 || rkCfg1.Map == M111)
01147         {
01148             // tri1 touching tri0 at a single point
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                 // edge0-edge1 intersection
01157                 GetEdgeEdgeIntersection(rkTri0.V[1],rkTri0.V[2],rkTri1.V[0],
01158                     rkTri1.V[1]);
01159             }
01160             else // rkCfg1.Map == m3
01161             {
01162                 // uedge-vface intersection
01163                 //Vector3<Real> akEdge0[2] = { rkTri0.V[1], rkTri0.V[2] };
01164                 //FindContactSetColinearLineTri(akEdge0,rkTri1,riQuantity,
01165                 //    akP);
01166             }
01167         }
01168         else // rkCfg0.Map == M3
01169         {
01170             if (rkCfg1.Map == M21)
01171             {
01172                 // face0-edge1 intersection
01173                 //Vector3<Real> akEdge1[2] = { rkTri1.V[0], rkTri1.V[1] };
01174                 //FindContactSetColinearLineTri(akEdge1,rkTri0,riQuantity,
01175                 //    akP);
01176             } 
01177             else // rkCfg1.Map == M3
01178             {
01179                 // face0-face1 intersection
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) // tri1 to the left of tri0
01186     {
01187         if (rkCfg1.Map == M21 || rkCfg1.Map == M111)
01188         {
01189             // tri1 touching tri0 at a single point
01190             m_iQuantity = 1;
01191             m_akPoint[0] = rkTri1.V[2];
01192         }
01193         else if (rkCfg0.Map == M12 || rkCfg0.Map == M111)
01194         {
01195             // tri0 touching tri1 at a single point
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                 // edge0-edge1 intersection
01204                 GetEdgeEdgeIntersection(rkTri0.V[0],rkTri0.V[1],rkTri1.V[1],
01205                     rkTri1.V[2]);
01206             }
01207             else // rkCfg0.Map == M3
01208             {
01209                 // edge1-face0 intersection
01210                 //Vector3<Real> akEdge1[2] = { rkTri1.V[1], rkTri1.V[2] };
01211                 //FindContactSetColinearLineTri(akEdge1,rkTri0,riQuantity,
01212                 //    akP);
01213             }
01214         }
01215         else // rkCfg1.Map == M3
01216         {
01217             if (rkCfg0.Map == M21)
01218             {
01219                 // edge0-face1 intersection
01220                 //Vector3<Real> akEdge0[2] = { rkTri0.V[0], rkTri0.V[1] };
01221                 //FindContactSetColinearLineTri(akEdge0,rkTri1,riQuantity,
01222                 //    akP);
01223             } 
01224             else // rkCfg0.Map == M
01225             {
01226                 // face0-face1 intersection
01227                 Plane3<Real> kPlane0(rkTri0.V[0],rkTri0.V[1],rkTri0.V[2]);
01228                 GetCoplanarIntersection(kPlane0,rkTri0,rkTri1);
01229             }
01230         }
01231     }
01232     else // reSide == CS_NONE
01233     {
01234         // triangles are already intersecting tranversely
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     // TO DO.
01245 }
01246 //----------------------------------------------------------------------------
01247 
01248 //----------------------------------------------------------------------------
01249 // explicit instantiation
01250 //----------------------------------------------------------------------------
01251 template WM4_FOUNDATION_ITEM
01252 class IntrTriangle3Triangle3<float>;
01253 
01254 template WM4_FOUNDATION_ITEM
01255 class IntrTriangle3Triangle3<double>;
01256 //----------------------------------------------------------------------------
01257 }

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