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