1 /* This file is part of StepCore library.
2 Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
3
4 StepCore library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 StepCore library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with StepCore; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include "collisionsolver.h"
20 #include "rigidbody.h"
21 #include "particle.h"
22
23 #include <algorithm>
24
25 #define EIGEN_USE_NEW_STDVECTOR
26 #include <Eigen/StdVector>
27
28 namespace StepCore {
29
30 // XXX: units for toleranceAbs and localError
31 STEPCORE_META_OBJECT(CollisionSolver, QT_TRANSLATE_NOOP("ObjectClass", "CollisionSolver"), "CollisionSolver", MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),
32 STEPCORE_PROPERTY_RW(double, toleranceAbs, QT_TRANSLATE_NOOP("PropertyName", "toleranceAbs"), STEPCORE_UNITS_1, QT_TRANSLATE_NOOP("PropertyDescription", "Allowed absolute tolerance"), toleranceAbs, setToleranceAbs)
33 STEPCORE_PROPERTY_R_D(double, localError, QT_TRANSLATE_NOOP("PropertyName", "localError"), STEPCORE_UNITS_1, QT_TRANSLATE_NOOP("PropertyDescription", "Maximal local error during last step"), localError))
34
35 STEPCORE_META_OBJECT(GJKCollisionSolver, QT_TRANSLATE_NOOP("ObjectClass", "GJKCollisionSolver"), "GJKCollisionSolver", 0,
36 STEPCORE_SUPER_CLASS(CollisionSolver),)
37
checkPolygonPolygon(Contact * contact)38 int GJKCollisionSolver::checkPolygonPolygon(Contact* contact)
39 {
40 BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
41 BasePolygon* polygon1 = static_cast<BasePolygon*>(contact->body1);
42
43 if(polygon0->vertexes().empty() || polygon1->vertexes().empty()) {
44 return contact->state = Contact::Unknown;
45 }
46
47 // Algorithm description can be found in
48 // "A Fast and Robust GJK Implementation for
49 // Collision Detection of Convex Objects"
50 // by Gino van den Bergen
51
52 Vector2dList vertexes[2];
53 vertexes[0].reserve(polygon0->vertexes().size());
54 vertexes[1].reserve(polygon1->vertexes().size());
55
56 const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
57 for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
58 it0 != p0_it_end; ++it0) {
59 vertexes[0].push_back(polygon0->pointLocalToWorld(*it0));
60 }
61
62 const Vector2dList::const_iterator p1_it_end = polygon1->vertexes().end();
63 for(Vector2dList::const_iterator it1 = polygon1->vertexes().begin();
64 it1 != p1_it_end; ++it1) {
65 vertexes[1].push_back(polygon1->pointLocalToWorld(*it1));
66 }
67
68 int wsize;
69 Vector2d w[3]; // Vertexes of current simplex
70 Vector2d v; // Closest point of current simplex
71 Vector2d s; // New support vertex in direction v
72
73 Vector2d vv[2]; // Closest points on the first and second objects
74 int wi[2][3]; // Indexes of vertexes corresponding to w
75 int si[2] = {0, 0}; // Indexes of vertexes corresponding to s
76
77 wsize = 1;
78 // Start with arbitrary vertex (TODO: cache the whole w simplex)
79 if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
80 wi[0][0] = contact->_w1[0];
81 wi[1][0] = contact->_w1[1];
82 } else {
83 wi[0][0] = 0;
84 wi[1][0] = 0;
85 }
86 vv[0] = vertexes[0][wi[0][0]];
87 vv[1] = vertexes[1][wi[1][0]];
88 w[0] = v = vv[1] - vv[0];
89
90 bool intersects = false;
91 unsigned int iteration = 0;
92 for(;; ++iteration) {
93 //STEPCORE_ASSERT_NOABORT( iteration < vertexes[0].size()*vertexes[1].size() );
94
95 double smin = v.squaredNorm();
96
97 // Check for penetration (part 1)
98 // If we are closer to the origin then given tolerance
99 // we should stop just now to avoid computational errors later
100 if(smin < _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance for penetration ?
101 intersects = true;
102 break;
103 }
104
105 // Find support vertex in direction v
106 // TODO: coherence optimization
107 bool sfound = false;
108 unsigned int vertex0_size = vertexes[0].size();
109 unsigned int vertex1_size = vertexes[1].size();
110
111 for(unsigned int i0=0; i0<vertex0_size; ++i0) {
112 for(unsigned int i1=0; i1<vertex1_size; ++i1) {
113 Vector2d sn = vertexes[1][i1] - vertexes[0][i0];
114 double scurr = sn.dot(v);
115 if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
116 smin = scurr;
117 s = sn;
118 si[0] = i0;
119 si[1] = i1;
120 sfound = true;
121 }
122 }
123 }
124
125 // If no support vertex have been found than we are at minimum
126 if(!sfound) {
127 if(wsize == 0) { // we have guessed right point
128 w[0] = v;
129 wi[0][0] = 0;
130 wi[1][0] = 0;
131 wsize = 1;
132 }
133 break;
134 }
135
136 // Check for penetration (part 2)
137 if(wsize == 2) {
138 // objects are penetrating if origin lies inside the simplex
139 // XXX: are there faster method to test it ?
140 Vector2d w02 = w[0] - s;
141 Vector2d w12 = w[1] - s;
142 double det = w02[0]*w12[1] - w02[1]*w12[0];
143 double det0 = -s[0]*w12[1] + s[1]*w12[0];
144 double det1 = -w02[0]* s[1] + w02[1]* s[0];
145 if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
146 w[wsize] = s;
147 wi[0][wsize] = si[0];
148 wi[1][wsize] = si[1];
149 ++wsize;
150 v.setZero();
151 intersects = true;
152 break;
153 }
154 }
155
156 // Find v = dist(conv(w+s))
157 double lambda = 0;
158 int ii = -1;
159 for(int i=0; i<wsize; ++i) {
160 double lambda0 = - (w[i]-s).dot(s) / (w[i]-s).squaredNorm();
161 if(lambda0 > 0) {
162 Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
163 if(vn.squaredNorm() < v.squaredNorm()) {
164 v = vn; ii = i;
165 lambda = lambda0;
166 }
167 }
168 }
169
170 if(ii >= 0) { // Closest simplex is line
171 vv[0] = vertexes[0][si[0]]*(1-lambda) + vertexes[0][wi[0][ii]]*lambda;
172 vv[1] = vertexes[1][si[1]]*(1-lambda) + vertexes[1][wi[1][ii]]*lambda;
173 if(wsize == 2) {
174 w[1-ii] = s;
175 wi[0][1-ii] = si[0];
176 wi[1][1-ii] = si[1];
177 } else {
178 w[wsize] = s;
179 wi[0][wsize] = si[0];
180 wi[1][wsize] = si[1];
181 ++wsize;
182 }
183 } else { // Closest simplex is vertex
184 STEPCORE_ASSERT_NOABORT(iteration == 0 || s.squaredNorm() < v.squaredNorm());
185
186 v = w[0] = s;
187 vv[0] = vertexes[0][si[0]];
188 vv[1] = vertexes[1][si[1]];
189 wi[0][0] = si[0];
190 wi[1][0] = si[1];
191 wsize = 1;
192 }
193 }
194
195 if(intersects) {
196 /*
197 qDebug("penetration detected");
198 qDebug("iteration = %d", iteration);
199 qDebug("simplexes:");
200 qDebug(" 1: 2:");
201 for(int i=0; i<wsize; ++i) {
202 qDebug(" %d %d", wi[0][i], wi[1][i]);
203 }
204 */
205 contact->distance = 0;
206 contact->normal.setZero();
207 contact->pointsCount = 0;
208 return contact->state = Contact::Intersected;
209 }
210
211 /*
212 qDebug("distance = %f", v.norm());
213 Vector2d v1 = v / v.norm();
214 qDebug("normal = (%f,%f)", v1[0], v1[1]);
215 qDebug("iteration = %d", iteration);
216 qDebug("simplexes:");
217 qDebug(" 1: 2:");
218 for(int i=0; i<wsize; ++i) {
219 qDebug(" %d %d", wi[0][i], wi[1][i]);
220 }
221 qDebug("contact points:");
222 qDebug(" (%f,%f) (%f,%f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
223 */
224
225 double vnorm = v.norm();
226 contact->distance = vnorm;
227 contact->normal = v/vnorm;
228 contact->pointsCount = 0;
229 contact->state = Contact::Separated;
230
231 contact->_w1[0] = wi[0][0];
232 contact->_w1[1] = wi[1][0];
233
234 if(vnorm > _toleranceAbs) return contact->state;
235
236 // If the objects are close enough we need to find contact manifold
237 // We are going to find simplexes (lines) that are 'most parallel'
238 // to contact plane and look for contact manifold among them. It
239 // works for almost all cases when adjacent polygon edges are
240 // not parallel
241 Vector2d vunit = v / vnorm;
242 Vector2d wm[2][2];
243
244 for(int i=0; i<2; ++i) {
245 wm[i][0] = vertexes[i][ wi[i][0] ];
246
247 if(wsize < 2 || wi[i][0] == wi[i][1]) { // vertex contact
248 // Check two adjacent edges
249 int ai1 = wi[i][0] - 1; if(ai1 < 0) ai1 = vertexes[i].size()-1;
250 Vector2d av1 = vertexes[i][ai1];
251 Vector2d dv1 = wm[i][0] - av1;
252 double dist1 = dv1.dot(vunit) * (i==0 ? 1 : -1);
253 double angle1 = dist1 / dv1.norm();
254
255 int ai2 = wi[i][0] + 1; if(ai2 >= (int) vertexes[i].size()) ai2 = 0;
256 Vector2d av2 = vertexes[i][ai2];
257 Vector2d dv2 = wm[i][0] - av2;
258 double dist2 = dv2.dot(vunit) * (i==0 ? 1 : -1);
259 double angle2 = dist2 / dv2.norm();
260
261 if(angle1 <= angle2 && dist1 < (_toleranceAbs-vnorm)/2) {
262 wm[i][1] = av1;
263 } else if(angle2 <= angle1 && dist2 < (_toleranceAbs-vnorm)/2) {
264 wm[i][1] = av2;
265 } else {
266 wm[i][1] = wm[i][0]; contact->pointsCount = 1;
267 break;
268 }
269 } else { // edge contact
270 wm[i][1] = vertexes[i][ wi[i][1] ];
271 }
272 }
273
274 // Find intersection of two lines
275 if(contact->pointsCount != 1) {
276 Vector2d vunit_o(-vunit[1], vunit[0]);
277 double wm_o[2][2];
278
279 for(int i=0; i<2; ++i) {
280 wm_o[i][0] = wm[i][0].dot(vunit_o);
281 wm_o[i][1] = wm[i][1].dot(vunit_o);
282
283 if(wm_o[i][0] > wm_o[i][1]) {
284 std::swap(wm_o[i][0], wm_o[i][1]);
285 std::swap(wm[i][0], wm[i][1]);
286 }
287 }
288
289 if(wm_o[0][0] > wm_o[1][0]) contact->points[0] = wm[0][0];
290 else contact->points[0] = wm[1][0];
291
292 if(wm_o[0][1] < wm_o[1][1]) contact->points[1] = wm[0][1];
293 else contact->points[1] = wm[1][1];
294
295 // TODO: interpolate to midpoint
296 if((contact->points[1] - contact->points[0]).norm() > _toleranceAbs) {
297 /*
298 for(int i=0; i<2; ++i) {
299 qDebug("contact%d: (%f,%f)", i, contact->points[i][0], contact->points[i][1]);
300 }
301 */
302 contact->pointsCount = 2;
303 }
304 /*
305 contact->vrel[0] = (polygon1->velocityWorld(contact->points[0]) -
306 polygon0->velocityWorld(contact->points[0])).dot(contact->normal);
307 contact->vrel[1] = (polygon1->velocityWorld(contact->points[1]) -
308 polygon0->velocityWorld(contact->points[1])).dot(contact->normal);
309 if(contact->vrel[0] < 0 || contact->vrel[1] < 0)
310 return contact->state = Contact::Colliding;
311 else if(contact->vrel[0] < _toleranceAbs || contact->vrel[1] < _toleranceAbs) // XXX: tolerance
312 return contact->state = Colliding::Contacted;
313 return contact->state = Contact::Separating;
314 }
315 */
316 }
317
318 if(contact->pointsCount != 2) {
319 contact->pointsCount = 1;
320 contact->points[0] = vv[0]; // TODO: interpolate vv[0] and vv[1]
321 //qDebug("contact is one point: (%f %f) (%f %f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
322 }
323
324 int pCount = contact->pointsCount;
325 for(int i=0; i<pCount; ++i) {
326 contact->vrel[i] = (polygon1->velocityWorld(contact->points[i]) -
327 polygon0->velocityWorld(contact->points[i])).dot(contact->normal);
328
329 if(contact->vrel[i] < 0)
330 contact->pointsState[i] = Contact::Colliding;
331 else if(contact->vrel[i] < _toleranceAbs) // XXX: tolerance
332 contact->pointsState[i] = Contact::Contacted;
333 else contact->pointsState[i] = Contact::Separating;
334
335 if(contact->pointsState[i] > contact->state)
336 contact->state = contact->pointsState[i];
337 }
338
339 contact->normalDerivative.setZero(); //XXX
340 return contact->state;
341 }
342
checkPolygonDisk(Contact * contact)343 int GJKCollisionSolver::checkPolygonDisk(Contact* contact)
344 {
345 BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
346 Disk* disk1 = static_cast<Disk*>(contact->body1);
347
348 if(polygon0->vertexes().empty()) {
349 return contact->state = Contact::Unknown;
350 }
351
352 // Simplier version of checkPolygonPolygon algorithm
353
354 Vector2dList vertexes;
355 vertexes.reserve(polygon0->vertexes().size());
356
357 const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
358 for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
359 it0 != p0_it_end; ++it0) {
360 vertexes.push_back(polygon0->pointLocalToWorld(*it0));
361 }
362
363 int wsize;
364 Vector2d w[3]; // Vertexes of current simplex
365 Vector2d v; // Closest point of current simplex
366 Vector2d s; // New support vertex in direction v
367
368 Vector2d vv; // Closest points on the polygon
369 int wi[3]; // Indexes of vertexes corresponding to w
370 int si = 0; // Indexes of vertexes corresponding to s
371
372 // Start with arbitrary vertex (TODO: cache the whole w simplex)
373 wsize = 1;
374 if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
375 wi[0] = contact->_w1[0];
376 } else {
377 wi[0] = 0;
378 }
379 vv = vertexes[wi[0]];
380 w[0] = v = disk1->position() - vv;
381
382 bool intersects = false;
383 unsigned int iteration = 0;
384 for(;; ++iteration) {
385 //STEPCORE_ASSERT_NOABORT( iteration < vertexes.size()*vertexes.size() );
386
387 double smin = v.squaredNorm();
388
389 // Check for penetration (part 1)
390 // If we are closer to the origin then given tolerance
391 // we should stop just now to avoid computational errors later
392 if(std::sqrt(smin) - disk1->radius() < _toleranceAbs*1e-2) { // XXX: separate tolerance for penetration ?
393 intersects = true;
394 break;
395 }
396
397 // Find support vertex in direction v
398 // TODO: coherence optimization
399 bool sfound = false;
400 unsigned int vertex_size = vertexes.size();
401
402 for(unsigned int i0=0; i0<vertex_size; ++i0) {
403 Vector2d sn = disk1->position() - vertexes[i0];
404 double scurr = sn.dot(v);
405 if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
406 smin = scurr;
407 s = sn;
408 si = i0;
409 sfound = true;
410 }
411 }
412
413 // If no support vertex have been found than we are at minimum
414 if(!sfound) {
415 if(wsize == 0) { // we have guessed right point
416 w[0] = v;
417 wi[0] = 0;
418 wsize = 1;
419 }
420 break;
421 }
422
423 // Check for penetration (part 2)
424 if(wsize == 2) {
425 // objects are penetrating if origin lies inside the simplex
426 // XXX: are there faster method to test it ?
427 Vector2d w02 = w[0] - s;
428 Vector2d w12 = w[1] - s;
429 Vector2d s0 = s - s.normalized() * disk1->radius();
430 double det = w02[0]*w12[1] - w02[1]*w12[0];
431 double det0 = -s0[0]*w12[1] + s0[1]*w12[0];
432 double det1 = -w02[0]* s0[1] + w02[1]* s0[0];
433 if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
434 w[wsize] = s;
435 wi[wsize] = si;
436 ++wsize;
437 v.setZero();
438 intersects = true;
439 break;
440 }
441 }
442
443 // Find v = dist(conv(w+s))
444 double lambda = 0;
445 int ii = -1;
446 for(int i=0; i<wsize; ++i) {
447 double lambda0 = - (w[i]-s).dot(s) / (w[i]-s).squaredNorm();
448 if(lambda0 > 0) {
449 Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
450 if(vn.squaredNorm() < v.squaredNorm()) {
451 v = vn; ii = i;
452 lambda = lambda0;
453 }
454 }
455 }
456
457 if(ii >= 0) { // Closest simplex is line
458 vv = vertexes[si]*(1-lambda) + vertexes[wi[ii]]*lambda;
459 if(wsize == 2) {
460 w[1-ii] = s;
461 wi[1-ii] = si;
462 } else {
463 w[wsize] = s;
464 wi[wsize] = si;
465 ++wsize;
466 }
467 } else { // Closest simplex is vertex
468 STEPCORE_ASSERT_NOABORT(iteration == 0 || s.squaredNorm() < v.squaredNorm());
469
470 v = w[0] = s;
471 vv = vertexes[si];
472 wi[0] = si;
473 wsize = 1;
474 }
475 }
476
477 if(intersects) {
478 /*qDebug("penetration detected");
479 qDebug("iteration = %d", iteration);
480 qDebug("simplexes:");
481 qDebug(" 1: 2:");
482 for(int i=0; i<wsize; ++i) {
483 qDebug(" %d %d", wi[i], wi[i]);
484 }*/
485 contact->distance = 0;
486 contact->normal.setZero();
487 contact->pointsCount = 0;
488 return contact->state = Contact::Intersected;
489 }
490
491 /*
492 qDebug("distance = %f", v.norm());
493 Vector2d v1 = v / v.norm();
494 qDebug("normal = (%f,%f)", v1[0], v1[1]);
495 qDebug("iteration = %d", iteration);
496 qDebug("simplexes:");
497 qDebug(" 1: 2:");
498 for(int i=0; i<wsize; ++i) {
499 qDebug(" %d %d", wi[i], wi[i]);
500 }
501 qDebug("contact points:");
502 qDebug(" (%f,%f) (%f,%f)", vv[0], vv[1], vv[0], vv[1]);
503 */
504
505 double vnorm = v.norm();
506 contact->distance = vnorm - disk1->radius();
507 contact->normal = v/vnorm;
508
509 contact->_w1[0] = wi[0];
510
511 if(contact->distance > _toleranceAbs) {
512 contact->pointsCount = 0;
513 contact->state = Contact::Separated;
514 return contact->state;
515 }/* else if(contact->distance < _toleranceAbs*1e-2) {
516 contact->state = Contact::Intersected;
517 return contact->state;
518 }*/
519
520 contact->pointsCount = 1;
521 contact->points[0] = disk1->position() - contact->normal * disk1->radius();
522 contact->vrel[0] = (disk1->velocity() -
523 polygon0->velocityWorld(contact->points[0])).dot(contact->normal);
524
525 if(contact->vrel[0] < 0)
526 contact->pointsState[0] = Contact::Colliding;
527 else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
528 contact->pointsState[0] = Contact::Contacted;
529 else contact->pointsState[0] = Contact::Separating;
530
531 contact->normalDerivative.setZero(); //XXX
532 contact->state = contact->pointsState[0];
533 return contact->state;
534 }
535
checkPolygonParticle(Contact * contact)536 int GJKCollisionSolver::checkPolygonParticle(Contact* contact)
537 {
538 BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
539 Particle* particle1 = static_cast<Particle*>(contact->body1);
540
541 if(polygon0->vertexes().empty()) {
542 return contact->state = Contact::Unknown;
543 }
544
545 // Simplier version of checkPolygonPolygon algorithm
546
547 Vector2dList vertexes;
548 vertexes.reserve(polygon0->vertexes().size());
549
550 const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
551 for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
552 it0 != p0_it_end; ++it0) {
553 vertexes.push_back(polygon0->pointLocalToWorld(*it0));
554 }
555
556 int wsize;
557 Vector2d w[3]; // Vertexes of current simplex
558 Vector2d v; // Closest point of current simplex
559 Vector2d s; // New support vertex in direction v
560
561 Vector2d vv; // Closest points on the polygon
562 int wi[3]; // Indexes of vertexes corresponding to w
563 int si = 0; // Indexes of vertexes corresponding to s
564
565 // Start with arbitrary vertex (TODO: cache the whole w simplex)
566 wsize = 1;
567 if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
568 wi[0] = contact->_w1[0];
569 } else {
570 wi[0] = 0;
571 }
572 vv = vertexes[wi[0]];
573 w[0] = v = particle1->position() - vv;
574
575 bool intersects = false;
576 unsigned int iteration = 0;
577 for(;; ++iteration) {
578 //STEPCORE_ASSERT_NOABORT( iteration < vertexes[0].size()*vertexes[1].size() );
579
580 double smin = v.squaredNorm();
581
582 // Check for penetration (part 1)
583 // If we are closer to the origin then given tolerance
584 // we should stop just now to avoid computational errors later
585 if(smin < _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance for penetration ?
586 intersects = true;
587 break;
588 }
589
590 // Find support vertex in direction v
591 // TODO: coherence optimization
592 bool sfound = false;
593 unsigned int vertex_size = vertexes.size();
594
595 for(unsigned int i0=0; i0<vertex_size; ++i0) {
596 Vector2d sn = particle1->position() - vertexes[i0];
597 double scurr = sn.dot(v);
598 if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
599 smin = scurr;
600 s = sn;
601 si = i0;
602 sfound = true;
603 }
604 }
605
606 // If no support vertex have been found than we are at minimum
607 if(!sfound) {
608 if(wsize == 0) { // we have guessed right point
609 w[0] = v;
610 wi[0] = 0;
611 wsize = 1;
612 }
613 break;
614 }
615
616 // Check for penetration (part 2)
617 if(wsize == 2) {
618 // objects are penetrating if origin lies inside the simplex
619 // XXX: are there faster method to test it ?
620 Vector2d w02 = w[0] - s;
621 Vector2d w12 = w[1] - s;
622 double det = w02[0]*w12[1] - w02[1]*w12[0];
623 double det0 = -s[0]*w12[1] + s[1]*w12[0];
624 double det1 = -w02[0]* s[1] + w02[1]* s[0];
625 if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
626 w[wsize] = s;
627 wi[wsize] = si;
628 ++wsize;
629 v.setZero();
630 intersects = true;
631 break;
632 }
633 }
634
635 // Find v = dist(conv(w+s))
636 double lambda = 0;
637 int ii = -1;
638 for(int i=0; i<wsize; ++i) {
639 double lambda0 = - (w[i]-s).dot(s) / (w[i]-s).squaredNorm();
640 if(lambda0 > 0) {
641 Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
642 if(vn.squaredNorm() < v.squaredNorm()) {
643 v = vn; ii = i;
644 lambda = lambda0;
645 }
646 }
647 }
648
649 if(ii >= 0) { // Closest simplex is line
650 vv = vertexes[si]*(1-lambda) + vertexes[wi[ii]]*lambda;
651 if(wsize == 2) {
652 w[1-ii] = s;
653 wi[1-ii] = si;
654 } else {
655 w[wsize] = s;
656 wi[wsize] = si;
657 ++wsize;
658 }
659 } else { // Closest simplex is vertex
660 STEPCORE_ASSERT_NOABORT(iteration == 0 || s.squaredNorm() < v.squaredNorm());
661
662 v = w[0] = s;
663 vv = vertexes[si];
664 wi[0] = si;
665 wsize = 1;
666 }
667 }
668
669 if(intersects) {
670 /*
671 qDebug("penetration detected");
672 qDebug("iteration = %d", iteration);
673 qDebug("simplexes:");
674 qDebug(" 1: 2:");
675 for(int i=0; i<wsize; ++i) {
676 qDebug(" %d %d", wi[0][i], wi[1][i]);
677 }
678 */
679 contact->distance = 0;
680 contact->normal.setZero();
681 contact->pointsCount = 0;
682 return contact->state = Contact::Intersected;
683 }
684
685 /*
686 qDebug("distance = %f", v.norm());
687 Vector2d v1 = v / v.norm();
688 qDebug("normal = (%f,%f)", v1[0], v1[1]);
689 qDebug("iteration = %d", iteration);
690 qDebug("simplexes:");
691 qDebug(" 1: 2:");
692 for(int i=0; i<wsize; ++i) {
693 qDebug(" %d %d", wi[0][i], wi[1][i]);
694 }
695 qDebug("contact points:");
696 qDebug(" (%f,%f) (%f,%f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
697 */
698
699 double vnorm = v.norm();
700 contact->distance = vnorm;
701 contact->normal = v/vnorm;
702
703 contact->_w1[0] = wi[0];
704
705 if(vnorm > _toleranceAbs) {
706 contact->pointsCount = 0;
707 contact->state = Contact::Separated;
708 return contact->state;
709 }
710
711 contact->pointsCount = 1;
712 contact->points[0] = particle1->position();
713 contact->vrel[0] = (particle1->velocity() -
714 polygon0->velocityWorld(contact->points[0])).dot(contact->normal);
715
716 if(contact->vrel[0] < 0)
717 contact->pointsState[0] = Contact::Colliding;
718 else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
719 contact->pointsState[0] = Contact::Contacted;
720 else contact->pointsState[0] = Contact::Separating;
721
722 contact->state = contact->pointsState[0];
723 return contact->state;
724 }
725
checkDiskDisk(Contact * contact)726 int GJKCollisionSolver::checkDiskDisk(Contact* contact)
727 {
728 Disk* disk0 = static_cast<Disk*>(contact->body0);
729 Disk* disk1 = static_cast<Disk*>(contact->body1);
730
731 Vector2d r = disk1->position() - disk0->position();
732 double rd = disk1->radius() + disk0->radius();
733 double rn = r.norm();
734 contact->normal = r/rn;
735 contact->distance = rn - rd;
736
737 if(contact->distance > _toleranceAbs) {
738 contact->state = Contact::Separated;
739 return contact->state;
740 } else if(contact->distance < 0) {
741 contact->state = Contact::Intersected;
742 return contact->state;
743 }
744
745 contact->pointsCount = 1;
746 contact->points[0] = disk0->position() + contact->normal * disk0->radius();
747
748 Vector2d v = disk1->velocity() - disk0->velocity();
749 contact->vrel[0] = v.dot(contact->normal);
750 contact->normalDerivative = v / rn - (v.dot(r)/rn/rn/rn) * r;
751
752 if(contact->vrel[0] < 0)
753 contact->pointsState[0] = Contact::Colliding;
754 else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
755 contact->pointsState[0] = Contact::Contacted;
756 else contact->pointsState[0] = Contact::Separating;
757
758 contact->normalDerivative.setZero(); //XXX
759 contact->state = contact->pointsState[0];
760 return contact->state;
761 }
762
checkDiskParticle(Contact * contact)763 int GJKCollisionSolver::checkDiskParticle(Contact* contact)
764 {
765 Disk* disk0 = static_cast<Disk*>(contact->body0);
766 Particle* particle1 = static_cast<Particle*>(contact->body1);
767
768 Vector2d r = particle1->position() - disk0->position();
769 double rd = disk0->radius();
770 double rn = r.norm();
771 contact->normal = r/rn;
772 contact->distance = rn - rd;
773
774 if(contact->distance > _toleranceAbs) {
775 contact->state = Contact::Separated;
776 return contact->state;
777 } else if(contact->distance < _toleranceAbs*1e-2) {
778 contact->state = Contact::Intersected;
779 return contact->state;
780 }
781
782 contact->pointsCount = 1;
783 contact->points[0] = disk0->position() + contact->normal * disk0->radius();
784
785 Vector2d v = particle1->velocity() - disk0->velocity();
786 contact->vrel[0] = v.dot(contact->normal);
787 contact->normalDerivative = v / rn - (v.dot(r)/rn/rn/rn) * r;
788
789 if(contact->vrel[0] < 0)
790 contact->pointsState[0] = Contact::Colliding;
791 else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
792 contact->pointsState[0] = Contact::Contacted;
793 else contact->pointsState[0] = Contact::Separating;
794
795 contact->state = contact->pointsState[0];
796 return contact->state;
797 }
798
799
800 /*
801 int GJKCollisionSolver::checkContact(Contact* contact)
802 {
803
804 if(contact->type == Contact::UnknownType) {
805 if(contact->body0->metaObject()->inherits<BasePolygon>()) {
806 if(contact->body1->metaObject()->inherits<BasePolygon>()) contact->type = Contact::PolygonPolygonType;
807 else if(contact->body1->metaObject()->inherits<Particle>()) contact->type = Contact::PolygonParticleType;
808 } else if(contact->body0->metaObject()->inherits<Particle>()) {
809 if(contact->body1->metaObject()->inherits<BasePolygon>()) {
810 std::swap(contact->body0, contact->body1);
811 contact->type = Contact::PolygonParticleType;
812 }
813 }
814 contact->state = Contact::Unknown;
815 }
816
817 if(contact->type == Contact::PolygonPolygonType) return checkPolygonPolygon(contact);
818 else if(contact->type == Contact::PolygonParticleType) return checkPolygonParticle(contact);
819 return contact->state = Contact::Unknown;
820 }*/
821
checkContact(Contact * contact)822 inline int GJKCollisionSolver::checkContact(Contact* contact)
823 {
824 if(contact->type == Contact::PolygonPolygonType) checkPolygonPolygon(contact);
825 else if(contact->type == Contact::PolygonDiskType) checkPolygonDisk(contact);
826 else if(contact->type == Contact::PolygonParticleType) checkPolygonParticle(contact);
827 else if(contact->type == Contact::DiskDiskType) checkDiskDisk(contact);
828 else if(contact->type == Contact::DiskParticleType) checkDiskParticle(contact);
829 else contact->state = Contact::Unknown;
830 return contact->state;
831 }
832
checkContacts(BodyList & bodies,bool collisions,int * retCount)833 int GJKCollisionSolver::checkContacts(BodyList& bodies, bool collisions, int* retCount)
834 {
835 int state = Contact::Unknown;
836 int count = 0;
837
838 checkCache(bodies);
839
840 // Detect and classify contacts...
841 // ... and count contact joints
842 const ContactValueList::iterator end = _contacts.end();
843 for(ContactValueList::iterator it = _contacts.begin(); it != end; ++it) {
844 Contact& contact = *it;
845
846 checkContact(&contact);
847
848 if(contact.state > state) state = contact.state;
849 if( (it->state == Contact::Contacted) || (collisions && it->state == Contact::Colliding) )
850 count += it->pointsCount;
851 }
852 if (retCount) *retCount = count;
853 return state;
854 }
855
getContactsInfo(ConstraintsInfo & info,bool collisions)856 void GJKCollisionSolver::getContactsInfo(ConstraintsInfo& info, bool collisions)
857 {
858 const ContactValueList::iterator end = _contacts.end();
859
860 int i = info.constraintsCount-1;
861 // Add contact joints
862 for(ContactValueList::iterator it = _contacts.begin(); it != end; ++it) {
863 Contact& contact = *it;
864 if(contact.state == Contact::Contacted) {
865 //qDebug("** resting contact, points: %d", contact.pointsCount);
866 for(int p=0; p<contact.pointsCount; ++p) {
867 // XXX: check signs !
868 // XXX: rotation and friction !
869 /*info.value[i0] = contact.normal[0] * contact.distance;
870 info.value[i1] = contact.normal[1] * contact.distance;
871 info.derivative[i0] = contact.normal[0] * contact.vrel[0];
872 info.derivative[i1] = contact.normal[1] * contact.vrel[0];*/
873 /*info.value[i] = contact.distance;
874 info.derivative[i] = contact.vrel[p];*/
875 ++i;
876 info.jacobian.coeffRef(i, contact.body0->variablesOffset() + RigidBody::PositionOffset) = -contact.normal[0];
877 info.jacobian.coeffRef(i, contact.body0->variablesOffset() + RigidBody::PositionOffset+1) = -contact.normal[1];
878 info.jacobian.coeffRef(i, contact.body1->variablesOffset() + RigidBody::PositionOffset) = contact.normal[0];
879 info.jacobian.coeffRef(i, contact.body1->variablesOffset() + RigidBody::PositionOffset+1) = contact.normal[1];
880
881 if(!collisions) {
882 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
883 RigidBody::PositionOffset) = ( -contact.normalDerivative[0]);
884 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
885 RigidBody::PositionOffset+1) = ( -contact.normalDerivative[1]);
886 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() +
887 RigidBody::PositionOffset) = ( contact.normalDerivative[0]);
888 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() +
889 RigidBody::PositionOffset+1) = ( contact.normalDerivative[1]);
890 }
891
892 if(contact.body0->metaObject()->inherits<RigidBody>()) {
893 Vector2d r = static_cast<RigidBody*>(contact.body0)->position() - contact.points[p];
894 Vector2d v = static_cast<RigidBody*>(contact.body0)->velocity();
895 double rn = r[0]*contact.normal[1] - r[1]*contact.normal[0];
896 double rd = v[0]*contact.normal[1] - v[1]*contact.normal[0] +
897 r[0]*contact.normalDerivative[1] - r[1]*contact.normalDerivative[0];
898 info.jacobian.coeffRef(i, contact.body0->variablesOffset() +
899 RigidBody::AngleOffset) = ( +rn);
900 if(!collisions)
901 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
902 RigidBody::AngleOffset) = ( +rd);
903 }
904
905 if(contact.body1->metaObject()->inherits<RigidBody>()) {
906 Vector2d r = static_cast<RigidBody*>(contact.body1)->position() - contact.points[p];
907 Vector2d v = static_cast<RigidBody*>(contact.body1)->velocity();
908 double rn = r[0]*contact.normal[1] - r[1]*contact.normal[0];
909 double rd = v[0]*contact.normal[1] - v[1]*contact.normal[0] +
910 r[0]*contact.normalDerivative[1] - r[1]*contact.normalDerivative[0];
911 info.jacobian.coeffRef(i, contact.body1->variablesOffset() + RigidBody::AngleOffset) = -rn;
912 if(!collisions)
913 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() + RigidBody::AngleOffset) = -rd;
914 }
915 info.forceMin[i] = 0;
916 }
917
918 } else if(collisions && contact.state == Contact::Colliding) {
919 //qDebug("** collision, points: %d", contact.pointsCount);
920 for(int p = 0; p<contact.pointsCount; ++p) {
921 ++i;
922 info.jacobian.coeffRef(i, contact.body0->variablesOffset() +
923 RigidBody::PositionOffset) = ( -contact.normal[0]);
924 info.jacobian.coeffRef(i, contact.body0->variablesOffset() +
925 RigidBody::PositionOffset+1) = ( -contact.normal[1]);
926 info.jacobian.coeffRef(i, contact.body1->variablesOffset() +
927 RigidBody::PositionOffset) = ( contact.normal[0]);
928 info.jacobian.coeffRef(i, contact.body1->variablesOffset() +
929 RigidBody::PositionOffset+1) = ( contact.normal[1]);
930
931 // jacobianDerivative is special in this case: it is not really
932 // a derivative, but rather just an expression that should be in
933 // constraint equation
934 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
935 RigidBody::PositionOffset) = ( -2*contact.normal[0]);
936 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
937 RigidBody::PositionOffset+1) = ( -2*contact.normal[1]);
938 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() +
939 RigidBody::PositionOffset) = ( 2*contact.normal[0]);
940 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() +
941 RigidBody::PositionOffset+1) = ( 2*contact.normal[1]);
942
943 if(contact.body0->metaObject()->inherits<RigidBody>()) {
944 Vector2d r = static_cast<RigidBody*>(contact.body0)->position() - contact.points[p];
945 double rn = r[0]*contact.normal[1] - r[1]*contact.normal[0];
946 info.jacobian.coeffRef(i, contact.body0->variablesOffset() +
947 RigidBody::AngleOffset) = ( +rn);
948 info.jacobianDerivative.coeffRef(i, contact.body0->variablesOffset() +
949 RigidBody::AngleOffset) = ( +2*rn);
950 }
951
952 if(contact.body1->metaObject()->inherits<RigidBody>()) {
953 Vector2d r = static_cast<RigidBody*>(contact.body1)->position() - contact.points[p];
954 double rn = r[0]*contact.normal[1] - r[1]*contact.normal[0];
955 info.jacobian.coeffRef(i, contact.body1->variablesOffset() +
956 RigidBody::AngleOffset) = ( -rn);
957 info.jacobianDerivative.coeffRef(i, contact.body1->variablesOffset() +
958 RigidBody::AngleOffset) = ( -2*rn);
959 }
960
961 info.forceMin[i] = 0;
962 info.collisionFlag = true;
963 }
964 }
965 }
966 }
967
solvePolygonPolygon(Contact * contact)968 int GJKCollisionSolver::solvePolygonPolygon(Contact* contact)
969 {
970 RigidBody* body0 = static_cast<RigidBody*>(contact->body0);
971 RigidBody* body1 = static_cast<RigidBody*>(contact->body1);
972
973 if(contact->pointsCount == 2 &&
974 contact->pointsState[0] == Contact::Colliding &&
975 contact->pointsState[1] == Contact::Colliding) {
976 qDebug("*********** Two-point collisions are still buggy!");
977 }
978
979 // calculate impulse
980 double b = 1; // coefficient of bounceness
981
982 int pointNum = (contact->pointsState[0] == Contact::Colliding ? 0 : 1);
983
984 double vrel = contact->vrel[pointNum];
985 STEPCORE_ASSERT_NOABORT( vrel < 0 );
986
987 Vector2d r0 = contact->points[pointNum] - body0->position();
988 Vector2d r1 = contact->points[pointNum] - body1->position();
989
990 double r0n = r0[0]*contact->normal[1] - r0[1]*contact->normal[0];
991 double r1n = r1[0]*contact->normal[1] - r1[1]*contact->normal[0];
992
993 double term0 = (Vector2d( -r0n*r0[1], r0n*r0[0] )).dot(contact->normal) / body0->inertia();
994 double term1 = (Vector2d( -r1n*r1[1], r1n*r1[0] )).dot(contact->normal) / body1->inertia();
995
996 double term2 = 1/body0->mass() + 1/body1->mass();
997
998 /*
999 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1000 body1->velocity()[0], body1->velocity()[1]);
1001 qDebug("body0=%p, body1=%p", body0, body1);
1002 qDebug("vrel=%f", vrel);
1003 qDebug("normal=(%f,%f)", contact->normal[0], contact->normal[1]);
1004 */
1005 Vector2d j = contact->normal * ( -(1+b)*vrel / (term0 + term1 + term2) );
1006 //qDebug("mass0=%f mass1=%f j=(%f,%f)", body0->mass(), body1->mass(), j[0], j[1]);
1007 body0->setVelocity(body0->velocity() - j / body0->mass());
1008 body1->setVelocity(body1->velocity() + j / body1->mass());
1009 body0->setAngularVelocity(body0->angularVelocity() - j.norm() * r0n / body0->inertia());
1010 body1->setAngularVelocity(body1->angularVelocity() + j.norm() * r1n / body1->inertia());
1011
1012 /*
1013 double vrel1 = (body1->velocityWorld(contact->points[pointNum]) -
1014 body0->velocityWorld(contact->points[pointNum])).dot(contact->normal);
1015 STEPCORE_ASSERT_NOABORT(vrel1 >= 0);
1016 qDebug("vrel1 = %f", vrel1);
1017 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1018 body1->velocity()[0], body1->velocity()[1]);
1019 qDebug(" ");
1020 */
1021 contact->pointsState[pointNum] = Contact::Separating;
1022 contact->state = Contact::Separating; // XXX
1023 return 2;//CollisionDetected;
1024 }
1025
solvePolygonDisk(Contact * contact)1026 int GJKCollisionSolver::solvePolygonDisk(Contact* contact)
1027 {
1028 RigidBody* body0 = static_cast<RigidBody*>(contact->body0);
1029 Disk* body1 = static_cast<Disk*>(contact->body1);
1030
1031 STEPCORE_ASSERT_NOABORT( contact->pointsCount == 1 );
1032
1033 // calculate impulse
1034 double b = 1; // coefficient of bounceness
1035
1036 double vrel = contact->vrel[0];
1037 STEPCORE_ASSERT_NOABORT( vrel < 0 );
1038
1039 Vector2d r0 = contact->points[0] - body0->position();
1040 double r0n = r0[0]*contact->normal[1] - r0[1]*contact->normal[0];
1041 double term0 = (Vector2d( -r0n*r0[1], r0n*r0[0])).dot(contact->normal) / body0->inertia();
1042
1043 double term2 = 1/body0->mass() + 1/body1->mass();
1044
1045 /*
1046 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1047 body1->velocity()[0], body1->velocity()[1]);
1048 qDebug("body0=%p, body1=%p", body0, body1);
1049 qDebug("vrel=%f", vrel);
1050 qDebug("normal=(%f,%f)", contact->normal[0], contact->normal[1]);
1051 */
1052 Vector2d j = contact->normal * ( -(1+b)*vrel / (term0 + term2) );
1053 //qDebug("mass0=%f mass1=%f j=(%f,%f)", body0->mass(), body1->mass(), j[0], j[1]);
1054 body0->setVelocity(body0->velocity() - j / body0->mass());
1055 body1->setVelocity(body1->velocity() + j / body1->mass());
1056 body0->setAngularVelocity(body0->angularVelocity() - j.norm() * r0n / body0->inertia());
1057
1058 /*
1059 double vrel1 = (body1->velocity() -
1060 body0->velocityWorld(contact->points[0])).dot(contact->normal);
1061 STEPCORE_ASSERT_NOABORT(vrel1 >= 0);
1062 qDebug("vrel1 = %f", vrel1);
1063 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1064 body1->velocity()[0], body1->velocity()[1]);
1065 qDebug(" ");
1066 */
1067 contact->pointsState[0] = Contact::Separating;
1068 contact->state = Contact::Separating; // XXX
1069 return 2;//CollisionDetected;
1070 }
1071
solvePolygonParticle(Contact * contact)1072 int GJKCollisionSolver::solvePolygonParticle(Contact* contact)
1073 {
1074 RigidBody* body0 = static_cast<RigidBody*>(contact->body0);
1075 Particle* body1 = static_cast<Particle*>(contact->body1);
1076
1077 STEPCORE_ASSERT_NOABORT( contact->pointsCount == 1 );
1078
1079 // calculate impulse
1080 double b = 1; // coefficient of bounceness
1081
1082 double vrel = contact->vrel[0];
1083 STEPCORE_ASSERT_NOABORT( vrel < 0 );
1084
1085 Vector2d r0 = contact->points[0] - body0->position();
1086 double r0n = r0[0]*contact->normal[1] - r0[1]*contact->normal[0];
1087 double term0 = (Vector2d( -r0n*r0[1], r0n*r0[0])).dot(contact->normal) / body0->inertia();
1088
1089 double term2 = 1/body0->mass() + 1/body1->mass();
1090
1091 /*
1092 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1093 body1->velocity()[0], body1->velocity()[1]);
1094 qDebug("body0=%p, body1=%p", body0, body1);
1095 qDebug("vrel=%f", vrel);
1096 qDebug("normal=(%f,%f)", contact->normal[0], contact->normal[1]);
1097 */
1098 Vector2d j = contact->normal * ( -(1+b)*vrel / (term0 + term2) );
1099 //qDebug("mass0=%f mass1=%f j=(%f,%f)", body0->mass(), body1->mass(), j[0], j[1]);
1100 body0->setVelocity(body0->velocity() - j / body0->mass());
1101 body1->setVelocity(body1->velocity() + j / body1->mass());
1102 body0->setAngularVelocity(body0->angularVelocity() - j.norm() * r0n / body0->inertia());
1103
1104 /*
1105 double vrel1 = (body1->velocity() -
1106 body0->velocityWorld(contact->points[0])).dot(contact->normal);
1107 STEPCORE_ASSERT_NOABORT(vrel1 >= 0);
1108 qDebug("vrel1 = %f", vrel1);
1109 qDebug("vel0=(%f,%f) vel1=(%f,%f)", body0->velocity()[0], body0->velocity()[1],
1110 body1->velocity()[0], body1->velocity()[1]);
1111 qDebug(" ");
1112 */
1113 contact->pointsState[0] = Contact::Separating;
1114 contact->state = Contact::Separating; // XXX
1115 return 2;//CollisionDetected;
1116 }
1117
solveDiskDisk(Contact * contact)1118 int GJKCollisionSolver::solveDiskDisk(Contact* contact)
1119 {
1120 Disk* disk0 = static_cast<Disk*>(contact->body0);
1121 Disk* disk1 = static_cast<Disk*>(contact->body1);
1122
1123 STEPCORE_ASSERT_NOABORT( contact->pointsCount == 1 );
1124
1125 // calculate impulse
1126 double b = 1; // coefficient of bounceness
1127 double vrel = contact->vrel[0];
1128 STEPCORE_ASSERT_NOABORT( vrel < 0 );
1129
1130 Vector2d j = contact->normal * ( -(1+b)*vrel / (1/disk0->mass() + 1/disk1->mass()) );
1131 disk0->setVelocity(disk0->velocity() - j / disk0->mass());
1132 disk1->setVelocity(disk1->velocity() + j / disk1->mass());
1133
1134 contact->pointsState[0] = Contact::Separating;
1135 contact->state = Contact::Separating; // XXX
1136 return 2;//CollisionDetected;
1137 }
1138
solveDiskParticle(Contact * contact)1139 int GJKCollisionSolver::solveDiskParticle(Contact* contact)
1140 {
1141 Disk* disk0 = static_cast<Disk*>(contact->body0);
1142 Particle* particle1 = static_cast<Particle*>(contact->body1);
1143
1144 STEPCORE_ASSERT_NOABORT( contact->pointsCount == 1 );
1145
1146 // calculate impulse
1147 double b = 1; // coefficient of bounceness
1148 double vrel = contact->vrel[0];
1149 STEPCORE_ASSERT_NOABORT( vrel < 0 );
1150
1151 Vector2d j = contact->normal * ( -(1+b)*vrel / (1/disk0->mass() + 1/particle1->mass()) );
1152 disk0->setVelocity(disk0->velocity() - j / disk0->mass());
1153 particle1->setVelocity(particle1->velocity() + j / particle1->mass());
1154
1155 contact->pointsState[0] = Contact::Separating;
1156 contact->state = Contact::Separating; // XXX
1157 return 2;//CollisionDetected;
1158 }
1159
solveCollisions(BodyList & bodies)1160 int GJKCollisionSolver::solveCollisions(BodyList& bodies)
1161 {
1162 int ret = 0;
1163
1164 // Detect and classify contacts
1165 ret = checkContacts(bodies);
1166 STEPCORE_ASSERT_NOABORT(ret != Contact::Intersected);
1167
1168 // Solve collisions
1169
1170 const ContactValueList::iterator end = _contacts.end();
1171 for(ContactValueList::iterator it = _contacts.begin(); it != end; ++it) {
1172 Contact& contact = *it;
1173
1174 if(contact.state != Contact::Colliding) continue;
1175
1176 if(contact.type == Contact::PolygonPolygonType) ret = solvePolygonPolygon(&contact);
1177 else if(contact.type == Contact::PolygonDiskType) ret = solvePolygonDisk(&contact);
1178 else if(contact.type == Contact::PolygonParticleType) ret = solvePolygonParticle(&contact);
1179 else if(contact.type == Contact::DiskDiskType) ret = solveDiskDisk(&contact);
1180 else if(contact.type == Contact::DiskParticleType) ret = solveDiskParticle(&contact);
1181 else { STEPCORE_ASSERT_NOABORT(0); }
1182
1183 }
1184
1185 return 0;
1186 }
1187
1188 #if 0
1189 int GJKCollisionSolver::solveConstraints(BodyList& /*bodies*/)
1190 {
1191
1192 return 0;
1193 }
1194 #endif
1195
checkCache(BodyList & bodies)1196 void GJKCollisionSolver::checkCache(BodyList& bodies)
1197 {
1198 if(!_contactsIsValid) {
1199 _contacts.clear();
1200
1201 BodyList::const_iterator end = bodies.end();
1202 for(BodyList::const_iterator i0 = bodies.begin(); i0 != end; ++i0) {
1203 for(BodyList::const_iterator i1 = i0+1; i1 != end; ++i1) {
1204 addContact(*i0, *i1);
1205 }
1206 }
1207
1208 _contactsIsValid = true;
1209 }
1210 }
1211
bodyAdded(BodyList & bodies,Body * body)1212 void GJKCollisionSolver::bodyAdded(BodyList& bodies, Body* body)
1213 {
1214 if(!_contactsIsValid) return;
1215
1216 BodyList::const_iterator end = bodies.end();
1217 for(BodyList::const_iterator i1 = bodies.begin(); i1 != end; ++i1)
1218 addContact(body, *i1);
1219 }
1220
bodyRemoved(BodyList &,Body * body)1221 void GJKCollisionSolver::bodyRemoved(BodyList&, Body* body)
1222 {
1223 if(!_contactsIsValid) return;
1224
1225 for(;;) {
1226 const ContactValueList::iterator end = _contacts.end();
1227 ContactValueList::iterator it = _contacts.begin();
1228 for(; it != end; ++it)
1229 if((*it).body0 == body || (*it).body1 == body) break;
1230 if(it != end) _contacts.erase(it);
1231 else break;
1232 }
1233 }
1234
addContact(Body * body0,Body * body1)1235 void GJKCollisionSolver::addContact(Body* body0, Body* body1)
1236 {
1237 int type = Contact::UnknownType;
1238
1239 if(body1->metaObject()->inherits<BasePolygon>() &&
1240 !body0->metaObject()->inherits<BasePolygon>()) {
1241 std::swap(body0, body1);
1242 }
1243
1244 if(body0->metaObject()->inherits<BasePolygon>()) {
1245 if(body1->metaObject()->inherits<BasePolygon>()) type = Contact::PolygonPolygonType;
1246 else if(body1->metaObject()->inherits<Disk>()) type = Contact::PolygonDiskType;
1247 else if(body1->metaObject()->inherits<Particle>()) type = Contact::PolygonParticleType;
1248 } else {
1249
1250 if(body1->metaObject()->inherits<Disk>() &&
1251 !body0->metaObject()->inherits<Disk>()) {
1252 std::swap(body0, body1);
1253 }
1254
1255 if(body0->metaObject()->inherits<Disk>()) {
1256 if(body1->metaObject()->inherits<Disk>()) type = Contact::DiskDiskType;
1257 else if(body1->metaObject()->inherits<Particle>()) type = Contact::DiskParticleType;
1258 }
1259
1260 }
1261
1262 if(type != Contact::UnknownType) {
1263 Contact contact;
1264 contact.type = type;
1265 contact.body0 = body0;
1266 contact.body1 = body1;
1267 contact.state = Contact::Unknown;
1268 _contacts.push_back(contact);
1269 }
1270 }
1271
resetCaches()1272 void GJKCollisionSolver::resetCaches()
1273 {
1274 _contactsIsValid = false;
1275 }
1276
1277 } // namespace StepCore
1278
1279