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 1 : 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 : class EntOBB final : public Ent
86 : {
87 : public:
88 : EntOBB(const physent *ent);
89 :
90 : vec contactface(const vec &wn, const vec &wdir) const;
91 : vec localsupportpoint(const vec &ln) const;
92 : vec supportpoint(const vec &n) const;
93 :
94 : float left() const;
95 : float right() const;
96 : float back() const;
97 : float front() const;
98 : float bottom() const;
99 : float top() const;
100 : private:
101 : matrix3 orient;
102 : float supportcoord(const vec &p) const;
103 : float supportcoordneg(const vec &p) const;
104 : };
105 :
106 : struct EntFuzzy : Ent
107 : {
108 0 : EntFuzzy(const physent *ent) : Ent(ent) {}
109 :
110 : float left() const;
111 : float right() const;
112 : float back() const;
113 : float front() const;
114 : float bottom() const;
115 : float top() const;
116 : };
117 :
118 : struct EntCylinder final : EntFuzzy
119 : {
120 0 : EntCylinder(const physent *ent) : EntFuzzy(ent) {}
121 :
122 : vec contactface(const vec &n, const vec &dir) const;
123 : vec supportpoint(const vec &n) const;
124 : };
125 :
126 : struct EntCapsule final : EntFuzzy
127 : {
128 0 : EntCapsule(const physent *ent) : EntFuzzy(ent) {}
129 :
130 : vec supportpoint(const vec &n) const;
131 : };
132 :
133 : struct EntEllipsoid final : EntFuzzy
134 : {
135 : EntEllipsoid(const physent *ent) : EntFuzzy(ent) {}
136 :
137 : vec supportpoint(const vec &dir) const;
138 : };
139 :
140 : struct Model
141 : {
142 : vec o, radius;
143 : matrix3 orient;
144 :
145 : Model(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll);
146 :
147 : /**
148 : * @brief Returns the origin vector of this object.
149 : *
150 : * This is the field model.o.
151 : *
152 : * @return copy of o
153 : */
154 : vec center() const;
155 : };
156 :
157 : struct ModelOBB final : Model
158 : {
159 0 : ModelOBB(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
160 0 : Model(ent, center, radius, yaw, pitch, roll)
161 0 : {}
162 :
163 : vec contactface(const vec &wn, const vec &wdir) const;
164 : vec supportpoint(const vec &n) const;
165 : };
166 :
167 :
168 : struct ModelEllipse final : Model
169 : {
170 0 : ModelEllipse(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
171 0 : Model(ent, center, radius, yaw, pitch, roll)
172 0 : {}
173 :
174 : vec contactface(const vec &wn, const vec &wdir) const;
175 : vec supportpoint(const vec &n) const;
176 : };
177 :
178 : //templates
179 : constexpr float boundarytolerance = 1e-3f;
180 :
181 : template<class T>
182 0 : bool collide(const T &p1, const EntOBB &p2)
183 : {
184 : // v0 = center of Minkowski difference
185 0 : vec v0 = p2.center().sub(p1.center());
186 0 : if(v0.iszero())
187 : {
188 0 : return true; // v0 and origin overlap ==> hit
189 : }
190 : // v1 = support in direction of origin
191 0 : vec n = vec(v0).neg();
192 0 : vec v1 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
193 0 : if(v1.dot(n) <= 0)
194 : {
195 0 : return false; // origin outside v1 support plane ==> miss
196 : }
197 : // v2 = support perpendicular to plane containing origin, v0 and v1
198 0 : n.cross(v1, v0);
199 0 : if(n.iszero())
200 : {
201 0 : return true; // v0, v1 and origin colinear (and origin inside v1 support plane) == > hit
202 : }
203 0 : vec v2 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
204 0 : if(v2.dot(n) <= 0)
205 : {
206 0 : return false; // origin outside v2 support plane ==> miss
207 : }
208 : // v3 = support perpendicular to plane containing v0, v1 and v2
209 0 : n.cross(v0, v1, v2);
210 : // If the origin is on the - side of the plane, reverse the direction of the plane
211 0 : if(n.dot(v0) > 0)
212 : {
213 0 : std::swap(v1, v2);
214 0 : n.neg();
215 : }
216 : ///
217 : // Phase One: Find a valid portal
218 :
219 0 : for(int i = 0; i < 100; ++i)
220 : {
221 : // Obtain the next support point
222 0 : vec v3 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
223 0 : if(v3.dot(n) <= 0)
224 : {
225 0 : return false; // origin outside v3 support plane ==> miss
226 : }
227 : // If origin is outside (v1,v0,v3), then portal is invalid -- eliminate v2 and find new support outside face
228 0 : vec v3xv0;
229 0 : v3xv0.cross(v3, v0);
230 0 : if(v1.dot(v3xv0) < 0)
231 : {
232 0 : v2 = v3;
233 0 : n.cross(v0, v1, v3);
234 0 : continue;
235 : }
236 : // If origin is outside (v3,v0,v2), then portal is invalid -- eliminate v1 and find new support outside face
237 0 : if(v2.dot(v3xv0) > 0)
238 : {
239 0 : v1 = v3;
240 0 : n.cross(v0, v3, v2);
241 0 : continue;
242 : }
243 : ///
244 : // Phase Two: Refine the portal
245 0 : for(int j = 0;; j++)
246 : {
247 : // Compute outward facing normal of the portal
248 0 : n.cross(v1, v2, v3);
249 :
250 : // If the origin is inside the portal, we have a hit
251 0 : if(n.dot(v1) >= 0)
252 : {
253 0 : return true;
254 : }
255 0 : n.normalize();
256 : // Find the support point in the direction of the portal's normal
257 0 : vec v4 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
258 : // If the origin is outside the support plane or the boundary is thin enough, we have a miss
259 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
260 : {
261 0 : return false;
262 : }
263 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
264 : // Note: We're taking advantage of the triple product identities here as an optimization
265 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
266 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
267 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
268 0 : vec v4xv0;
269 0 : v4xv0.cross(v4, v0);
270 0 : if(v1.dot(v4xv0) > 0)
271 : {
272 0 : if(v2.dot(v4xv0) > 0)
273 : {
274 0 : v1 = v4; // Inside v1 & inside v2 ==> eliminate v1
275 : }
276 : else
277 : {
278 0 : v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
279 : }
280 : }
281 : else
282 : {
283 0 : if(v3.dot(v4xv0) > 0)
284 : {
285 0 : v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
286 : }
287 : else
288 : {
289 0 : v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
290 : }
291 : }
292 : }
293 : }
294 0 : return false;
295 : }
296 :
297 : template<class T>
298 0 : bool collide(const EntOBB &p1, const T &p2, vec *contactnormal, vec *contactpoint1, vec *contactpoint2)
299 : {
300 : // v0 = center of Minkowski sum
301 0 : const vec v01 = p1.center();
302 0 : const vec v02 = p2.center();
303 0 : vec v0 = vec(v02).sub(v01);
304 :
305 : // Avoid case where centers overlap -- any direction is fine in this case
306 0 : if(v0.iszero())
307 : {
308 0 : v0 = vec(0, 0, 1e-5f);
309 : }
310 : // v1 = support in direction of origin
311 0 : vec n = vec(v0).neg();
312 0 : vec v11 = p1.supportpoint(vec(n).neg());
313 0 : vec v12 = p2.supportpoint(n);
314 0 : vec v1 = vec(v12).sub(v11);
315 0 : if(v1.dot(n) <= 0)
316 : {
317 0 : if(contactnormal)
318 : {
319 0 : *contactnormal = n;
320 : }
321 0 : return false;
322 : }
323 : // v2 - support perpendicular to v1,v0
324 0 : n.cross(v1, v0);
325 0 : if(n.iszero())
326 : {
327 0 : n = vec(v1).sub(v0);
328 0 : n.normalize();
329 0 : if(contactnormal)
330 : {
331 0 : *contactnormal = n;
332 : }
333 0 : if(contactpoint1)
334 : {
335 0 : *contactpoint1 = v11;
336 : }
337 0 : if(contactpoint2)
338 : {
339 0 : *contactpoint2 = v12;
340 : }
341 0 : return true;
342 : }
343 0 : vec v21 = p1.supportpoint(vec(n).neg());
344 0 : vec v22 = p2.supportpoint(n);
345 0 : vec v2 = vec(v22).sub(v21);
346 0 : if(v2.dot(n) <= 0)
347 : {
348 0 : if(contactnormal)
349 : {
350 0 : *contactnormal = n;
351 : }
352 0 : return false;
353 : }
354 : // Determine whether origin is on + or - side of plane (v1,v0,v2)
355 0 : n.cross(v0, v1, v2);
356 : // If the origin is on the - side of the plane, reverse the direction of the plane
357 0 : if(n.dot(v0) > 0)
358 : {
359 0 : std::swap(v1, v2);
360 0 : std::swap(v11, v21);
361 0 : std::swap(v12, v22);
362 0 : n.neg();
363 : }
364 : ///
365 : // Phase One: Identify a portal
366 0 : for(int i = 0; i < 100; ++i)
367 : {
368 : // Obtain the support point in a direction perpendicular to the existing plane
369 : // Note: This point is guaranteed to lie off the plane
370 0 : vec v31 = p1.supportpoint(vec(n).neg());
371 0 : vec v32 = p2.supportpoint(n);
372 0 : vec v3 = vec(v32).sub(v31);
373 0 : if(v3.dot(n) <= 0)
374 : {
375 0 : if(contactnormal) *contactnormal = n;
376 0 : return false;
377 : }
378 : // If origin is outside (v1,v0,v3), then eliminate v2 and loop
379 0 : vec v3xv0;
380 0 : v3xv0.cross(v3, v0);
381 0 : if(v1.dot(v3xv0) < 0)
382 : {
383 0 : v2 = v3;
384 0 : v21 = v31;
385 0 : v22 = v32;
386 0 : n.cross(v0, v1, v3);
387 0 : continue;
388 : }
389 : // If origin is outside (v3,v0,v2), then eliminate v1 and loop
390 0 : if(v2.dot(v3xv0) > 0)
391 : {
392 0 : v1 = v3;
393 0 : v11 = v31;
394 0 : v12 = v32;
395 0 : n.cross(v0, v3, v2);
396 0 : continue;
397 : }
398 0 : bool hit = false;
399 : ///
400 : // Phase Two: Refine the portal
401 : // We are now inside of a wedge...
402 0 : for(int j = 0;; j++)
403 : {
404 : // Compute normal of the wedge face
405 0 : n.cross(v1, v2, v3);
406 : // Can this happen??? Can it be handled more cleanly?
407 0 : if(n.iszero())
408 : {
409 0 : return true;
410 : }
411 0 : n.normalize();
412 : // If the origin is inside the wedge, we have a hit
413 0 : if(n.dot(v1) >= 0 && !hit)
414 : {
415 0 : if(contactnormal)
416 : {
417 0 : *contactnormal = n;
418 : }
419 : // Compute the barycentric coordinates of the origin
420 0 : if(contactpoint1 || contactpoint2)
421 : {
422 0 : float b0 = v3.scalartriple(v1, v2),
423 0 : b1 = v0.scalartriple(v3, v2),
424 0 : b2 = v3.scalartriple(v0, v1),
425 0 : b3 = v0.scalartriple(v2, v1),
426 0 : sum = b0 + b1 + b2 + b3;
427 0 : if(sum <= 0)
428 : {
429 0 : b0 = 0;
430 0 : b1 = n.scalartriple(v2, v3);
431 0 : b2 = n.scalartriple(v3, v1);
432 0 : b3 = n.scalartriple(v1, v2);
433 0 : sum = b1 + b2 + b3;
434 : }
435 0 : if(contactpoint1)
436 : {
437 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);
438 : }
439 0 : if(contactpoint2)
440 : {
441 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);
442 : }
443 : }
444 : // HIT!!!
445 0 : hit = true;
446 : }
447 : // Find the support point in the direction of the wedge face
448 0 : vec v41 = p1.supportpoint(vec(n).neg());
449 0 : vec v42 = p2.supportpoint(n);
450 0 : vec v4 = vec(v42).sub(v41);
451 : // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
452 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
453 : {
454 0 : if(contactnormal)
455 : {
456 0 : *contactnormal = n;
457 : }
458 0 : return hit;
459 : }
460 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
461 : // Note: We're taking advantage of the triple product identities here as an optimization
462 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
463 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
464 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
465 0 : vec v4xv0;
466 0 : v4xv0.cross(v4, v0);
467 0 : if(v1.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d1 = (v4,v0,v1)
468 : {
469 0 : if(v2.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d2 = (v4,v0,v2)
470 : {
471 : // Inside d1 & inside d2 ==> eliminate v1
472 0 : v1 = v4;
473 0 : v11 = v41;
474 0 : v12 = v42;
475 : }
476 : else
477 : {
478 : // Inside d1 & outside d2 ==> eliminate v3
479 0 : v3 = v4;
480 0 : v31 = v41;
481 0 : v32 = v42;
482 : }
483 : }
484 : else
485 : {
486 0 : if(v3.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d3 = (v4,v0,v3)
487 : {
488 : // Outside d1 & inside d3 ==> eliminate v2
489 0 : v2 = v4;
490 0 : v21 = v41;
491 0 : v22 = v42;
492 : }
493 : else
494 : {
495 : // Outside d1 & outside d3 ==> eliminate v1
496 0 : v1 = v4;
497 0 : v11 = v41;
498 0 : v12 = v42;
499 : }
500 : }
501 : }
502 : }
503 0 : return false;
504 : }
505 : }
506 :
507 : #endif
|
