Line data Source code
1 : /**
2 : * @file mpr.cpp
3 : * @brief Minkowski Portal Refinement implementation
4 : *
5 : * This code is based off the Minkowski Portal Refinement algorithm by Gary Snethen
6 : * in XenoCollide & Game Programming Gems 7.
7 : *
8 : * Minkowski Portal Refinement is a way of finding whether two hulls intersect
9 : * efficiently, useful for finding if two models are intersecting quickly
10 : *
11 : * used in physics.cpp and for model-related collision purposes
12 : */
13 : #include "../libprimis-headers/cube.h"
14 : #include "../../shared/geomexts.h"
15 :
16 : #include "octaworld.h"
17 :
18 : #include "mpr.h"
19 :
20 : namespace mpr
21 : {
22 : //CubePlanes
23 :
24 1 : vec CubePlanes::center() const
25 : {
26 1 : return p.o;
27 : }
28 :
29 3 : vec CubePlanes::supportpoint(const vec &n) const
30 : {
31 3 : int besti = 7;
32 3 : float bestd = n.dot(p.v[7]);
33 24 : for(int i = 0; i < 7; ++i)
34 : {
35 21 : float d = n.dot(p.v[i]);
36 21 : if(d > bestd)
37 : {
38 3 : besti = i;
39 3 : bestd = d;
40 : }
41 : }
42 3 : return p.v[besti];
43 : }
44 :
45 : //SolidCube
46 :
47 2 : vec SolidCube::center() const
48 : {
49 2 : return vec(o).add(size/2);
50 : }
51 :
52 3 : vec SolidCube::supportpoint (const vec &n) const
53 : {
54 3 : vec p(o);
55 3 : if(n.x > 0)
56 : {
57 2 : p.x += size;
58 : }
59 3 : if(n.y > 0)
60 : {
61 3 : p.y += size;
62 : }
63 3 : if(n.z > 0)
64 : {
65 3 : p.z += size;
66 : }
67 3 : return p;
68 : }
69 :
70 : //Ent
71 :
72 3 : vec Ent::center() const
73 : {
74 3 : return vec(ent->o.x, ent->o.y, ent->o.z + (ent->aboveeye - ent->eyeheight)/2);
75 : }
76 :
77 : //EntOBB
78 :
79 7 : EntOBB::EntOBB(const physent *ent) : Ent(ent)
80 : {
81 7 : orient.setyaw(ent->yaw/RAD);
82 7 : }
83 :
84 0 : vec EntOBB::contactface(const vec &wn, const vec &wdir) const
85 : {
86 0 : vec n = orient.transform(wn).div(vec(ent->xradius, ent->yradius, (ent->aboveeye + ent->eyeheight)/2)),
87 0 : dir = orient.transform(wdir),
88 0 : an(std::fabs(n.x), std::fabs(n.y), dir.z ? std::fabs(n.z) : 0),
89 0 : fn(0, 0, 0);
90 0 : if(an.x > an.y)
91 : {
92 0 : if(an.x > an.z)
93 : {
94 0 : fn.x = n.x*dir.x < 0 ? (n.x > 0 ? 1 : -1) : 0;
95 : }
96 0 : else if(an.z > 0)
97 : {
98 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
99 : }
100 : }
101 0 : else if(an.y > an.z)
102 : {
103 0 : fn.y = n.y*dir.y < 0 ? (n.y > 0 ? 1 : -1) : 0;
104 : }
105 0 : else if(an.z > 0)
106 : {
107 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
108 : }
109 0 : return orient.transposedtransform(fn);
110 : }
111 :
112 9 : vec EntOBB::localsupportpoint(const vec &ln) const
113 : {
114 9 : return vec(ln.x > 0 ? ent->xradius : -ent->xradius,
115 9 : ln.y > 0 ? ent->yradius : -ent->yradius,
116 18 : ln.z > 0 ? ent->aboveeye : -ent->eyeheight);
117 : }
118 :
119 1 : vec EntOBB::supportpoint(const vec &n) const
120 : {
121 1 : return orient.transposedtransform(localsupportpoint(orient.transform(n))).add(ent->o);
122 : }
123 :
124 4 : float EntOBB::supportcoordneg(const vec &p) const
125 : {
126 4 : return localsupportpoint(vec(p).neg()).dot(p);
127 : }
128 :
129 4 : float EntOBB::supportcoord(const vec &p) const
130 : {
131 4 : return localsupportpoint(p).dot(p);
132 : }
133 :
134 2 : float EntOBB::left() const { return supportcoordneg(orient.a) + ent->o.x; }
135 2 : float EntOBB::right() const { return supportcoord(orient.a) + ent->o.x; }
136 2 : float EntOBB::back() const { return supportcoordneg(orient.b) + ent->o.y; }
137 2 : float EntOBB::front() const { return supportcoord(orient.b) + ent->o.y; }
138 1 : float EntOBB::bottom() const { return ent->o.z - ent->eyeheight; }
139 1 : float EntOBB::top() const { return ent->o.z + ent->aboveeye; }
140 :
141 : //EntFuzzy
142 :
143 1 : float EntFuzzy::left() const { return ent->o.x - ent->radius; }
144 1 : float EntFuzzy::right() const { return ent->o.x + ent->radius; }
145 1 : float EntFuzzy::back() const { return ent->o.y - ent->radius; }
146 1 : float EntFuzzy::front() const { return ent->o.y + ent->radius; }
147 2 : float EntFuzzy::bottom() const { return ent->o.z - ent->eyeheight; }
148 2 : float EntFuzzy::top() const { return ent->o.z + ent->aboveeye; }
149 :
150 : //EntCylinder
151 :
152 0 : vec EntCylinder::contactface(const vec &n, const vec &dir) const
153 : {
154 0 : float dxy = n.dot2(n)/(ent->radius*ent->radius),
155 0 : dz = n.z*n.z*4/(ent->aboveeye + ent->eyeheight);
156 0 : vec fn(0, 0, 0);
157 0 : if(dz > dxy && dir.z)
158 : {
159 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
160 : }
161 0 : else if(n.dot2(dir) < 0)
162 : {
163 0 : fn.x = n.x;
164 0 : fn.y = n.y;
165 0 : fn.normalize();
166 : }
167 0 : return fn;
168 : }
169 :
170 0 : vec EntCylinder::supportpoint(const vec &n) const
171 : {
172 0 : vec p(ent->o);
173 0 : if(n.z > 0)
174 : {
175 0 : p.z += ent->aboveeye;
176 : }
177 : else
178 : {
179 0 : p.z -= ent->eyeheight;
180 : }
181 0 : if(n.x || n.y)
182 : {
183 0 : float r = ent->radius / n.magnitude2();
184 0 : p.x += n.x*r;
185 0 : p.y += n.y*r;
186 : }
187 0 : return p;
188 : }
189 :
190 : //EntCapsule
191 :
192 0 : vec EntCapsule::supportpoint(const vec &n) const
193 : {
194 0 : vec p(ent->o);
195 0 : if(n.z > 0)
196 : {
197 0 : p.z += ent->aboveeye - ent->radius;
198 : }
199 : else
200 : {
201 0 : p.z -= ent->eyeheight - ent->radius;
202 : }
203 0 : p.add(vec(n).mul(ent->radius / n.magnitude()));
204 0 : return p;
205 : }
206 :
207 : //EntEllipsoid
208 :
209 0 : vec EntEllipsoid::supportpoint(const vec &dir) const
210 : {
211 0 : vec p(ent->o),
212 0 : n = vec(dir).normalize();
213 0 : p.x += ent->radius*n.x;
214 0 : p.y += ent->radius*n.y;
215 0 : p.z += (ent->aboveeye + ent->eyeheight)/2*(1 + n.z) - ent->eyeheight;
216 0 : return p;
217 : }
218 :
219 : //Model
220 3 : Model::~Model() {}
221 :
222 3 : Model::Model(const vec &ent, const vec ¢er, const vec &radius, int yaw, int pitch, int roll) : o(ent), radius(radius)
223 : {
224 3 : orient.identity();
225 3 : if(roll)
226 : {
227 0 : orient.rotate_around_y(sincosmod360(roll));
228 : }
229 3 : if(pitch)
230 : {
231 0 : orient.rotate_around_x(sincosmod360(-pitch));
232 : }
233 3 : if(yaw)
234 : {
235 1 : orient.rotate_around_z(sincosmod360(-yaw));
236 : }
237 3 : o.add(orient.transposedtransform(center));
238 3 : }
239 :
240 1 : vec Model::center() const
241 : {
242 1 : return o;
243 : }
244 :
245 : //ModelOBB
246 :
247 0 : vec ModelOBB::contactface(const vec &wn, const vec &wdir) const
248 : {
249 0 : vec n = orient.transform(wn).div(radius),
250 0 : dir = orient.transform(wdir),
251 0 : an(std::fabs(n.x), std::fabs(n.y), dir.z ? std::fabs(n.z) : 0),
252 0 : fn(0, 0, 0);
253 0 : if(an.x > an.y)
254 : {
255 0 : if(an.x > an.z)
256 : {
257 0 : fn.x = n.x*dir.x < 0 ? (n.x > 0 ? 1 : -1) : 0;
258 : }
259 0 : else if(an.z > 0)
260 : {
261 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
262 : }
263 : }
264 0 : else if(an.y > an.z)
265 : {
266 0 : fn.y = n.y*dir.y < 0 ? (n.y > 0 ? 1 : -1) : 0;
267 : }
268 0 : else if(an.z > 0)
269 : {
270 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
271 : }
272 0 : return orient.transposedtransform(fn);
273 : }
274 :
275 0 : vec ModelOBB::supportpoint(const vec &n) const
276 : {
277 0 : vec ln = orient.transform(n),
278 0 : p(0, 0, 0);
279 0 : if(ln.x > 0)
280 : {
281 0 : p.x += radius.x;
282 : }
283 : else
284 : {
285 0 : p.x -= radius.x;
286 : }
287 0 : if(ln.y > 0)
288 : {
289 0 : p.y += radius.y;
290 : }
291 : else
292 : {
293 0 : p.y -= radius.y;
294 : }
295 0 : if(ln.z > 0)
296 : {
297 0 : p.z += radius.z;
298 : }
299 : else
300 : {
301 0 : p.z -= radius.z;
302 : }
303 0 : return orient.transposedtransform(p).add(o);
304 : }
305 :
306 : //ModelEllipse
307 :
308 0 : vec ModelEllipse::contactface(const vec &wn, const vec &wdir) const
309 : {
310 0 : vec n = orient.transform(wn).div(radius),
311 0 : dir = orient.transform(wdir);
312 0 : float dxy = n.dot2(n),
313 0 : dz = n.z*n.z;
314 0 : vec fn(0, 0, 0);
315 0 : if(dz > dxy && dir.z)
316 : {
317 0 : fn.z = n.z*dir.z < 0 ? (n.z > 0 ? 1 : -1) : 0;
318 : }
319 0 : else if(n.dot2(dir) < 0)
320 : {
321 0 : fn.x = n.x*radius.y;
322 0 : fn.y = n.y*radius.x;
323 0 : fn.normalize();
324 : }
325 0 : return orient.transposedtransform(fn);
326 : }
327 :
328 0 : vec ModelEllipse::supportpoint(const vec &n) const
329 : {
330 0 : vec ln = orient.transform(n),
331 0 : p(0, 0, 0);
332 0 : if(ln.z > 0)
333 : {
334 0 : p.z += radius.z;
335 : }
336 : else
337 : {
338 0 : p.z -= radius.z;
339 : }
340 0 : if(ln.x || ln.y)
341 : {
342 0 : float r = ln.magnitude2();
343 0 : p.x += ln.x*radius.x/r;
344 0 : p.y += ln.y*radius.y/r;
345 : }
346 0 : return orient.transposedtransform(p).add(o);
347 : }
348 : }
|
