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