libs/mathlib/bbox.c

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