initial
[xonotic/netradiant.git] / libs / mathlib / bbox.c
1 /*
2 Copyright (C) 2001-2006, William Joseph.
3 All Rights Reserved.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #include <float.h>
23
24 #include "mathlib.h"
25
26 const aabb_t g_aabb_null = {
27   { 0, 0, 0, },
28   { -FLT_MAX, -FLT_MAX, -FLT_MAX, },
29 };
30
31 void aabb_construct_for_vec3(aabb_t *aabb, const vec3_t min, const vec3_t max)
32 {
33   VectorMid(min, max, aabb->origin);
34   VectorSubtract(max, aabb->origin, aabb->extents);
35 }
36
37 void aabb_clear(aabb_t *aabb)
38 {
39   VectorCopy(g_aabb_null.origin, aabb->origin);
40   VectorCopy(g_aabb_null.extents, aabb->extents);
41 }
42
43 void aabb_extend_by_point(aabb_t *aabb, const vec3_t point)
44 {
45 #if 1
46   int i;
47   vec_t min, max, displacement;
48   for(i=0; i<3; i++)
49   {
50     displacement = point[i] - aabb->origin[i];
51     if(fabs(displacement) > aabb->extents[i])
52     {
53       if(aabb->extents[i] < 0) // degenerate
54       {
55         min = max = point[i];
56       }
57       else if(displacement > 0)
58       {
59         min = aabb->origin[i] - aabb->extents[i];
60         max = aabb->origin[i] + displacement;
61       }
62       else
63       {
64         max = aabb->origin[i] + aabb->extents[i];
65         min = aabb->origin[i] + displacement;
66       }
67       aabb->origin[i] = (min + max) * 0.5f;
68       aabb->extents[i] = max - aabb->origin[i];
69     }
70   }
71 #else
72   unsigned int i;
73   for(i=0; i<3; ++i)
74   {
75     if(aabb->extents[i] < 0) // degenerate
76     {
77       aabb->origin[i] = point[i];
78       aabb->extents[i] = 0;
79     }
80     else
81     {
82       vec_t displacement = point[i] - aabb->origin[i];
83       vec_t increment = (vec_t)fabs(displacement) - aabb->extents[i];
84       if(increment > 0)
85       {
86         increment *= (vec_t)((displacement > 0) ? 0.5 : -0.5);
87         aabb->extents[i] += increment;
88         aabb->origin[i] += increment;
89       }
90     }
91   }
92 #endif
93 }
94
95 void aabb_extend_by_aabb(aabb_t *aabb, const aabb_t *aabb_src)
96 {
97   int i;
98   vec_t min, max, displacement, difference;
99   for(i=0; i<3; i++)
100   {
101     displacement = aabb_src->origin[i] - aabb->origin[i];
102     difference = aabb_src->extents[i] - aabb->extents[i];
103     if(aabb->extents[i] < 0
104       || difference >= fabs(displacement))
105     {
106       // 2nd contains 1st
107       aabb->extents[i] = aabb_src->extents[i];
108       aabb->origin[i] = aabb_src->origin[i];
109     }
110     else if(aabb_src->extents[i] < 0
111       || -difference >= fabs(displacement))
112     {
113       // 1st contains 2nd
114       continue;
115     }
116     else
117     {
118       // not contained
119       if(displacement > 0)
120       {
121         min = aabb->origin[i] - aabb->extents[i];
122         max = aabb_src->origin[i] + aabb_src->extents[i];
123       }
124       else
125       {
126         min = aabb_src->origin[i] - aabb_src->extents[i];
127         max = aabb->origin[i] + aabb->extents[i];
128       }
129       aabb->origin[i] = (min + max) * 0.5f;
130       aabb->extents[i] = max - aabb->origin[i];
131     }
132   }
133 }
134
135 void aabb_extend_by_vec3(aabb_t *aabb, vec3_t extension)
136 {
137   VectorAdd(aabb->extents, extension, aabb->extents);
138 }
139
140 int aabb_test_point(const aabb_t *aabb, const vec3_t point)
141 {
142   int i;
143   for(i=0; i<3; i++)
144     if(fabs(point[i] - aabb->origin[i]) >= aabb->extents[i])
145       return 0;
146   return 1;
147 }
148
149 int aabb_test_aabb(const aabb_t *aabb, const aabb_t *aabb_src)
150 {
151   int i;
152   for (i=0; i<3; i++)
153     if ( fabs(aabb_src->origin[i] - aabb->origin[i]) > (fabs(aabb->extents[i]) + fabs(aabb_src->extents[i])) )
154       return 0;
155   return 1;
156 }
157
158 int aabb_test_plane(const aabb_t *aabb, const float *plane)
159 {
160   float fDist, fIntersect;
161
162   // calc distance of origin from plane
163   fDist = DotProduct(plane, aabb->origin) + plane[3];
164   
165    // calc extents distance relative to plane normal
166   fIntersect = (vec_t)(fabs(plane[0] * aabb->extents[0]) + fabs(plane[1] * aabb->extents[1]) + fabs(plane[2] * aabb->extents[2]));
167   // accept if origin is less than or equal to this distance
168   if (fabs(fDist) < fIntersect) return 1; // partially inside
169   else if (fDist < 0) return 2; // totally inside
170   return 0; // totally outside
171 }
172
173 /* 
174 Fast Ray-Box Intersection
175 by Andrew Woo
176 from "Graphics Gems", Academic Press, 1990
177 */
178
179 #define NUMDIM  3
180 #define RIGHT   0
181 #define LEFT    1
182 #define MIDDLE  2
183
184 int aabb_intersect_ray(const aabb_t *aabb, const ray_t *ray, vec3_t intersection)
185 {
186         int inside = 1;
187         char quadrant[NUMDIM];
188         register int i;
189         int whichPlane;
190         double maxT[NUMDIM];
191         double candidatePlane[NUMDIM];
192   
193   const float *origin = ray->origin;
194   const float *direction = ray->direction;
195
196         /* Find candidate planes; this loop can be avoided if
197         rays cast all from the eye(assume perpsective view) */
198         for (i=0; i<NUMDIM; i++)
199   {
200                 if(origin[i] < (aabb->origin[i] - aabb->extents[i]))
201     {
202                         quadrant[i] = LEFT;
203                         candidatePlane[i] = (aabb->origin[i] - aabb->extents[i]);
204                         inside = 0;
205                 }
206     else if (origin[i] > (aabb->origin[i] + aabb->extents[i]))
207     {
208                         quadrant[i] = RIGHT;
209                         candidatePlane[i] = (aabb->origin[i] + aabb->extents[i]);
210                         inside = 0;
211                 }
212     else
213     {
214                         quadrant[i] = MIDDLE;
215     }
216   }
217
218         /* Ray origin inside bounding box */
219         if(inside == 1)
220   {
221                 VectorCopy(ray->origin, intersection);
222                 return 1;
223         }
224
225
226         /* Calculate T distances to candidate planes */
227         for (i = 0; i < NUMDIM; i++)
228   {
229                 if (quadrant[i] != MIDDLE && direction[i] !=0.)
230                         maxT[i] = (candidatePlane[i] - origin[i]) / direction[i];
231                 else
232                         maxT[i] = -1.;
233   }
234
235         /* Get largest of the maxT's for final choice of intersection */
236         whichPlane = 0;
237         for (i = 1; i < NUMDIM; i++)
238                 if (maxT[whichPlane] < maxT[i])
239                         whichPlane = i;
240
241         /* Check final candidate actually inside box */
242         if (maxT[whichPlane] < 0.)
243     return 0;
244         for (i = 0; i < NUMDIM; i++)
245   {
246                 if (whichPlane != i)
247     {
248                         intersection[i] = (vec_t)(origin[i] + maxT[whichPlane] * direction[i]);
249                         if (fabs(intersection[i] - aabb->origin[i]) > aabb->extents[i])
250                                 return 0;
251                 }
252     else
253     {
254                         intersection[i] = (vec_t)candidatePlane[i];
255                 }
256   }
257
258         return 1;                               /* ray hits box */
259 }
260
261 int aabb_test_ray(const aabb_t* aabb, const ray_t* ray)
262 {
263  vec3_t displacement, ray_absolute;
264  vec_t f;
265  
266  displacement[0] = ray->origin[0] - aabb->origin[0];
267  if(fabs(displacement[0]) > aabb->extents[0] && displacement[0] * ray->direction[0] >= 0.0f)
268    return 0;
269  
270  displacement[1] = ray->origin[1] - aabb->origin[1];
271  if(fabs(displacement[1]) > aabb->extents[1] && displacement[1] * ray->direction[1] >= 0.0f)
272    return 0;
273  
274  displacement[2] = ray->origin[2] - aabb->origin[2];
275  if(fabs(displacement[2]) > aabb->extents[2] && displacement[2] * ray->direction[2] >= 0.0f)
276    return 0;
277  
278  ray_absolute[0] = (float)fabs(ray->direction[0]);
279  ray_absolute[1] = (float)fabs(ray->direction[1]);
280  ray_absolute[2] = (float)fabs(ray->direction[2]);
281
282  f = ray->direction[1] * displacement[2] - ray->direction[2] * displacement[1];
283  if((float)fabs(f) > aabb->extents[1] * ray_absolute[2] + aabb->extents[2] * ray_absolute[1])
284    return 0;
285
286  f = ray->direction[2] * displacement[0] - ray->direction[0] * displacement[2];
287  if((float)fabs(f) > aabb->extents[0] * ray_absolute[2] + aabb->extents[2] * ray_absolute[0])
288    return 0;
289
290  f = ray->direction[0] * displacement[1] - ray->direction[1] * displacement[0];
291  if((float)fabs(f) > aabb->extents[0] * ray_absolute[1] + aabb->extents[1] * ray_absolute[0])
292    return 0;
293  
294  return 1;
295 }
296
297 void aabb_orthogonal_transform(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
298 {
299   VectorCopy(src->origin, dst->origin);
300   m4x4_transform_point(transform, dst->origin);
301
302   dst->extents[0] = (vec_t)(fabs(transform[0]  * src->extents[0])
303                           + fabs(transform[4]  * src->extents[1])
304                           + fabs(transform[8]  * src->extents[2]));
305   dst->extents[1] = (vec_t)(fabs(transform[1]  * src->extents[0])
306                           + fabs(transform[5]  * src->extents[1])
307                           + fabs(transform[9]  * src->extents[2]));
308   dst->extents[2] = (vec_t)(fabs(transform[2]  * src->extents[0])
309                           + fabs(transform[6]  * src->extents[1])
310                           + fabs(transform[10] * src->extents[2]));
311 }
312
313 void aabb_for_bbox(aabb_t *aabb, const bbox_t *bbox)
314 {
315         int i;
316         vec3_t temp[3];
317
318   VectorCopy(bbox->aabb.origin, aabb->origin);
319
320         // calculate the AABB extents in local coord space from the OBB extents and axes
321         VectorScale(bbox->axes[0], bbox->aabb.extents[0], temp[0]);
322         VectorScale(bbox->axes[1], bbox->aabb.extents[1], temp[1]);
323         VectorScale(bbox->axes[2], bbox->aabb.extents[2], temp[2]);
324         for(i=0;i<3;i++) aabb->extents[i] = (vec_t)(fabs(temp[0][i]) + fabs(temp[1][i]) + fabs(temp[2][i]));
325 }
326
327 void aabb_for_area(aabb_t *aabb, vec3_t area_tl, vec3_t area_br, int axis)
328 {
329   aabb_clear(aabb);
330   aabb->extents[axis] = FLT_MAX;
331   aabb_extend_by_point(aabb, area_tl);
332   aabb_extend_by_point(aabb, area_br);
333 }
334
335 int aabb_oriented_intersect_plane(const aabb_t *aabb, const m4x4_t transform, const vec_t* plane)
336 {
337   vec_t fDist, fIntersect;
338
339   // calc distance of origin from plane
340   fDist = DotProduct(plane, aabb->origin) + plane[3];
341
342   // calc extents distance relative to plane normal
343   fIntersect = (vec_t)(fabs(aabb->extents[0] * DotProduct(plane, transform))
344     + fabs(aabb->extents[1] * DotProduct(plane, transform+4))
345     + fabs(aabb->extents[2] * DotProduct(plane, transform+8)));
346   // accept if origin is less than this distance
347   if (fabs(fDist) < fIntersect) return 1; // partially inside
348   else if (fDist < 0) return 2; // totally inside
349   return 0; // totally outside
350 }
351
352 void aabb_corners(const aabb_t* aabb, vec3_t corners[8])
353 {
354   vec3_t min, max;
355   VectorSubtract(aabb->origin, aabb->extents, min);
356   VectorAdd(aabb->origin, aabb->extents, max);
357   VectorSet(corners[0], min[0], max[1], max[2]);
358   VectorSet(corners[1], max[0], max[1], max[2]);
359   VectorSet(corners[2], max[0], min[1], max[2]);
360   VectorSet(corners[3], min[0], min[1], max[2]);
361   VectorSet(corners[4], min[0], max[1], min[2]);
362   VectorSet(corners[5], max[0], max[1], min[2]);
363   VectorSet(corners[6], max[0], min[1], min[2]);
364   VectorSet(corners[7], min[0], min[1], min[2]);
365 }
366
367
368 void bbox_update_radius(bbox_t *bbox)
369 {
370   bbox->radius = VectorLength(bbox->aabb.extents);
371 }
372
373 void aabb_for_transformed_aabb(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
374 {
375   if(src->extents[0] < 0
376     || src->extents[1] < 0
377     || src->extents[2] < 0)
378   {
379     aabb_clear(dst);
380     return;
381   }
382
383   VectorCopy(src->origin, dst->origin);
384   m4x4_transform_point(transform, dst->origin);
385
386   dst->extents[0] = (vec_t)(fabs(transform[0]  * src->extents[0])
387                           + fabs(transform[4]  * src->extents[1])
388                           + fabs(transform[8]  * src->extents[2]));
389   dst->extents[1] = (vec_t)(fabs(transform[1]  * src->extents[0])
390                           + fabs(transform[5]  * src->extents[1])
391                           + fabs(transform[9]  * src->extents[2]));
392   dst->extents[2] = (vec_t)(fabs(transform[2]  * src->extents[0])
393                           + fabs(transform[6]  * src->extents[1])
394                           + fabs(transform[10] * src->extents[2]));
395 }
396
397 void bbox_for_oriented_aabb(bbox_t *bbox, const aabb_t *aabb, const m4x4_t matrix, const vec3_t euler, const vec3_t scale)
398 {
399         double rad[3];
400         double pi_180 = Q_PI / 180;
401   double A, B, C, D, E, F, AD, BD;
402   
403         VectorCopy(aabb->origin, bbox->aabb.origin);
404         
405   m4x4_transform_point(matrix, bbox->aabb.origin);
406
407         bbox->aabb.extents[0] = aabb->extents[0] * scale[0];
408         bbox->aabb.extents[1] = aabb->extents[1] * scale[1];
409         bbox->aabb.extents[2] = aabb->extents[2] * scale[2];
410
411   rad[0] = euler[0] * pi_180;
412         rad[1] = euler[1] * pi_180;
413         rad[2] = euler[2] * pi_180;
414
415   A       = cos(rad[0]);
416   B       = sin(rad[0]);
417   C       = cos(rad[1]);
418   D       = sin(rad[1]);
419   E       = cos(rad[2]);
420   F       = sin(rad[2]);
421
422   AD      =   A * -D;
423   BD      =   B * -D;
424
425         bbox->axes[0][0] = (vec_t)(C*E);
426         bbox->axes[0][1] = (vec_t)(-BD*E + A*F);
427         bbox->axes[0][2] = (vec_t)(AD*E + B*F);
428         bbox->axes[1][0] = (vec_t)(-C*F);
429         bbox->axes[1][1] = (vec_t)(BD*F + A*E);
430         bbox->axes[1][2] = (vec_t)(-AD*F + B*E);
431         bbox->axes[2][0] = (vec_t)D;
432         bbox->axes[2][1] = (vec_t)(-B*C);
433         bbox->axes[2][2] = (vec_t)(A*C);
434
435   bbox_update_radius(bbox);
436 }
437
438 int bbox_intersect_plane(const bbox_t *bbox, const vec_t* plane)
439 {
440   vec_t fDist, fIntersect;
441
442   // calc distance of origin from plane
443   fDist = DotProduct(plane, bbox->aabb.origin) + plane[3];
444
445         // trivial accept/reject using bounding sphere
446         if (fabs(fDist) > bbox->radius)
447         {
448                 if (fDist < 0)
449                         return 2; // totally inside
450                 else
451                         return 0; // totally outside
452         }
453
454   // calc extents distance relative to plane normal
455   fIntersect = (vec_t)(fabs(bbox->aabb.extents[0] * DotProduct(plane, bbox->axes[0]))
456              + fabs(bbox->aabb.extents[1] * DotProduct(plane, bbox->axes[1]))
457              + fabs(bbox->aabb.extents[2] * DotProduct(plane, bbox->axes[2])));
458   // accept if origin is less than this distance
459   if (fabs(fDist) < fIntersect) return 1; // partially inside
460   else if (fDist < 0) return 2; // totally inside
461   return 0; // totally outside
462 }