libs/mathlib/bbox.c

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