src/Collision.cpp

Go to the documentation of this file.

#ifndef DOLOG
//#define DOLOG
#undef  DOLOG
#endif


#include "Collision.h"

#include <sstream>
#include <iomanip>


std::string Collision::dump() const
{
    std::stringstream s;
    s << "[" << std::setw(6) << std::setprecision(5) << time << " * " << first->name() << " vs " << second->name() << " ]";
    return s.str();
}


void BallCollision::collide()
{/*{{{*/
    assert(first);
    assert(second);
    assert(first->applyPhysics());
    assert(second->applyPhysics());

    LLOG("colliding balls" << dump());
    if (first->velocity().length() < PHYS_EPS && second->velocity().length() < PHYS_EPS)
    {
        LOG("Zeroing");
        first->setVelocity(Vec3f(0, 0, 0));
        second->setVelocity(Vec3f(0, 0, 0));
        first->setAngularVelocity(Vec3f(0, 0, 0));
        second->setAngularVelocity(Vec3f(0, 0, 0));
//        return;
    }
    first->updateProperties(time);
    second->updateProperties(time);

    // calculate velocity change

    // collision plane normal
    Vec3f n = (first->position() - second->position());
    LLOG("distance = " << std::setprecision(8) << n.length());
    Vec3f spreadV(0, 0, 0);
    if (n.length() < BALL_RADIUS*2 + EPSILON)
    {
        // avoid gluing
        LOG("avoid gluing");
        spreadV = n.normal() * powf(fabs(BALL_RADIUS*2 - n.length() + PHYS_EPS), SPREAD_POW) * SPREAD_MAX_V;
    }
    n.normalize();

    LLOG("plane normal : " << n);
    LLOG("spread velocity : " << spreadV);

    // normal and tangential components of the velocities
    Vec3f vn1 = -n * first->velocity().dot(-n);
    Vec3f vn2 = n * second->velocity().dot(n);
    Vec3f vt1 = first->velocity() - vn1;
    Vec3f vt2 = second->velocity() - vn2;
    LLOG("obj1 : vn1 = " << vn1 << " (" << vn1.length() << ")  vt1 = " << vt1 << " (" << vt1.length() << ")");
    LLOG("obj2 : vn2 = " << vn2 << " (" << vn2.length() << ")  vt2 = " << vt2 << " (" << vt2.length() << ")");

    // calculate new velocities using formulas obtained from
    // conservation of linear momentum and conservation of kinetic energy
    // assume equal masses -> most terms cancel out, normal velocities are exchanged
    assert(fabs(first->mass() - second->mass()) < EPSILON);
    Vec3f v1_new = vt1 + (vn2 * COLLISION_LOSS) + spreadV;
    Vec3f v2_new = vt2 + (vn1 * COLLISION_LOSS) - spreadV;
    LLOG("new velocities : v1_new = " << v1_new << " (" << v1_new.length() << ")");
    LLOG("                 v2_new = " << v2_new << " (" << v2_new.length() << ")");

    // calculate angular velocity change

    Vec3f r1 = -n * BALL_RADIUS;
    Vec3f r2 = n * BALL_RADIUS;
    
    // difference of perimeter velocities
    Vec3f vpr = r2.cross(second->angularVelocity()) - r1.cross(first->angularVelocity());
    Vec3f force1(0, 0, 0);
    if ((vpr + vt1).length() > EPSILON)
        force1 = (vpr + vt1).normal() * (- MUE_BALLS) * first->mass() * (vn2 - vn1 * COLLISION_LOSS).length() / T_IMPULSE;
    Vec3f force2(0, 0, 0);
    if ((- vpr + vt2).length() > EPSILON)
        force2 = (- vpr + vt2).normal() * (- MUE_BALLS) * second->mass() * (vn1 - vn2 * COLLISION_LOSS).length() / T_IMPULSE;
    // delta angular velocity
    Vec3f dw1 = r1.cross(force1) * 2.5f * T_IMPULSE / (first->mass() * BALL_RADIUS * BALL_RADIUS);
    Vec3f dw2 = r2.cross(force2) * 2.5f * T_IMPULSE / (second->mass() * BALL_RADIUS * BALL_RADIUS);
    // FIXME limit dw-s
    LLOG("vpr = " << vpr << " (" << vpr.length() << ")");
    LLOG("F1 = " << force1 << " (" << force1.length() << ")  F2 = " << force2 << " (" << force2.length() << ")");
    LLOG("dw1 = " << dw1 << " (" << dw1.length() << ")  dw2 = " << dw2 << " (" << dw2.length() << ")");

    first->setVelocity(v1_new);
    second->setVelocity(v2_new);
    first->setAngularVelocity(first->angularVelocity() + dw1);
    second->setAngularVelocity(second->angularVelocity() + dw2);
    LLOG("\n");
}/*}}}*/


void BorderCollision::collide()
{/*{{{*/
    assert(first);
    assert(first->applyPhysics());
    assert(second);
    assert(!second->applyPhysics());

    LLOG("colliding with border " << dump());
    first->updateProperties(time);
    TableBorder * border = dynamic_cast<TableBorder *>(second);
    assert(border);

    LLOG("distance = " << (border->xAligned() ? first->position().z() : first->position().x()) - border->dist());

    // calculate velocity change

    // collision plane normal
    Vec3f n = border->normal();
    LLOG("plane normal : " << n);

    // normal and tangential components of the velocities
    Vec3f vn = -n * first->velocity().dot(-n);
    Vec3f vt = first->velocity() - vn;
    LLOG("obj : vn = " << vn << " (" << vn.length() << ")  vt = " << vt << " (" << vt.length() << ")");

    // calculate new velocities using formulas obtained from
    // conservation of linear momentum and conservation of kinetic energy
    // assume infinite mass of border -> normal velocity is reflected
    Vec3f v_new = vt - (vn * COLLISION_LOSS);
    LLOG("new velocity : v_new = " << v_new << " (" << v_new.length() << ")");

    // calculate angular velocity change

    Vec3f r = -n * BALL_RADIUS;

    // perimeter velocity
    Vec3f vpr = - r.cross(first->angularVelocity());
    Vec3f force(0, 0, 0);
    if ((vpr + vt).length() > EPSILON)
        force = (vpr + vt).normal() * (- MUE_SLIDING) * first->mass() * (vn + vn * COLLISION_LOSS).length() / T_IMPULSE;
    // delta angular velocity
    Vec3f dw = r.cross(force) * 2.5f * T_IMPULSE / (first->mass() * BALL_RADIUS * BALL_RADIUS);
    if (dw.length() > first->angularVelocity().length()*2*COLLISION_LOSS)
        dw = first->angularVelocity() * -2 * COLLISION_LOSS;
    LLOG("r   = " << r << " (" << r.length() << ")");
    LLOG("vpr = " << vpr << " (" << vpr.length() << ")");
    LLOG("F   = " << force << " (" << force.length() << ")");
    LLOG("dw  = " << dw << " (" << dw.length() << ")");

    first->setVelocity(v_new);
    first->setAngularVelocity(first->angularVelocity() + dw);
    LLOG("result");
//    first->dump();  // FIXME DEBUG
    LLOG("\n");
}/*}}}*/

---