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 }