Line data Source code
1 : #ifndef MPR_H_
2 : #define MPR_H_
3 :
4 : struct clipplanes;
5 :
6 : namespace mpr
7 : {
8 : struct CubePlanes
9 : {
10 : const clipplanes &p;
11 :
12 0 : CubePlanes(const clipplanes &p) : p(p) {}
13 :
14 : vec center() const;
15 : vec supportpoint(const vec &n) const;
16 : };
17 :
18 : struct SolidCube
19 : {
20 : vec o;
21 : int size;
22 :
23 : SolidCube(float x, float y, float z, int size) : o(x, y, z), size(size) {}
24 : SolidCube(const vec &o, int size) : o(o), size(size) {}
25 0 : SolidCube(const ivec &o, int size) : o(o), size(size) {}
26 :
27 : vec center() const;
28 : vec supportpoint(const vec &n) const;
29 : };
30 :
31 : struct Ent
32 : {
33 : const physent *ent;
34 :
35 0 : Ent(const physent *ent) : ent(ent) {}
36 :
37 : vec center() const;
38 : };
39 :
40 : class EntOBB : public Ent
41 : {
42 : public:
43 : EntOBB(const physent *ent);
44 :
45 : vec contactface(const vec &wn, const vec &wdir) const;
46 : vec localsupportpoint(const vec &ln) const;
47 : vec supportpoint(const vec &n) const;
48 :
49 : float left() const;
50 : float right() const;
51 : float back() const;
52 : float front() const;
53 : float bottom() const;
54 : float top() const;
55 : private:
56 : matrix3 orient;
57 : float supportcoord(const vec &p) const;
58 : float supportcoordneg(const vec &p) const;
59 : };
60 :
61 : struct EntFuzzy : Ent
62 : {
63 0 : EntFuzzy(const physent *ent) : Ent(ent) {}
64 :
65 : float left() const;
66 : float right() const;
67 : float back() const;
68 : float front() const;
69 : float bottom() const;
70 : float top() const;
71 : };
72 :
73 : struct EntCylinder : EntFuzzy
74 : {
75 0 : EntCylinder(const physent *ent) : EntFuzzy(ent) {}
76 :
77 : vec contactface(const vec &n, const vec &dir) const;
78 : vec supportpoint(const vec &n) const;
79 : };
80 :
81 : struct EntCapsule : EntFuzzy
82 : {
83 0 : EntCapsule(const physent *ent) : EntFuzzy(ent) {}
84 :
85 : vec supportpoint(const vec &n) const;
86 : };
87 :
88 : struct EntEllipsoid : EntFuzzy
89 : {
90 : EntEllipsoid(const physent *ent) : EntFuzzy(ent) {}
91 :
92 : vec supportpoint(const vec &dir) const;
93 : };
94 :
95 : struct Model
96 : {
97 : vec o, radius;
98 : matrix3 orient;
99 :
100 : Model(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll);
101 :
102 : vec center() const;
103 : };
104 :
105 : struct ModelOBB : Model
106 : {
107 0 : ModelOBB(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
108 0 : Model(ent, center, radius, yaw, pitch, roll)
109 0 : {}
110 :
111 : vec contactface(const vec &wn, const vec &wdir) const;
112 : vec supportpoint(const vec &n) const;
113 : };
114 :
115 :
116 : struct ModelEllipse : Model
117 : {
118 0 : ModelEllipse(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) :
119 0 : Model(ent, center, radius, yaw, pitch, roll)
120 0 : {}
121 :
122 : vec contactface(const vec &wn, const vec &wdir) const;
123 : vec supportpoint(const vec &n) const;
124 : };
125 :
126 : //templates
127 : const float boundarytolerance = 1e-3f;
128 :
129 : template<class T>
130 0 : bool collide(const T &p1, const EntOBB &p2)
131 : {
132 : // v0 = center of Minkowski difference
133 0 : vec v0 = p2.center().sub(p1.center());
134 0 : if(v0.iszero())
135 : {
136 0 : return true; // v0 and origin overlap ==> hit
137 : }
138 : // v1 = support in direction of origin
139 0 : vec n = vec(v0).neg();
140 0 : vec v1 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
141 0 : if(v1.dot(n) <= 0)
142 : {
143 0 : return false; // origin outside v1 support plane ==> miss
144 : }
145 : // v2 = support perpendicular to plane containing origin, v0 and v1
146 0 : n.cross(v1, v0);
147 0 : if(n.iszero())
148 : {
149 0 : return true; // v0, v1 and origin colinear (and origin inside v1 support plane) == > hit
150 : }
151 0 : vec v2 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
152 0 : if(v2.dot(n) <= 0)
153 : {
154 0 : return false; // origin outside v2 support plane ==> miss
155 : }
156 : // v3 = support perpendicular to plane containing v0, v1 and v2
157 0 : n.cross(v0, v1, v2);
158 : // If the origin is on the - side of the plane, reverse the direction of the plane
159 0 : if(n.dot(v0) > 0)
160 : {
161 0 : std::swap(v1, v2);
162 0 : n.neg();
163 : }
164 : ///
165 : // Phase One: Find a valid portal
166 :
167 0 : for(int i = 0; i < 100; ++i)
168 : {
169 : // Obtain the next support point
170 0 : vec v3 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
171 0 : if(v3.dot(n) <= 0)
172 : {
173 0 : return false; // origin outside v3 support plane ==> miss
174 : }
175 : // If origin is outside (v1,v0,v3), then portal is invalid -- eliminate v2 and find new support outside face
176 0 : vec v3xv0;
177 0 : v3xv0.cross(v3, v0);
178 0 : if(v1.dot(v3xv0) < 0)
179 : {
180 0 : v2 = v3;
181 0 : n.cross(v0, v1, v3);
182 0 : continue;
183 : }
184 : // If origin is outside (v3,v0,v2), then portal is invalid -- eliminate v1 and find new support outside face
185 0 : if(v2.dot(v3xv0) > 0)
186 : {
187 0 : v1 = v3;
188 0 : n.cross(v0, v3, v2);
189 0 : continue;
190 : }
191 : ///
192 : // Phase Two: Refine the portal
193 0 : for(int j = 0;; j++)
194 : {
195 : // Compute outward facing normal of the portal
196 0 : n.cross(v1, v2, v3);
197 :
198 : // If the origin is inside the portal, we have a hit
199 0 : if(n.dot(v1) >= 0)
200 : {
201 0 : return true;
202 : }
203 0 : n.normalize();
204 : // Find the support point in the direction of the portal's normal
205 0 : vec v4 = p2.supportpoint(n).sub(p1.supportpoint(vec(n).neg()));
206 : // If the origin is outside the support plane or the boundary is thin enough, we have a miss
207 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
208 : {
209 0 : return false;
210 : }
211 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
212 : // Note: We're taking advantage of the triple product identities here as an optimization
213 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
214 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
215 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
216 0 : vec v4xv0;
217 0 : v4xv0.cross(v4, v0);
218 0 : if(v1.dot(v4xv0) > 0)
219 : {
220 0 : if(v2.dot(v4xv0) > 0)
221 : {
222 0 : v1 = v4; // Inside v1 & inside v2 ==> eliminate v1
223 : }
224 : else
225 : {
226 0 : v3 = v4; // Inside v1 & outside v2 ==> eliminate v3
227 : }
228 : }
229 : else
230 : {
231 0 : if(v3.dot(v4xv0) > 0)
232 : {
233 0 : v2 = v4; // Outside v1 & inside v3 ==> eliminate v2
234 : }
235 : else
236 : {
237 0 : v1 = v4; // Outside v1 & outside v3 ==> eliminate v1
238 : }
239 : }
240 : }
241 : }
242 0 : return false;
243 : }
244 :
245 : template<class T>
246 0 : bool collide(const EntOBB &p1, const T &p2, vec *contactnormal, vec *contactpoint1, vec *contactpoint2)
247 : {
248 : // v0 = center of Minkowski sum
249 0 : vec v01 = p1.center();
250 0 : vec v02 = p2.center();
251 0 : vec v0 = vec(v02).sub(v01);
252 :
253 : // Avoid case where centers overlap -- any direction is fine in this case
254 0 : if(v0.iszero())
255 : {
256 0 : v0 = vec(0, 0, 1e-5f);
257 : }
258 : // v1 = support in direction of origin
259 0 : vec n = vec(v0).neg();
260 0 : vec v11 = p1.supportpoint(vec(n).neg());
261 0 : vec v12 = p2.supportpoint(n);
262 0 : vec v1 = vec(v12).sub(v11);
263 0 : if(v1.dot(n) <= 0)
264 : {
265 0 : if(contactnormal)
266 : {
267 0 : *contactnormal = n;
268 : }
269 0 : return false;
270 : }
271 : // v2 - support perpendicular to v1,v0
272 0 : n.cross(v1, v0);
273 0 : if(n.iszero())
274 : {
275 0 : n = vec(v1).sub(v0);
276 0 : n.normalize();
277 0 : if(contactnormal)
278 : {
279 0 : *contactnormal = n;
280 : }
281 0 : if(contactpoint1)
282 : {
283 0 : *contactpoint1 = v11;
284 : }
285 0 : if(contactpoint2)
286 : {
287 0 : *contactpoint2 = v12;
288 : }
289 0 : return true;
290 : }
291 0 : vec v21 = p1.supportpoint(vec(n).neg());
292 0 : vec v22 = p2.supportpoint(n);
293 0 : vec v2 = vec(v22).sub(v21);
294 0 : if(v2.dot(n) <= 0)
295 : {
296 0 : if(contactnormal)
297 : {
298 0 : *contactnormal = n;
299 : }
300 0 : return false;
301 : }
302 : // Determine whether origin is on + or - side of plane (v1,v0,v2)
303 0 : n.cross(v0, v1, v2);
304 : // If the origin is on the - side of the plane, reverse the direction of the plane
305 0 : if(n.dot(v0) > 0)
306 : {
307 0 : std::swap(v1, v2);
308 0 : std::swap(v11, v21);
309 0 : std::swap(v12, v22);
310 0 : n.neg();
311 : }
312 : ///
313 : // Phase One: Identify a portal
314 0 : for(int i = 0; i < 100; ++i)
315 : {
316 : // Obtain the support point in a direction perpendicular to the existing plane
317 : // Note: This point is guaranteed to lie off the plane
318 0 : vec v31 = p1.supportpoint(vec(n).neg());
319 0 : vec v32 = p2.supportpoint(n);
320 0 : vec v3 = vec(v32).sub(v31);
321 0 : if(v3.dot(n) <= 0)
322 : {
323 0 : if(contactnormal) *contactnormal = n;
324 0 : return false;
325 : }
326 : // If origin is outside (v1,v0,v3), then eliminate v2 and loop
327 0 : vec v3xv0;
328 0 : v3xv0.cross(v3, v0);
329 0 : if(v1.dot(v3xv0) < 0)
330 : {
331 0 : v2 = v3;
332 0 : v21 = v31;
333 0 : v22 = v32;
334 0 : n.cross(v0, v1, v3);
335 0 : continue;
336 : }
337 : // If origin is outside (v3,v0,v2), then eliminate v1 and loop
338 0 : if(v2.dot(v3xv0) > 0)
339 : {
340 0 : v1 = v3;
341 0 : v11 = v31;
342 0 : v12 = v32;
343 0 : n.cross(v0, v3, v2);
344 0 : continue;
345 : }
346 0 : bool hit = false;
347 : ///
348 : // Phase Two: Refine the portal
349 : // We are now inside of a wedge...
350 0 : for(int j = 0;; j++)
351 : {
352 : // Compute normal of the wedge face
353 0 : n.cross(v1, v2, v3);
354 : // Can this happen??? Can it be handled more cleanly?
355 0 : if(n.iszero())
356 : {
357 0 : return true;
358 : }
359 0 : n.normalize();
360 : // If the origin is inside the wedge, we have a hit
361 0 : if(n.dot(v1) >= 0 && !hit)
362 : {
363 0 : if(contactnormal)
364 : {
365 0 : *contactnormal = n;
366 : }
367 : // Compute the barycentric coordinates of the origin
368 0 : if(contactpoint1 || contactpoint2)
369 : {
370 0 : float b0 = v3.scalartriple(v1, v2),
371 0 : b1 = v0.scalartriple(v3, v2),
372 0 : b2 = v3.scalartriple(v0, v1),
373 0 : b3 = v0.scalartriple(v2, v1),
374 0 : sum = b0 + b1 + b2 + b3;
375 0 : if(sum <= 0)
376 : {
377 0 : b0 = 0;
378 0 : b1 = n.scalartriple(v2, v3);
379 0 : b2 = n.scalartriple(v3, v1);
380 0 : b3 = n.scalartriple(v1, v2);
381 0 : sum = b1 + b2 + b3;
382 : }
383 0 : if(contactpoint1)
384 : {
385 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);
386 : }
387 0 : if(contactpoint2)
388 : {
389 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);
390 : }
391 : }
392 : // HIT!!!
393 0 : hit = true;
394 : }
395 : // Find the support point in the direction of the wedge face
396 0 : vec v41 = p1.supportpoint(vec(n).neg());
397 0 : vec v42 = p2.supportpoint(n);
398 0 : vec v4 = vec(v42).sub(v41);
399 : // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
400 0 : if(v4.dot(n) <= 0 || vec(v4).sub(v3).dot(n) <= boundarytolerance || j > 100)
401 : {
402 0 : if(contactnormal)
403 : {
404 0 : *contactnormal = n;
405 : }
406 0 : return hit;
407 : }
408 : // Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0)
409 : // Note: We're taking advantage of the triple product identities here as an optimization
410 : // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0)
411 : // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0)
412 : // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0)
413 0 : vec v4xv0;
414 0 : v4xv0.cross(v4, v0);
415 0 : if(v1.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d1 = (v4,v0,v1)
416 : {
417 0 : if(v2.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d2 = (v4,v0,v2)
418 : {
419 : // Inside d1 & inside d2 ==> eliminate v1
420 0 : v1 = v4;
421 0 : v11 = v41;
422 0 : v12 = v42;
423 : }
424 : else
425 : {
426 : // Inside d1 & outside d2 ==> eliminate v3
427 0 : v3 = v4;
428 0 : v31 = v41;
429 0 : v32 = v42;
430 : }
431 : }
432 : else
433 : {
434 0 : if(v3.dot(v4xv0) > 0) // Compute the tetrahedron dividing face d3 = (v4,v0,v3)
435 : {
436 : // Outside d1 & inside d3 ==> eliminate v2
437 0 : v2 = v4;
438 0 : v21 = v41;
439 0 : v22 = v42;
440 : }
441 : else
442 : {
443 : // Outside d1 & outside d3 ==> eliminate v1
444 0 : v1 = v4;
445 0 : v11 = v41;
446 0 : v12 = v42;
447 : }
448 : }
449 : }
450 : }
451 0 : return false;
452 : }
453 : }
454 :
455 : #endif
|
