OBB_Disjoint.h
Go to the documentation of this file.
00001 /*************************************************************************\
00002 
00003   Copyright 1999 The University of North Carolina at Chapel Hill.
00004   All Rights Reserved.
00005 
00006   Permission to use, copy, modify and distribute this software and its
00007   documentation for educational, research and non-profit purposes, without
00008   fee, and without a written agreement is hereby granted, provided that the
00009   above copyright notice and the following three paragraphs appear in all
00010   copies.
00011 
00012   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
00013   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
00014   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
00015   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
00016   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
00017   DAMAGES.
00018 
00019   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
00020   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
00022   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
00023   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
00024   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00025 
00026   The authors may be contacted via:
00027 
00028   US Mail:             S. Gottschalk
00029                        Department of Computer Science
00030                        Sitterson Hall, CB #3175
00031                        University of N. Carolina
00032                        Chapel Hill, NC 27599-3175
00033 
00034   Phone:               (919)962-1749
00035 
00036   EMail:               geom@cs.unc.edu
00037 
00038 
00039 \**************************************************************************/
00040 
00041 #ifndef PQP_OBB_DISJOINT
00042 #define PQP_OBB_DISJOINT
00043 
00044 #include "MatVec.h"
00045 #include "PQP_Compile.h"
00046 
00047 // int
00048 // obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3]);
00049 //
00050 // This is a test between two boxes, box A and box B.  It is assumed that
00051 // the coordinate system is aligned and centered on box A.  The 3x3
00052 // matrix B specifies box B's orientation with respect to box A.
00053 // Specifically, the columns of B are the basis vectors (axis vectors) of
00054 // box B.  The center of box B is located at the vector T.  The
00055 // dimensions of box B are given in the array b.  The orientation and
00056 // placement of box A, in this coordinate system, are the identity matrix
00057 // and zero vector, respectively, so they need not be specified.  The
00058 // dimensions of box A are given in array a.
00059 
00060 inline
00061 int
00062 obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3])
00063 {
00064   register PQP_REAL t, s;
00065   register int r;
00066   PQP_REAL Bf[3][3];
00067   const PQP_REAL reps = (PQP_REAL)1e-6;
00068   
00069   // Bf = fabs(B)
00070   Bf[0][0] = myfabs(B[0][0]);  Bf[0][0] += reps;
00071   Bf[0][1] = myfabs(B[0][1]);  Bf[0][1] += reps;
00072   Bf[0][2] = myfabs(B[0][2]);  Bf[0][2] += reps;
00073   Bf[1][0] = myfabs(B[1][0]);  Bf[1][0] += reps;
00074   Bf[1][1] = myfabs(B[1][1]);  Bf[1][1] += reps;
00075   Bf[1][2] = myfabs(B[1][2]);  Bf[1][2] += reps;
00076   Bf[2][0] = myfabs(B[2][0]);  Bf[2][0] += reps;
00077   Bf[2][1] = myfabs(B[2][1]);  Bf[2][1] += reps;
00078   Bf[2][2] = myfabs(B[2][2]);  Bf[2][2] += reps;
00079 
00080   // if any of these tests are one-sided, then the polyhedra are disjoint
00081   r = 1;
00082 
00083   // A1 x A2 = A0
00084   t = myfabs(T[0]);
00085   
00086   r &= (t <= 
00087           (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
00088   if (!r) return 1;
00089   
00090   // B1 x B2 = B0
00091   s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0];
00092   t = myfabs(s);
00093 
00094   r &= ( t <=
00095           (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
00096   if (!r) return 2;
00097     
00098   // A2 x A0 = A1
00099   t = myfabs(T[1]);
00100   
00101   r &= ( t <= 
00102           (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
00103   if (!r) return 3;
00104 
00105   // A0 x A1 = A2
00106   t = myfabs(T[2]);
00107 
00108   r &= ( t <= 
00109           (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
00110   if (!r) return 4;
00111 
00112   // B2 x B0 = B1
00113   s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1];
00114   t = myfabs(s);
00115 
00116   r &= ( t <=
00117           (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
00118   if (!r) return 5;
00119 
00120   // B0 x B1 = B2
00121   s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2];
00122   t = myfabs(s);
00123 
00124   r &= ( t <=
00125           (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
00126   if (!r) return 6;
00127 
00128   // A0 x B0
00129   s = T[2] * B[1][0] - T[1] * B[2][0];
00130   t = myfabs(s);
00131   
00132   r &= ( t <= 
00133         (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
00134          b[1] * Bf[0][2] + b[2] * Bf[0][1]));
00135   if (!r) return 7;
00136   
00137   // A0 x B1
00138   s = T[2] * B[1][1] - T[1] * B[2][1];
00139   t = myfabs(s);
00140 
00141   r &= ( t <=
00142         (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
00143          b[0] * Bf[0][2] + b[2] * Bf[0][0]));
00144   if (!r) return 8;
00145 
00146   // A0 x B2
00147   s = T[2] * B[1][2] - T[1] * B[2][2];
00148   t = myfabs(s);
00149 
00150   r &= ( t <=
00151           (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
00152            b[0] * Bf[0][1] + b[1] * Bf[0][0]));
00153   if (!r) return 9;
00154 
00155   // A1 x B0
00156   s = T[0] * B[2][0] - T[2] * B[0][0];
00157   t = myfabs(s);
00158 
00159   r &= ( t <=
00160           (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
00161            b[1] * Bf[1][2] + b[2] * Bf[1][1]));
00162   if (!r) return 10;
00163 
00164   // A1 x B1
00165   s = T[0] * B[2][1] - T[2] * B[0][1];
00166   t = myfabs(s);
00167 
00168   r &= ( t <=
00169           (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
00170            b[0] * Bf[1][2] + b[2] * Bf[1][0]));
00171   if (!r) return 11;
00172 
00173   // A1 x B2
00174   s = T[0] * B[2][2] - T[2] * B[0][2];
00175   t = myfabs(s);
00176 
00177   r &= (t <=
00178           (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
00179            b[0] * Bf[1][1] + b[1] * Bf[1][0]));
00180   if (!r) return 12;
00181 
00182   // A2 x B0
00183   s = T[1] * B[0][0] - T[0] * B[1][0];
00184   t = myfabs(s);
00185 
00186   r &= (t <=
00187           (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
00188            b[1] * Bf[2][2] + b[2] * Bf[2][1]));
00189   if (!r) return 13;
00190 
00191   // A2 x B1
00192   s = T[1] * B[0][1] - T[0] * B[1][1];
00193   t = myfabs(s);
00194 
00195   r &= ( t <=
00196           (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
00197            b[0] * Bf[2][2] + b[2] * Bf[2][0]));
00198   if (!r) return 14;
00199 
00200   // A2 x B2
00201   s = T[1] * B[0][2] - T[0] * B[1][2];
00202   t = myfabs(s);
00203 
00204   r &= ( t <=
00205           (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
00206            b[0] * Bf[2][1] + b[1] * Bf[2][0]));
00207   if (!r) return 15;
00208 
00209   return 0;  // should equal 0
00210 }
00211 
00212 #endif
00213 
00214 
00215 
00216 


jskeus
Author(s): JSK Alumnis
autogenerated on Tue Mar 7 2017 04:04:34