coeng

annotate src/geom.cc @ 7:af24cfbdf9b6

find changesets by author, revision, files, or words in the commit message
adding collision detection
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 14 Feb 2015 10:08:00 +0200
parents
children 8cce82794f90
rev   line source
nuclear@7 1 #include <float.h>
nuclear@7 2 #include <algorithm>
nuclear@7 3 #include "geom.h"
nuclear@7 4
nuclear@7 5 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit);
nuclear@7 6 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit);
nuclear@7 7 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit);
nuclear@7 8
nuclear@7 9 GeomShape::GeomShape()
nuclear@7 10 {
nuclear@7 11 type = GEOM_UNKNOWN;
nuclear@7 12 }
nuclear@7 13
nuclear@7 14 GeomShape::~GeomShape()
nuclear@7 15 {
nuclear@7 16 }
nuclear@7 17
nuclear@7 18 GeomShape::Type GeomShape::get_type() const
nuclear@7 19 {
nuclear@7 20 return type;
nuclear@7 21 }
nuclear@7 22
nuclear@7 23 // ---- Sphere class ----
nuclear@7 24
nuclear@7 25 Sphere::Sphere()
nuclear@7 26 {
nuclear@7 27 type = GEOM_SPHERE;
nuclear@7 28 radius = 1.0f;
nuclear@7 29 }
nuclear@7 30
nuclear@7 31 Sphere::Sphere(const Vector3 &c, float rad)
nuclear@7 32 : center(c)
nuclear@7 33 {
nuclear@7 34 type = GEOM_SPHERE;
nuclear@7 35 radius = rad;
nuclear@7 36 }
nuclear@7 37
nuclear@7 38 bool Sphere::intersect(const Ray &ray, HitPoint *hit) const
nuclear@7 39 {
nuclear@7 40 float a = dot_product(ray.dir, ray.dir);
nuclear@7 41 float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +
nuclear@7 42 2.0 * ray.dir.y * (ray.origin.y - center.y) +
nuclear@7 43 2.0 * ray.dir.z * (ray.origin.z - center.z);
nuclear@7 44 float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) -
nuclear@7 45 2.0 * dot_product(ray.origin, center) - radius * radius;
nuclear@7 46
nuclear@7 47 float discr = b * b - 4.0 * a * c;
nuclear@7 48 if(discr < 1e-4) {
nuclear@7 49 return false;
nuclear@7 50 }
nuclear@7 51
nuclear@7 52 float sqrt_discr = sqrt(discr);
nuclear@7 53 float t0 = (-b + sqrt_discr) / (2.0 * a);
nuclear@7 54 float t1 = (-b - sqrt_discr) / (2.0 * a);
nuclear@7 55
nuclear@7 56 if(t0 < 1e-4)
nuclear@7 57 t0 = t1;
nuclear@7 58 if(t1 < 1e-4)
nuclear@7 59 t1 = t0;
nuclear@7 60
nuclear@7 61 float t = t0 < t1 ? t0 : t1;
nuclear@7 62 if(t < 1e-4) {
nuclear@7 63 return false;
nuclear@7 64 }
nuclear@7 65
nuclear@7 66 // fill the HitPoint structure
nuclear@7 67 if(hit) {
nuclear@7 68 hit->shape = this;
nuclear@7 69 hit->data = 0;
nuclear@7 70 hit->dist = t;
nuclear@7 71 hit->pos = ray.origin + ray.dir * t;
nuclear@7 72 hit->normal = (hit->pos - center) / radius;
nuclear@7 73 }
nuclear@7 74 return true;
nuclear@7 75 }
nuclear@7 76
nuclear@7 77 bool Sphere::collide(const GeomShape *geom, HitPoint *hit) const
nuclear@7 78 {
nuclear@7 79 switch(geom->get_type()) {
nuclear@7 80 case GEOM_SPHERE:
nuclear@7 81 return col_sphere_sphere(this, (const Sphere*)geom, hit);
nuclear@7 82 case GEOM_PLANE:
nuclear@7 83 return col_sphere_plane(this, (const Plane*)geom, hit);
nuclear@7 84 default:
nuclear@7 85 break;
nuclear@7 86 }
nuclear@7 87 return false;
nuclear@7 88 }
nuclear@7 89
nuclear@7 90 float Sphere::distance(const Vector3 &pt) const
nuclear@7 91 {
nuclear@7 92 return sqrt(distance_sq(pt));
nuclear@7 93 }
nuclear@7 94
nuclear@7 95 float Sphere::distance_sq(const Vector3 &pt) const
nuclear@7 96 {
nuclear@7 97 return (pt - center).length_sq();
nuclear@7 98 }
nuclear@7 99
nuclear@7 100 // ---- Plane class ----
nuclear@7 101
nuclear@7 102 Plane::Plane()
nuclear@7 103 : normal(0, 1, 0)
nuclear@7 104 {
nuclear@7 105 type = GEOM_PLANE;
nuclear@7 106 dist = 0.0f;
nuclear@7 107 }
nuclear@7 108
nuclear@7 109 Plane::Plane(const Vector3 &n, float d)
nuclear@7 110 : normal(n)
nuclear@7 111 {
nuclear@7 112 type = GEOM_PLANE;
nuclear@7 113 dist = d;
nuclear@7 114
nuclear@7 115 normal.normalize();
nuclear@7 116 }
nuclear@7 117
nuclear@7 118 Plane::Plane(float a, float b, float c, float d)
nuclear@7 119 : normal(a, b, c)
nuclear@7 120 {
nuclear@7 121 type = GEOM_PLANE;
nuclear@7 122 dist = d;
nuclear@7 123
nuclear@7 124 normal.normalize();
nuclear@7 125 }
nuclear@7 126
nuclear@7 127 Plane::Plane(const Vector3 &pos, const Vector3 &norm)
nuclear@7 128 : normal(norm)
nuclear@7 129 {
nuclear@7 130 type = GEOM_PLANE;
nuclear@7 131
nuclear@7 132 dist = 0.0f;
nuclear@7 133 dist = distance(pos);
nuclear@7 134 }
nuclear@7 135
nuclear@7 136 Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3)
nuclear@7 137 {
nuclear@7 138 type = GEOM_PLANE;
nuclear@7 139
nuclear@7 140 normal = cross_product(p2 - p1, p3 - p1).normalized();
nuclear@7 141 dist = 0.0f;
nuclear@7 142 dist = distance(p1);
nuclear@7 143 }
nuclear@7 144
nuclear@7 145
nuclear@7 146 bool Plane::intersect(const Ray &ray, HitPoint *hit) const
nuclear@7 147 {
nuclear@7 148 Vector3 pt = Vector3(0, 0, 0) + normal * dist;
nuclear@7 149
nuclear@7 150 float ndotdir = dot_product(normal, ray.dir);
nuclear@7 151 if(fabs(ndotdir) < 1e-4) {
nuclear@7 152 return false;
nuclear@7 153 }
nuclear@7 154
nuclear@7 155 if(hit) {
nuclear@7 156 Vector3 ptdir = pt - ray.origin;
nuclear@7 157 float t = dot_product(normal, ptdir) / ndotdir;
nuclear@7 158
nuclear@7 159 hit->pos = ray.origin + ray.dir * t;
nuclear@7 160 hit->normal = normal;
nuclear@7 161 hit->shape = this;
nuclear@7 162 }
nuclear@7 163 return true;
nuclear@7 164 }
nuclear@7 165
nuclear@7 166 bool Plane::collide(const GeomShape *geom, HitPoint *hit) const
nuclear@7 167 {
nuclear@7 168 switch(geom->get_type()) {
nuclear@7 169 case GEOM_SPHERE:
nuclear@7 170 {
nuclear@7 171 bool res = col_sphere_plane((const Sphere*)geom, this, hit);
nuclear@7 172 if(hit) {
nuclear@7 173 hit->normal = -hit->normal;
nuclear@7 174 }
nuclear@7 175 return res;
nuclear@7 176 }
nuclear@7 177
nuclear@7 178 case GEOM_PLANE:
nuclear@7 179 return col_plane_plane(this, (const Plane*)geom, hit);
nuclear@7 180
nuclear@7 181 default:
nuclear@7 182 break;
nuclear@7 183 }
nuclear@7 184 return false;
nuclear@7 185 }
nuclear@7 186
nuclear@7 187 float Plane::distance(const Vector3 &v) const
nuclear@7 188 {
nuclear@7 189 Vector3 pt = normal * dist;
nuclear@7 190 return dot_product(v - pt, normal);
nuclear@7 191 }
nuclear@7 192
nuclear@7 193 float Plane::distance_sq(const Vector3 &v) const
nuclear@7 194 {
nuclear@7 195 float d = distance(v);
nuclear@7 196 return d * d;
nuclear@7 197 }
nuclear@7 198
nuclear@7 199
nuclear@7 200 // collision detection functions
nuclear@7 201
nuclear@7 202 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit)
nuclear@7 203 {
nuclear@7 204 float sum_rad = a->radius + b->radius;
nuclear@7 205 Vector3 dir = b->center - a->center;
nuclear@7 206 float dist_sq = dir.length_sq();
nuclear@7 207 bool res = dist_sq <= sum_rad * sum_rad;
nuclear@7 208
nuclear@7 209 if(res && hit) {
nuclear@7 210 hit->pos = a->center + dir * 0.5;
nuclear@7 211 hit->shape = b;
nuclear@7 212 hit->normal = -dir;
nuclear@7 213 }
nuclear@7 214 return res;
nuclear@7 215 }
nuclear@7 216
nuclear@7 217 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit)
nuclear@7 218 {
nuclear@7 219 fprintf(stderr, "%s: not implemented\n", __FUNCTION__);
nuclear@7 220 return false;
nuclear@7 221 }
nuclear@7 222
nuclear@7 223 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit)
nuclear@7 224 {
nuclear@7 225 float dist = plane->distance(sph->center);
nuclear@7 226 bool res = fabs(dist) <= sph->radius;
nuclear@7 227
nuclear@7 228 if(res && hit) {
nuclear@7 229 Vector3 planept = plane->normal * plane->dist;
nuclear@7 230 Vector3 sphdir = sph->center - planept;
nuclear@7 231 if(dot_product(sphdir, plane->normal) >= 0.0f) {
nuclear@7 232 hit->normal = plane->normal;
nuclear@7 233 } else {
nuclear@7 234 hit->normal = -plane->normal;
nuclear@7 235 }
nuclear@7 236 hit->pos = sph->center - plane->normal * fabs(dist);
nuclear@7 237 hit->shape = plane;
nuclear@7 238 }
nuclear@7 239 return res;
nuclear@7 240 }