Line data Source code
1 : #ifndef MPR_H_
2 : #define MPR_H_
3 :
4 : struct clipplanes;
5 :
6 : namespace mpr
7 : {
8 : struct CubePlanes final
9 : {
10 : const clipplanes &p;
11 :
12 5 : CubePlanes(const clipplanes &p) : p(p) {}
13 :
14 : /**
15 : * @brief Returns the value of the origin element of this object's clipplanes field.
16 : *
17 : * @return copy of this.p.o
18 : */
19 : vec center() const;
20 :
21 : /**
22 : * @brief Returns element in clipplanes vec array with largest dot product
23 : *
24 : * Calculates the dot product of n with the elements of the clipplanes
25 : * field p. Returns the value of the element with the largest dot product,
26 : * or the last element if all have the same dot product.
27 : *
28 : * @param n the parameter to dot with vec elements
29 : *
30 : * @return the values of the largest dot product element
31 : */
32 : vec supportpoint(const vec &n) const;
33 : };
34 :
35 : struct SolidCube final
36 : {
37 : vec o;
38 : int size;
39 :
40 6 : SolidCube(float x, float y, float z, int size) : o(x, y, z), size(size) {}
41 1 : SolidCube(const vec &o, int size) : o(o), size(size) {}
42 1 : SolidCube(const ivec &o, int size) : o(o), size(size) {}
43 :
44 : /**
45 : * @brief Returns SolidCube::o plus half of size.
46 : *
47 : * Returns value equal to elementwise sum of size/2 and o. That is, half
48 : * of size is added to each of o's x, y, z fields.
49 : *
50 : * @return vec containing sum of o, size/2
51 : */
52 : vec center() const;
53 :
54 : /**
55 : * @brief Returns o added to size in channels specified by passed vec.
56 : *
57 : * For fields in n that are greater than zero, adds size to that field in
58 : * o. The values of n are irrelevant other than whether they are greater
59 : * than zero.
60 : *
61 : * @return vec containing values of o conditionally summed with size
62 : */
63 : vec supportpoint(const vec &n) const;
64 : };
65 :
66 : struct Ent
67 : {
68 : const physent *ent;
69 :
70 20 : Ent(const physent *ent) : ent(ent) {}
71 :
72 : /**
73 : * @brief Returns the center of this ent, modified by the ent's height.
74 : *
75 : * Returns values containing the x and y positions of the physent inside
76 : * this ent, and the z position plus the eye correction. The eye
77 : * correction is half of the difference between the aboveeye and
78 : * eyeheight fields.
79 : *
80 : * @return the adjusted center position of the ent
81 : */
82 : vec center() const;
83 : };
84 :
85 : /**
86 : * @brief Entity Oriented Bounded Box
87 : *
88 : * The orientation of the box is set by the `orient` matrix and the size of
89 : * the bounding box is set by the `ent->xradius`, `ent->yradius`, and
90 : * `ent->aboveeye`/`ent->eyeheight`.
91 : *
92 : * The orient matrix is set by the yaw of the entity `ent`.
93 : */
94 : class EntOBB final : public Ent
95 : {
96 : public:
97 : /**
98 : * @brief Constructs this EntOBB, setting orient yaw direction field.
99 : *
100 : * Sets the yaw field to the yaw angle in the physent passed, converted
101 : * to radians.
102 : *
103 : * @param ent the physent associated with this EntOBB
104 : */
105 : EntOBB(const physent *ent);
106 :
107 : vec contactface(const vec &wn, const vec &wdir) const;
108 :
109 : vec supportpoint(const vec &n) const;
110 :
111 : float left() const;
112 : float right() const;
113 : float back() const;
114 : float front() const;
115 : float bottom() const;
116 : float top() const;
117 : private:
118 : matrix3 orient;
119 : float supportcoord(const vec &p) const;
120 : float supportcoordneg(const vec &p) const;
121 :
122 : /**
123 : * @brief Returns ent sizes modified by passed ln parameter
124 : *
125 : * Returns vec(ent->xradius, ent->yradius, ent->eyeheight|-ent->aboveeye) with the signs of
126 : * the components set by the signs of the components of `ln`. Values passed
127 : * to `ln` are disregarded except for sign. Zero values of components of `ln`
128 : * result in a negative sign.
129 : *
130 : * @param ln the vec to get sign information from
131 : *
132 : * @return position of ent modified by signs from ln
133 : */
134 : vec localsupportpoint(const vec &ln) const;
135 : };
136 :
137 : /**
138 : * @brief Unoriented bounding box entity
139 : *
140 : * EntFuzzy objects have their x,y boundaries (left,right,back,front) set by
141 : * the radius field, and the z boundaries by the aboveeye/eyeheight
142 : * fields. The specific x/y radii fields are not used.
143 : */
144 : struct EntFuzzy : Ent
145 : {
146 9 : EntFuzzy(const physent *ent) : Ent(ent) {}
147 :
148 : float left() const;
149 : float right() const;
150 : float back() const;
151 : float front() const;
152 : float bottom() const;
153 : float top() const;
154 : };
155 :
156 : struct EntCylinder final : EntFuzzy
157 : {
158 0 : EntCylinder(const physent *ent) : EntFuzzy(ent) {}
159 :
160 : vec contactface(const vec &n, const vec &dir) const;
161 : vec supportpoint(const vec &n) const;
162 : };
163 :
164 : struct EntCapsule final : EntFuzzy
165 : {
166 0 : EntCapsule(const physent *ent) : EntFuzzy(ent) {}
167 :
168 : vec supportpoint(const vec &n) const;
169 : };
170 :
171 : struct EntEllipsoid final : EntFuzzy
172 : {
173 : EntEllipsoid(const physent *ent) : EntFuzzy(ent) {}
174 :
175 : vec supportpoint(const vec &dir) const;
176 : };
177 :
178 : struct Model
179 : {
180 : vec o, radius;
181 : matrix3 orient;
182 :
183 : /**
184 : * @brief Creates a new object with specified position and orientation.
185 : *
186 : * Sets position to ent + center, with center modified by the rotation matrix
187 : * defined by the Euler angle parameters `yaw` `pitch` `roll`.
188 : *
189 : * @param ent base position of ent
190 : * @param center center offset from ent, modified by Euler angles
191 : * @param radius radius of entity in basis directions
192 : * @param yaw yaw parameter of rotation for center
193 : * @param pitch pitch parameter of rotation for center
194 : * @param roll roll parameter of rotation for center
195 : */
196 : Model(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll);
197 :
198 : /**
199 : * @brief Returns the origin vector of this object.
200 : *
201 : * This is the field model.o.
202 : *
203 : * @return copy of o
204 : */
205 : vec center() const;
206 : };
207 :
208 : struct ModelOBB final : Model
209 : {
210 0 : ModelOBB(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
211 0 : Model(ent, center, radius, yaw, pitch, roll)
212 0 : {}
213 :
214 : vec contactface(const vec &wn, const vec &wdir) const;
215 : vec supportpoint(const vec &n) const;
216 : };
217 :
218 :
219 : struct ModelEllipse final : Model
220 : {
221 0 : ModelEllipse(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
222 0 : Model(ent, center, radius, yaw, pitch, roll)
223 0 : {}
224 :
225 : vec contactface(const vec &wn, const vec &wdir) const;
226 : vec supportpoint(const vec &n) const;
227 : };
228 :
229 : //templates
230 : constexpr float boundarytolerance = 1e-3f;
231 :
232 : template<class T>
233 0 : bool collide(const T &p1, const EntOBB &p2)
234 : {
235 : // v0 = center of Minkowski difference
236 0 : vec v0 = p2.center().sub(p1.center());
237 0 : if(v0.iszero())
238 : {
239 0 : return true; // v0 and origin overlap ==> hit
240 : }
241 : // v1 = support in direction of origin
242 0 : vec n = vec(v0).neg();
243 0 : vec v1 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
244 0 : if(v1.dot(n) <= 0)
245 : {
246 0 : return false; // origin outside v1 support plane ==> miss
247 : }
248 : // v2 = support perpendicular to plane containing origin, v0 and v1
249 0 : n.cross(v1, v0);
250 0 : if(n.iszero())
251 : {
252 0 : return true; // v0, v1 and origin colinear (and origin inside v1 support plane) == > hit
253 : }
254 0 : vec v2 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
255 0 : if(v2.dot(n) <= 0)
256 : {
257 0 : return false; // origin outside v2 support plane ==> miss
258 : }
259 : // v3 = support perpendicular to plane containing v0, v1 and v2
260 0 : n.cross(v0, v1, v2);
261 : // If the origin is on the - side of the plane, reverse the direction of the plane
262 0 : if(n.dot(v0) > 0)
263 : {
264 0 : std::swap(v1, v2);
265 0 : n.neg();
266 : }
267 : ///
268 : // Phase One: Find a valid portal
269 :
270 0 : for(int i = 0; i < 100; ++i)
271 : {
272 : // Obtain the next support point
273 0 : vec v3 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
274 0 : if(v3.dot(n) <= 0)
275 : {
276 0 : return false; // origin outside v3 support plane ==> miss
277 : }
278 : // If origin is outside (v1,v0,v3), then portal is invalid -- eliminate v2 and find new support outside face
279 0 : vec v3xv0;
280 0 : v3xv0.cross(v3, v0);
281 0 : if(v1.dot(v3xv0) < 0)
282 : {
283 0 : v2 = v3;
284 0 : n.cross(v0, v1, v3);
285 0 : continue;
286 : }
287 : // If origin is outside (v3,v0,v2), then portal is invalid -- eliminate v1 and find new support outside face
288 0 : if(v2.dot(v3xv0) > 0)
289 : {
290 0 : v1 = v3;
291 0 : n.cross(v0, v3, v2);
292 0 : continue;
293 : }
294 : ///
295 : // Phase Two: Refine the portal
296 0 : for(int j = 0;; j++)
297 : {
298 : // Compute outward facing normal of the portal
299 0 : n.cross(v1, v2, v3);
300 :
301 : // If the origin is inside the portal, we have a hit
302 0 : if(n.dot(v1) >= 0)
303 : {
304 0 : return true;
305 : }
306 0 : n.normalize();
307 : // Find the support point in the direction of the portal's normal
308 0 : vec v4 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
309 : // If the origin is outside the support plane or the boundary is thin enough, we have a miss
310 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
311 : {
312 0 : return false;
313 : }
314 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
315 : // Note: We're taking advantage of the triple product identities here as an optimization
316 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
317 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
318 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
319 0 : vec v4xv0;
320 0 : v4xv0.cross(v4, v0);
321 0 : if(v1.dot(v4xv0) > 0)
322 : {
323 0 : if(v2.dot(v4xv0) > 0)
324 : {
325 0 : v1 = v4; // Inside v1 & inside v2 ==> eliminate v1
326 : }
327 : else
328 : {
329 0 : v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
330 : }
331 : }
332 : else
333 : {
334 0 : if(v3.dot(v4xv0) > 0)
335 : {
336 0 : v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
337 : }
338 : else
339 : {
340 0 : v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
341 : }
342 : }
343 : }
344 : }
345 0 : return false;
346 : }
347 :
348 : template<class T>
349 0 : bool collide(const EntOBB &p1, const T &p2, vec *contactnormal, vec *contactpoint1, vec *contactpoint2)
350 : {
351 : // v0 = center of Minkowski sum
352 0 : const vec v01 = p1.center();
353 0 : const vec v02 = p2.center();
354 0 : vec v0 = vec(v02).sub(v01);
355 :
356 : // Avoid case where centers overlap -- any direction is fine in this case
357 0 : if(v0.iszero())
358 : {
359 0 : v0 = vec(0, 0, 1e-5f);
360 : }
361 : // v1 = support in direction of origin
362 0 : vec n = vec(v0).neg();
363 0 : vec v11 = p1.supportpoint(vec(n).neg());
364 0 : vec v12 = p2.supportpoint(n);
365 0 : vec v1 = vec(v12).sub(v11);
366 0 : if(v1.dot(n) <= 0)
367 : {
368 0 : if(contactnormal)
369 : {
370 0 : *contactnormal = n;
371 : }
372 0 : return false;
373 : }
374 : // v2 - support perpendicular to v1,v0
375 0 : n.cross(v1, v0);
376 0 : if(n.iszero())
377 : {
378 0 : n = vec(v1).sub(v0);
379 0 : n.normalize();
380 0 : if(contactnormal)
381 : {
382 0 : *contactnormal = n;
383 : }
384 0 : if(contactpoint1)
385 : {
386 0 : *contactpoint1 = v11;
387 : }
388 0 : if(contactpoint2)
389 : {
390 0 : *contactpoint2 = v12;
391 : }
392 0 : return true;
393 : }
394 0 : vec v21 = p1.supportpoint(vec(n).neg());
395 0 : vec v22 = p2.supportpoint(n);
396 0 : vec v2 = vec(v22).sub(v21);
397 0 : if(v2.dot(n) <= 0)
398 : {
399 0 : if(contactnormal)
400 : {
401 0 : *contactnormal = n;
402 : }
403 0 : return false;
404 : }
405 : // Determine whether origin is on + or - side of plane (v1,v0,v2)
406 0 : n.cross(v0, v1, v2);
407 : // If the origin is on the - side of the plane, reverse the direction of the plane
408 0 : if(n.dot(v0) > 0)
409 : {
410 0 : std::swap(v1, v2);
411 0 : std::swap(v11, v21);
412 0 : std::swap(v12, v22);
413 0 : n.neg();
414 : }
415 : ///
416 : // Phase One: Identify a portal
417 0 : for(int i = 0; i < 100; ++i)
418 : {
419 : // Obtain the support point in a direction perpendicular to the existing plane
420 : // Note: This point is guaranteed to lie off the plane
421 0 : vec v31 = p1.supportpoint(vec(n).neg());
422 0 : vec v32 = p2.supportpoint(n);
423 0 : vec v3 = vec(v32).sub(v31);
424 0 : if(v3.dot(n) <= 0)
425 : {
426 0 : if(contactnormal) *contactnormal = n;
427 0 : return false;
428 : }
429 : // If origin is outside (v1,v0,v3), then eliminate v2 and loop
430 0 : vec v3xv0;
431 0 : v3xv0.cross(v3, v0);
432 0 : if(v1.dot(v3xv0) < 0)
433 : {
434 0 : v2 = v3;
435 0 : v21 = v31;
436 0 : v22 = v32;
437 0 : n.cross(v0, v1, v3);
438 0 : continue;
439 : }
440 : // If origin is outside (v3,v0,v2), then eliminate v1 and loop
441 0 : if(v2.dot(v3xv0) > 0)
442 : {
443 0 : v1 = v3;
444 0 : v11 = v31;
445 0 : v12 = v32;
446 0 : n.cross(v0, v3, v2);
447 0 : continue;
448 : }
449 0 : bool hit = false;
450 : ///
451 : // Phase Two: Refine the portal
452 : // We are now inside of a wedge...
453 0 : for(int j = 0;; j++)
454 : {
455 : // Compute normal of the wedge face
456 0 : n.cross(v1, v2, v3);
457 : // Can this happen??? Can it be handled more cleanly?
458 0 : if(n.iszero())
459 : {
460 0 : return true;
461 : }
462 0 : n.normalize();
463 : // If the origin is inside the wedge, we have a hit
464 0 : if(n.dot(v1) >= 0 && !hit)
465 : {
466 0 : if(contactnormal)
467 : {
468 0 : *contactnormal = n;
469 : }
470 : // Compute the barycentric coordinates of the origin
471 0 : if(contactpoint1 || contactpoint2)
472 : {
473 0 : float b0 = v3.scalartriple(v1, v2),
474 0 : b1 = v0.scalartriple(v3, v2),
475 0 : b2 = v3.scalartriple(v0, v1),
476 0 : b3 = v0.scalartriple(v2, v1),
477 0 : sum = b0 + b1 + b2 + b3;
478 0 : if(sum <= 0)
479 : {
480 0 : b0 = 0;
481 0 : b1 = n.scalartriple(v2, v3);
482 0 : b2 = n.scalartriple(v3, v1);
483 0 : b3 = n.scalartriple(v1, v2);
484 0 : sum = b1 + b2 + b3;
485 : }
486 0 : if(contactpoint1)
487 : {
488 0 : *contactpoint1 = (vec(v01).mul(b0).add(vec(v11).mul(b1)).add(vec(v21).mul(b2)).add(vec(v31).mul(b3))).mul(1.0f/sum);
489 : }
490 0 : if(contactpoint2)
491 : {
492 0 : *contactpoint2 = (vec(v02).mul(b0).add(vec(v12).mul(b1)).add(vec(v22).mul(b2)).add(vec(v32).mul(b3))).mul(1.0f/sum);
493 : }
494 : }
495 : // HIT!!!
496 0 : hit = true;
497 : }
498 : // Find the support point in the direction of the wedge face
499 0 : vec v41 = p1.supportpoint(vec(n).neg());
500 0 : vec v42 = p2.supportpoint(n);
501 0 : vec v4 = vec(v42).sub(v41);
502 : // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
503 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
504 : {
505 0 : if(contactnormal)
506 : {
507 0 : *contactnormal = n;
508 : }
509 0 : return hit;
510 : }
511 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
512 : // Note: We're taking advantage of the triple product identities here as an optimization
513 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
514 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
515 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
516 0 : vec v4xv0;
517 0 : v4xv0.cross(v4, v0);
518 0 : if(v1.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d1 = (v4,v0,v1)
519 : {
520 0 : if(v2.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d2 = (v4,v0,v2)
521 : {
522 : // Inside d1 & inside d2 ==> eliminate v1
523 0 : v1 = v4;
524 0 : v11 = v41;
525 0 : v12 = v42;
526 : }
527 : else
528 : {
529 : // Inside d1 & outside d2 ==> eliminate v3
530 0 : v3 = v4;
531 0 : v31 = v41;
532 0 : v32 = v42;
533 : }
534 : }
535 : else
536 : {
537 0 : if(v3.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d3 = (v4,v0,v3)
538 : {
539 : // Outside d1 & inside d3 ==> eliminate v2
540 0 : v2 = v4;
541 0 : v21 = v41;
542 0 : v22 = v42;
543 : }
544 : else
545 : {
546 : // Outside d1 & outside d3 ==> eliminate v1
547 0 : v1 = v4;
548 0 : v11 = v41;
549 0 : v12 = v42;
550 : }
551 : }
552 : }
553 : }
554 0 : return false;
555 : }
556 : }
557 :
558 : #endif
|
