Wm4DistSegment3Segment3.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.1 (2006/12/02)
00016 
00017 #include "Wm4FoundationPCH.h"
00018 #include "Wm4DistSegment3Segment3.h"
00019 
00020 namespace Wm4
00021 {
00022 //----------------------------------------------------------------------------
00023 template <class Real>
00024 DistSegment3Segment3<Real>::DistSegment3Segment3 (
00025     const Segment3<Real>& rkSegment0, const Segment3<Real>& rkSegment1)
00026     :
00027     m_rkSegment0(rkSegment0),
00028     m_rkSegment1(rkSegment1)
00029 {
00030 }
00031 //----------------------------------------------------------------------------
00032 template <class Real>
00033 const Segment3<Real>& DistSegment3Segment3<Real>::GetSegment0 () const
00034 {
00035     return m_rkSegment0;
00036 }
00037 //----------------------------------------------------------------------------
00038 template <class Real>
00039 const Segment3<Real>& DistSegment3Segment3<Real>::GetSegment1 () const
00040 {
00041     return m_rkSegment1;
00042 }
00043 //----------------------------------------------------------------------------
00044 template <class Real>
00045 Real DistSegment3Segment3<Real>::Get ()
00046 {
00047     Real fSqrDist = GetSquared();
00048     return Math<Real>::Sqrt(fSqrDist);
00049 }
00050 //----------------------------------------------------------------------------
00051 template <class Real>
00052 Real DistSegment3Segment3<Real>::GetSquared ()
00053 {
00054     Vector3<Real> kDiff = m_rkSegment0.Origin - m_rkSegment1.Origin;
00055     Real fA01 = -m_rkSegment0.Direction.Dot(m_rkSegment1.Direction);
00056     Real fB0 = kDiff.Dot(m_rkSegment0.Direction);
00057     Real fB1 = -kDiff.Dot(m_rkSegment1.Direction);
00058     Real fC = kDiff.SquaredLength();
00059     Real fDet = Math<Real>::FAbs((Real)1.0 - fA01*fA01);
00060     Real fS0, fS1, fSqrDist, fExtDet0, fExtDet1, fTmpS0, fTmpS1;
00061 
00062     if (fDet >= Math<Real>::ZERO_TOLERANCE)
00063     {
00064         // segments are not parallel
00065         fS0 = fA01*fB1-fB0;
00066         fS1 = fA01*fB0-fB1;
00067         fExtDet0 = m_rkSegment0.Extent*fDet;
00068         fExtDet1 = m_rkSegment1.Extent*fDet;
00069 
00070         if (fS0 >= -fExtDet0)
00071         {
00072             if (fS0 <= fExtDet0)
00073             {
00074                 if (fS1 >= -fExtDet1)
00075                 {
00076                     if (fS1 <= fExtDet1)  // region 0 (interior)
00077                     {
00078                         // minimum at two interior points of 3D lines
00079                         Real fInvDet = ((Real)1.0)/fDet;
00080                         fS0 *= fInvDet;
00081                         fS1 *= fInvDet;
00082                         fSqrDist = fS0*(fS0+fA01*fS1+((Real)2.0)*fB0) +
00083                             fS1*(fA01*fS0+fS1+((Real)2.0)*fB1)+fC;
00084                     }
00085                     else  // region 3 (side)
00086                     {
00087                         fS1 = m_rkSegment1.Extent;
00088                         fTmpS0 = -(fA01*fS1+fB0);
00089                         if (fTmpS0 < -m_rkSegment0.Extent)
00090                         {
00091                             fS0 = -m_rkSegment0.Extent;
00092                             fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00093                                 fS1*(fS1+((Real)2.0)*fB1)+fC;
00094                         }
00095                         else if (fTmpS0 <= m_rkSegment0.Extent)
00096                         {
00097                             fS0 = fTmpS0;
00098                             fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00099                         }
00100                         else
00101                         {
00102                             fS0 = m_rkSegment0.Extent;
00103                             fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00104                                 fS1*(fS1+((Real)2.0)*fB1)+fC;
00105                         }
00106                     }
00107                 }
00108                 else  // region 7 (side)
00109                 {
00110                     fS1 = -m_rkSegment1.Extent;
00111                     fTmpS0 = -(fA01*fS1+fB0);
00112                     if (fTmpS0 < -m_rkSegment0.Extent)
00113                     {
00114                         fS0 = -m_rkSegment0.Extent;
00115                         fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00116                             fS1*(fS1+((Real)2.0)*fB1)+fC;
00117                     }
00118                     else if (fTmpS0 <= m_rkSegment0.Extent)
00119                     {
00120                         fS0 = fTmpS0;
00121                         fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00122                     }
00123                     else
00124                     {
00125                         fS0 = m_rkSegment0.Extent;
00126                         fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00127                             fS1*(fS1+((Real)2.0)*fB1)+fC;
00128                     }
00129                 }
00130             }
00131             else
00132             {
00133                 if (fS1 >= -fExtDet1)
00134                 {
00135                     if (fS1 <= fExtDet1)  // region 1 (side)
00136                     {
00137                         fS0 = m_rkSegment0.Extent;
00138                         fTmpS1 = -(fA01*fS0+fB1);
00139                         if (fTmpS1 < -m_rkSegment1.Extent)
00140                         {
00141                             fS1 = -m_rkSegment1.Extent;
00142                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00143                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00144                         }
00145                         else if (fTmpS1 <= m_rkSegment1.Extent)
00146                         {
00147                             fS1 = fTmpS1;
00148                             fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)+fC;
00149                         }
00150                         else
00151                         {
00152                             fS1 = m_rkSegment1.Extent;
00153                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00154                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00155                         }
00156                     }
00157                     else  // region 2 (corner)
00158                     {
00159                         fS1 = m_rkSegment1.Extent;
00160                         fTmpS0 = -(fA01*fS1+fB0);
00161                         if (fTmpS0 < -m_rkSegment0.Extent)
00162                         {
00163                             fS0 = -m_rkSegment0.Extent;
00164                             fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00165                                 fS1*(fS1+((Real)2.0)*fB1)+fC;
00166                         }
00167                         else if (fTmpS0 <= m_rkSegment0.Extent)
00168                         {
00169                             fS0 = fTmpS0;
00170                             fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00171                         }
00172                         else
00173                         {
00174                             fS0 = m_rkSegment0.Extent;
00175                             fTmpS1 = -(fA01*fS0+fB1);
00176                             if (fTmpS1 < -m_rkSegment1.Extent)
00177                             {
00178                                 fS1 = -m_rkSegment1.Extent;
00179                                 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00180                                     fS0*(fS0+((Real)2.0)*fB0)+fC;
00181                             }
00182                             else if (fTmpS1 <= m_rkSegment1.Extent)
00183                             {
00184                                 fS1 = fTmpS1;
00185                                 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
00186                                     + fC;
00187                             }
00188                             else
00189                             {
00190                                 fS1 = m_rkSegment1.Extent;
00191                                 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00192                                     fS0*(fS0+((Real)2.0)*fB0)+fC;
00193                             }
00194                         }
00195                     }
00196                 }
00197                 else  // region 8 (corner)
00198                 {
00199                     fS1 = -m_rkSegment1.Extent;
00200                     fTmpS0 = -(fA01*fS1+fB0);
00201                     if (fTmpS0 < -m_rkSegment0.Extent)
00202                     {
00203                         fS0 = -m_rkSegment0.Extent;
00204                         fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00205                             fS1*(fS1+((Real)2.0)*fB1)+fC;
00206                     }
00207                     else if (fTmpS0 <= m_rkSegment0.Extent)
00208                     {
00209                         fS0 = fTmpS0;
00210                         fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00211                     }
00212                     else
00213                     {
00214                         fS0 = m_rkSegment0.Extent;
00215                         fTmpS1 = -(fA01*fS0+fB1);
00216                         if (fTmpS1 > m_rkSegment1.Extent)
00217                         {
00218                             fS1 = m_rkSegment1.Extent;
00219                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00220                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00221                         }
00222                         else if (fTmpS1 >= -m_rkSegment1.Extent)
00223                         {
00224                             fS1 = fTmpS1;
00225                             fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
00226                                 + fC;
00227                         }
00228                         else
00229                         {
00230                             fS1 = -m_rkSegment1.Extent;
00231                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00232                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00233                         }
00234                     }
00235                 }
00236             }
00237         }
00238         else 
00239         {
00240             if (fS1 >= -fExtDet1)
00241             {
00242                 if (fS1 <= fExtDet1)  // region 5 (side)
00243                 {
00244                     fS0 = -m_rkSegment0.Extent;
00245                     fTmpS1 = -(fA01*fS0+fB1);
00246                     if (fTmpS1 < -m_rkSegment1.Extent)
00247                     {
00248                         fS1 = -m_rkSegment1.Extent;
00249                         fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00250                             fS0*(fS0+((Real)2.0)*fB0)+fC;
00251                     }
00252                     else if (fTmpS1 <= m_rkSegment1.Extent)
00253                     {
00254                         fS1 = fTmpS1;
00255                         fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)+fC;
00256                     }
00257                     else
00258                     {
00259                         fS1 = m_rkSegment1.Extent;
00260                         fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00261                             fS0*(fS0+((Real)2.0)*fB0)+fC;
00262                     }
00263                 }
00264                 else  // region 4 (corner)
00265                 {
00266                     fS1 = m_rkSegment1.Extent;
00267                     fTmpS0 = -(fA01*fS1+fB0);
00268                     if (fTmpS0 > m_rkSegment0.Extent)
00269                     {
00270                         fS0 = m_rkSegment0.Extent;
00271                         fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00272                             fS1*(fS1+((Real)2.0)*fB1)+fC;
00273                     }
00274                     else if (fTmpS0 >= -m_rkSegment0.Extent)
00275                     {
00276                         fS0 = fTmpS0;
00277                         fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00278                     }
00279                     else
00280                     {
00281                         fS0 = -m_rkSegment0.Extent;
00282                         fTmpS1 = -(fA01*fS0+fB1);
00283                         if (fTmpS1 < -m_rkSegment1.Extent)
00284                         {
00285                             fS1 = -m_rkSegment1.Extent;
00286                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00287                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00288                         }
00289                         else if (fTmpS1 <= m_rkSegment1.Extent)
00290                         {
00291                             fS1 = fTmpS1;
00292                             fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
00293                                 + fC;
00294                         }
00295                         else
00296                         {
00297                             fS1 = m_rkSegment1.Extent;
00298                             fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00299                                 fS0*(fS0+((Real)2.0)*fB0)+fC;
00300                         }
00301                     }
00302                 }
00303             }
00304             else   // region 6 (corner)
00305             {
00306                 fS1 = -m_rkSegment1.Extent;
00307                 fTmpS0 = -(fA01*fS1+fB0);
00308                 if (fTmpS0 > m_rkSegment0.Extent)
00309                 {
00310                     fS0 = m_rkSegment0.Extent;
00311                     fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
00312                         fS1*(fS1+((Real)2.0)*fB1)+fC;
00313                 }
00314                 else if (fTmpS0 >= -m_rkSegment0.Extent)
00315                 {
00316                     fS0 = fTmpS0;
00317                     fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
00318                 }
00319                 else
00320                 {
00321                     fS0 = -m_rkSegment0.Extent;
00322                     fTmpS1 = -(fA01*fS0+fB1);
00323                     if (fTmpS1 < -m_rkSegment1.Extent)
00324                     {
00325                         fS1 = -m_rkSegment1.Extent;
00326                         fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00327                             fS0*(fS0+((Real)2.0)*fB0)+fC;
00328                     }
00329                     else if (fTmpS1 <= m_rkSegment1.Extent)
00330                     {
00331                         fS1 = fTmpS1;
00332                         fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
00333                             + fC;
00334                     }
00335                     else
00336                     {
00337                         fS1 = m_rkSegment1.Extent;
00338                         fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
00339                             fS0*(fS0+((Real)2.0)*fB0)+fC;
00340                     }
00341                 }
00342             }
00343         }
00344     }
00345     else
00346     {
00347         // The segments are parallel.  The average b0 term is designed to
00348         // ensure symmetry of the function.  That is, dist(seg0,seg1) and
00349         // dist(seg1,seg0) should produce the same number.
00350         Real fE0pE1 = m_rkSegment0.Extent + m_rkSegment1.Extent;
00351         Real fSign = (fA01 > (Real)0.0 ? (Real)-1.0 : (Real)1.0);
00352         Real fB0Avr = ((Real)0.5)*(fB0 - fSign*fB1);
00353         Real fLambda = -fB0Avr;
00354         if (fLambda < -fE0pE1)
00355         {
00356             fLambda = -fE0pE1;
00357         }
00358         else if (fLambda > fE0pE1)
00359         {
00360             fLambda = fE0pE1;
00361         }
00362 
00363         fS1 = -fSign*fLambda*m_rkSegment1.Extent/fE0pE1;
00364         fS0 = fLambda + fSign*fS1;
00365         fSqrDist = fLambda*(fLambda + ((Real)2.0)*fB0Avr) + fC;
00366     }
00367 
00368     m_kClosestPoint0 = m_rkSegment0.Origin + fS0*m_rkSegment0.Direction;
00369     m_kClosestPoint1 = m_rkSegment1.Origin + fS1*m_rkSegment1.Direction;
00370     m_fSegment0Parameter = fS0;
00371     m_fSegment1Parameter = fS1;
00372     return Math<Real>::FAbs(fSqrDist);
00373 }
00374 //----------------------------------------------------------------------------
00375 template <class Real>
00376 Real DistSegment3Segment3<Real>::Get (Real fS1,
00377     const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
00378 {
00379     Vector3<Real> kMOrigin0 = m_rkSegment0.Origin + fS1*rkVelocity0;
00380     Vector3<Real> kMOrigin1 = m_rkSegment1.Origin + fS1*rkVelocity1;
00381     Segment3<Real> kMSegment0(kMOrigin0,m_rkSegment0.Direction,
00382         m_rkSegment0.Extent);
00383     Segment3<Real> kMSegment1(kMOrigin1,m_rkSegment1.Direction,
00384         m_rkSegment1.Extent);
00385     return DistSegment3Segment3<Real>(kMSegment0,kMSegment1).Get();
00386 }
00387 //----------------------------------------------------------------------------
00388 template <class Real>
00389 Real DistSegment3Segment3<Real>::GetSquared (Real fS1,
00390     const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
00391 {
00392     Vector3<Real> kMOrigin0 = m_rkSegment0.Origin + fS1*rkVelocity0;
00393     Vector3<Real> kMOrigin1 = m_rkSegment1.Origin + fS1*rkVelocity1;
00394     Segment3<Real> kMSegment0(kMOrigin0,m_rkSegment0.Direction,
00395         m_rkSegment0.Extent);
00396     Segment3<Real> kMSegment1(kMOrigin1,m_rkSegment1.Direction,
00397         m_rkSegment1.Extent);
00398     return DistSegment3Segment3<Real>(kMSegment0,kMSegment1).GetSquared();
00399 }
00400 //----------------------------------------------------------------------------
00401 template <class Real>
00402 Real DistSegment3Segment3<Real>::GetSegment0Parameter () const
00403 {
00404     return m_fSegment0Parameter;
00405 }
00406 //----------------------------------------------------------------------------
00407 template <class Real>
00408 Real DistSegment3Segment3<Real>::GetSegment1Parameter () const
00409 {
00410     return m_fSegment1Parameter;
00411 }
00412 //----------------------------------------------------------------------------
00413 
00414 //----------------------------------------------------------------------------
00415 // explicit instantiation
00416 //----------------------------------------------------------------------------
00417 template WM4_FOUNDATION_ITEM
00418 class DistSegment3Segment3<float>;
00419 
00420 template WM4_FOUNDATION_ITEM
00421 class DistSegment3Segment3<double>;
00422 //----------------------------------------------------------------------------
00423 }