pqp
 All Classes
OBB_Disjoint.h
1 /*************************************************************************\
2 
3  Copyright 1999 The University of North Carolina at Chapel Hill.
4  All Rights Reserved.
5 
6  Permission to use, copy, modify and distribute this software and its
7  documentation for educational, research and non-profit purposes, without
8  fee, and without a written agreement is hereby granted, provided that the
9  above copyright notice and the following three paragraphs appear in all
10  copies.
11 
12  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17  DAMAGES.
18 
19  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
20  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
22  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
23  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
24  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 
26  The authors may be contacted via:
27 
28  US Mail: S. Gottschalk
29  Department of Computer Science
30  Sitterson Hall, CB #3175
31  University of N. Carolina
32  Chapel Hill, NC 27599-3175
33 
34  Phone: (919)962-1749
35 
36  EMail: geom@cs.unc.edu
37 
38 
39 \**************************************************************************/
40 
41 #ifndef PQP_OBB_DISJOINT
42 #define PQP_OBB_DISJOINT
43 
44 #include "MatVec.h"
45 #include "PQP_Compile.h"
46 
47 // int
48 // obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3]);
49 //
50 // This is a test between two boxes, box A and box B. It is assumed that
51 // the coordinate system is aligned and centered on box A. The 3x3
52 // matrix B specifies box B's orientation with respect to box A.
53 // Specifically, the columns of B are the basis vectors (axis vectors) of
54 // box B. The center of box B is located at the vector T. The
55 // dimensions of box B are given in the array b. The orientation and
56 // placement of box A, in this coordinate system, are the identity matrix
57 // and zero vector, respectively, so they need not be specified. The
58 // dimensions of box A are given in array a.
59 
60 inline
61 int
62 obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3])
63 {
64  register PQP_REAL t, s;
65  register int r;
66  PQP_REAL Bf[3][3];
67  const PQP_REAL reps = (PQP_REAL)1e-6;
68 
69  // Bf = fabs(B)
70  Bf[0][0] = myfabs(B[0][0]); Bf[0][0] += reps;
71  Bf[0][1] = myfabs(B[0][1]); Bf[0][1] += reps;
72  Bf[0][2] = myfabs(B[0][2]); Bf[0][2] += reps;
73  Bf[1][0] = myfabs(B[1][0]); Bf[1][0] += reps;
74  Bf[1][1] = myfabs(B[1][1]); Bf[1][1] += reps;
75  Bf[1][2] = myfabs(B[1][2]); Bf[1][2] += reps;
76  Bf[2][0] = myfabs(B[2][0]); Bf[2][0] += reps;
77  Bf[2][1] = myfabs(B[2][1]); Bf[2][1] += reps;
78  Bf[2][2] = myfabs(B[2][2]); Bf[2][2] += reps;
79 
80  // if any of these tests are one-sided, then the polyhedra are disjoint
81  r = 1;
82 
83  // A1 x A2 = A0
84  t = myfabs(T[0]);
85 
86  r &= (t <=
87  (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
88  if (!r) return 1;
89 
90  // B1 x B2 = B0
91  s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0];
92  t = myfabs(s);
93 
94  r &= ( t <=
95  (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
96  if (!r) return 2;
97 
98  // A2 x A0 = A1
99  t = myfabs(T[1]);
100 
101  r &= ( t <=
102  (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
103  if (!r) return 3;
104 
105  // A0 x A1 = A2
106  t = myfabs(T[2]);
107 
108  r &= ( t <=
109  (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
110  if (!r) return 4;
111 
112  // B2 x B0 = B1
113  s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1];
114  t = myfabs(s);
115 
116  r &= ( t <=
117  (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
118  if (!r) return 5;
119 
120  // B0 x B1 = B2
121  s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2];
122  t = myfabs(s);
123 
124  r &= ( t <=
125  (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
126  if (!r) return 6;
127 
128  // A0 x B0
129  s = T[2] * B[1][0] - T[1] * B[2][0];
130  t = myfabs(s);
131 
132  r &= ( t <=
133  (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
134  b[1] * Bf[0][2] + b[2] * Bf[0][1]));
135  if (!r) return 7;
136 
137  // A0 x B1
138  s = T[2] * B[1][1] - T[1] * B[2][1];
139  t = myfabs(s);
140 
141  r &= ( t <=
142  (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
143  b[0] * Bf[0][2] + b[2] * Bf[0][0]));
144  if (!r) return 8;
145 
146  // A0 x B2
147  s = T[2] * B[1][2] - T[1] * B[2][2];
148  t = myfabs(s);
149 
150  r &= ( t <=
151  (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
152  b[0] * Bf[0][1] + b[1] * Bf[0][0]));
153  if (!r) return 9;
154 
155  // A1 x B0
156  s = T[0] * B[2][0] - T[2] * B[0][0];
157  t = myfabs(s);
158 
159  r &= ( t <=
160  (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
161  b[1] * Bf[1][2] + b[2] * Bf[1][1]));
162  if (!r) return 10;
163 
164  // A1 x B1
165  s = T[0] * B[2][1] - T[2] * B[0][1];
166  t = myfabs(s);
167 
168  r &= ( t <=
169  (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
170  b[0] * Bf[1][2] + b[2] * Bf[1][0]));
171  if (!r) return 11;
172 
173  // A1 x B2
174  s = T[0] * B[2][2] - T[2] * B[0][2];
175  t = myfabs(s);
176 
177  r &= (t <=
178  (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
179  b[0] * Bf[1][1] + b[1] * Bf[1][0]));
180  if (!r) return 12;
181 
182  // A2 x B0
183  s = T[1] * B[0][0] - T[0] * B[1][0];
184  t = myfabs(s);
185 
186  r &= (t <=
187  (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
188  b[1] * Bf[2][2] + b[2] * Bf[2][1]));
189  if (!r) return 13;
190 
191  // A2 x B1
192  s = T[1] * B[0][1] - T[0] * B[1][1];
193  t = myfabs(s);
194 
195  r &= ( t <=
196  (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
197  b[0] * Bf[2][2] + b[2] * Bf[2][0]));
198  if (!r) return 14;
199 
200  // A2 x B2
201  s = T[1] * B[0][2] - T[0] * B[1][2];
202  t = myfabs(s);
203 
204  r &= ( t <=
205  (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
206  b[0] * Bf[2][1] + b[1] * Bf[2][0]));
207  if (!r) return 15;
208 
209  return 0; // should equal 0
210 }
211 
212 #endif
213 
214 
215 
216