libmove3d
3.13.0
|
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