• Skip to content
• Skip to link menu

KDE 4.2 API Reference

• KDE API Reference
• kdeedu

• Sitemap
• Contact Us

 

step/stepcore

collisionsolver.cc

Go to the documentation of this file.

/* This file is part of StepCore library.
   Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>

   StepCore library is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   StepCore library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with StepCore; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "collisionsolver.h"
#include "rigidbody.h"
#include "particle.h"

#include <algorithm>

namespace StepCore {

// XXX: units for toleranceAbs and localError
STEPCORE_META_OBJECT(CollisionSolver, "CollisionSolver", MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),
    STEPCORE_PROPERTY_RW(double, toleranceAbs, STEPCORE_UNITS_1, "Allowed absolute tolerance", toleranceAbs, setToleranceAbs)
    STEPCORE_PROPERTY_R_D(double, localError, STEPCORE_UNITS_1, "Maximal local error during last step", localError))

STEPCORE_META_OBJECT(GJKCollisionSolver, "GJKCollisionSolver", 0,
                        STEPCORE_SUPER_CLASS(CollisionSolver),)

int GJKCollisionSolver::checkPolygonPolygon(Contact* contact)
{
    BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
    BasePolygon* polygon1 = static_cast<BasePolygon*>(contact->body1);

    if(polygon0->vertexes().empty() || polygon1->vertexes().empty()) {
        return contact->state = Contact::Unknown;
    }

    // Algorithm description can be found in 
    // "A Fast and Robust GJK Implementation for
    //    Collision Detection of Convex Objects"
    //    by Gino van den Bergen

    Vector2dList vertexes[2];
    vertexes[0].reserve(polygon0->vertexes().size());
    vertexes[1].reserve(polygon1->vertexes().size());

    const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
    for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
                                        it0 != p0_it_end; ++it0) {
        vertexes[0].push_back(polygon0->pointLocalToWorld(*it0));
    }

    const Vector2dList::const_iterator p1_it_end = polygon1->vertexes().end();
    for(Vector2dList::const_iterator it1 = polygon1->vertexes().begin();
                                        it1 != p1_it_end; ++it1) {
        vertexes[1].push_back(polygon1->pointLocalToWorld(*it1));
    }

    int wsize;
    Vector2d w[3];  // Vertexes of current simplex
    Vector2d v;     // Closest point of current simplex
    Vector2d s;     // New support vertex in direction v

    Vector2d vv[2]; // Closest points on the first and second objects
    int wi[2][3];   // Indexes of vertexes corresponding to w
    int si[2];      // Indexes of vertexes corresponding to s

    wsize = 1;
    // Start with arbitrary vertex (TODO: cache the whole w simplex)
    if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
        wi[0][0] = contact->_w1[0];
        wi[1][0] = contact->_w1[1];
    } else {
        wi[0][0] = 0;
        wi[1][0] = 0;
    }
    vv[0] = vertexes[0][wi[0][0]];
    vv[1] = vertexes[1][wi[1][0]];
    w[0] = v = vv[1] - vv[0];

    bool intersects = false;
    unsigned int iteration = 0;
    for(;; ++iteration) {
        //STEPCORE_ASSERT_NOABORT( iteration < vertexes[0].size()*vertexes[1].size() );

        double smin = v.norm2();

        // Check for penetration (part 1)
        // If we are closer to the origin then given tolerance
        // we should stop just now to avoid computational errors later
        if(smin < _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance for penetration ?
            intersects = true;
            break;
        }

        // Find support vertex in direction v
        // TODO: coherence optimization
        bool sfound = false;
        unsigned int vertex0_size = vertexes[0].size();
        unsigned int vertex1_size = vertexes[1].size();

        for(unsigned int i0=0; i0<vertex0_size; ++i0) {
            for(unsigned int i1=0; i1<vertex1_size; ++i1) {
                Vector2d sn = vertexes[1][i1] - vertexes[0][i0];
                double scurr = v.innerProduct(sn);
                if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
                    smin = scurr;
                    s = sn;
                    si[0] = i0;
                    si[1] = i1;
                    sfound = true;
                }
            }
        }

        // If no support vertex have been found than we are at minimum
        if(!sfound) {
            if(wsize == 0) { // we have guessed right point
                w[0] = v;
                wi[0][0] = 0;
                wi[1][0] = 0;
                wsize = 1;
            }
            break;
        }

        // Check for penetration (part 2)
        if(wsize == 2) {
            // objects are penetrating if origin lies inside the simplex
            // XXX: are there faster method to test it ?
            Vector2d w02 = w[0] - s;
            Vector2d w12 = w[1] - s;
            double det  =  w02[0]*w12[1] - w02[1]*w12[0];
            double det0 =   -s[0]*w12[1] +   s[1]*w12[0];
            double det1 = -w02[0]*  s[1] + w02[1]*  s[0];
            if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
                w[wsize] = s;
                wi[0][wsize] = si[0];
                wi[1][wsize] = si[1];
                ++wsize;
                v.setZero();
                intersects = true;
                break;
            }
        }

        // Find v = dist(conv(w+s))
        double lambda = 0;
        int ii = -1;
        for(int i=0; i<wsize; ++i) {
            double lambda0 = - s.innerProduct(w[i]-s) / (w[i]-s).norm2();
            if(lambda0 > 0) {
                Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
                if(vn.norm2() < v.norm2()) {
                    v = vn; ii = i;
                    lambda = lambda0;
                }
            }
        }

        if(ii >= 0) { // Closest simplex is line
            vv[0] = vertexes[0][si[0]]*(1-lambda) + vertexes[0][wi[0][ii]]*lambda;
            vv[1] = vertexes[1][si[1]]*(1-lambda) + vertexes[1][wi[1][ii]]*lambda;
            if(wsize == 2) {
                w[1-ii] = s;
                wi[0][1-ii] = si[0];
                wi[1][1-ii] = si[1];
            } else {
                w[wsize] = s;
                wi[0][wsize] = si[0];
                wi[1][wsize] = si[1];
                ++wsize;
            }
        } else { // Closest simplex is vertex
            STEPCORE_ASSERT_NOABORT(iteration == 0 || s.norm2() < v.norm2());

            v = w[0] = s;
            vv[0] = vertexes[0][si[0]];
            vv[1] = vertexes[1][si[1]];
            wi[0][0] = si[0];
            wi[1][0] = si[1];
            wsize = 1;
        }
    }

    if(intersects) {
        /*
        qDebug("penetration detected");
        qDebug("iteration = %d", iteration);
        qDebug("simplexes:");
        qDebug("    1:   2:");
        for(int i=0; i<wsize; ++i) {
            qDebug("    %d    %d", wi[0][i], wi[1][i]);
        }
        */
        contact->distance = 0;
        contact->normal.setZero();
        contact->pointsCount = 0;
        return contact->state = Contact::Intersected;
    }

    /*
    qDebug("distance = %f", v.norm());
    Vector2d v1 = v / v.norm();
    qDebug("normal = (%f,%f)", v1[0], v1[1]);
    qDebug("iteration = %d", iteration);
    qDebug("simplexes:");
    qDebug("    1:   2:");
    for(int i=0; i<wsize; ++i) {
        qDebug("    %d    %d", wi[0][i], wi[1][i]);
    }
    qDebug("contact points:");
    qDebug("    (%f,%f)    (%f,%f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
    */

    double vnorm = v.norm();
    contact->distance = vnorm;
    contact->normal = v/vnorm;
    contact->pointsCount = 0;
    contact->state = Contact::Separated;

    contact->_w1[0] = wi[0][0];
    contact->_w1[1] = wi[1][0];

    if(vnorm > _toleranceAbs) return contact->state;

    // If the objects are close enough we need to find contact manifold
    // We are going to find simplexes (lines) that are 'most parallel'
    // to contact plane and look for contact manifold among them. It
    // works for almost all cases when adjacent polygon edges are
    // not parallel
    Vector2d vunit = v / vnorm;
    Vector2d wm[2][2];

    for(int i=0; i<2; ++i) {
        wm[i][0] = vertexes[i][ wi[i][0] ];

        if(wsize < 2 || wi[i][0] == wi[i][1]) { // vertex contact
            // Check two adjacent edges
            int ai1 = wi[i][0] - 1; if(ai1 < 0) ai1 = vertexes[i].size()-1;
            Vector2d av1 = vertexes[i][ai1];
            Vector2d dv1 = wm[i][0] - av1;
            double dist1 = vunit.innerProduct( dv1 ) * (i==0 ? 1 : -1);
            double angle1 = dist1 / dv1.norm();

            int ai2 = wi[i][0] + 1; if(ai2 >= (int) vertexes[i].size()) ai2 = 0;
            Vector2d av2 = vertexes[i][ai2];
            Vector2d dv2 = wm[i][0] - av2;
            double dist2 = vunit.innerProduct( dv2 ) * (i==0 ? 1 : -1);
            double angle2 = dist2 / dv2.norm();

            if(angle1 <= angle2 && dist1 < (_toleranceAbs-vnorm)/2) {
                wm[i][1] = av1;
            } else if(angle2 <= angle1 && dist2 < (_toleranceAbs-vnorm)/2) {
                wm[i][1] = av2;
            } else {
                wm[i][1] = wm[i][0]; contact->pointsCount = 1;
                break;
            }
        } else { // edge contact
            wm[i][1] = vertexes[i][ wi[i][1] ];
        }
    }

    // Find intersection of two lines
    if(contact->pointsCount != 1) {
        Vector2d vunit_o(-vunit[1], vunit[0]);
        double wm_o[2][2];

        for(int i=0; i<2; ++i) {
            wm_o[i][0] = vunit_o.innerProduct(wm[i][0]);
            wm_o[i][1] = vunit_o.innerProduct(wm[i][1]);

            if(wm_o[i][0] > wm_o[i][1]) {
                std::swap(wm_o[i][0], wm_o[i][1]);
                std::swap(wm[i][0], wm[i][1]);
            }
        }

        if(wm_o[0][0] > wm_o[1][0]) contact->points[0] = wm[0][0];
        else contact->points[0] = wm[1][0];

        if(wm_o[0][1] < wm_o[1][1]) contact->points[1] = wm[0][1];
        else contact->points[1] = wm[1][1];

        // TODO: interpolate to midpoint
        if((contact->points[1] - contact->points[0]).norm() > _toleranceAbs) {
            /*
            for(int i=0; i<2; ++i) {
                qDebug("contact%d: (%f,%f)", i, contact->points[i][0], contact->points[i][1]);
            }
            */
            contact->pointsCount = 2;
        }
        /*
            contact->vrel[0] = contact->normal.innerProduct(
                                polygon1->velocityWorld(contact->points[0]) -
                                polygon0->velocityWorld(contact->points[0]));
            contact->vrel[1] = contact->normal.innerProduct(
                                polygon1->velocityWorld(contact->points[1]) -
                                polygon0->velocityWorld(contact->points[1]));
            if(contact->vrel[0] < 0 || contact->vrel[1] < 0)
                return contact->state = Contact::Colliding;
            else if(contact->vrel[0] < _toleranceAbs || contact->vrel[1] < _toleranceAbs) // XXX: tolerance
                return contact->state = Colliding::Contacted;
            return contact->state = Contact::Separating;
        }
        */
    }

    if(contact->pointsCount != 2) {
        contact->pointsCount = 1;
        contact->points[0] = vv[0]; // TODO: interpolate vv[0] and vv[1]
        //qDebug("contact is one point: (%f %f) (%f %f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
    }

    int pCount = contact->pointsCount;
    for(int i=0; i<pCount; ++i) {
        contact->vrel[i] = contact->normal.innerProduct(
                        polygon1->velocityWorld(contact->points[i]) -
                        polygon0->velocityWorld(contact->points[i]));

        if(contact->vrel[i] < 0)
            contact->pointsState[i] = Contact::Colliding;
        else if(contact->vrel[i] < _toleranceAbs) // XXX: tolerance
            contact->pointsState[i] = Contact::Contacted;
        else contact->pointsState[i] = Contact::Separating;

        if(contact->pointsState[i] > contact->state)
            contact->state = contact->pointsState[i];
    }

    contact->normalDerivative.setZero(); //XXX
    return contact->state;
}

int GJKCollisionSolver::checkPolygonDisk(Contact* contact)
{
    BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
    Disk* disk1 = static_cast<Disk*>(contact->body1);

    if(polygon0->vertexes().empty()) {
        return contact->state = Contact::Unknown;
    }

    // Simplier version of checkPolygonPolygon algorithm

    Vector2dList vertexes;
    vertexes.reserve(polygon0->vertexes().size());

    const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
    for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
                                        it0 != p0_it_end; ++it0) {
        vertexes.push_back(polygon0->pointLocalToWorld(*it0));
    }

    int wsize;
    Vector2d w[3];  // Vertexes of current simplex
    Vector2d v;     // Closest point of current simplex
    Vector2d s;     // New support vertex in direction v

    Vector2d vv; // Closest points on the polygon
    int wi[3];   // Indexes of vertexes corresponding to w
    int si;      // Indexes of vertexes corresponding to s

    // Start with arbitrary vertex (TODO: cache the whole w simplex)
    wsize = 1;
    if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
        wi[0] = contact->_w1[0];
    } else {
        wi[0] = 0;
    }
    vv = vertexes[wi[0]];
    w[0] = v = disk1->position() - vv;

    bool intersects = false;
    unsigned int iteration = 0;
    for(;; ++iteration) {
        //STEPCORE_ASSERT_NOABORT( iteration < vertexes.size()*vertexes.size() );

        double smin = v.norm2();

        // Check for penetration (part 1)
        // If we are closer to the origin then given tolerance
        // we should stop just now to avoid computational errors later
        if(std::sqrt(smin) - disk1->radius() < _toleranceAbs*1e-2) { // XXX: separate tolerance for penetration ?
            intersects = true;
            break;
        }

        // Find support vertex in direction v
        // TODO: coherence optimization
        bool sfound = false;
        unsigned int vertex_size = vertexes.size();

        for(unsigned int i0=0; i0<vertex_size; ++i0) {
            Vector2d sn = disk1->position() - vertexes[i0];
            double scurr = v.innerProduct(sn);
            if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
                smin = scurr;
                s = sn;
                si = i0;
                sfound = true;
            }
        }

        // If no support vertex have been found than we are at minimum
        if(!sfound) {
            if(wsize == 0) { // we have guessed right point
                w[0] = v;
                wi[0] = 0;
                wsize = 1;
            }
            break;
        }

        // Check for penetration (part 2)
        if(wsize == 2) {
            // objects are penetrating if origin lies inside the simplex
            // XXX: are there faster method to test it ?
            Vector2d w02 = w[0] - s;
            Vector2d w12 = w[1] - s;
            Vector2d s0 = s - s.unit() * disk1->radius();
            double det  =  w02[0]*w12[1] - w02[1]*w12[0];
            double det0 =  -s0[0]*w12[1] +  s0[1]*w12[0];
            double det1 = -w02[0]* s0[1] + w02[1]* s0[0];
            if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
                w[wsize] = s;
                wi[wsize] = si;
                ++wsize;
                v.setZero();
                intersects = true;
                break;
            }
        }

        // Find v = dist(conv(w+s))
        double lambda = 0;
        int ii = -1;
        for(int i=0; i<wsize; ++i) {
            double lambda0 = - s.innerProduct(w[i]-s) / (w[i]-s).norm2();
            if(lambda0 > 0) {
                Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
                if(vn.norm2() < v.norm2()) {
                    v = vn; ii = i;
                    lambda = lambda0;
                }
            }
        }

        if(ii >= 0) { // Closest simplex is line
            vv = vertexes[si]*(1-lambda) + vertexes[wi[ii]]*lambda;
            if(wsize == 2) {
                w[1-ii] = s;
                wi[1-ii] = si;
            } else {
                w[wsize] = s;
                wi[wsize] = si;
                ++wsize;
            }
        } else { // Closest simplex is vertex
            STEPCORE_ASSERT_NOABORT(iteration == 0 || s.norm2() < v.norm2());

            v = w[0] = s;
            vv = vertexes[si];
            wi[0] = si;
            wsize = 1;
        }
    }

    if(intersects) {
        /*qDebug("penetration detected");
        qDebug("iteration = %d", iteration);
        qDebug("simplexes:");
        qDebug("    1:   2:");
        for(int i=0; i<wsize; ++i) {
            qDebug("    %d    %d", wi[i], wi[i]);
        }*/
        contact->distance = 0;
        contact->normal.setZero();
        contact->pointsCount = 0;
        return contact->state = Contact::Intersected;
    }

    /*
    qDebug("distance = %f", v.norm());
    Vector2d v1 = v / v.norm();
    qDebug("normal = (%f,%f)", v1[0], v1[1]);
    qDebug("iteration = %d", iteration);
    qDebug("simplexes:");
    qDebug("    1:   2:");
    for(int i=0; i<wsize; ++i) {
        qDebug("    %d    %d", wi[i], wi[i]);
    }
    qDebug("contact points:");
    qDebug("    (%f,%f)    (%f,%f)", vv[0], vv[1], vv[0], vv[1]);
    */

    double vnorm = v.norm();
    contact->distance = vnorm - disk1->radius();
    contact->normal = v/vnorm;

    contact->_w1[0] = wi[0];

    if(contact->distance > _toleranceAbs) {
        contact->pointsCount = 0;
        contact->state = Contact::Separated;
        return contact->state;
    }/* else if(contact->distance < _toleranceAbs*1e-2) {
        contact->state = Contact::Intersected;
        return contact->state;
    }*/

    contact->pointsCount = 1;
    contact->points[0] = disk1->position() - contact->normal * disk1->radius();
    contact->vrel[0] = contact->normal.innerProduct(
                        disk1->velocity() - 
                        polygon0->velocityWorld(contact->points[0]));

    if(contact->vrel[0] < 0)
        contact->pointsState[0] = Contact::Colliding;
    else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
        contact->pointsState[0] = Contact::Contacted;
    else contact->pointsState[0] = Contact::Separating;

    contact->normalDerivative.setZero(); //XXX
    contact->state = contact->pointsState[0];
    return contact->state;
}

int GJKCollisionSolver::checkPolygonParticle(Contact* contact)
{
    BasePolygon* polygon0 = static_cast<BasePolygon*>(contact->body0);
    Particle* particle1 = static_cast<Particle*>(contact->body1);

    if(polygon0->vertexes().empty()) {
        return contact->state = Contact::Unknown;
    }

    // Simplier version of checkPolygonPolygon algorithm

    Vector2dList vertexes;
    vertexes.reserve(polygon0->vertexes().size());

    const Vector2dList::const_iterator p0_it_end = polygon0->vertexes().end();
    for(Vector2dList::const_iterator it0 = polygon0->vertexes().begin();
                                        it0 != p0_it_end; ++it0) {
        vertexes.push_back(polygon0->pointLocalToWorld(*it0));
    }

    int wsize;
    Vector2d w[3];  // Vertexes of current simplex
    Vector2d v;     // Closest point of current simplex
    Vector2d s;     // New support vertex in direction v

    Vector2d vv; // Closest points on the polygon
    int wi[3];   // Indexes of vertexes corresponding to w
    int si;      // Indexes of vertexes corresponding to s

    // Start with arbitrary vertex (TODO: cache the whole w simplex)
    wsize = 1;
    if(contact->state >= Contact::Separating && contact->state < Contact::Intersected) {
        wi[0] = contact->_w1[0];
    } else {
        wi[0] = 0;
    }
    vv = vertexes[wi[0]];
    w[0] = v = particle1->position() - vv;

    bool intersects = false;
    unsigned int iteration = 0;
    for(;; ++iteration) {
        //STEPCORE_ASSERT_NOABORT( iteration < vertexes[0].size()*vertexes[1].size() );

        double smin = v.norm2();

        // Check for penetration (part 1)
        // If we are closer to the origin then given tolerance
        // we should stop just now to avoid computational errors later
        if(smin < _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance for penetration ?
            intersects = true;
            break;
        }

        // Find support vertex in direction v
        // TODO: coherence optimization
        bool sfound = false;
        unsigned int vertex_size = vertexes.size();

        for(unsigned int i0=0; i0<vertex_size; ++i0) {
            Vector2d sn = particle1->position() - vertexes[i0];
            double scurr = v.innerProduct(sn);
            if(smin - scurr > _toleranceAbs*_toleranceAbs*1e-4) { // XXX: separate tolerance ?
                smin = scurr;
                s = sn;
                si = i0;
                sfound = true;
            }
        }

        // If no support vertex have been found than we are at minimum
        if(!sfound) {
            if(wsize == 0) { // we have guessed right point
                w[0] = v;
                wi[0] = 0;
                wsize = 1;
            }
            break;
        }

        // Check for penetration (part 2)
        if(wsize == 2) {
            // objects are penetrating if origin lies inside the simplex
            // XXX: are there faster method to test it ?
            Vector2d w02 = w[0] - s;
            Vector2d w12 = w[1] - s;
            double det  =  w02[0]*w12[1] - w02[1]*w12[0];
            double det0 =   -s[0]*w12[1] +   s[1]*w12[0];
            double det1 = -w02[0]*  s[1] + w02[1]*  s[0];
            if(det0/det > 0 && det1/det > 0) { // XXX: tolerance
                w[wsize] = s;
                wi[wsize] = si;
                ++wsize;
                v.setZero();
                intersects = true;
                break;
            }
        }

        // Find v = dist(conv(w+s))
        double lambda = 0;
        int ii = -1;
        for(int i=0; i<wsize; ++i) {
            double lambda0 = - s.innerProduct(w[i]-s) / (w[i]-s).norm2();
            if(lambda0 > 0) {
                Vector2d vn = s*(1-lambda0) + w[i]*lambda0;
                if(vn.norm2() < v.norm2()) {
                    v = vn; ii = i;
                    lambda = lambda0;
                }
            }
        }

        if(ii >= 0) { // Closest simplex is line
            vv = vertexes[si]*(1-lambda) + vertexes[wi[ii]]*lambda;
            if(wsize == 2) {
                w[1-ii] = s;
                wi[1-ii] = si;
            } else {
                w[wsize] = s;
                wi[wsize] = si;
                ++wsize;
            }
        } else { // Closest simplex is vertex
            STEPCORE_ASSERT_NOABORT(iteration == 0 || s.norm2() < v.norm2());

            v = w[0] = s;
            vv = vertexes[si];
            wi[0] = si;
            wsize = 1;
        }
    }

    if(intersects) {
        /*
        qDebug("penetration detected");
        qDebug("iteration = %d", iteration);
        qDebug("simplexes:");
        qDebug("    1:   2:");
        for(int i=0; i<wsize; ++i) {
            qDebug("    %d    %d", wi[0][i], wi[1][i]);
        }
        */
        contact->distance = 0;
        contact->normal.setZero();
        contact->pointsCount = 0;
        return contact->state = Contact::Intersected;
    }

    /*
    qDebug("distance = %f", v.norm());
    Vector2d v1 = v / v.norm();
    qDebug("normal = (%f,%f)", v1[0], v1[1]);
    qDebug("iteration = %d", iteration);
    qDebug("simplexes:");
    qDebug("    1:   2:");
    for(int i=0; i<wsize; ++i) {
        qDebug("    %d    %d", wi[0][i], wi[1][i]);
    }
    qDebug("contact points:");
    qDebug("    (%f,%f)    (%f,%f)", vv[0][0], vv[0][1], vv[1][0], vv[1][1]);
    */

    double vnorm = v.norm();
    contact->distance = vnorm;
    contact->normal = v/vnorm;

    contact->_w1[0] = wi[0];

    if(vnorm > _toleranceAbs) {
        contact->pointsCount = 0;
        contact->state = Contact::Separated;
        return contact->state;
    }

    contact->pointsCount = 1;
    contact->points[0] = particle1->position();
    contact->vrel[0] = contact->normal.innerProduct(
                        particle1->velocity() - 
                        polygon0->velocityWorld(contact->points[0]));

    if(contact->vrel[0] < 0)
        contact->pointsState[0] = Contact::Colliding;
    else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
        contact->pointsState[0] = Contact::Contacted;
    else contact->pointsState[0] = Contact::Separating;

    contact->state = contact->pointsState[0];
    return contact->state;
}

int GJKCollisionSolver::checkDiskDisk(Contact* contact)
{
    Disk* disk0 = static_cast<Disk*>(contact->body0);
    Disk* disk1 = static_cast<Disk*>(contact->body1);

    Vector2d r = disk1->position() - disk0->position();
    double rd = disk1->radius() + disk0->radius();
    double rn = r.norm();
    contact->normal = r/rn;
    contact->distance = rn - rd;

    if(contact->distance > _toleranceAbs) {
        contact->state = Contact::Separated;
        return contact->state;
    } else if(contact->distance < 0) {
        contact->state = Contact::Intersected;
        return contact->state;
    }

    contact->pointsCount = 1;
    contact->points[0] = disk0->position() + contact->normal * disk0->radius();

    Vector2d v = disk1->velocity() - disk0->velocity();
    contact->vrel[0] = contact->normal.innerProduct(v);
    contact->normalDerivative = v / rn - (r.innerProduct(v)/rn/rn/rn) * r;

    if(contact->vrel[0] < 0)
        contact->pointsState[0] = Contact::Colliding;
    else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
        contact->pointsState[0] = Contact::Contacted;
    else contact->pointsState[0] = Contact::Separating;

    contact->normalDerivative.setZero(); //XXX
    contact->state = contact->pointsState[0];
    return contact->state;
}

int GJKCollisionSolver::checkDiskParticle(Contact* contact)
{
    Disk* disk0 = static_cast<Disk*>(contact->body0);
    Particle* particle1 = static_cast<Particle*>(contact->body1);

    Vector2d r = particle1->position() - disk0->position();
    double rd = disk0->radius();
    double rn = r.norm();
    contact->normal = r/rn;
    contact->distance = rn - rd;

    if(contact->distance > _toleranceAbs) {
        contact->state = Contact::Separated;
        return contact->state;
    } else if(contact->distance < _toleranceAbs*1e-2) {
        contact->state = Contact::Intersected;
        return contact->state;
    }

    contact->pointsCount = 1;
    contact->points[0] = disk0->position() + contact->normal * disk0->radius();

    Vector2d v = particle1->velocity() - disk0->velocity();
    contact->vrel[0] = contact->normal.innerProduct(v);
    contact->normalDerivative = v / rn - (r.innerProduct(v)/rn/rn/rn) * r;

    if(contact->vrel[0] < 0)
        contact->pointsState[0] = Contact::Colliding;
    else if(contact->vrel[0] < _toleranceAbs) // XXX: tolerance
        contact->pointsState[0] = Contact::Contacted;
    else contact->pointsState[0] = Contact::Separating;

    contact->state = contact->pointsState[0];
    return contact->state;
}


/*
int GJKCollisionSolver::checkContact(Contact* contact)
{

    if(contact->type == Contact::UnknownType) {
        if(contact->body0->metaObject()->inherits<BasePolygon>()) {
            if(contact->body1->metaObject()->inherits<BasePolygon>()) contact->type = Contact::PolygonPolygonType;
            else if(contact->body1->metaObject()->inherits<Particle>()) contact->type = Contact::PolygonParticleType;
        } else if(contact->body0->metaObject()->inherits<Particle>()) {
            if(contact->body1->metaObject()->inherits<BasePolygon>()) {
                std::swap(contact->body0, contact->body1);
                contact->type = Contact::PolygonParticleType;
            }
        }
        contact->state = Contact::Unknown;
    }

    if(contact->type == Contact::PolygonPolygonType) return checkPolygonPolygon(contact);
    else if(contact->type == Contact::PolygonParticleType) return checkPolygonParticle(contact);
    return contact->state = Contact::Unknown;
}*/

inline int GJKCollisionSolver::checkContact(Contact* contact)
{
    if(contact->type == Contact::PolygonPolygonType) checkPolygonPolygon(contact);
    else if(contact->type == Contact::PolygonDiskType) checkPolygonDisk(contact);
    else if(contact->type == Contact::PolygonParticleType) checkPolygonParticle(contact);
    else if(contact->type == Contact::DiskDiskType) checkDiskDisk(contact);
    else if(contact->type == Contact::DiskParticleType) checkDiskParticle(contact);
    else contact->state = Contact::Unknown;
    return contact->state;
}

int GJKCollisionSolver::checkContacts(BodyList& bodies, ConstraintsInfo* info, bool collisions)
{
    int state = Contact::Unknown;

    checkCache(bodies);

    // Detect and classify contacts
    const ContactValueList::iterator end = _contacts.end();
    for(ContactValueList::iterator it = _contacts.begin(); it != end; ++it) {
        Contact& contact = *it;

        checkContact(&contact);

        if(contact.state > state) state = contact.state;
        if(contact.state == Contact::Intersected) return state;
    }

    if(info) {
        // Add contact joints
        for(ContactValueList::iterator it = _contacts.begin(); it != end; ++it) {
            Contact& contact = *it;
            if(contact.state == Contact::Contacted) {
                //qDebug("** resting contact, points: %d", contact.pointsCount);
                for(int p=0; p<contact.pointsCount; ++p) {
                    int i = info->addContact();
                    // XXX: check signs !
                    // XXX: rotation and friction !
                    /*info->value[i0] = contact.normal[0] * contact.distance;
                    info->value[i1] = contact.normal[1] * contact.distance;
                    info->derivative[i0] = contact.normal[0] * contact.vrel[0];
                    info->derivative[i1] = contact.normal[1] * contact.vrel[0];*/
                    /*info->value[i] = contact.distance;
                    info->derivative[i] = contact.vrel[p];*/
                    
                    info->jacobian.row(i).w(contact.body0->variablesOffset() +
                                            RigidBody::PositionOffset, -contact.normal[0]);
                    info->jacobian.row(i).w(contact.body0->variablesOffset() +
                                            RigidBody::PositionOffset+1, -contact.normal[1]);
                    info->jacobian.row(i).w(contact.body1->variablesOffset() +
                                            RigidBody::PositionOffset, contact.normal[0]);
                    info->jacobian.row(i).w(contact.body1->variablesOffset() +
                                            RigidBody::PositionOffset+1, contact.normal[1]);

                    if(!collisions) {
                        info->jacobianDerivative.row(i).w(contact.body0->variablesOffset() +
                                                RigidBody::PositionOffset, -contact.normalDerivative[0]);
                        info->jacobianDerivative.row(i).w(contact.body0->variablesOffset() +