ld33_umonster
view src/geom.cc @ 7:92d662deb66e
capsule distance seems broken
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Tue, 25 Aug 2015 00:38:00 +0300 |
| parents | 4a6683050e29 |
| children | bed39534d471 |
line source
1 #include <assert.h>
2 #include <float.h>
3 #include <algorithm>
4 #include "geom.h"
6 GeomObject::~GeomObject()
7 {
8 }
11 Sphere::Sphere()
12 {
13 radius = 1.0;
14 }
16 Sphere::Sphere(const Vector3 ¢, float radius)
17 : center(cent)
18 {
19 this->radius = radius;
20 }
22 void Sphere::set_union(const GeomObject *obj1, const GeomObject *obj2)
23 {
24 const Sphere *sph1 = dynamic_cast<const Sphere*>(obj1);
25 const Sphere *sph2 = dynamic_cast<const Sphere*>(obj2);
27 if(!sph1 || !sph2) {
28 fprintf(stderr, "Sphere::set_union: arguments must be spheres");
29 return;
30 }
32 float dist = (sph1->center - sph2->center).length();
33 float surf_dist = dist - (sph1->radius + sph2->radius);
34 float d1 = sph1->radius + surf_dist / 2.0;
35 float d2 = sph2->radius + surf_dist / 2.0;
36 float t = d1 / (d1 + d2);
38 if(t < 0.0) t = 0.0;
39 if(t > 1.0) t = 1.0;
41 center = sph1->center * t + sph2->center * (1.0 - t);
42 radius = std::max(dist * t + sph2->radius, dist * (1.0f - t) + sph1->radius);
43 }
45 void Sphere::set_intersection(const GeomObject *obj1, const GeomObject *obj2)
46 {
47 fprintf(stderr, "Sphere::intersection undefined\n");
48 }
50 bool Sphere::intersect(const Ray &ray, HitPoint *hit) const
51 {
52 float a = dot_product(ray.dir, ray.dir);
53 float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +
54 2.0 * ray.dir.y * (ray.origin.y - center.y) +
55 2.0 * ray.dir.z * (ray.origin.z - center.z);
56 float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) -
57 2.0 * dot_product(ray.origin, center) - radius * radius;
59 float discr = b * b - 4.0 * a * c;
60 if(discr < 1e-4) {
61 return false;
62 }
64 float sqrt_discr = sqrt(discr);
65 float t0 = (-b + sqrt_discr) / (2.0 * a);
66 float t1 = (-b - sqrt_discr) / (2.0 * a);
68 if(t0 < 1e-4)
69 t0 = t1;
70 if(t1 < 1e-4)
71 t1 = t0;
73 float t = t0 < t1 ? t0 : t1;
74 if(t < 1e-4) {
75 return false;
76 }
78 // fill the HitPoint structure
79 if(hit) {
80 hit->obj = this;
81 hit->dist = t;
82 hit->pos = ray.origin + ray.dir * t;
83 hit->normal = (hit->pos - center) / radius;
84 }
85 return true;
86 }
89 AABox::AABox()
90 {
91 }
93 AABox::AABox(const Vector3 &vmin, const Vector3 &vmax)
94 : min(vmin), max(vmax)
95 {
96 }
98 void AABox::set_union(const GeomObject *obj1, const GeomObject *obj2)
99 {
100 const AABox *box1 = dynamic_cast<const AABox*>(obj1);
101 const AABox *box2 = dynamic_cast<const AABox*>(obj2);
103 if(!box1 || !box2) {
104 fprintf(stderr, "AABox::set_union: arguments must be AABoxes too\n");
105 return;
106 }
108 min.x = std::min(box1->min.x, box2->min.x);
109 min.y = std::min(box1->min.y, box2->min.y);
110 min.z = std::min(box1->min.z, box2->min.z);
112 max.x = std::max(box1->max.x, box2->max.x);
113 max.y = std::max(box1->max.y, box2->max.y);
114 max.z = std::max(box1->max.z, box2->max.z);
115 }
117 void AABox::set_intersection(const GeomObject *obj1, const GeomObject *obj2)
118 {
119 const AABox *box1 = dynamic_cast<const AABox*>(obj1);
120 const AABox *box2 = dynamic_cast<const AABox*>(obj2);
122 if(!box1 || !box2) {
123 fprintf(stderr, "AABox::set_intersection: arguments must be AABoxes too\n");
124 return;
125 }
127 for(int i=0; i<3; i++) {
128 min[i] = std::max(box1->min[i], box2->min[i]);
129 max[i] = std::min(box1->max[i], box2->max[i]);
131 if(max[i] < min[i]) {
132 max[i] = min[i];
133 }
134 }
135 }
137 bool AABox::intersect(const Ray &ray, HitPoint *hit) const
138 {
139 Vector3 param[2] = {min, max};
140 Vector3 inv_dir(1.0 / ray.dir.x, 1.0 / ray.dir.y, 1.0 / ray.dir.z);
141 int sign[3] = {inv_dir.x < 0, inv_dir.y < 0, inv_dir.z < 0};
143 float tmin = (param[sign[0]].x - ray.origin.x) * inv_dir.x;
144 float tmax = (param[1 - sign[0]].x - ray.origin.x) * inv_dir.x;
145 float tymin = (param[sign[1]].y - ray.origin.y) * inv_dir.y;
146 float tymax = (param[1 - sign[1]].y - ray.origin.y) * inv_dir.y;
148 if(tmin > tymax || tymin > tmax) {
149 return false;
150 }
151 if(tymin > tmin) {
152 tmin = tymin;
153 }
154 if(tymax < tmax) {
155 tmax = tymax;
156 }
158 float tzmin = (param[sign[2]].z - ray.origin.z) * inv_dir.z;
159 float tzmax = (param[1 - sign[2]].z - ray.origin.z) * inv_dir.z;
161 if(tmin > tzmax || tzmin > tmax) {
162 return false;
163 }
164 if(tzmin > tmin) {
165 tmin = tzmin;
166 }
167 if(tzmax < tmax) {
168 tmax = tzmax;
169 }
171 float t = tmin < 1e-4 ? tmax : tmin;
172 if(t >= 1e-4) {
174 if(hit) {
175 hit->obj = this;
176 hit->dist = t;
177 hit->pos = ray.origin + ray.dir * t;
179 float min_dist = FLT_MAX;
180 Vector3 offs = min + (max - min) / 2.0;
181 Vector3 local_hit = hit->pos - offs;
183 static const Vector3 axis[] = {
184 Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)
185 };
186 //int tcidx[][2] = {{2, 1}, {0, 2}, {0, 1}};
188 for(int i=0; i<3; i++) {
189 float dist = fabs((max[i] - offs[i]) - fabs(local_hit[i]));
190 if(dist < min_dist) {
191 min_dist = dist;
192 hit->normal = axis[i] * (local_hit[i] < 0.0 ? 1.0 : -1.0);
193 //hit->texcoord = Vector2(hit->pos[tcidx[i][0]], hit->pos[tcidx[i][1]]);
194 }
195 }
196 }
197 return true;
198 }
199 return false;
201 }
203 Plane::Plane()
204 : normal(0.0, 1.0, 0.0)
205 {
206 }
208 Plane::Plane(const Vector3 &p, const Vector3 &norm)
209 : pt(p)
210 {
211 normal = norm.normalized();
212 }
214 Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3)
215 : pt(p1)
216 {
217 normal = cross_product(p2 - p1, p3 - p1).normalized();
218 }
220 Plane::Plane(const Vector3 &normal, float dist)
221 {
222 this->normal = normal.normalized();
223 pt = this->normal * dist;
224 }
226 void Plane::set_union(const GeomObject *obj1, const GeomObject *obj2)
227 {
228 fprintf(stderr, "Plane::set_union undefined\n");
229 }
231 void Plane::set_intersection(const GeomObject *obj1, const GeomObject *obj2)
232 {
233 fprintf(stderr, "Plane::set_intersection undefined\n");
234 }
236 bool Plane::intersect(const Ray &ray, HitPoint *hit) const
237 {
238 float ndotdir = dot_product(normal, ray.dir);
239 if(fabs(ndotdir) < 1e-4) {
240 return false;
241 }
243 if(hit) {
244 Vector3 ptdir = pt - ray.origin;
245 float t = dot_product(normal, ptdir) / ndotdir;
247 hit->pos = ray.origin + ray.dir * t;
248 hit->normal = normal;
249 hit->obj = this;
250 }
251 return true;
252 }
254 float sphere_distance(const Vector3 ¢, float rad, const Vector3 &pt)
255 {
256 return (pt - cent).length() - rad;
257 }
259 float capsule_distance(const Vector3 &a, float ra, const Vector3 &b, float rb, const Vector3 &pt)
260 {
261 Vector3 ab_dir = b - a;
263 if(fabs(ab_dir.length_sq()) < 1e-5) {
264 // if a == b, the capsule is a sphere with radius the maximum of the capsule radii
265 return sphere_distance(a, std::max(ra, rb), pt);
266 }
267 float ab_len = ab_dir.length();
269 Vector3 ap_dir = pt - a;
270 Vector3 rotaxis = cross_product(ab_dir, ap_dir).normalized();
272 Matrix4x4 rmat;
273 rmat.set_rotation(rotaxis, M_PI / 2.0);
274 Vector3 right = ab_dir.transformed(rmat) / ab_len;
276 // XXX I think this check is redundant, always false, due to the cross product order
277 //assert(dot_product(right, ab_dir) >= 0.0);
278 if(dot_product(right, ab_dir) < 0.0) {
279 right = -right;
280 }
281 Vector3 aa = a + right * ra;
282 Vector3 bb = b + right * rb;
284 // project pt to the line segment bb-aa, see if the projection lies within the interval [0, 1)
285 Vector3 aabb_dir = bb - aa;
286 float aabb_len = aabb_dir.length();
287 Vector3 aap_dir = pt - aa;
289 float t = dot_product(aap_dir, aabb_dir / aabb_len) / aabb_len;
290 if(t < 0.0) {
291 return sphere_distance(a, ra, pt);
292 }
293 if(t >= 1.0) {
294 return sphere_distance(b, rb, pt);
295 }
297 Vector3 ppt = aa + aabb_dir * t;
298 Vector3 norm = ppt - pt;
299 float dist = norm.length();
301 if(dot_product(norm, right) < 0.0) {
302 // inside the cone
303 dist = -dist;
304 }
305 return dist;
306 }