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