zenilib: zenilib/jni/external/zenilib/zeni/Collision.cpp Source File

zenilib 0.5.3.0

- Main Page
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Groups Pages
Collision.cpp
Go to the documentation of this file.
1 /* This file is part of the Zenipex Library (zenilib).
2 * Copyright (C) 2011 Mitchell Keith Bloch (bazald).
3 *
4 * zenilib is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU Lesser General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * zenilib is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU Lesser General Public License for more details.
13 *
14 * You should have received a copy of the GNU Lesser General Public License
15 * along with zenilib. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include <zeni.h>
19
20 #include <algorithm>
21 #include <cmath>
22
23 #include <Zeni/Define.h>
24
25 namespace Zeni {
26
27 namespace Collision {
28
29 /* Begin Helpers and Templates
30 *
31 * Hard stuff largely derived from the description here:
32 * http://www.softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
33 */
34
35 inline std::pair<float, float> unpoof(const std::pair<float, float> &lhs, const float &radii) {
36 return std::make_pair(std::max(0.0f, lhs.first - radii), lhs.second);
37 }
38
39 inline float unpoof(const float &lhs, const float &radii) {
40 return std::max(0.0f, lhs - radii);
41 }
42
43 inline void absolute_float_unclamp(float &value, const float &new_value) {
44 const float abs_value = float(fabs(new_value));
45 if(value < abs_value)
46 value = abs_value;
47 }
48
49 template <typename VALUE_TYPE>
50 void simple_clamp(VALUE_TYPE &value, const VALUE_TYPE &lower, const VALUE_TYPE &upper) {
51 if(value < lower)
52 value = lower;
53 else if(value > upper)
54 value = upper;
55 }
56
57 template <typename VALUE_TYPE>
58 void double_unclamp(VALUE_TYPE &min_value, VALUE_TYPE &max_value, const VALUE_TYPE &new_value) {
59 if(min_value > new_value)
60 min_value = new_value;
61 else if(max_value < new_value)
62 max_value = new_value;
63 }
64
65 template <typename LINE_TYPE>
66 std::pair<float, float> nearest_point(const LINE_TYPE &lhs, const Point3f &rhs) {
67 const Vector3f w = rhs - lhs.get_end_point_a();
68 const Vector3f &u = lhs.get_direction();
69
70 const float &uu = lhs.get_direction2();
71 const float uw = u * w;
72
73 Vector3f closest_point = lhs.get_end_point_a() - rhs;
74
75 if(LINE_TYPE::has_lower_bound() && uw < 0.0f)
76 return std::make_pair(closest_point.magnitude(), 0.0f);
77 else if(LINE_TYPE::has_upper_bound() && uw > uu)
78 return std::make_pair((closest_point + u).magnitude(), 1.0f);
79
80 const float t = uw / uu;
81 return std::make_pair((closest_point + t * u).magnitude(), t);
82 }
83
84 template <typename LINE_TYPE>
85 std::pair<float, float> nearest_point(const LINE_TYPE &lhs, const Plane &rhs) {
86 const Vector3f w = rhs.get_point() - lhs.get_end_point_a();
87 const Vector3f &u = lhs.get_direction();
88 const Vector3f &n = rhs.get_normal();
89
90 const float t = (n * w) / (n * u);
91 if(LINE_TYPE::has_lower_bound() && t < 0.0f)
92 return std::make_pair(rhs.shortest_distance(lhs.get_end_point_a()), 0.0f);
93 else if(LINE_TYPE::has_upper_bound() && t > 1.0f)
94 return std::make_pair(rhs.shortest_distance(lhs.get_end_point_a() + u), 1.0f);
95
96 return std::make_pair(rhs.shortest_distance(lhs.get_end_point_a() + t * u), t);
97 }
98
99 template <typename LINE_TYPE1, typename LINE_TYPE2>
100 std::pair<float, float> nearest_point(const LINE_TYPE1 &lhs, const LINE_TYPE2 &rhs) {
101 const Vector3f w = lhs.get_end_point_a() - rhs.get_end_point_a();
102 const Vector3f &u = lhs.get_direction();
103 const Vector3f &v = rhs.get_direction();
104
105 const float &uu = lhs.get_direction2();
106 const float uv = u * v;
107 const float &vv = rhs.get_direction2();
108 const float uw = u * w;
109 const float vw = v * w;
110
111 const float denom = uu * vv - uv * uv;
112 float sc_numer = uv * vw - vv * uw, sc_denom;
113 float tc_numer = uu * vw - uv * uw, tc_denom;
114
115 if(denom > ZENI_COLLISION_EPSILON) {
116 sc_denom = denom;
117
118 if(LINE_TYPE1::has_lower_bound() && sc_numer < 0.0f) {
119 sc_numer = 0.0f;
120 tc_numer = vw;
121 tc_denom = vv;
122 }
123 else if(LINE_TYPE1::has_upper_bound() && sc_numer > sc_denom) {
124 sc_numer = sc_denom;
125 tc_numer = vw + uv;
126 tc_denom = vv;
127 }
128 else
129 tc_denom = denom;
130 }
131 else {
132 sc_numer = 0.0f;
133 sc_denom = 1.0f;
134 tc_numer = vw;
135 tc_denom = vv;
136 }
137
138 Vector3f min_dist(w);
139 float final_numer;
140
141 if(LINE_TYPE2::has_lower_bound() && tc_numer < 0.0f)
142 final_numer = -uw;
143 else if(LINE_TYPE2::has_upper_bound() && tc_numer > tc_denom) {
144 min_dist -= v;
145 final_numer = uv - uw;
146 }
147 else {
148 const float t = sc_numer / sc_denom;
149 return std::make_pair((min_dist + t * u - (tc_numer / tc_denom) * v).magnitude(), t);
150 }
151
152 if(LINE_TYPE1::has_lower_bound() && final_numer < 0.0f)
153 return std::make_pair(min_dist.magnitude(), 0.0f);
154 else if(LINE_TYPE1::has_upper_bound() && final_numer > uu)
155 return std::make_pair((min_dist + u).magnitude(), 1.0f);
156
157 const float t = final_numer / uu;
158 return std::make_pair((min_dist + t * u).magnitude(), t);
159 }
160
161 template <typename LINE_TYPE>
162 std::pair<float, float> nearest_point(const LINE_TYPE &lhs, const Parallelepiped &rhs) {
163 const Vector3f end_point_a = rhs.get_convert_to() * (lhs.get_end_point_a() - rhs.get_point());
164 const Vector3f end_point_b = rhs.get_convert_to() * (lhs.get_end_point_b() - rhs.get_point());
165 const Vector3f direction = end_point_b - end_point_a;
166
167 const Vector3f tmin = -end_point_a.divide_by(direction);
168 const Vector3f tmax = (Vector3f(1.0f, 1.0f, 1.0f) - end_point_a).divide_by(direction);
169
170 const Vector3f real_min(std::min(tmin.i, tmax.i), std::min(tmin.j, tmax.j), std::min(tmin.k, tmax.k));
171 const Vector3f real_max(std::max(tmin.i, tmax.i), std::max(tmin.j, tmax.j), std::max(tmin.k, tmax.k));
172
175 int valid_axes = 0;
176 float invalid_axes_distance2 = 0.0f;
177 float min_max = END_OF_TIME;
178 float max_min = END_OF_TIME;
179
180 if(fabs(direction.i) > ZENI_COLLISION_EPSILON) {
181 if(!valid_axes || real_max.i < min_max) min_max = real_max.i;
182 if(!valid_axes || real_min.i > max_min) max_min = real_min.i;
183 ++valid_axes;
184 }
185 else if(end_point_a.i < 0.0f) {
186 const Vector3f diff = (rhs.get_convert_from() * Vector3f(end_point_a.i, 0.0f, 0.0f));
187 invalid_axes_distance2 += diff * diff;
188 }
189 else if(end_point_a.i > 1.0f) {
190 const Vector3f diff = (rhs.get_convert_from() * Vector3f(end_point_a.i - 1.0f, 0.0f, 0.0f));
191 invalid_axes_distance2 += diff * diff;
192 }
193
194 if(fabs(direction.j) > ZENI_COLLISION_EPSILON) {
195 if(!valid_axes || real_max.j < min_max) min_max = real_max.j;
196 if(!valid_axes || real_min.j > max_min) max_min = real_min.j;
197 ++valid_axes;
198 }
199 else if(end_point_a.j < 0.0f) {
200 const Vector3f diff = (rhs.get_convert_from() * Vector3f(0.0f, end_point_a.j, 0.0f));
201 invalid_axes_distance2 += diff * diff;
202 }
203 else if(end_point_a.j > 1.0f) {
204 const Vector3f diff = (rhs.get_convert_from() * Vector3f(0.0f, end_point_a.j - 1.0f, 0.0f));
205 invalid_axes_distance2 += diff * diff;
206 }
207
208 if(fabs(direction.k) > ZENI_COLLISION_EPSILON) {
209 if(!valid_axes || real_max.k < min_max) min_max = real_max.k;
210 if(!valid_axes || real_min.k > max_min) max_min = real_min.k;
211 ++valid_axes;
212 }
213 else if(end_point_a.k < 0.0f) {
214 const Vector3f diff = (rhs.get_convert_from() * Vector3f(0.0f, 0.0f, end_point_a.k));
215 invalid_axes_distance2 += diff * diff;
216 }
217 else if(end_point_a.k > 1.0f) {
218 const Vector3f diff = (rhs.get_convert_from() * Vector3f(0.0f, 0.0f, end_point_a.k - 1.0f));
219 invalid_axes_distance2 += diff * diff;
220 }
221
222 if(!valid_axes)
223 return std::make_pair(invalid_axes_distance2, 0.0f);
224
227 if(LINE_TYPE::has_lower_bound() && min_max < 0.0f)
228 min_max = 0.0f;
229 else if(LINE_TYPE::has_upper_bound() && min_max > 1.0f)
230 min_max = 1.0f;
231
232 if(LINE_TYPE::has_lower_bound() && max_min < 0.0f)
233 max_min = 0.0f;
234 else if(LINE_TYPE::has_upper_bound() && max_min > 1.0f)
235 max_min = 1.0f;
236
237 if(min_max > max_min)
238 return std::make_pair(sqrt(invalid_axes_distance2), max_min);
239
240 Point3f closest_point = end_point_a + min_max * direction;
241 simple_clamp(closest_point.x, 0.0f, 1.0f);
242 simple_clamp(closest_point.y, 0.0f, 1.0f);
243 simple_clamp(closest_point.z, 0.0f, 1.0f);
244 closest_point = rhs.get_point() + rhs.get_convert_from() * Vector3f(closest_point);
245
246 const Vector3f valid_axes_distance = lhs.get_end_point_a() + min_max * lhs.get_direction() - closest_point;
247 const float valid_axes_distance2 = valid_axes_distance * valid_axes_distance;
248 const float total_distance = float(sqrt(invalid_axes_distance2 + valid_axes_distance2));
249
250 return std::make_pair(total_distance, min_max);
251 }
252
253 /* End Helpers and Templates
254 */
255
256 Sphere::Sphere(const Point3f &center_, const float &radius_)
257 : center(center_),
258 radius(radius_)
259 {
260 }
261
262 float Sphere::shortest_distance(const Sphere &rhs) const {
263 return unpoof((center - rhs.center).magnitude(), radius + rhs.radius);
264 }
265 float Sphere::shortest_distance(const Point3f &rhs) const {
266 return unpoof((center - rhs).magnitude(), radius);
267 }
268
269 Plane::Plane(const Point3f &point_, const Vector3f &normal_)
270 : point(point_),
271 normal(normal_.normalized())
272 {
273 }
274
275 float Plane::shortest_distance(const Plane &rhs) const {
276 const Vector3f line_normal = normal % rhs.normal;
277 if(line_normal.magnitude() < ZENI_COLLISION_EPSILON)
278 return shortest_distance(rhs.point);
279
280 return 0.0f;
281 }
282
283 float Plane::shortest_distance(const Point3f &rhs) const {
284 return float(fabs((point - rhs) * normal));
285 }
286
287 float Plane::shortest_distance(const Sphere &rhs) const {
288 return unpoof(shortest_distance(rhs.get_center()), rhs.get_radius());
289 }
290
291 Line::Line(const Point3f &end_point_a_, const Point3f &end_point_b_)
292 : end_point_a(end_point_a_),
293 end_point_b(end_point_b_),
294 direction(end_point_b - end_point_a),
295 direction2(direction * direction)
296 {
297 }
298
299 Line::Line(const Point3f &end_point_a_, const Vector3f &direction_vector_)
300 : end_point_a(end_point_a_),
301 end_point_b(end_point_a_ + direction_vector_),
302 direction(direction_vector_),
303 direction2(direction * direction)
304 {
305 }
306
307 float Line::shortest_distance(const Point3f &rhs) const {
308 return Collision::nearest_point(*this, rhs).first;
309 }
310 float Line::shortest_distance(const Sphere &rhs) const {
311 return unpoof(shortest_distance(rhs.get_center()), rhs.get_radius());
312 }
313 float Line::shortest_distance(const Plane &rhs) const {
314 return Collision::nearest_point(*this, rhs).first;
315 }
316 float Line::shortest_distance(const Line &rhs) const {
317 return Collision::nearest_point(*this, rhs).first;
318 }
319 float Line::shortest_distance(const Line_Segment &rhs) const {
320 return Collision::nearest_point(*this, rhs).first;
321 }
322 float Line::shortest_distance(const Parallelepiped &rhs) const {
323 return Collision::nearest_point(*this, rhs).first;
324 }
325
326 Ray::Ray(const Point3f &end_point_a_, const Point3f &end_point_b_)
327 : end_point_a(end_point_a_),
328 end_point_b(end_point_b_),
329 direction(end_point_b - end_point_a),
330 direction2(direction * direction)
331 {
332 }
333
334 Ray::Ray(const Point3f &end_point_a_, const Vector3f &direction_vector_)
335 : end_point_a(end_point_a_),
336 end_point_b(end_point_a_ + direction_vector_),
337 direction(direction_vector_),
338 direction2(direction * direction)
339 {
340 }
341
342 std::pair<float, float> Ray::nearest_point(const Point3f &rhs) const {
343 return Collision::nearest_point(*this, rhs);
344 }
345 std::pair<float, float> Ray::nearest_point(const Sphere &rhs) const {
346 return unpoof(nearest_point(rhs.get_center()), rhs.get_radius());
347 }
348 std::pair<float, float> Ray::nearest_point(const Plane &rhs) const {
349 return Collision::nearest_point(*this, rhs);
350 }
351 std::pair<float, float> Ray::nearest_point(const Line &rhs) const {
352 return Collision::nearest_point(*this, rhs);
353 }
354 std::pair<float, float> Ray::nearest_point(const Ray &rhs) const {
355 return Collision::nearest_point(*this, rhs);
356 }
357 std::pair<float, float> Ray::nearest_point(const Line_Segment &rhs) const {
358 return Collision::nearest_point(*this, rhs);
359 }
360 std::pair<float, float> Ray::nearest_point(const Parallelepiped &rhs) const {
361 return Collision::nearest_point(*this, rhs);
362 }
363
364 Line_Segment::Line_Segment(const Point3f &end_point_a_, const Point3f &end_point_b_)
365 : end_point_a(end_point_a_),
366 end_point_b(end_point_b_),
367 direction(end_point_b - end_point_a),
368 direction2(direction * direction)
369 {
370 }
371
372 std::pair<float, float> Line_Segment::nearest_point(const Point3f &rhs) const {
373 return Collision::nearest_point(*this, rhs);
374 }
375 std::pair<float, float> Line_Segment::nearest_point(const Sphere &rhs) const {
376 return unpoof(nearest_point(rhs.get_center()), rhs.get_radius());
377 }
378 std::pair<float, float> Line_Segment::nearest_point(const Plane &rhs) const {
379 return Collision::nearest_point(*this, rhs);
380 }
381 std::pair<float, float> Line_Segment::nearest_point(const Line &rhs) const {
382 return Collision::nearest_point(*this, rhs);
383 }
384 std::pair<float, float> Line_Segment::nearest_point(const Ray &rhs) const {
385 return Collision::nearest_point(*this, rhs);
386 }
387 std::pair<float, float> Line_Segment::nearest_point(const Line_Segment &rhs) const {
388 return Collision::nearest_point(*this, rhs);
389 }
390 std::pair<float, float> Line_Segment::nearest_point(const Parallelepiped &rhs) const {
391 return Collision::nearest_point(*this, rhs);
392 }
393
394 Infinite_Cylinder::Infinite_Cylinder(const Point3f &end_point_a_, const Point3f &end_point_b_,
395 const float &radius_)
396 : line(end_point_a_, end_point_b_),
397 radius(radius_)
398 {
399 }
400
401 Infinite_Cylinder::Infinite_Cylinder(const Point3f &end_point_a_, const Vector3f &direction_vector_,
402 const float &radius_)
403 : line(end_point_a_, direction_vector_),
404 radius(radius_)
405 {
406 }
407
408 float Infinite_Cylinder::shortest_distance(const Point3f &rhs) const {
409 return unpoof(line.shortest_distance(rhs), radius);
410 }
411 float Infinite_Cylinder::shortest_distance(const Sphere &rhs) const {
412 return unpoof(line.shortest_distance(rhs), radius);
413 }
414 float Infinite_Cylinder::shortest_distance(const Plane &rhs) const {
415 return unpoof(line.shortest_distance(rhs), radius);
416 }
417 float Infinite_Cylinder::shortest_distance(const Infinite_Cylinder &rhs) const {
418 return unpoof(line.shortest_distance(rhs.line), radius + rhs.radius);
419 }
420 float Infinite_Cylinder::shortest_distance(const Line &rhs) const {
421 return unpoof(line.shortest_distance(rhs), radius);
422 }
423 float Infinite_Cylinder::shortest_distance(const Ray &rhs) const {
424 return unpoof(line.shortest_distance(rhs), radius);
425 }
426 float Infinite_Cylinder::shortest_distance(const Line_Segment &rhs) const {
427 return unpoof(line.shortest_distance(rhs), radius);
428 }
429
430 Capsule::Capsule(const Point3f &end_point_a_, const Point3f &end_point_b_,
431 const float &radius_)
432 : line_segment(end_point_a_, end_point_b_),
433 radius(radius_)
434 {
435 }
436
437 std::pair<float, float> Capsule::nearest_point(const Point3f &rhs) const {
438 return unpoof(line_segment.nearest_point(rhs), radius);
439 }
440 std::pair<float, float> Capsule::nearest_point(const Sphere &rhs) const {
441 return unpoof(line_segment.nearest_point(rhs), radius);
442 }
443 std::pair<float, float> Capsule::nearest_point(const Plane &rhs) const {
444 return unpoof(line_segment.nearest_point(rhs), radius);
445 }
446 std::pair<float, float> Capsule::nearest_point(const Line &rhs) const {
447 return unpoof(line_segment.nearest_point(rhs), radius);
448 }
449 std::pair<float, float> Capsule::nearest_point(const Ray &rhs) const {
450 return unpoof(line_segment.nearest_point(rhs), radius);
451 }
452 std::pair<float, float> Capsule::nearest_point(const Line_Segment &rhs) const {
453 return unpoof(line_segment.nearest_point(rhs), radius);
454 }
455 std::pair<float, float> Capsule::nearest_point(const Infinite_Cylinder &rhs) const {
456 return unpoof(line_segment.nearest_point(reinterpret_cast<const Line_Segment &>(rhs)), radius + rhs.get_radius());
457 }
458 std::pair<float, float> Capsule::nearest_point(const Capsule &rhs) const {
459 return unpoof(line_segment.nearest_point(rhs.line_segment), radius + rhs.radius);
460 }
461 std::pair<float, float> Capsule::nearest_point(const Parallelepiped &rhs) const {
462 return unpoof(line_segment.nearest_point(rhs), radius);
463 }
464
465 Parallelepiped::Parallelepiped(const Point3f &point_, const Vector3f &edge_a_,
466 const Vector3f &edge_b_, const Vector3f &edge_c_)
467 : point(point_),
468 edge_a(edge_a_),
469 edge_b(edge_b_),
470 edge_c(edge_c_),
471 convert_from(edge_a, edge_b, edge_c),
472 convert_to(convert_from.inverted()),
473 center(point),
474 normal_a((edge_b % edge_c).normalized()),
475 normal_b((edge_c % edge_a).normalized()),
476 normal_c((edge_a % edge_b).normalized())
477 {
478 const Vector3f ha = 0.5f * edge_a;
479 const Vector3f hb = 0.5f * edge_b;
480 const Vector3f hc = 0.5f * edge_c;
481
482 center += ha + hb + hc;
483
484 absolute_float_unclamp(extents.i, ha * normal_a);
485 absolute_float_unclamp(extents.i, hb * normal_a);
486 absolute_float_unclamp(extents.i, hc * normal_a);
487 absolute_float_unclamp(extents.j, ha * normal_b);
488 absolute_float_unclamp(extents.j, hb * normal_b);
489 absolute_float_unclamp(extents.j, hc * normal_b);
490 absolute_float_unclamp(extents.k, ha * normal_c);
491 absolute_float_unclamp(extents.k, hb * normal_c);
492 absolute_float_unclamp(extents.k, hc * normal_c);
493 }
494
495 float Parallelepiped::shortest_distance(const Parallelepiped &rhs) const {
496 const Vector3f &a = extents;
497 const Point3f &Pa = center;
498 const Vector3f * const A = &normal_a;
499
500 const Vector3f &b = rhs.extents;
501 const Point3f &Pb = rhs.center;
502 const Vector3f * const B = &rhs.normal_a;
503
504 // translation, in parent frame
505 const Vector3f v = Pb - Pa;
506
507 // translation, in A's frame
508 const Vector3f T(v * A[0], v * A[1], v * A[2]);
509
510 // B's basis with respect to A's local frame (rotation matrix)
511 float R[3][3];
512 for(int j = 0; j < 3; ++j)
513 for(int i = 0; i < 3; ++i)
514 R[j][i] = A[j] * B[i];
515
516 // generate the fabs(rotation matrix)
517 const float abs_R[3][3] = {{float(fabs(R[0][0])), float(fabs(R[0][1])), float(fabs(R[0][2]))},
518 {float(fabs(R[1][0])), float(fabs(R[1][1])), float(fabs(R[1][2]))},
519 {float(fabs(R[2][0])), float(fabs(R[2][1])), float(fabs(R[2][2]))}};
520
521 /* ALGORITHM: Use the separating axis test for all 15 potential
522 * separating axes. If a separating axis could not be found, the two
523 * boxes overlap.
524 */
525
526 float rv = 0.0f;
527
528 // A and B's 6 basis vectors (3 each)
529 for(int i = 0; i < 3; ++i) {
530 {
531 const float &ra = a[i];
532 const float rb = b[0] * abs_R[i][0] + b[1] * abs_R[i][1] + b[2] * abs_R[i][2];
533 const float t = float(fabs(T[i]));
534
535 if(t > ra + rb)
536 rv = std::max(rv, t - (ra + rb));
537 }
538
539 {
540 const float ra = a[0] * abs_R[0][i] + a[1] * abs_R[1][i] + a[2] * abs_R[2][i];
541 const float &rb = b[i];
542 const float t = float(fabs(T[0] * R[0][i] + T[1] * R[1][i] + T[2] * R[2][i]));
543
544 if(t > ra + rb)
545 rv = std::max(rv, t - (ra + rb));
546 }
547 }
548
549 /* 9 cross products
550 *
551 * L = A/j[0,1,2] x B/i[0,1,2]
552 *
553 * row by row | column by column
554 * j = [0,1,2] | i = [0,1,2]
555 * u = [1,0,0] | m = [1,0,0]
556 * v = [2,2,1] | n = [2,2,1]
557 */
558 for(int j = 0, u = 1, v = 2; j < 3; ++j) {
559 for(int i = 0, m = 1, n = 2; i < 3; ++i) {
560 const float ra = a[u] * abs_R[v][i] + a[v] * abs_R[u][i];
561 const float rb = b[m] * abs_R[j][n] + b[n] * abs_R[j][m];
562 const float t = float(fabs(T[u] * R[v][i] - T[v] * R[u][i]));
563
564 if(t > ra + rb)
565 rv = std::max(rv, t - (ra + rb));
566
567 if(!i) --m; else --n;
568 }
569
570 if(!j) --u; else --v;
571 }
572
573 // no separating axis found, indicating that the two boxes overlap
574 return rv;
575 }
576
577 float Parallelepiped::some_distance(const Parallelepiped &rhs) const {
578 const Vector3f &a = extents;
579 const Point3f &Pa = center;
580 const Vector3f * const A = &normal_a;
581
582 const Vector3f &b = rhs.extents;
583 const Point3f &Pb = rhs.center;
584 const Vector3f * const B = &rhs.normal_a;
585
586 // translation, in parent frame
587 const Vector3f v = Pb - Pa;
588
589 // translation, in A's frame
590 const Vector3f T(v * A[0], v * A[1], v * A[2]);
591
592 // B's basis with respect to A's local frame (rotation matrix)
593 float R[3][3];
594 for(int j = 0; j < 3; ++j)
595 for(int i = 0; i < 3; ++i)
596 R[j][i] = A[j] * B[i];
597
598 // generate the fabs(rotation matrix)
599 const float abs_R[3][3] = {{float(fabs(R[0][0])), float(fabs(R[0][1])), float(fabs(R[0][2]))},
600 {float(fabs(R[1][0])), float(fabs(R[1][1])), float(fabs(R[1][2]))},
601 {float(fabs(R[2][0])), float(fabs(R[2][1])), float(fabs(R[2][2]))}};
602
603 /* ALGORITHM: Use the separating axis test for all 15 potential
604 * separating axes. If a separating axis could not be found, the two
605 * boxes overlap.
606 */
607
608 // A and B's 6 basis vectors (3 each)
609 for(int i = 0; i < 3; ++i) {
610 {
611 const float &ra = a[i];
612 const float rb = b[0] * abs_R[i][0] + b[1] * abs_R[i][1] + b[2] * abs_R[i][2];
613 const float t = float(fabs(T[i]));
614
615 if(t > ra + rb)
616 return t - (ra + rb);
617 }
618
619 {
620 const float ra = a[0] * abs_R[0][i] + a[1] * abs_R[1][i] + a[2] * abs_R[2][i];
621 const float &rb = b[i];
622 const float t = float(fabs(T[0] * R[0][i] + T[1] * R[1][i] + T[2] * R[2][i]));
623
624 if(t > ra + rb)
625 return t - (ra + rb);
626 }
627 }
628
629 /* 9 cross products
630 *
631 * L = A/j[0,1,2] x B/i[0,1,2]
632 *
633 * row by row | column by column
634 * j = [0,1,2] | i = [0,1,2]
635 * u = [1,0,0] | m = [1,0,0]
636 * v = [2,2,1] | n = [2,2,1]
637 */
638 for(int j = 0, u = 1, v = 2; j < 3; ++j) {
639 for(int i = 0, m = 1, n = 2; i < 3; ++i) {
640 const float ra = a[u] * abs_R[v][i] + a[v] * abs_R[u][i];
641 const float rb = b[m] * abs_R[j][n] + b[n] * abs_R[j][m];
642 const float t = float(fabs(T[u] * R[v][i] - T[v] * R[u][i]));
643
644 if(t > ra + rb)
645 return t - (ra + rb);
646
647 if(!i) --m; else --n;
648 }
649
650 if(!j) --u; else --v;
651 }
652
653 // no separating axis found, indicating that the two boxes overlap
654 return 0.0f;
655 }
656
657 float Parallelepiped::shortest_distance(const Point3f &rhs) const {
658 const Point3f converted_point = convert_to * (rhs - point);
659
660 Point3f nearest_point = converted_point;
661 simple_clamp(nearest_point.x, 0.0f, 1.0f);
662 simple_clamp(nearest_point.y, 0.0f, 1.0f);
663 simple_clamp(nearest_point.z, 0.0f, 1.0f);
664
665 const Vector3f difference = convert_from * (converted_point - nearest_point);
666
667 return difference.magnitude();
668 }
669
670 float Parallelepiped::shortest_distance(const Plane &rhs) const {
671 const Point3f &p = rhs.get_point();
672 const Vector3f &n = rhs.get_normal();
673
674 const float diff_001 = edge_c * n;
675 const float diff_010 = edge_b * n;
676 const float diff_011 = diff_010 + diff_001;
677 const float diff_100 = edge_a * n;
678 const float diff_101 = diff_100 + diff_001;
679 const float diff_110 = diff_100 + diff_010;
680 const float diff_111 = diff_100 + diff_011;
681
682 const float t_000 = (point - p) * n;
683 const float t_001 = t_000 + diff_001;
684 const float t_010 = t_000 + diff_010;
685 const float t_011 = t_000 + diff_011;
686 const float t_100 = t_000 + diff_100;
687 const float t_101 = t_000 + diff_101;
688 const float t_110 = t_000 + diff_110;
689 const float t_111 = t_000 + diff_111;
690
691 float min_t = t_000;
692 float max_t = t_000;
693
694 double_unclamp(min_t, max_t, t_001);
695 double_unclamp(min_t, max_t, t_010);
696 double_unclamp(min_t, max_t, t_011);
697 double_unclamp(min_t, max_t, t_100);
698 double_unclamp(min_t, max_t, t_101);
699 double_unclamp(min_t, max_t, t_110);
700 double_unclamp(min_t, max_t, t_111);
701
702 if(min_t > 0.0f)
703 return min_t;
704 if(max_t < 0.0f)
705 return -max_t;
706 return 0.0f;
707 }
708
709 float Parallelepiped::shortest_distance(const Sphere &rhs) const {
710 return unpoof(shortest_distance(rhs.get_center()), rhs.get_radius());
711 }
712 float Parallelepiped::shortest_distance(const Infinite_Cylinder &rhs) const {
713 return unpoof(shortest_distance(reinterpret_cast<const Line &>(rhs)), rhs.get_radius());
714 }
715
716 }
717
718 }
719
720 #include <Zeni/Undefine.h>
Zeni::Collision::Plane::shortest_distance
float shortest_distance(const Plane &rhs) const
Definition: Collision.cpp:275
Zeni::Collision::nearest_point
std::pair< float, float > nearest_point(const LINE_TYPE &lhs, const Point3f &rhs)
Definition: Collision.cpp:66
Zeni::Collision::Line::Line
Line()
Definition: Collision.h:320
Zeni::Collision::Capsule::Capsule
Capsule()
Definition: Collision.h:384
f
GLclampf f
Definition: glew.h:3390
normalized
GLint GLenum GLboolean normalized
Definition: glew.h:1891
Zeni::Collision::Capsule
Collision Capsule.
Definition: Collision.h:382
Zeni::Collision::Line
Collision Line.
Definition: Collision.h:318
n
GLclampd n
Definition: glew.h:7287
Zeni::Collision::Sphere::get_radius
const float & get_radius() const
Definition: Collision.h:200
Zeni::Collision::Plane::Plane
Plane()
Definition: Collision.h:209
t
GLdouble GLdouble t
Definition: glew.h:1384
ZENI_COLLISION_EPSILON
#define ZENI_COLLISION_EPSILON
Definition: Define.h:79
j
int32_t j
Definition: e_log.c:102
Zeni::Vector3f::divide_by
Vector3f divide_by(const Vector3f &rhs) const
Divide corresponding members.
Definition: Vector3f.hxx:174
a
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:8736
Zeni::Collision::Plane::get_normal
const Vector3f & get_normal() const
Definition: Collision.h:226
Zeni::Collision::simple_clamp
void simple_clamp(VALUE_TYPE &value, const VALUE_TYPE &lower, const VALUE_TYPE &upper)
Definition: Collision.cpp:50
Undefine.h
Zeni::Collision::Line::shortest_distance
float shortest_distance(const Line &rhs) const
Definition: Collision.cpp:316
Zeni::Collision::double_unclamp
void double_unclamp(VALUE_TYPE &min_value, VALUE_TYPE &max_value, const VALUE_TYPE &new_value)
Definition: Collision.cpp:58
Zeni::Collision::Infinite_Cylinder::shortest_distance
float shortest_distance(const Infinite_Cylinder &rhs) const
Definition: Collision.cpp:417
Zeni::Collision::Capsule::nearest_point
std::pair< float, float > nearest_point(const Capsule &rhs) const
Returns &lt;distance, interpolation value [0.0f, 1.0f]&gt;
Definition: Collision.cpp:458
Zeni::Vector3f::magnitude
float magnitude() const
Get the magnitude of the vector.
Definition: Vector3f.hxx:142
Zeni::Collision::Sphere::shortest_distance
float shortest_distance(const Sphere &rhs) const
Definition: Collision.cpp:262
Zeni::Collision::Line_Segment
Collision Line Segment.
Definition: Collision.h:233
Zeni::Collision::Infinite_Cylinder::get_radius
const float & get_radius() const
Definition: Collision.h:375
Zeni::Collision::Infinite_Cylinder
Collision Infinite Cylinder.
Definition: Collision.h:352
Zeni::Point3f
A 3D Point represented with floats.
Definition: Coordinate.h:133
Zeni::Collision::Sphere::get_center
const Point3f & get_center() const
Definition: Collision.h:199
Zeni::Collision::unpoof
std::pair< float, float > unpoof(const std::pair< float, float > &lhs, const float &radii)
Definition: Collision.cpp:35
Zeni::Vector3f
A Featureful 3-Space Vector Class.
Definition: Vector3f.h:58
u
ALuint u
Definition: alMain.h:58
v
const GLdouble * v
Definition: glew.h:1377
Zeni::Collision::Parallelepiped::get_convert_from
const Matrix4f & get_convert_from() const
Definition: Collision.h:444
Define.h
Zeni::Collision::Sphere
Collision Sphere.
Definition: Collision.h:181
p
GLfloat GLfloat p
Definition: glew.h:14938
END_OF_TIME
#define END_OF_TIME
Definition: Define.h:133
Zeni::Point3f::y
float y
Definition: Coordinate.h:152
Zeni::Collision::Parallelepiped
Collision Parallelepiped.
Definition: Collision.h:421
Zeni::Vector3f::k
float k
Definition: Vector3f.h:119
Zeni::Point3f::z
float z
Definition: Coordinate.h:152
Zeni::Collision::Ray
Collision Ray.
Definition: Collision.h:275
Zeni::Collision::Plane::get_point
const Point3f & get_point() const
Definition: Collision.h:225
Zeni::Collision::Parallelepiped::Parallelepiped
Parallelepiped()
Definition: Collision.h:423
Zeni::Collision::Ray::Ray
Ray()
Definition: Collision.h:277
Zeni::Collision::Parallelepiped::shortest_distance
float shortest_distance(const Parallelepiped &rhs) const
Definition: Collision.cpp:495
Zeni::Vector3f::j
float j
Definition: Vector3f.h:115
Zeni::Collision::Sphere::Sphere
Sphere()
Definition: Collision.h:183
Zeni::Collision::Line_Segment::Line_Segment
Line_Segment()
Definition: Collision.h:235
value
EGLSurface EGLint void ** value
Definition: eglext.h:301
Zeni::Collision::Parallelepiped::get_convert_to
const Matrix4f & get_convert_to() const
Definition: Collision.h:445
Zeni::Collision::Plane
Collision Plane.
Definition: Collision.h:207
Zeni::Collision::Line_Segment::nearest_point
std::pair< float, float > nearest_point(const Line_Segment &rhs) const
Returns &lt;distance, interpolation value [0.0f, 1.0f]&gt;
Definition: Collision.cpp:387
b
GLdouble GLdouble GLdouble b
Definition: glew.h:8383
w
GLint GLint GLint GLint GLint w
Definition: gl2ext.h:1215
min
#define min(x, y)
Definition: os.h:75
Zeni::Collision::Parallelepiped::get_point
const Point3f & get_point() const
Definition: Collision.h:440
zeni.h
R
R
Definition: e_log.c:153
i
int i
Definition: pngrutil.c:1377
max
#define max(x, y)
Definition: os.h:79
Zeni::Collision::absolute_float_unclamp
void absolute_float_unclamp(float &value, const float &new_value)
Definition: Collision.cpp:43
Zeni::Vector3f::i
float i
Definition: Vector3f.h:111
fabs
double fabs(double x)
Definition: s_fabs.c:29
m
#define m(i, j)
Zeni::Collision::Ray::nearest_point
std::pair< float, float > nearest_point(const Ray &rhs) const
Returns &lt;distance, interpolation value [0.0f, inf)&gt;
Definition: Collision.cpp:354
Zeni::Collision::Infinite_Cylinder::Infinite_Cylinder
Infinite_Cylinder()
Definition: Collision.h:354
Zeni::Point3f::x
float x
Definition: Coordinate.h:152
m
const GLdouble * m
Definition: glew.h:8385