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 }