git.mutantstargoat.com Git - laserbrain_demo/blob - src/geom.cc

   1 #include <stdlib.h>
   2 #include <float.h>
   3 #include <algorithm>
   4 #include "geom.h"
   5 #include "app.h"
   6
   7 #define SPHERE(ptr)     ((Sphere*)ptr)
   8 #define AABOX(ptr)      ((AABox*)ptr)
   9 #define BOX(ptr)        ((Box*)ptr)
  10 #define PLANE(ptr)      ((Plane*)ptr)
  11
  12 GeomObject::GeomObject()
  13 {
  14         type = GOBJ_UNKNOWN;
  15 }
  16
  17 GeomObject::~GeomObject()
  18 {
  19 }
  20
  21 bool GeomObject::valid() const
  22 {
  23         return true;
  24 }
  25
  26 void GeomObject::invalidate()
  27 {
  28 }
  29
  30 float GeomObject::distance(const Vec3 &v) const
  31 {
  32         return sqrt(distance_sq(v));
  33 }
  34
  35 float GeomObject::signed_distance(const Vec3 &v) const
  36 {
  37         return sqrt(signed_distance_sq(v));
  38 }
  39
  40 Sphere::Sphere()
  41 {
  42         type = GOBJ_SPHERE;
  43         radius = 1.0;
  44 }
  45
  46 Sphere::Sphere(const Vec3 &cent, float radius)
  47         : center(cent)
  48 {
  49         type = GOBJ_SPHERE;
  50         this->radius = radius;
  51 }
  52
  53 bool Sphere::valid() const
  54 {
  55         return radius >= 0.0f;
  56 }
  57
  58 void Sphere::invalidate()
  59 {
  60         center = Vec3(0, 0, 0);
  61         radius = -1;
  62 }
  63
  64 bool Sphere::intersect(const Ray &ray, HitPoint *hit) const
  65 {
  66         float a = dot(ray.dir, ray.dir);
  67         float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +
  68                 2.0 * ray.dir.y * (ray.origin.y - center.y) +
  69                 2.0 * ray.dir.z * (ray.origin.z - center.z);
  70         float c = dot(ray.origin, ray.origin) + dot(center, center) -
  71                 2.0 * dot(ray.origin, center) - radius * radius;
  72
  73         float discr = b * b - 4.0 * a * c;
  74         if(discr < 1e-4) {
  75                 return false;
  76         }
  77
  78         float sqrt_discr = sqrt(discr);
  79         float t0 = (-b + sqrt_discr) / (2.0 * a);
  80         float t1 = (-b - sqrt_discr) / (2.0 * a);
  81
  82         if(t0 < 1e-4)
  83                 t0 = t1;
  84         if(t1 < 1e-4)
  85                 t1 = t0;
  86
  87         float t = t0 < t1 ? t0 : t1;
  88         if(t < 1e-4) {
  89                 return false;
  90         }
  91
  92         // fill the HitPoint structure
  93         if(hit) {
  94                 hit->obj = this;
  95                 hit->dist = t;
  96                 hit->pos = ray.origin + ray.dir * t;
  97                 hit->normal = (hit->pos - center) / radius;
  98         }
  99         return true;
 100 }
 101
 102 bool Sphere::contains(const Vec3 &pt) const
 103 {
 104         return length_sq(pt - center) <= radius * radius;
 105 }
 106
 107 float Sphere::distance_sq(const Vec3 &v) const
 108 {
 109         return std::max(length_sq(v - center) - radius * radius, 0.0f);
 110 }
 111
 112 float Sphere::signed_distance_sq(const Vec3 &v) const
 113 {
 114         return length_sq(v - center) - radius * radius;
 115 }
 116
 117 AABox::AABox()
 118 {
 119         type = GOBJ_AABOX;
 120 }
 121
 122 AABox::AABox(const Vec3 &vmin, const Vec3 &vmax)
 123         : min(vmin), max(vmax)
 124 {
 125         type = GOBJ_AABOX;
 126 }
 127
 128 bool AABox::valid() const
 129 {
 130         return min.x <= max.x && min.y <= max.y && min.z <= max.z;
 131 }
 132
 133 void AABox::invalidate()
 134 {
 135         min = Vec3(FLT_MAX, FLT_MAX, FLT_MAX);
 136         max = Vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
 137 }
 138
 139 Vec3 AABox::get_corner(int idx) const
 140 {
 141         Vec3 v[] = {min, max};
 142         static const int xidx[] = {0, 1, 1, 0, 0, 1, 1, 0};
 143         static const int yidx[] = {0, 0, 0, 0, 1, 1, 1, 1};
 144         static const int zidx[] = {0, 0, 1, 1, 0, 0, 1, 1};
 145         return Vec3(v[xidx[idx]].x, v[yidx[idx]].y, v[zidx[idx]].z);
 146 }
 147
 148 Plane AABox::get_plane(int pidx) const
 149 {
 150         Vec3 c = (max - min) * 0.5f;
 151         switch(pidx) {
 152         case AABOX_PLANE_PX:
 153                 return Plane(Vec3(max.x, c.y, c.z), Vec3(1, 0, 0));
 154         case AABOX_PLANE_NX:
 155                 return Plane(Vec3(min.x, c.y, c.z), Vec3(-1, 0, 0));
 156         case AABOX_PLANE_PY:
 157                 return Plane(Vec3(c.x, max.x, c.z), Vec3(0, 1, 0));
 158         case AABOX_PLANE_NY:
 159                 return Plane(Vec3(c.x, min.x, c.z), Vec3(0, -1, 0));
 160         case AABOX_PLANE_PZ:
 161                 return Plane(Vec3(c.x, c.y, max.z), Vec3(0, 0, 1));
 162         case AABOX_PLANE_NZ:
 163                 return Plane(Vec3(c.x, c.y, min.z), Vec3(0, 0, -1));
 164         default:
 165                 break;
 166         }
 167         abort();
 168         return Plane();
 169 }
 170
 171 bool AABox::intersect(const Ray &ray, HitPoint *hit) const
 172 {
 173         Vec3 param[2] = {min, max};
 174 #ifndef NDEBUG
 175         Vec3 inv_dir;
 176         if(fpexcept_enabled) {
 177                 inv_dir.x = ray.dir.x == 0.0f ? 1.0f : 1.0f / ray.dir.x;
 178                 inv_dir.y = ray.dir.y == 0.0f ? 1.0f : 1.0f / ray.dir.y;
 179                 inv_dir.z = ray.dir.z == 0.0f ? 1.0f : 1.0f / ray.dir.z;
 180         } else {
 181                 inv_dir = Vec3(1.0 / ray.dir.x, 1.0 / ray.dir.y, 1.0 / ray.dir.z);
 182         }
 183 #else
 184         Vec3 inv_dir(1.0 / ray.dir.x, 1.0 / ray.dir.y, 1.0 / ray.dir.z);
 185 #endif
 186         int sign[3] = {inv_dir.x < 0, inv_dir.y < 0, inv_dir.z < 0};
 187
 188         float tmin = (param[sign[0]].x - ray.origin.x) * inv_dir.x;
 189         float tmax = (param[1 - sign[0]].x - ray.origin.x) * inv_dir.x;
 190         float tymin = (param[sign[1]].y - ray.origin.y) * inv_dir.y;
 191         float tymax = (param[1 - sign[1]].y - ray.origin.y) * inv_dir.y;
 192
 193         if(tmin > tymax || tymin > tmax) {
 194                 return false;
 195         }
 196         if(tymin > tmin) {
 197                 tmin = tymin;
 198         }
 199         if(tymax < tmax) {
 200                 tmax = tymax;
 201         }
 202
 203         float tzmin = (param[sign[2]].z - ray.origin.z) * inv_dir.z;
 204         float tzmax = (param[1 - sign[2]].z - ray.origin.z) * inv_dir.z;
 205
 206         if(tmin > tzmax || tzmin > tmax) {
 207                 return false;
 208         }
 209         if(tzmin > tmin) {
 210                 tmin = tzmin;
 211         }
 212         if(tzmax < tmax) {
 213                 tmax = tzmax;
 214         }
 215
 216         float t = tmin < 1e-4 ? tmax : tmin;
 217         if(t >= 1e-4) {
 218
 219                 if(hit) {
 220                         hit->obj = this;
 221                         hit->dist = t;
 222                         hit->pos = ray.origin + ray.dir * t;
 223
 224                         float min_dist = FLT_MAX;
 225                         Vec3 offs = min + (max - min) / 2.0;
 226                         Vec3 local_hit = hit->pos - offs;
 227
 228                         static const Vec3 axis[] = {
 229                                 Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)
 230                         };
 231                         //int tcidx[][2] = {{2, 1}, {0, 2}, {0, 1}};
 232
 233                         for(int i=0; i<3; i++) {
 234                                 float dist = fabs((max[i] - offs[i]) - fabs(local_hit[i]));
 235                                 if(dist < min_dist) {
 236                                         min_dist = dist;
 237                                         hit->normal = axis[i] * (local_hit[i] < 0.0 ? 1.0 : -1.0);
 238                                         //hit->texcoord = Vec2(hit->pos[tcidx[i][0]], hit->pos[tcidx[i][1]]);
 239                                 }
 240                         }
 241                 }
 242                 return true;
 243         }
 244         return false;
 245 }
 246
 247 bool AABox::contains(const Vec3 &v) const
 248 {
 249         return v.x >= min.x && v.y >= min.y && v.z >= min.z &&
 250                 v.x <= max.x && v.y <= max.y && v.z <= max.z;
 251 }
 252
 253 #define SQ(x)   ((x) * (x))
 254 float AABox::distance_sq(const Vec3 &v) const
 255 {
 256         float dsq = 0.0f;
 257
 258         for(int i=0; i<3; i++) {
 259                 if(v[i] < min[i]) dsq += SQ(min[i] - v[i]);
 260                 if(v[i] > max[i]) dsq += SQ(v[i] - max[i]);
 261         }
 262         return dsq;
 263 }
 264
 265 float AABox::signed_distance_sq(const Vec3 &v) const
 266 {
 267         if(!contains(v)) {
 268                 return distance_sq(v);
 269         }
 270
 271         float dsq = 0.0f;
 272         for(int i=0; i<3; i++) {
 273                 float dmin = v[i] - min[i];
 274                 float dmax = max[i] - v[i];
 275                 dsq -= dmin < dmax ? SQ(dmin) : SQ(dmax);
 276         }
 277         return dsq;
 278 }
 279
 280 Box::Box()
 281 {
 282         type = GOBJ_BOX;
 283 }
 284
 285 Box::Box(const AABox &aabox, const Mat4 &xform)
 286         : xform(xform)
 287 {
 288         type = GOBJ_BOX;
 289         min = aabox.min;
 290         max = aabox.max;
 291 }
 292
 293 void Box::invalidate()
 294 {
 295         AABox::invalidate();
 296         xform = Mat4::identity;
 297 }
 298
 299 Box::Box(const Vec3 &min, const Vec3 &max)
 300         : AABox(min, max)
 301 {
 302         type = GOBJ_BOX;
 303 }
 304
 305 Box::Box(const Vec3 &min, const Vec3 &max, const Mat4 &xform)
 306         : AABox(min, max), xform(xform)
 307 {
 308         type = GOBJ_BOX;
 309 }
 310
 311 // XXX all this shit is completely untested
 312 Box::Box(const Vec3 &pos, const Vec3 &vi, const Vec3 &vj, const Vec3 &vk)
 313 {
 314         type = GOBJ_BOX;
 315         float ilen = length(vi);
 316         float jlen = length(vj);
 317         float klen = length(vk);
 318
 319         min = Vec3(-ilen, -jlen, -klen);
 320         max = Vec3(ilen, jlen, klen);
 321
 322         float si = ilen == 0.0 ? 1.0 : 1.0 / ilen;
 323         float sj = jlen == 0.0 ? 1.0 : 1.0 / jlen;
 324         float sk = klen == 0.0 ? 1.0 : 1.0 / klen;
 325
 326         xform = Mat4(vi * si, vj * sj, vk * sk);
 327         xform.translate(pos);
 328 }
 329
 330 Box::Box(const Vec3 *varr, int vcount)
 331 {
 332         type = GOBJ_BOX;
 333         calc_bounding_aabox(this, varr, vcount);
 334 }
 335
 336 Vec3 Box::get_corner(int idx) const
 337 {
 338         return xform * AABox::get_corner(idx);
 339 }
 340
 341 bool Box::intersect(const Ray &ray, HitPoint *hit) const
 342 {
 343         Mat4 inv_xform = inverse(xform);
 344         Mat4 dir_inv_xform = inv_xform.upper3x3();
 345         Mat4 dir_xform = transpose(dir_inv_xform);
 346         Ray local_ray = Ray(inv_xform * ray.origin, dir_inv_xform * ray.dir);
 347
 348         bool res = AABox::intersect(local_ray, hit);
 349         if(!res || !hit) return res;
 350
 351         hit->pos = xform * hit->pos;
 352         hit->normal = dir_xform * hit->normal;
 353         hit->local_ray = local_ray;
 354         hit->ray = ray;
 355         return true;
 356 }
 357
 358 bool Box::contains(const Vec3 &pt) const
 359 {
 360         // XXX is it faster to extract 6 planes and do dot products? sounds marginal
 361         return AABox::contains(inverse(xform) * pt);
 362 }
 363
 364 float Box::distance_sq(const Vec3 &v) const
 365 {
 366         return 0.0f;    // TODO
 367 }
 368
 369 float Box::signed_distance_sq(const Vec3 &v) const
 370 {
 371         return 0.0f;    // TODO
 372 }
 373
 374 Plane::Plane()
 375         : normal(0.0, 1.0, 0.0)
 376 {
 377         type = GOBJ_PLANE;
 378 }
 379
 380 Plane::Plane(const Vec3 &p, const Vec3 &norm)
 381         : pt(p)
 382 {
 383         type = GOBJ_PLANE;
 384         normal = normalize(norm);
 385 }
 386
 387 Plane::Plane(const Vec3 &p1, const Vec3 &p2, const Vec3 &p3)
 388         : pt(p1)
 389 {
 390         type = GOBJ_PLANE;
 391         normal = normalize(cross(p2 - p1, p3 - p1));
 392 }
 393
 394 Plane::Plane(const Vec3 &normal, float dist)
 395 {
 396         type = GOBJ_PLANE;
 397         this->normal = normalize(normal);
 398         pt = this->normal * dist;
 399 }
 400
 401 bool Plane::intersect(const Ray &ray, HitPoint *hit) const
 402 {
 403         float ndotdir = dot(normal, ray.dir);
 404         if(fabs(ndotdir) < 1e-4) {
 405                 return false;
 406         }
 407
 408         if(hit) {
 409                 Vec3 ptdir = pt - ray.origin;
 410                 float t = dot(normal, ptdir) / ndotdir;
 411
 412                 hit->dist = t;
 413                 hit->pos = ray.origin + ray.dir * t;
 414                 hit->normal = normal;
 415                 hit->obj = this;
 416         }
 417         return true;
 418 }
 419
 420 bool Plane::contains(const Vec3 &v) const
 421 {
 422         return dot(v, normal) <= 0.0;
 423 }
 424
 425 float Plane::distance(const Vec3 &v) const
 426 {
 427         return std::max(dot(v - pt, normal), 0.0f);
 428 }
 429
 430 float Plane::signed_distance(const Vec3 &v) const
 431 {
 432         return dot(v - pt, normal);
 433 }
 434
 435 float Plane::distance_sq(const Vec3 &v) const
 436 {
 437         float d = distance(v);
 438         return d * d;
 439 }
 440
 441 float Plane::signed_distance_sq(const Vec3 &v) const
 442 {
 443         float d = distance(v);
 444         return dot(v, normal) >= 0.0f ? d * d : -(d * d);
 445 }
 446
 447
 448 Disc::Disc()
 449 {
 450         type = GOBJ_DISC;
 451         radius = 1.0;
 452 }
 453
 454 Disc::Disc(const Vec3 &pt, const Vec3 &normal, float rad)
 455         : Plane(pt, normal)
 456 {
 457         type = GOBJ_DISC;
 458         radius = rad;
 459 }
 460
 461 Disc::Disc(const Vec3 &normal, float dist, float rad)
 462         : Plane(normal, dist)
 463 {
 464         type = GOBJ_DISC;
 465         radius = rad;
 466 }
 467
 468 bool Disc::valid() const
 469 {
 470         return radius >= 0.0f;
 471 }
 472
 473 void Disc::invalidate()
 474 {
 475         radius = -1;
 476 }
 477
 478 bool Disc::intersect(const Ray &ray, HitPoint *hit) const
 479 {
 480         HitPoint phit;
 481         if(Plane::intersect(ray, &phit)) {
 482                 if(length_sq(phit.pos - pt) <= radius * radius) {
 483                         *hit = phit;
 484                         return true;
 485                 }
 486         }
 487         return false;
 488 }
 489
 490 bool Disc::contains(const Vec3 &pt) const
 491 {
 492         Vec3 pj = proj_point_plane(pt, *this);
 493         return length_sq(pj - this->pt) <= radius * radius;
 494 }
 495
 496 float Disc::distance_sq(const Vec3 &v) const
 497 {
 498         return 0.0;     // TODO
 499 }
 500
 501 float Disc::signed_distance_sq(const Vec3 &v) const
 502 {
 503         return 0.0;     // TODO
 504 }
 505
 506
 507 Vec3 proj_point_plane(const Vec3 &pt, const Plane &plane)
 508 {
 509         float dist = plane.signed_distance(pt);
 510         return pt - plane.normal * dist;
 511 }
 512
 513 // ---- bounding sphere calculations ----
 514
 515 bool calc_bounding_sphere(Sphere *sph, const GeomObject *obj)
 516 {
 517         if(!obj->valid()) {
 518                 sph->invalidate();
 519                 return true;
 520         }
 521
 522         switch(obj->type) {
 523         case GOBJ_SPHERE:
 524                 *sph = *(Sphere*)obj;
 525                 break;
 526
 527         case GOBJ_AABOX:
 528                 sph->center = (AABOX(obj)->min + AABOX(obj)->max) * 0.5;
 529                 sph->radius = length(AABOX(obj)->max - AABOX(obj)->min) * 0.5;
 530                 break;
 531
 532         case GOBJ_BOX:
 533                 sph->center = (BOX(obj)->min + BOX(obj)->max) * 0.5 + BOX(obj)->xform.get_translation();
 534                 sph->radius = length(BOX(obj)->max - BOX(obj)->min) * 0.5;
 535                 break;
 536
 537         case GOBJ_PLANE:
 538         default:
 539                 return false;
 540         }
 541         return true;
 542 }
 543
 544 bool calc_bounding_sphere(Sphere *sph, const GeomObject *a, const GeomObject *b)
 545 {
 546         Sphere bsa, bsb;
 547
 548         if(!calc_bounding_sphere(&bsa, a) || !calc_bounding_sphere(&bsb, b)) {
 549                 return false;
 550         }
 551
 552         float dist = length(bsa.center - bsb.center);
 553         float surf_dist = dist - (bsa.radius + bsb.radius);
 554         float d1 = bsa.radius + surf_dist / 2.0;
 555         float d2 = bsb.radius + surf_dist / 2.0;
 556         float t = d1 / (d1 + d2);
 557
 558         if(t < 0.0) t = 0.0;
 559         if(t > 1.0) t = 1.0;
 560
 561         sph->center = bsa.center * t + bsb.center * (1.0 - t);
 562         sph->radius = std::max(dist * t + bsb.radius, dist * (1.0f - t) + bsa.radius);
 563         return true;
 564 }
 565
 566 bool calc_bounding_sphere(Sphere *sph, const GeomObject **objv, int num)
 567 {
 568         if(num <= 0) return false;
 569
 570         if(!calc_bounding_sphere(sph, objv[0])) {
 571                 return false;
 572         }
 573
 574         for(int i=1; i<num; i++) {
 575                 if(!calc_bounding_sphere(sph, sph, objv[i])) {
 576                         return false;
 577                 }
 578         }
 579         return true;
 580 }
 581
 582 bool calc_bounding_sphere(Sphere *sph, const Vec3 *v, int num, const Mat4 &xform)
 583 {
 584         if(num <= 0) return false;
 585
 586         sph->center = Vec3(0.0, 0.0, 0.0);
 587         for(int i=0; i<num; i++) {
 588                 sph->center += xform * v[i];
 589         }
 590         sph->center /= (float)num;
 591
 592         float rad_sq = 0.0f;
 593         for(int i=0; i<num; i++) {
 594                 Vec3 dir = xform * v[i] - sph->center;
 595                 rad_sq = std::max(rad_sq, dot(dir, dir));
 596         }
 597         sph->radius = sqrt(rad_sq);
 598         return true;
 599 }
 600
 601 bool calc_bounding_aabox(AABox *box, const GeomObject *obj)
 602 {
 603         if(!obj->valid()) {
 604                 box->invalidate();
 605                 return true;
 606         }
 607
 608         switch(obj->type) {
 609         case GOBJ_AABOX:
 610                 *box = *(AABox*)obj;
 611                 break;
 612
 613         case GOBJ_BOX:
 614                 {
 615                         Vec3 v[8];
 616                         for(int i=0; i<8; i++) {
 617                                 v[i] = BOX(obj)->get_corner(i);
 618                         }
 619                         calc_bounding_aabox(box, v, 8);
 620                 }
 621                 break;
 622
 623         case GOBJ_SPHERE:
 624                 {
 625                         float r = SPHERE(obj)->radius;
 626                         box->min = SPHERE(obj)->center - Vec3(r, r, r);
 627                         box->max = SPHERE(obj)->center + Vec3(r, r, r);
 628                 }
 629                 break;
 630
 631         case GOBJ_PLANE:
 632         default:
 633                 return false;
 634         }
 635         return true;
 636 }
 637
 638 bool calc_bounding_aabox(AABox *box, const GeomObject *a, const GeomObject *b)
 639 {
 640         AABox bba, bbb;
 641
 642         if(!calc_bounding_aabox(&bba, a) || !calc_bounding_aabox(&bbb, b)) {
 643                 return false;
 644         }
 645
 646         for(int i=0; i<3; i++) {
 647                 box->min[i] = std::min(bba.min[i], bbb.min[i]);
 648                 box->max[i] = std::max(bba.max[i], bbb.max[i]);
 649         }
 650         return true;
 651 }
 652
 653 bool calc_bounding_aabox(AABox *box, const GeomObject **objv, int num)
 654 {
 655         if(num <= 0) return false;
 656
 657         if(!calc_bounding_aabox(box, objv[0])) {
 658                 return false;
 659         }
 660
 661         for(int i=1; i<num; i++) {
 662                 if(!calc_bounding_aabox(box, box, objv[i])) {
 663                         return false;
 664                 }
 665         }
 666         return true;
 667 }
 668
 669 bool calc_bounding_aabox(AABox *box, const Vec3 *v, int num, const Mat4 &xform)
 670 {
 671         if(num <= 0) return false;
 672
 673         box->min = box->max = xform * v[0];
 674         for(int i=1; i<num; i++) {
 675                 Vec3 p = xform * v[i];
 676
 677                 for(int j=0; j<3; j++) {
 678                         box->min[j] = std::min(box->min[j], p[j]);
 679                         box->max[j] = std::max(box->max[j], p[j]);
 680                 }
 681         }
 682         return true;
 683 }
 684
 685 bool calc_bounding_box(Box *box, const GeomObject *obj)
 686 {
 687         if(!obj->valid()) {
 688                 box->invalidate();
 689                 return true;
 690         }
 691
 692         switch(obj->type) {
 693         case GOBJ_BOX:
 694                 *box = *(Box*)obj;
 695                 break;
 696
 697         case GOBJ_AABOX:
 698                 box->min = BOX(obj)->min;
 699                 box->max = BOX(obj)->max;
 700                 box->xform = Mat4::identity;
 701                 break;
 702
 703         case GOBJ_SPHERE:
 704                 {
 705                         float r = SPHERE(obj)->radius;
 706                         box->min = SPHERE(obj)->center - Vec3(r, r, r);
 707                         box->max = SPHERE(obj)->center + Vec3(r, r, r);
 708                         box->xform = Mat4::identity;
 709                 }
 710                 break;
 711
 712         case GOBJ_PLANE:
 713         default:
 714                 return false;
 715         }
 716         return true;
 717 }
 718
 719 bool intersect_sphere_sphere(Disc *result, const Sphere &a, const Sphere &b)
 720 {
 721         Vec3 dir = b.center - a.center;
 722
 723         float dist_sq = length_sq(dir);
 724         if(dist_sq <= 1e-8) return false;
 725
 726         float rsum = a.radius + b.radius;
 727         float rdif = fabs(a.radius - b.radius);
 728         if(dist_sq > rsum * rsum || dist_sq < rdif * rdif) {
 729                 return false;
 730         }
 731
 732         float dist = sqrt(dist_sq);
 733         float t = (dist_sq + a.radius * a.radius - b.radius * b.radius) / (2.0 * sqrt(dist_sq));
 734
 735         result->pt = a.center + dir * t;
 736         result->normal = dir / dist;
 737         result->radius = sin(acos(t)) * a.radius;
 738         return true;
 739 }
 740
 741 bool intersect_plane_plane(Ray *result, const Plane &a, const Plane &b)
 742 {
 743         return false;   // TODO
 744 }
 745
 746 bool intersect_sphere_plane(Sphere *result, const Sphere &s, const Plane &p)
 747 {
 748         return false;   // TODO
 749 }
 750
 751 bool intersect_plane_sphere(Sphere *result, const Plane &p, const Sphere &s)
 752 {
 753         return false;   // TODO
 754 }
 755
 756 bool intersect_aabox_aabox(AABox *res, const AABox &a, const AABox &b)
 757 {
 758         for(int i=0; i<3; i++) {
 759                 res->min[i] = std::max(a.min[i], b.min[i]);
 760                 res->max[i] = std::min(a.max[i], b.max[i]);
 761
 762                 if(res->max[i] < res->min[i]) {
 763                         res->max[i] = res->min[i];
 764                 }
 765         }
 766         return res->min.x != res->max.x && res->min.y != res->max.y && res->min.z != res->max.z;
 767 }
 768
 769 bool collision_sphere_aabox(const Sphere &s, const AABox &b)
 770 {
 771         return b.distance_sq(s.center) <= s.radius * s.radius;
 772 }