RSS.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "fcl/BV/RSS.h"
00038 #include "fcl/BVH/BVH_utility.h"
00039 #include <iostream>
00040 namespace fcl
00041 {
00042 
00044 void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b)
00045 {
00046   if(val < a) val = a;
00047   else if(val > b) val = b;
00048 }
00049 
00058 void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
00059 {
00060   FCL_REAL denom = 1 - A_dot_B * A_dot_B;
00061 
00062   if(denom == 0) t = 0;
00063   else
00064   {
00065     t = (A_dot_T - B_dot_T * A_dot_B) / denom;
00066     clipToRange(t, 0, a);
00067   }
00068 
00069   u = t * A_dot_B - B_dot_T;
00070   if(u < 0)
00071   {
00072     u = 0;
00073     t = A_dot_T;
00074     clipToRange(t, 0, a);
00075   }
00076   else if(u > b)
00077   {
00078     u = b;
00079     t = u * A_dot_B + A_dot_T;
00080     clipToRange(t, 0, a);
00081   }
00082 }
00083 
00089 bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
00090 {
00091   if(fabs(Anorm_dot_B) < 1e-7) return false;
00092 
00093   FCL_REAL t, u, v;
00094 
00095   u = -Anorm_dot_T / Anorm_dot_B;
00096   clipToRange(u, 0, b);
00097 
00098   t = u * A_dot_B + A_dot_T;
00099   clipToRange(t, 0, a);
00100 
00101   v = t * A_dot_B - B_dot_T;
00102 
00103   if(Anorm_dot_B > 0)
00104   {
00105     if(v > (u + 1e-7)) return true;
00106   }
00107   else
00108   {
00109     if(v < (u - 1e-7)) return true;
00110   }
00111   return false;
00112 }
00113 
00114 
00116 FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL)
00117 {
00118   FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
00119 
00120   A0_dot_B0 = Rab(0, 0);
00121   A0_dot_B1 = Rab(0, 1);
00122   A1_dot_B0 = Rab(1, 0);
00123   A1_dot_B1 = Rab(1, 1);
00124 
00125   FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
00126   FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
00127 
00128   aA0_dot_B0 = a[0] * A0_dot_B0;
00129   aA0_dot_B1 = a[0] * A0_dot_B1;
00130   aA1_dot_B0 = a[1] * A1_dot_B0;
00131   aA1_dot_B1 = a[1] * A1_dot_B1;
00132   bA0_dot_B0 = b[0] * A0_dot_B0;
00133   bA1_dot_B0 = b[0] * A1_dot_B0;
00134   bA0_dot_B1 = b[1] * A0_dot_B1;
00135   bA1_dot_B1 = b[1] * A1_dot_B1;
00136 
00137   Vec3f Tba = Rab.transposeTimes(Tab);
00138 
00139   Vec3f S;
00140   FCL_REAL t, u;
00141 
00142   // determine if any edge pair contains the closest points
00143 
00144   FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x;
00145   FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x;
00146   FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
00147 
00148   ALL_x = -Tba[0];
00149   ALU_x = ALL_x + aA1_dot_B0;
00150   AUL_x = ALL_x + aA0_dot_B0;
00151   AUU_x = ALU_x + aA0_dot_B0;
00152 
00153   if(ALL_x < ALU_x)
00154   {
00155     LA1_lx = ALL_x;
00156     LA1_ux = ALU_x;
00157     UA1_lx = AUL_x;
00158     UA1_ux = AUU_x;
00159   }
00160   else
00161   {
00162     LA1_lx = ALU_x;
00163     LA1_ux = ALL_x;
00164     UA1_lx = AUU_x;
00165     UA1_ux = AUL_x;
00166   }
00167 
00168   BLL_x = Tab[0];
00169   BLU_x = BLL_x + bA0_dot_B1;
00170   BUL_x = BLL_x + bA0_dot_B0;
00171   BUU_x = BLU_x + bA0_dot_B0;
00172 
00173   if(BLL_x < BLU_x)
00174   {
00175     LB1_lx = BLL_x;
00176     LB1_ux = BLU_x;
00177     UB1_lx = BUL_x;
00178     UB1_ux = BUU_x;
00179   }
00180   else
00181   {
00182     LB1_lx = BLU_x;
00183     LB1_ux = BLL_x;
00184     UB1_lx = BUU_x;
00185     UB1_ux = BUL_x;
00186   }
00187 
00188   // UA1, UB1
00189 
00190   if((UA1_ux > b[0]) && (UB1_ux > a[0]))
00191   {
00192     if(((UA1_lx > b[0]) ||
00193         inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
00194                   A1_dot_B1, aA0_dot_B1 - Tba[1],
00195                   -Tab[1] - bA1_dot_B0))
00196        &&
00197        ((UB1_lx > a[0]) ||
00198         inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
00199                   A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
00200     {
00201       segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
00202                 Tba[1] - aA0_dot_B1);
00203 
00204       S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
00205       S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
00206       S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
00207 
00208       if(P && Q)
00209       {
00210         P->setValue(a[0], t, 0);
00211         *Q = S + (*P);
00212       }
00213 
00214       return S.length();
00215     }
00216   }
00217 
00218 
00219   // UA1, LB1
00220 
00221   if((UA1_lx < 0) && (LB1_ux > a[0]))
00222   {
00223     if(((UA1_ux < 0) ||
00224         inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
00225                   A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
00226        &&
00227        ((LB1_lx > a[0]) ||
00228         inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
00229                   A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
00230     {
00231       segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
00232 
00233       S[0] = Tab[0] + Rab(0, 1) * u - a[0];
00234       S[1] = Tab[1] + Rab(1, 1) * u - t;
00235       S[2] = Tab[2] + Rab(2, 1) * u;
00236 
00237       if(P && Q)
00238       {
00239         P->setValue(a[0], t, 0);
00240         *Q = S + (*P);
00241       }
00242 
00243       return S.length();
00244     }
00245   }
00246 
00247   // LA1, UB1
00248 
00249   if((LA1_ux > b[0]) && (UB1_lx < 0))
00250   {
00251     if(((LA1_lx > b[0]) ||
00252         inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
00253                   A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
00254        &&
00255        ((UB1_ux < 0) ||
00256         inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
00257                   A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
00258     {
00259       segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
00260 
00261       S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
00262       S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
00263       S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
00264 
00265       if(P && Q)
00266       {
00267         P->setValue(0, t, 0);
00268         *Q = S + (*P);
00269       }
00270 
00271       return S.length();
00272     }
00273   }
00274 
00275   // LA1, LB1
00276 
00277   if((LA1_lx < 0) && (LB1_lx < 0))
00278   {
00279     if (((LA1_ux < 0) ||
00280          inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
00281                    -Tba[1], -Tab[1]))
00282         &&
00283         ((LB1_ux < 0) ||
00284          inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
00285                    Tab[1], Tba[1])))
00286     {
00287       segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
00288 
00289       S[0] = Tab[0] + Rab(0, 1) * u;
00290       S[1] = Tab[1] + Rab(1, 1) * u - t;
00291       S[2] = Tab[2] + Rab(2, 1) * u;
00292 
00293       if(P && Q)
00294       {
00295         P->setValue(0, t, 0);
00296         *Q = S + (*P);
00297       }
00298 
00299       return S.length();
00300     }
00301   }
00302 
00303   FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y;
00304 
00305   ALL_y = -Tba[1];
00306   ALU_y = ALL_y + aA1_dot_B1;
00307   AUL_y = ALL_y + aA0_dot_B1;
00308   AUU_y = ALU_y + aA0_dot_B1;
00309 
00310   FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
00311 
00312   if(ALL_y < ALU_y)
00313   {
00314     LA1_ly = ALL_y;
00315     LA1_uy = ALU_y;
00316     UA1_ly = AUL_y;
00317     UA1_uy = AUU_y;
00318   }
00319   else
00320   {
00321     LA1_ly = ALU_y;
00322     LA1_uy = ALL_y;
00323     UA1_ly = AUU_y;
00324     UA1_uy = AUL_y;
00325   }
00326 
00327   if(BLL_x < BUL_x)
00328   {
00329     LB0_lx = BLL_x;
00330     LB0_ux = BUL_x;
00331     UB0_lx = BLU_x;
00332     UB0_ux = BUU_x;
00333   }
00334   else
00335   {
00336     LB0_lx = BUL_x;
00337     LB0_ux = BLL_x;
00338     UB0_lx = BUU_x;
00339     UB0_ux = BLU_x;
00340   }
00341 
00342   // UA1, UB0
00343 
00344   if((UA1_uy > b[1]) && (UB0_ux > a[0]))
00345   {
00346     if(((UA1_ly > b[1]) ||
00347         inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
00348                   A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
00349        &&
00350        ((UB0_lx > a[0]) ||
00351         inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
00352                   A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
00353     {
00354       segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
00355                 Tba[0] - aA0_dot_B0);
00356 
00357       S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ;
00358       S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
00359       S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
00360 
00361       if(P && Q)
00362       {
00363         P->setValue(a[0], t, 0);
00364         *Q = S + (*P);
00365       }
00366 
00367       return S.length();
00368     }
00369   }
00370 
00371   // UA1, LB0
00372 
00373   if((UA1_ly < 0) && (LB0_ux > a[0]))
00374   {
00375     if(((UA1_uy < 0) ||
00376         inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
00377                   aA0_dot_B0 - Tba[0], -Tab[1]))
00378        &&
00379        ((LB0_lx > a[0]) ||
00380         inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
00381                   A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
00382     {
00383       segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
00384 
00385       S[0] = Tab[0] + Rab(0, 0) * u - a[0];
00386       S[1] = Tab[1] + Rab(1, 0) * u - t;
00387       S[2] = Tab[2] + Rab(2, 0) * u;
00388 
00389       if(P && Q)
00390       {
00391         P->setValue(a[0], t, 0);
00392         *Q = S + (*P);
00393       }
00394 
00395       return S.length();
00396     }
00397   }
00398 
00399   // LA1, UB0
00400 
00401   if((LA1_uy > b[1]) && (UB0_lx < 0))
00402   {
00403     if(((LA1_ly > b[1]) ||
00404         inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
00405                   A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
00406        &&
00407 
00408        ((UB0_ux < 0) ||
00409         inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
00410                   Tab[1] + bA1_dot_B1, Tba[0])))
00411     {
00412       segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
00413 
00414       S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
00415       S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
00416       S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
00417 
00418       if(P && Q)
00419       {
00420         P->setValue(0, t, 0);
00421         *Q = S + (*P);
00422       }
00423 
00424 
00425       return S.length();
00426     }
00427   }
00428 
00429   // LA1, LB0
00430 
00431   if((LA1_ly < 0) && (LB0_lx < 0))
00432   {
00433     if(((LA1_uy < 0) ||
00434         inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
00435                   -Tba[0], -Tab[1]))
00436        &&
00437        ((LB0_ux < 0) ||
00438         inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
00439                   Tab[1], Tba[0])))
00440     {
00441       segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
00442 
00443       S[0] = Tab[0] + Rab(0, 0) * u;
00444       S[1] = Tab[1] + Rab(1, 0) * u - t;
00445       S[2] = Tab[2] + Rab(2, 0) * u;
00446 
00447       if(P&& Q)
00448       {
00449         P->setValue(0, t, 0);
00450         *Q = S + (*P);
00451       }
00452 
00453       return S.length();
00454     }
00455   }
00456 
00457   FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y;
00458 
00459   BLL_y = Tab[1];
00460   BLU_y = BLL_y + bA1_dot_B1;
00461   BUL_y = BLL_y + bA1_dot_B0;
00462   BUU_y = BLU_y + bA1_dot_B0;
00463 
00464   FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
00465 
00466   if(ALL_x < AUL_x)
00467   {
00468     LA0_lx = ALL_x;
00469     LA0_ux = AUL_x;
00470     UA0_lx = ALU_x;
00471     UA0_ux = AUU_x;
00472   }
00473   else
00474   {
00475     LA0_lx = AUL_x;
00476     LA0_ux = ALL_x;
00477     UA0_lx = AUU_x;
00478     UA0_ux = ALU_x;
00479   }
00480 
00481   if(BLL_y < BLU_y)
00482   {
00483     LB1_ly = BLL_y;
00484     LB1_uy = BLU_y;
00485     UB1_ly = BUL_y;
00486     UB1_uy = BUU_y;
00487   }
00488   else
00489   {
00490     LB1_ly = BLU_y;
00491     LB1_uy = BLL_y;
00492     UB1_ly = BUU_y;
00493     UB1_uy = BUL_y;
00494   }
00495 
00496   // UA0, UB1
00497 
00498   if((UA0_ux > b[0]) && (UB1_uy > a[1]))
00499   {
00500     if(((UA0_lx > b[0]) ||
00501         inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
00502                   A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
00503        &&
00504        ((UB1_ly > a[1]) ||
00505         inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
00506                   A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
00507     {
00508       segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
00509                 Tba[1] - aA1_dot_B1);
00510 
00511       S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
00512       S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
00513       S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
00514 
00515       if(P && Q)
00516       {
00517         P->setValue(t, a[1], 0);
00518         *Q = S + (*P);
00519       }
00520 
00521       return S.length();
00522     }
00523   }
00524 
00525   // UA0, LB1
00526 
00527   if((UA0_lx < 0) && (LB1_uy > a[1]))
00528   {
00529     if(((UA0_ux < 0) ||
00530         inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
00531                   aA1_dot_B1 - Tba[1], -Tab[0]))
00532        &&
00533        ((LB1_ly > a[1]) ||
00534         inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
00535                   Tba[1] - aA1_dot_B1)))
00536     {
00537       segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
00538 
00539       S[0] = Tab[0] + Rab(0, 1) * u - t;
00540       S[1] = Tab[1] + Rab(1, 1) * u - a[1];
00541       S[2] = Tab[2] + Rab(2, 1) * u;
00542 
00543       if(P && Q)
00544       {
00545         P->setValue(t, a[1], 0);
00546         *Q = S + (*P);
00547       }
00548 
00549       return S.length();
00550     }
00551   }
00552 
00553   // LA0, UB1
00554 
00555   if((LA0_ux > b[0]) && (UB1_ly < 0))
00556   {
00557     if(((LA0_lx > b[0]) ||
00558         inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
00559                   -bA0_dot_B0 - Tab[0]))
00560        &&
00561        ((UB1_uy < 0) ||
00562         inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
00563                   Tab[0] + bA0_dot_B0, Tba[1])))
00564     {
00565       segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
00566 
00567       S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
00568       S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
00569       S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
00570 
00571       if(P && Q)
00572       {
00573         P->setValue(t, 0, 0);
00574         *Q = S + (*P);
00575       }
00576 
00577       return S.length();
00578     }
00579   }
00580 
00581   // LA0, LB1
00582 
00583   if((LA0_lx < 0) && (LB1_ly < 0))
00584   {
00585     if(((LA0_ux < 0) ||
00586         inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
00587                   -Tab[0]))
00588        &&
00589        ((LB1_uy < 0) ||
00590         inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
00591                   Tab[0], Tba[1])))
00592     {
00593       segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
00594 
00595       S[0] = Tab[0] + Rab(0, 1) * u - t;
00596       S[1] = Tab[1] + Rab(1, 1) * u;
00597       S[2] = Tab[2] + Rab(2, 1) * u;
00598 
00599       if(P && Q)
00600       {
00601         P->setValue(t, 0, 0);
00602         *Q = S + (*P);
00603       }
00604 
00605       return S.length();
00606     }
00607   }
00608 
00609   FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
00610 
00611   if(ALL_y < AUL_y)
00612   {
00613     LA0_ly = ALL_y;
00614     LA0_uy = AUL_y;
00615     UA0_ly = ALU_y;
00616     UA0_uy = AUU_y;
00617   }
00618   else
00619   {
00620     LA0_ly = AUL_y;
00621     LA0_uy = ALL_y;
00622     UA0_ly = AUU_y;
00623     UA0_uy = ALU_y;
00624   }
00625 
00626   if(BLL_y < BUL_y)
00627   {
00628     LB0_ly = BLL_y;
00629     LB0_uy = BUL_y;
00630     UB0_ly = BLU_y;
00631     UB0_uy = BUU_y;
00632   }
00633   else
00634   {
00635     LB0_ly = BUL_y;
00636     LB0_uy = BLL_y;
00637     UB0_ly = BUU_y;
00638     UB0_uy = BLU_y;
00639   }
00640 
00641   // UA0, UB0
00642 
00643   if((UA0_uy > b[1]) && (UB0_uy > a[1]))
00644   {
00645     if(((UA0_ly > b[1]) ||
00646         inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
00647                   A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
00648        &&
00649        ((UB0_ly > a[1]) ||
00650         inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
00651                   Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
00652     {
00653       segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
00654                 Tba[0] - aA1_dot_B0);
00655 
00656       S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
00657       S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
00658       S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
00659 
00660       if(P && Q)
00661       {
00662         P->setValue(t, a[1], 0);
00663         *Q = S + (*P);
00664       }
00665 
00666       return S.length();
00667     }
00668   }
00669 
00670   // UA0, LB0
00671 
00672   if((UA0_ly < 0) && (LB0_uy > a[1]))
00673   {
00674     if(((UA0_uy < 0) ||
00675         inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
00676                   aA1_dot_B0 - Tba[0], -Tab[0]))
00677        &&
00678        ((LB0_ly > a[1]) ||
00679         inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
00680                   A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
00681     {
00682       segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
00683 
00684       S[0] = Tab[0] + Rab(0, 0) * u - t;
00685       S[1] = Tab[1] + Rab(1, 0) * u - a[1];
00686       S[2] = Tab[2] + Rab(2, 0) * u;
00687 
00688       if(P && Q)
00689       {
00690         P->setValue(t, a[1], 0);
00691         *Q = S + (*P);
00692       }
00693 
00694       return S.length();
00695     }
00696   }
00697 
00698   // LA0, UB0
00699 
00700   if((LA0_uy > b[1]) && (UB0_ly < 0))
00701   {
00702     if(((LA0_ly > b[1]) ||
00703         inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
00704                   -Tab[0] - bA0_dot_B1))
00705        &&
00706 
00707        ((UB0_uy < 0) ||
00708         inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
00709                   Tab[0] + bA0_dot_B1, Tba[0])))
00710     {
00711       segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
00712 
00713       S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
00714       S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
00715       S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
00716 
00717       if(P && Q)
00718       {
00719         P->setValue(t, 0, 0);
00720         *Q = S + (*P);
00721       }
00722 
00723       return S.length();
00724     }
00725   }
00726 
00727   // LA0, LB0
00728 
00729   if((LA0_ly < 0) && (LB0_ly < 0))
00730   {
00731     if(((LA0_uy < 0) ||
00732         inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
00733                   -Tba[0], -Tab[0]))
00734        &&
00735        ((LB0_uy < 0) ||
00736         inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
00737                   Tab[0], Tba[0])))
00738     {
00739       segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
00740 
00741       S[0] = Tab[0] + Rab(0, 0) * u - t;
00742       S[1] = Tab[1] + Rab(1, 0) * u;
00743       S[2] = Tab[2] + Rab(2, 0) * u;
00744 
00745       if(P && Q)
00746       {
00747         P->setValue(t, 0, 0);
00748         *Q = S + (*P);
00749       }
00750 
00751       return S.length();
00752     }
00753   }
00754 
00755   // no edges passed, take max separation along face normals
00756 
00757   FCL_REAL sep1, sep2;
00758 
00759   if(Tab[2] > 0.0)
00760   {
00761     sep1 = Tab[2];
00762     if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
00763     if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
00764   }
00765   else
00766   {
00767     sep1 = -Tab[2];
00768     if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
00769     if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
00770   }
00771 
00772   if(Tba[2] < 0)
00773   {
00774     sep2 = -Tba[2];
00775     if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
00776     if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
00777   }
00778   else
00779   {
00780     sep2 = Tba[2];
00781     if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
00782     if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
00783   }
00784 
00785   if(sep1 >= sep2 && sep1 >= 0)
00786   {
00787     if(Tab[2] > 0)
00788       S.setValue(0, 0, sep1);
00789     else
00790       S.setValue(0, 0, -sep1);
00791 
00792     if(P && Q)
00793     {
00794       *Q = S;
00795       P->setValue(0);
00796     }
00797   }
00798 
00799   if(sep2 >= sep1 && sep2 >= 0)
00800   {
00801     Vec3f Q_(Tab[0], Tab[1], Tab[2]);
00802     Vec3f P_;
00803     if(Tba[2] < 0)
00804     {
00805       P_[0] = Rab(0, 2) * sep2 + Tab[0];
00806       P_[1] = Rab(1, 2) * sep2 + Tab[1];
00807       P_[2] = Rab(2, 2) * sep2 + Tab[2];
00808     }
00809     else
00810     {
00811       P_[0] = -Rab(0, 2) * sep2 + Tab[0];
00812       P_[1] = -Rab(1, 2) * sep2 + Tab[1];
00813       P_[2] = -Rab(2, 2) * sep2 + Tab[2];
00814     }
00815 
00816     S = Q_ - P_;
00817 
00818     if(P && Q)
00819     {
00820       *P = P_;
00821       *Q = Q_;
00822     }
00823   }
00824 
00825   FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2);
00826   return (sep > 0 ? sep : 0);
00827 }
00828 
00829 
00830 
00831 bool RSS::overlap(const RSS& other) const
00832 {
00836 
00838   Vec3f t = other.Tr - Tr; 
00839 
00841   Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2]));
00842 
00844   Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
00845              axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
00846              axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
00847 
00848   FCL_REAL dist = rectDistance(R, T, l, other.l);
00849   return (dist <= (r + other.r));
00850 }
00851 
00852 bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
00853 {
00854   Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
00855                 R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
00856                 R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
00857 
00858   Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
00859              R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
00860              R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
00861 
00862   Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
00863   Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
00864 
00865   FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
00866   return (dist <= (b1.r + b2.r));
00867 }
00868 
00869 bool RSS::contain(const Vec3f& p) const
00870 {
00871   Vec3f local_p = p - Tr;
00872   FCL_REAL proj0 = local_p.dot(axis[0]);
00873   FCL_REAL proj1 = local_p.dot(axis[1]);
00874   FCL_REAL proj2 = local_p.dot(axis[2]);
00875   FCL_REAL abs_proj2 = fabs(proj2);
00876   Vec3f proj(proj0, proj1, proj2);
00877 
00879   if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
00880   {
00881     return (abs_proj2 < r);
00882   }
00883   else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
00884   {
00885     FCL_REAL y = (proj1 > 0) ? l[1] : 0;
00886     Vec3f v(proj0, y, 0);
00887     return ((proj - v).sqrLength() < r * r);
00888   }
00889   else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
00890   {
00891     FCL_REAL x = (proj0 > 0) ? l[0] : 0;
00892     Vec3f v(x, proj1, 0);
00893     return ((proj - v).sqrLength() < r * r);
00894   }
00895   else
00896   {
00897     FCL_REAL x = (proj0 > 0) ? l[0] : 0;
00898     FCL_REAL y = (proj1 > 0) ? l[1] : 0;
00899     Vec3f v(x, y, 0);
00900     return ((proj - v).sqrLength() < r * r);
00901   }
00902 }
00903 
00904 RSS& RSS::operator += (const Vec3f& p)
00905 {
00906   Vec3f local_p = p - Tr;
00907   FCL_REAL proj0 = local_p.dot(axis[0]);
00908   FCL_REAL proj1 = local_p.dot(axis[1]);
00909   FCL_REAL proj2 = local_p.dot(axis[2]);
00910   FCL_REAL abs_proj2 = fabs(proj2);
00911   Vec3f proj(proj0, proj1, proj2);
00912 
00913   // projection is within the rectangle
00914   if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
00915   {
00916     if(abs_proj2 < r)
00917       ; // do nothing
00918     else
00919     {
00920       r = 0.5 * (r + abs_proj2); // enlarge the r
00921       // change RSS origin position
00922       if(proj2 > 0)
00923         Tr[2] += 0.5 * (abs_proj2 - r);
00924       else
00925         Tr[2] -= 0.5 * (abs_proj2 - r);
00926     }
00927   }
00928   else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
00929   {
00930     FCL_REAL y = (proj1 > 0) ? l[1] : 0;
00931     Vec3f v(proj0, y, 0);
00932     FCL_REAL new_r_sqr = (proj - v).sqrLength();
00933     if(new_r_sqr < r * r)
00934       ; // do nothing
00935     else
00936     {
00937       if(abs_proj2 < r)
00938       {
00939         FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
00940         l[1] += delta_y;
00941         if(proj1 < 0)
00942           Tr[1] -= delta_y;
00943       }
00944       else
00945       {
00946         FCL_REAL delta_y = fabs(proj1 - y);
00947         l[1] += delta_y;
00948         if(proj1 < 0)
00949           Tr[1] -= delta_y;
00950 
00951         if(proj2 > 0)
00952           Tr[2] += 0.5 * (abs_proj2 - r);
00953         else
00954           Tr[2] -= 0.5 * (abs_proj2 - r);
00955       }
00956     }
00957   }
00958   else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
00959   {
00960     FCL_REAL x = (proj0 > 0) ? l[0] : 0;
00961     Vec3f v(x, proj1, 0);
00962     FCL_REAL new_r_sqr = (proj - v).sqrLength();
00963     if(new_r_sqr < r * r)
00964       ; // do nothing
00965     else
00966     {
00967       if(abs_proj2 < r)
00968       {
00969         FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
00970         l[0] += delta_x;
00971         if(proj0 < 0)
00972           Tr[0] -= delta_x;
00973       }
00974       else
00975       {
00976         FCL_REAL delta_x = fabs(proj0 - x);
00977         l[0] += delta_x;
00978         if(proj0 < 0)
00979           Tr[0] -= delta_x;
00980 
00981         if(proj2 > 0)
00982           Tr[2] += 0.5 * (abs_proj2 - r);
00983         else
00984           Tr[2] -= 0.5 * (abs_proj2 - r);
00985       }
00986     }
00987   }
00988   else
00989   {
00990     FCL_REAL x = (proj0 > 0) ? l[0] : 0;
00991     FCL_REAL y = (proj1 > 0) ? l[1] : 0;
00992     Vec3f v(x, y, 0);
00993     FCL_REAL new_r_sqr = (proj - v).sqrLength();
00994     if(new_r_sqr < r * r)
00995       ; // do nothing
00996     else
00997     {
00998       if(abs_proj2 < r)
00999       {
01000         FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2);
01001         FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag;
01002 
01003         FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
01004         FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
01005         l[0] += delta_x;
01006         l[1] += delta_y;
01007 
01008         if(proj0 < 0 && proj1 < 0)
01009         {
01010           Tr[0] -= delta_x;
01011           Tr[1] -= delta_y;
01012         }
01013       }
01014       else
01015       {
01016         FCL_REAL delta_x = fabs(proj0 - x);
01017         FCL_REAL delta_y = fabs(proj1 - y);
01018 
01019         l[0] += delta_x;
01020         l[1] += delta_y;
01021 
01022         if(proj0 < 0 && proj1 < 0)
01023         {
01024           Tr[0] -= delta_x;
01025           Tr[1] -= delta_y;
01026         }
01027 
01028         if(proj2 > 0)
01029           Tr[2] += 0.5 * (abs_proj2 - r);
01030         else
01031           Tr[2] -= 0.5 * (abs_proj2 - r);
01032       }
01033     }
01034   }
01035 
01036   return *this;
01037 }
01038 
01039 RSS RSS::operator + (const RSS& other) const
01040 {
01041   RSS bv;
01042 
01043   Vec3f v[16];
01044   Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
01045   Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
01046   Vec3f d0_neg = other.axis[0] * (-other.r);
01047   Vec3f d1_neg = other.axis[1] * (-other.r);
01048   Vec3f d2_pos = other.axis[2] * other.r;
01049   Vec3f d2_neg = other.axis[2] * (-other.r);
01050 
01051   v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
01052   v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
01053   v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
01054   v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
01055   v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
01056   v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
01057   v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
01058   v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
01059 
01060   d0_pos = axis[0] * (l[0] + r);
01061   d1_pos = axis[1] * (l[1] + r);
01062   d0_neg = axis[0] * (-r);
01063   d1_neg = axis[1] * (-r);
01064   d2_pos = axis[2] * r;
01065   d2_neg = axis[2] * (-r);
01066 
01067   v[8] = Tr + d0_pos + d1_pos + d2_pos;
01068   v[9] = Tr + d0_pos + d1_pos + d2_neg;
01069   v[10] = Tr + d0_pos + d1_neg + d2_pos;
01070   v[11] = Tr + d0_pos + d1_neg + d2_neg;
01071   v[12] = Tr + d0_neg + d1_pos + d2_pos;
01072   v[13] = Tr + d0_neg + d1_pos + d2_neg;
01073   v[14] = Tr + d0_neg + d1_neg + d2_pos;
01074   v[15] = Tr + d0_neg + d1_neg + d2_neg;
01075 
01076 
01077   Matrix3f M; // row first matrix
01078   Vec3f E[3]; // row first eigen-vectors
01079   Matrix3f::U s[3] = {0, 0, 0};
01080 
01081   getCovariance(v, NULL, NULL, NULL, 16, M);
01082   eigen(M, s, E);
01083 
01084   int min, mid, max;
01085   if(s[0] > s[1]) { max = 0; min = 1; }
01086   else { min = 0; max = 1; }
01087   if(s[2] < s[min]) { mid = min; min = 2; }
01088   else if(s[2] > s[max]) { mid = max; max = 2; }
01089   else { mid = 2; }
01090 
01091   // column first matrix, as the axis in RSS
01092   bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
01093   bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
01094   bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
01095                       E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
01096                       E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
01097 
01098   // set rss origin, rectangle size and radius
01099   getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
01100 
01101   return bv;
01102 }
01103 
01104 FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
01105 {
01106   // compute what transform [R,T] that takes us from cs1 to cs2.
01107   // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
01108   // First compute the rotation part, then translation part
01109   Vec3f t = other.Tr - Tr; // T2 - T1
01110   Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
01111   Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
01112              axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
01113              axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
01114 
01115   FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
01116   dist -= (r + other.r);
01117   return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
01118 }
01119 
01120 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
01121 {
01122   Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
01123                 R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
01124                 R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
01125 
01126   Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
01127              R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
01128              R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
01129 
01130   Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
01131 
01132   Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
01133 
01134   FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
01135   dist -= (b1.r + b2.r);
01136   return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
01137 }
01138 
01139 RSS translate(const RSS& bv, const Vec3f& t)
01140 {
01141   RSS res(bv);
01142   res.Tr += t;
01143   return res;
01144 }
01145 
01146 
01147 
01148 
01149 
01150 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30