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

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