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