coeng

annotate src/geom.cc @ 8:8cce82794f90

find changesets by author, revision, files, or words in the commit message
seems to work nicely
author John Tsiombikas <nuclear@member.fsf.org>
date Sun, 15 Feb 2015 05:14:20 +0200
parents af24cfbdf9b6
children
rev   line source
nuclear@8 1 #include "opengl.h"
nuclear@7 2 #include <float.h>
nuclear@7 3 #include <algorithm>
nuclear@7 4 #include "geom.h"
nuclear@7 5
nuclear@7 6 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit);
nuclear@7 7 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit);
nuclear@7 8 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit);
nuclear@7 9
nuclear@7 10 GeomShape::GeomShape()
nuclear@7 11 {
nuclear@7 12 type = GEOM_UNKNOWN;
nuclear@7 13 }
nuclear@7 14
nuclear@7 15 GeomShape::~GeomShape()
nuclear@7 16 {
nuclear@7 17 }
nuclear@7 18
nuclear@7 19 GeomShape::Type GeomShape::get_type() const
nuclear@7 20 {
nuclear@7 21 return type;
nuclear@7 22 }
nuclear@7 23
nuclear@8 24 void GeomShape::set_transform(const Matrix4x4 &m)
nuclear@8 25 {
nuclear@8 26 xform = m;
nuclear@8 27 inv_xform = xform.inverse();
nuclear@8 28 }
nuclear@8 29
nuclear@8 30 const Matrix4x4 &GeomShape::get_transform() const
nuclear@8 31 {
nuclear@8 32 return xform;
nuclear@8 33 }
nuclear@8 34
nuclear@8 35 const Matrix4x4 &GeomShape::get_inv_transform() const
nuclear@8 36 {
nuclear@8 37 return inv_xform;
nuclear@8 38 }
nuclear@8 39
nuclear@7 40 // ---- Sphere class ----
nuclear@7 41
nuclear@7 42 Sphere::Sphere()
nuclear@7 43 {
nuclear@7 44 type = GEOM_SPHERE;
nuclear@7 45 radius = 1.0f;
nuclear@7 46 }
nuclear@7 47
nuclear@7 48 Sphere::Sphere(const Vector3 &c, float rad)
nuclear@7 49 : center(c)
nuclear@7 50 {
nuclear@7 51 type = GEOM_SPHERE;
nuclear@7 52 radius = rad;
nuclear@7 53 }
nuclear@7 54
nuclear@8 55 bool Sphere::contains(const Vector3 &pt) const
nuclear@7 56 {
nuclear@8 57 return distance_sq(pt) <= 0.0;
nuclear@8 58 }
nuclear@8 59
nuclear@8 60 bool Sphere::intersect(const Ray &inray, HitPoint *hit) const
nuclear@8 61 {
nuclear@8 62 Ray ray = inray.transformed(inv_xform);
nuclear@8 63
nuclear@7 64 float a = dot_product(ray.dir, ray.dir);
nuclear@7 65 float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +
nuclear@7 66 2.0 * ray.dir.y * (ray.origin.y - center.y) +
nuclear@7 67 2.0 * ray.dir.z * (ray.origin.z - center.z);
nuclear@7 68 float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) -
nuclear@7 69 2.0 * dot_product(ray.origin, center) - radius * radius;
nuclear@7 70
nuclear@7 71 float discr = b * b - 4.0 * a * c;
nuclear@7 72 if(discr < 1e-4) {
nuclear@7 73 return false;
nuclear@7 74 }
nuclear@7 75
nuclear@7 76 float sqrt_discr = sqrt(discr);
nuclear@7 77 float t0 = (-b + sqrt_discr) / (2.0 * a);
nuclear@7 78 float t1 = (-b - sqrt_discr) / (2.0 * a);
nuclear@7 79
nuclear@7 80 if(t0 < 1e-4)
nuclear@7 81 t0 = t1;
nuclear@7 82 if(t1 < 1e-4)
nuclear@7 83 t1 = t0;
nuclear@7 84
nuclear@7 85 float t = t0 < t1 ? t0 : t1;
nuclear@7 86 if(t < 1e-4) {
nuclear@7 87 return false;
nuclear@7 88 }
nuclear@7 89
nuclear@7 90 // fill the HitPoint structure
nuclear@7 91 if(hit) {
nuclear@7 92 hit->shape = this;
nuclear@7 93 hit->data = 0;
nuclear@7 94 hit->dist = t;
nuclear@8 95
nuclear@8 96 Vector3 local_pos = ray.origin + ray.dir * t;
nuclear@8 97 hit->pos = inray.origin + inray.dir * t;
nuclear@8 98 hit->normal = (local_pos - center) / radius;
nuclear@7 99 }
nuclear@7 100 return true;
nuclear@7 101 }
nuclear@7 102
nuclear@7 103 bool Sphere::collide(const GeomShape *geom, HitPoint *hit) const
nuclear@7 104 {
nuclear@7 105 switch(geom->get_type()) {
nuclear@7 106 case GEOM_SPHERE:
nuclear@7 107 return col_sphere_sphere(this, (const Sphere*)geom, hit);
nuclear@7 108 case GEOM_PLANE:
nuclear@7 109 return col_sphere_plane(this, (const Plane*)geom, hit);
nuclear@7 110 default:
nuclear@7 111 break;
nuclear@7 112 }
nuclear@7 113 return false;
nuclear@7 114 }
nuclear@7 115
nuclear@7 116 float Sphere::distance(const Vector3 &pt) const
nuclear@7 117 {
nuclear@8 118 Vector3 local_pt = pt.transformed(inv_xform);
nuclear@8 119 return (local_pt - center).length() - radius;
nuclear@7 120 }
nuclear@7 121
nuclear@7 122 float Sphere::distance_sq(const Vector3 &pt) const
nuclear@7 123 {
nuclear@8 124 Vector3 local_pt = pt.transformed(inv_xform);
nuclear@8 125 return (local_pt - center).length_sq() - radius * radius;
nuclear@8 126 }
nuclear@8 127
nuclear@8 128 void Sphere::draw() const
nuclear@8 129 {
nuclear@8 130 // TODO
nuclear@7 131 }
nuclear@7 132
nuclear@7 133 // ---- Plane class ----
nuclear@7 134
nuclear@7 135 Plane::Plane()
nuclear@7 136 : normal(0, 1, 0)
nuclear@7 137 {
nuclear@7 138 type = GEOM_PLANE;
nuclear@7 139 dist = 0.0f;
nuclear@7 140 }
nuclear@7 141
nuclear@7 142 Plane::Plane(const Vector3 &n, float d)
nuclear@7 143 : normal(n)
nuclear@7 144 {
nuclear@7 145 type = GEOM_PLANE;
nuclear@7 146 dist = d;
nuclear@7 147
nuclear@7 148 normal.normalize();
nuclear@7 149 }
nuclear@7 150
nuclear@7 151 Plane::Plane(float a, float b, float c, float d)
nuclear@7 152 : normal(a, b, c)
nuclear@7 153 {
nuclear@7 154 type = GEOM_PLANE;
nuclear@7 155 dist = d;
nuclear@7 156
nuclear@7 157 normal.normalize();
nuclear@7 158 }
nuclear@7 159
nuclear@7 160 Plane::Plane(const Vector3 &pos, const Vector3 &norm)
nuclear@7 161 : normal(norm)
nuclear@7 162 {
nuclear@7 163 type = GEOM_PLANE;
nuclear@7 164
nuclear@7 165 dist = 0.0f;
nuclear@7 166 dist = distance(pos);
nuclear@7 167 }
nuclear@7 168
nuclear@7 169 Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3)
nuclear@7 170 {
nuclear@7 171 type = GEOM_PLANE;
nuclear@7 172
nuclear@7 173 normal = cross_product(p2 - p1, p3 - p1).normalized();
nuclear@7 174 dist = 0.0f;
nuclear@7 175 dist = distance(p1);
nuclear@7 176 }
nuclear@7 177
nuclear@8 178 bool Plane::contains(const Vector3 &pt) const
nuclear@8 179 {
nuclear@8 180 return distance(pt) <= 0.0;
nuclear@8 181 }
nuclear@7 182
nuclear@8 183 bool Plane::intersect(const Ray &inray, HitPoint *hit) const
nuclear@7 184 {
nuclear@8 185 Ray ray = inray.transformed(inv_xform);
nuclear@8 186 Vector3 pt = normal * dist;
nuclear@7 187
nuclear@7 188 float ndotdir = dot_product(normal, ray.dir);
nuclear@7 189 if(fabs(ndotdir) < 1e-4) {
nuclear@7 190 return false;
nuclear@7 191 }
nuclear@7 192
nuclear@7 193 if(hit) {
nuclear@7 194 Vector3 ptdir = pt - ray.origin;
nuclear@7 195 float t = dot_product(normal, ptdir) / ndotdir;
nuclear@7 196
nuclear@8 197 hit->pos = inray.origin + inray.dir * t;
nuclear@7 198 hit->normal = normal;
nuclear@7 199 hit->shape = this;
nuclear@7 200 }
nuclear@7 201 return true;
nuclear@7 202 }
nuclear@7 203
nuclear@7 204 bool Plane::collide(const GeomShape *geom, HitPoint *hit) const
nuclear@7 205 {
nuclear@7 206 switch(geom->get_type()) {
nuclear@7 207 case GEOM_SPHERE:
nuclear@7 208 {
nuclear@7 209 bool res = col_sphere_plane((const Sphere*)geom, this, hit);
nuclear@7 210 if(hit) {
nuclear@7 211 hit->normal = -hit->normal;
nuclear@7 212 }
nuclear@7 213 return res;
nuclear@7 214 }
nuclear@7 215
nuclear@7 216 case GEOM_PLANE:
nuclear@7 217 return col_plane_plane(this, (const Plane*)geom, hit);
nuclear@7 218
nuclear@7 219 default:
nuclear@7 220 break;
nuclear@7 221 }
nuclear@7 222 return false;
nuclear@7 223 }
nuclear@7 224
nuclear@7 225 float Plane::distance(const Vector3 &v) const
nuclear@7 226 {
nuclear@7 227 Vector3 pt = normal * dist;
nuclear@8 228 return dot_product(v.transformed(inv_xform) - pt, normal);
nuclear@7 229 }
nuclear@7 230
nuclear@7 231 float Plane::distance_sq(const Vector3 &v) const
nuclear@7 232 {
nuclear@7 233 float d = distance(v);
nuclear@7 234 return d * d;
nuclear@7 235 }
nuclear@7 236
nuclear@8 237 void Plane::draw() const
nuclear@8 238 {
nuclear@8 239 // TODO
nuclear@8 240 }
nuclear@8 241
nuclear@8 242 // ---- AABox ----
nuclear@8 243
nuclear@8 244 AABox::AABox()
nuclear@8 245 {
nuclear@8 246 type = GEOM_AABOX;
nuclear@8 247 }
nuclear@8 248
nuclear@8 249 AABox::AABox(const Vector3 &vmin, const Vector3 &vmax)
nuclear@8 250 : min(vmin), max(vmax)
nuclear@8 251 {
nuclear@8 252 type = GEOM_AABOX;
nuclear@8 253 }
nuclear@8 254
nuclear@8 255 void AABox::set_union(const GeomShape *obj1, const GeomShape *obj2)
nuclear@8 256 {
nuclear@8 257 const AABox *box1 = (const AABox*)obj1;
nuclear@8 258 const AABox *box2 = (const AABox*)obj2;
nuclear@8 259
nuclear@8 260 if(!box1 || !box2 || obj1->get_type() != GEOM_AABOX || obj2->get_type() != GEOM_AABOX) {
nuclear@8 261 fprintf(stderr, "AABox::set_union: arguments must be AABoxes too\n");
nuclear@8 262 return;
nuclear@8 263 }
nuclear@8 264
nuclear@8 265 min.x = std::min(box1->min.x, box2->min.x);
nuclear@8 266 min.y = std::min(box1->min.y, box2->min.y);
nuclear@8 267 min.z = std::min(box1->min.z, box2->min.z);
nuclear@8 268
nuclear@8 269 max.x = std::max(box1->max.x, box2->max.x);
nuclear@8 270 max.y = std::max(box1->max.y, box2->max.y);
nuclear@8 271 max.z = std::max(box1->max.z, box2->max.z);
nuclear@8 272 }
nuclear@8 273
nuclear@8 274 void AABox::set_intersection(const GeomShape *obj1, const GeomShape *obj2)
nuclear@8 275 {
nuclear@8 276 const AABox *box1 = (const AABox*)obj1;
nuclear@8 277 const AABox *box2 = (const AABox*)obj2;
nuclear@8 278
nuclear@8 279 if(!box1 || !box2 || obj1->get_type() != GEOM_AABOX || obj2->get_type() != GEOM_AABOX) {
nuclear@8 280 fprintf(stderr, "AABox::set_union: arguments must be AABoxes too\n");
nuclear@8 281 return;
nuclear@8 282 }
nuclear@8 283
nuclear@8 284 for(int i=0; i<3; i++) {
nuclear@8 285 min[i] = std::max(box1->min[i], box2->min[i]);
nuclear@8 286 max[i] = std::min(box1->max[i], box2->max[i]);
nuclear@8 287
nuclear@8 288 if(max[i] < min[i]) {
nuclear@8 289 max[i] = min[i];
nuclear@8 290 }
nuclear@8 291 }
nuclear@8 292 }
nuclear@8 293
nuclear@8 294 bool AABox::contains(const Vector3 &pt) const
nuclear@8 295 {
nuclear@8 296 return pt.x >= min.x && pt.x <= max.x &&
nuclear@8 297 pt.y >= min.y && pt.y <= max.y &&
nuclear@8 298 pt.z >= min.z && pt.z <= max.z;
nuclear@8 299 }
nuclear@8 300
nuclear@8 301 bool AABox::intersect(const Ray &inray, HitPoint *hit) const
nuclear@8 302 {
nuclear@8 303 Ray ray = inray.transformed(inv_xform);
nuclear@8 304
nuclear@8 305 Vector3 param[2] = {min, max};
nuclear@8 306 Vector3 inv_dir(1.0 / ray.dir.x, 1.0 / ray.dir.y, 1.0 / ray.dir.z);
nuclear@8 307 int sign[3] = {inv_dir.x < 0, inv_dir.y < 0, inv_dir.z < 0};
nuclear@8 308
nuclear@8 309 float tmin = (param[sign[0]].x - ray.origin.x) * inv_dir.x;
nuclear@8 310 float tmax = (param[1 - sign[0]].x - ray.origin.x) * inv_dir.x;
nuclear@8 311 float tymin = (param[sign[1]].y - ray.origin.y) * inv_dir.y;
nuclear@8 312 float tymax = (param[1 - sign[1]].y - ray.origin.y) * inv_dir.y;
nuclear@8 313
nuclear@8 314 if(tmin > tymax || tymin > tmax) {
nuclear@8 315 return false;
nuclear@8 316 }
nuclear@8 317 if(tymin > tmin) {
nuclear@8 318 tmin = tymin;
nuclear@8 319 }
nuclear@8 320 if(tymax < tmax) {
nuclear@8 321 tmax = tymax;
nuclear@8 322 }
nuclear@8 323
nuclear@8 324 float tzmin = (param[sign[2]].z - ray.origin.z) * inv_dir.z;
nuclear@8 325 float tzmax = (param[1 - sign[2]].z - ray.origin.z) * inv_dir.z;
nuclear@8 326
nuclear@8 327 if(tmin > tzmax || tzmin > tmax) {
nuclear@8 328 return false;
nuclear@8 329 }
nuclear@8 330 if(tzmin > tmin) {
nuclear@8 331 tmin = tzmin;
nuclear@8 332 }
nuclear@8 333 if(tzmax < tmax) {
nuclear@8 334 tmax = tzmax;
nuclear@8 335 }
nuclear@8 336
nuclear@8 337 float t = tmin < 1e-4 ? tmax : tmin;
nuclear@8 338 if(t >= 1e-4) {
nuclear@8 339
nuclear@8 340 if(hit) {
nuclear@8 341 hit->shape = this;
nuclear@8 342 hit->dist = t;
nuclear@8 343 hit->pos = inray.origin + inray.dir * t;
nuclear@8 344
nuclear@8 345 float min_dist = FLT_MAX;
nuclear@8 346 Vector3 offs = min + (max - min) / 2.0;
nuclear@8 347 Vector3 local_hit = hit->pos - offs;
nuclear@8 348
nuclear@8 349 static const Vector3 axis[] = {
nuclear@8 350 Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)
nuclear@8 351 };
nuclear@8 352 //int tcidx[][2] = {{2, 1}, {0, 2}, {0, 1}};
nuclear@8 353
nuclear@8 354 for(int i=0; i<3; i++) {
nuclear@8 355 float dist = fabs((max[i] - offs[i]) - fabs(local_hit[i]));
nuclear@8 356 if(dist < min_dist) {
nuclear@8 357 min_dist = dist;
nuclear@8 358 hit->normal = axis[i] * (local_hit[i] < 0.0 ? 1.0 : -1.0);
nuclear@8 359 //hit->texcoord = Vector2(hit->pos[tcidx[i][0]], hit->pos[tcidx[i][1]]);
nuclear@8 360 }
nuclear@8 361 }
nuclear@8 362 hit->normal.transform(Matrix3x3(xform));
nuclear@8 363 }
nuclear@8 364 return true;
nuclear@8 365 }
nuclear@8 366 return false;
nuclear@8 367 }
nuclear@8 368
nuclear@8 369 bool AABox::collide(const GeomShape *geom, HitPoint *hit) const
nuclear@8 370 {
nuclear@8 371 fprintf(stderr, "%s: not implemented yet\n", __FUNCTION__);
nuclear@8 372 return false;
nuclear@8 373 }
nuclear@8 374
nuclear@8 375 float AABox::distance(const Vector3 &pt) const
nuclear@8 376 {
nuclear@8 377 float d = distance_sq(pt);
nuclear@8 378 return d * d;
nuclear@8 379 }
nuclear@8 380
nuclear@8 381 float AABox::distance_sq(const Vector3 &world_pt) const
nuclear@8 382 {
nuclear@8 383 Vector3 pt = pt.transformed(inv_xform);
nuclear@8 384
nuclear@8 385 float dx = std::max(pt.x - max.x, min.x - pt.x);
nuclear@8 386 float dy = std::max(pt.y - max.y, min.y - pt.y);
nuclear@8 387 float dz = std::max(pt.z - max.z, min.z - pt.z);
nuclear@8 388
nuclear@8 389 return Vector3(dx, dy, dz).length_sq();
nuclear@8 390 }
nuclear@8 391
nuclear@8 392 void AABox::draw() const
nuclear@8 393 {
nuclear@8 394 }
nuclear@8 395
nuclear@7 396
nuclear@7 397 // collision detection functions
nuclear@7 398
nuclear@7 399 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit)
nuclear@7 400 {
nuclear@8 401 const Matrix4x4 &xforma = a->get_transform();
nuclear@8 402 const Matrix4x4 &xformb = b->get_transform();
nuclear@8 403
nuclear@8 404 Vector3 ca = a->center.transformed(xforma);
nuclear@8 405 Vector3 cb = b->center.transformed(xformb);
nuclear@8 406 float rada = a->radius * xforma.get_scaling().x;
nuclear@8 407 float radb = b->radius * xformb.get_scaling().x;
nuclear@8 408
nuclear@8 409 float sum_rad = rada + radb;
nuclear@8 410 Vector3 dir = cb - ca;
nuclear@7 411 float dist_sq = dir.length_sq();
nuclear@7 412 bool res = dist_sq <= sum_rad * sum_rad;
nuclear@7 413
nuclear@7 414 if(res && hit) {
nuclear@8 415 hit->pos = ca + dir * 0.5;
nuclear@7 416 hit->shape = b;
nuclear@8 417 hit->normal = -dir.normalized();
nuclear@7 418 }
nuclear@7 419 return res;
nuclear@7 420 }
nuclear@7 421
nuclear@7 422 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit)
nuclear@7 423 {
nuclear@7 424 fprintf(stderr, "%s: not implemented\n", __FUNCTION__);
nuclear@7 425 return false;
nuclear@7 426 }
nuclear@7 427
nuclear@7 428 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit)
nuclear@7 429 {
nuclear@8 430 const Matrix4x4 &xform = sph->get_transform();
nuclear@8 431
nuclear@8 432 Vector3 sph_center = sph->center.transformed(xform);
nuclear@8 433 float sph_rad = sph->radius * xform.get_scaling().x;
nuclear@8 434
nuclear@8 435 float dist = plane->distance(sph_center);
nuclear@8 436 bool res = fabs(dist) <= sph_rad;
nuclear@7 437
nuclear@7 438 if(res && hit) {
nuclear@8 439 Vector3 planept = (plane->normal * plane->dist).transformed(plane->get_transform());
nuclear@8 440 Vector3 planenorm = plane->normal.transformed(Matrix3x3(plane->get_transform()));
nuclear@8 441
nuclear@8 442 Vector3 sphdir = sph_center - planept;
nuclear@8 443 if(dot_product(sphdir, planenorm) >= 0.0f) {
nuclear@8 444 hit->normal = planenorm;
nuclear@7 445 } else {
nuclear@8 446 hit->normal = -planenorm;
nuclear@7 447 }
nuclear@8 448 hit->pos = sph_center - planenorm * fabs(dist);
nuclear@7 449 hit->shape = plane;
nuclear@7 450 }
nuclear@7 451 return res;
nuclear@7 452 }