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 : virtual ~Model() = 0;
208 : };
209 :
210 : struct ModelOBB final : Model
211 : {
212 0 : ModelOBB(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
213 0 : Model(ent, center, radius, yaw, pitch, roll)
214 0 : {}
215 :
216 : vec contactface(const vec &wn, const vec &wdir) const;
217 : vec supportpoint(const vec &n) const;
218 : };
219 :
220 :
221 : struct ModelEllipse final : Model
222 : {
223 3 : ModelEllipse(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
224 3 : Model(ent, center, radius, yaw, pitch, roll)
225 3 : {}
226 :
227 : vec contactface(const vec &wn, const vec &wdir) const;
228 : vec supportpoint(const vec &n) const;
229 : };
230 :
231 : //templates
232 : constexpr float boundarytolerance = 1e-3f;
233 :
234 : template<class T>
235 0 : bool collide(const T &p1, const EntOBB &p2)
236 : {
237 : // v0 = center of Minkowski difference
238 0 : vec v0 = p2.center().sub(p1.center());
239 0 : if(v0.iszero())
240 : {
241 0 : return true; // v0 and origin overlap ==> hit
242 : }
243 : // v1 = support in direction of origin
244 0 : vec n = vec(v0).neg();
245 0 : vec v1 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
246 0 : if(v1.dot(n) <= 0)
247 : {
248 0 : return false; // origin outside v1 support plane ==> miss
249 : }
250 : // v2 = support perpendicular to plane containing origin, v0 and v1
251 0 : n.cross(v1, v0);
252 0 : if(n.iszero())
253 : {
254 0 : return true; // v0, v1 and origin colinear (and origin inside v1 support plane) == > hit
255 : }
256 0 : vec v2 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
257 0 : if(v2.dot(n) <= 0)
258 : {
259 0 : return false; // origin outside v2 support plane ==> miss
260 : }
261 : // v3 = support perpendicular to plane containing v0, v1 and v2
262 0 : n.cross(v0, v1, v2);
263 : // If the origin is on the - side of the plane, reverse the direction of the plane
264 0 : if(n.dot(v0) > 0)
265 : {
266 0 : std::swap(v1, v2);
267 0 : n.neg();
268 : }
269 : ///
270 : // Phase One: Find a valid portal
271 :
272 0 : for(int i = 0; i < 100; ++i)
273 : {
274 : // Obtain the next support point
275 0 : vec v3 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
276 0 : if(v3.dot(n) <= 0)
277 : {
278 0 : return false; // origin outside v3 support plane ==> miss
279 : }
280 : // If origin is outside (v1,v0,v3), then portal is invalid -- eliminate v2 and find new support outside face
281 0 : vec v3xv0;
282 0 : v3xv0.cross(v3, v0);
283 0 : if(v1.dot(v3xv0) < 0)
284 : {
285 0 : v2 = v3;
286 0 : n.cross(v0, v1, v3);
287 0 : continue;
288 : }
289 : // If origin is outside (v3,v0,v2), then portal is invalid -- eliminate v1 and find new support outside face
290 0 : if(v2.dot(v3xv0) > 0)
291 : {
292 0 : v1 = v3;
293 0 : n.cross(v0, v3, v2);
294 0 : continue;
295 : }
296 : ///
297 : // Phase Two: Refine the portal
298 0 : for(int j = 0;; j++)
299 : {
300 : // Compute outward facing normal of the portal
301 0 : n.cross(v1, v2, v3);
302 :
303 : // If the origin is inside the portal, we have a hit
304 0 : if(n.dot(v1) >= 0)
305 : {
306 0 : return true;
307 : }
308 0 : n.normalize();
309 : // Find the support point in the direction of the portal's normal
310 0 : const vec v4 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
311 : // If the origin is outside the support plane or the boundary is thin enough, we have a miss
312 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
313 : {
314 0 : return false;
315 : }
316 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
317 : // Note: We're taking advantage of the triple product identities here as an optimization
318 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
319 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
320 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
321 0 : vec v4xv0;
322 0 : v4xv0.cross(v4, v0);
323 0 : if(v1.dot(v4xv0) > 0)
324 : {
325 0 : if(v2.dot(v4xv0) > 0)
326 : {
327 0 : v1 = v4; // Inside v1 & inside v2 ==> eliminate v1
328 : }
329 : else
330 : {
331 0 : v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
332 : }
333 : }
334 : else
335 : {
336 0 : if(v3.dot(v4xv0) > 0)
337 : {
338 0 : v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
339 : }
340 : else
341 : {
342 0 : v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
343 : }
344 : }
345 : }
346 : }
347 0 : return false;
348 : }
349 :
350 : template<class T>
351 0 : bool collide(const EntOBB &p1, const T &p2, vec *contactnormal, vec *contactpoint1, vec *contactpoint2)
352 : {
353 : // v0 = center of Minkowski sum
354 0 : const vec v01 = p1.center(),
355 0 : v02 = p2.center();
356 0 : vec v0 = vec(v02).sub(v01);
357 :
358 : // Avoid case where centers overlap -- any direction is fine in this case
359 0 : if(v0.iszero())
360 : {
361 0 : v0 = vec(0, 0, 1e-5f);
362 : }
363 : // v1 = support in direction of origin
364 0 : vec n = vec(v0).neg(),
365 0 : v11 = p1.supportpoint(vec(n).neg()),
366 0 : v12 = p2.supportpoint(n),
367 0 : v1 = vec(v12).sub(v11);
368 0 : if(v1.dot(n) <= 0)
369 : {
370 0 : if(contactnormal)
371 : {
372 0 : *contactnormal = n;
373 : }
374 0 : return false;
375 : }
376 : // v2 - support perpendicular to v1,v0
377 0 : n.cross(v1, v0);
378 0 : if(n.iszero())
379 : {
380 0 : n = vec(v1).sub(v0);
381 0 : n.normalize();
382 0 : if(contactnormal)
383 : {
384 0 : *contactnormal = n;
385 : }
386 0 : if(contactpoint1)
387 : {
388 0 : *contactpoint1 = v11;
389 : }
390 0 : if(contactpoint2)
391 : {
392 0 : *contactpoint2 = v12;
393 : }
394 0 : return true;
395 : }
396 0 : vec v21 = p1.supportpoint(vec(n).neg()),
397 0 : v22 = p2.supportpoint(n),
398 0 : v2 = vec(v22).sub(v21);
399 0 : if(v2.dot(n) <= 0)
400 : {
401 0 : if(contactnormal)
402 : {
403 0 : *contactnormal = n;
404 : }
405 0 : return false;
406 : }
407 : // Determine whether origin is on + or - side of plane (v1,v0,v2)
408 0 : n.cross(v0, v1, v2);
409 : // If the origin is on the - side of the plane, reverse the direction of the plane
410 0 : if(n.dot(v0) > 0)
411 : {
412 0 : std::swap(v1, v2);
413 0 : std::swap(v11, v21);
414 0 : std::swap(v12, v22);
415 0 : n.neg();
416 : }
417 : ///
418 : // Phase One: Identify a portal
419 0 : for(int i = 0; i < 100; ++i)
420 : {
421 : // Obtain the support point in a direction perpendicular to the existing plane
422 : // Note: This point is guaranteed to lie off the plane
423 0 : vec v31 = p1.supportpoint(vec(n).neg()),
424 0 : v32 = p2.supportpoint(n),
425 0 : v3 = vec(v32).sub(v31);
426 0 : if(v3.dot(n) <= 0)
427 : {
428 0 : if(contactnormal) *contactnormal = n;
429 0 : return false;
430 : }
431 : // If origin is outside (v1,v0,v3), then eliminate v2 and loop
432 0 : vec v3xv0;
433 0 : v3xv0.cross(v3, v0);
434 0 : if(v1.dot(v3xv0) < 0)
435 : {
436 0 : v2 = v3;
437 0 : v21 = v31;
438 0 : v22 = v32;
439 0 : n.cross(v0, v1, v3);
440 0 : continue;
441 : }
442 : // If origin is outside (v3,v0,v2), then eliminate v1 and loop
443 0 : if(v2.dot(v3xv0) > 0)
444 : {
445 0 : v1 = v3;
446 0 : v11 = v31;
447 0 : v12 = v32;
448 0 : n.cross(v0, v3, v2);
449 0 : continue;
450 : }
451 0 : bool hit = false;
452 : ///
453 : // Phase Two: Refine the portal
454 : // We are now inside of a wedge...
455 0 : for(int j = 0;; j++)
456 : {
457 : // Compute normal of the wedge face
458 0 : n.cross(v1, v2, v3);
459 : // Can this happen??? Can it be handled more cleanly?
460 0 : if(n.iszero())
461 : {
462 0 : return true;
463 : }
464 0 : n.normalize();
465 : // If the origin is inside the wedge, we have a hit
466 0 : if(n.dot(v1) >= 0 && !hit)
467 : {
468 0 : if(contactnormal)
469 : {
470 0 : *contactnormal = n;
471 : }
472 : // Compute the barycentric coordinates of the origin
473 0 : if(contactpoint1 || contactpoint2)
474 : {
475 0 : float b0 = v3.scalartriple(v1, v2),
476 0 : b1 = v0.scalartriple(v3, v2),
477 0 : b2 = v3.scalartriple(v0, v1),
478 0 : b3 = v0.scalartriple(v2, v1),
479 0 : sum = b0 + b1 + b2 + b3;
480 0 : if(sum <= 0)
481 : {
482 0 : b0 = 0;
483 0 : b1 = n.scalartriple(v2, v3);
484 0 : b2 = n.scalartriple(v3, v1);
485 0 : b3 = n.scalartriple(v1, v2);
486 0 : sum = b1 + b2 + b3;
487 : }
488 0 : if(contactpoint1)
489 : {
490 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);
491 : }
492 0 : if(contactpoint2)
493 : {
494 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);
495 : }
496 : }
497 : // HIT!!!
498 0 : hit = true;
499 : }
500 : // Find the support point in the direction of the wedge face
501 0 : const vec v41 = p1.supportpoint(vec(n).neg()),
502 0 : v42 = p2.supportpoint(n),
503 0 : v4 = vec(v42).sub(v41);
504 : // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
505 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
506 : {
507 0 : if(contactnormal)
508 : {
509 0 : *contactnormal = n;
510 : }
511 0 : return hit;
512 : }
513 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
514 : // Note: We're taking advantage of the triple product identities here as an optimization
515 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
516 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
517 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
518 0 : vec v4xv0;
519 0 : v4xv0.cross(v4, v0);
520 0 : if(v1.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d1 = (v4,v0,v1)
521 : {
522 0 : if(v2.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d2 = (v4,v0,v2)
523 : {
524 : // Inside d1 & inside d2 ==> eliminate v1
525 0 : v1 = v4;
526 0 : v11 = v41;
527 0 : v12 = v42;
528 : }
529 : else
530 : {
531 : // Inside d1 & outside d2 ==> eliminate v3
532 0 : v3 = v4;
533 0 : v31 = v41;
534 0 : v32 = v42;
535 : }
536 : }
537 : else
538 : {
539 0 : if(v3.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d3 = (v4,v0,v3)
540 : {
541 : // Outside d1 & inside d3 ==> eliminate v2
542 0 : v2 = v4;
543 0 : v21 = v41;
544 0 : v22 = v42;
545 : }
546 : else
547 : {
548 : // Outside d1 & outside d3 ==> eliminate v1
549 0 : v1 = v4;
550 0 : v11 = v41;
551 0 : v12 = v42;
552 : }
553 : }
554 : }
555 : }
556 0 : return false;
557 : }
558 : }
559 :
560 : #endif
|
