Source

pygame / src / pgCollision.c

marcus ed81767 





















marcus f5142b2 
minz 8bc337b 

marcus ed81767 






minz f6ce9e1 
minz bd7a8e4 
minz 8bc337b 



minz 16f240e 





minz 8bc337b 


marcus 91cc95d 
minz f7a1c4a 

marcus 91cc95d 
minz f7a1c4a 







marcus 91cc95d 
marcus ed81767 
minz 1068654 
marcus ed81767 
minz 1068654 
marcus ed81767 












minz 8bc337b 
marcus ed81767 
minz 8bc337b 

marcus 91cc95d 
minz f7a1c4a 
marcus 91cc95d 
minz f7a1c4a 

marcus 91cc95d 
marcus ed81767 
minz 8bc337b 
marcus ed81767 















































minz 658a25b 
minz 8bc337b 
marcus ed81767 


minz 8bc337b 
marcus ed81767 



minz 8bc337b 
marcus ed81767 
minz 8bc337b 
marcus ed81767 







minz 8bc337b 
marcus ed81767 
minz 8bc337b 

marcus ed81767 
minz 11a720f 
marcus 91cc95d 
minz f7a1c4a 




marcus 91cc95d 
minz 7cb9b1a 
minz 38f97de 
minz 7010fd4 
marcus ed81767 






minz 34361b4 
marcus ed81767 



minz 8bc337b 
marcus ed81767 


minz 8bc337b 
marcus ed81767 
minz 2102fd0 
marcus 91cc95d 
minz f7a1c4a 


marcus 91cc95d 
marcus ed81767 






minz 118c8e6 
marcus ed81767 

minz 11a720f 
marcus ed81767 








minz 34361b4 
marcus ed81767 

minz f7a1c4a 
marcus ed81767 


minz f7a1c4a 
marcus ed81767 






minz 34361b4 
minz 38f97de 


minz adf4936 
minz 11a720f 
marcus ed81767 

minz adf4936 
marcus ed81767 















































minz adf4936 
marcus 91cc95d 




marcus ed81767 












minz adf4936 
marcus ed81767 










minz 11a720f 

marcus 91cc95d 


marcus ed81767 

minz 11a720f 
marcus ed81767 



minz 11a720f 

marcus 91cc95d 






marcus ed81767 
minz adf4936 
marcus ed81767 
minz f7a1c4a 
marcus ed81767 


marcus 8f54fd3 
marcus ed81767 
minz 34361b4 
marcus ed81767 



minz adf4936 

marcus 91cc95d 

marcus ed81767 
minz f6ce9e1 
marcus ed81767 
minz f6ce9e1 
/*
  pygame physics - Pygame physics module

  Copyright (C) 2008 Zhang Fan

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

  This 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
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <assert.h>
#include "pgDeclare.h"
#include "pgVector2.h"
#include "pgCollision.h"

static int _LiangBarskey_Internal(double p, double q, double* u1, double* u2);
static void _UpdateV(PyJointObject* joint, double step);
static void _ContactDestroy(PyJointObject* contact);
static PyObject* _ContactNewInternal(PyTypeObject *type, PyObject *args,
    PyObject *kwds);
static PyJointObject* _ContactNew(PyBodyObject* refBody,
    PyBodyObject* incidBody);

// We borrow this graph from Box2D Lite
// Box vertex and edge numbering:
//
//        ^ y
//        |
//        e3
//   v3 ----- v2
//    |        |
// e0 |        | e2  --> x
//    |        |
//   v0 ----- v1
//        e1


/**
 * _LiangBarskey_Internal is an internal function used for Liang-Barskey clipping function
 * 
 *
 * @param p Start point of the directed line segment to be clipped 
 * @param q End point of the directed line segment to be clipped
 * @param u1 Start parameter of clipped line
 * @param u2 End parameter of clipped line
 * @return If there is no overlap, return 0. Otherwise return 1
 * note: ans_p1 = p1 + u1*(p2-p1);
 *       ans_p2 = p2 + u2*(p2-2p);
 * that's why we need u1 and u2.
 */
static int _LiangBarskey_Internal(double p, double q, double* u1, double* u2)
{
    double val;

    if(IS_NEAR_ZERO(p))
    {
        if(q < 0)
            return 0;
        return 1;
    }
    
    val = q/p;
    
    if(p < 0)
        *u1 = MAX(*u1, val);
    else
        *u2 = MIN(*u2, val);

    return 1; 
}

/**
 * update the two bodies' velocity connected by PyContact
 *
 * @param joint It's a PyContact
 * @param step Time step
 */
static void _UpdateV(PyJointObject* joint, double step)
{
    PyContact *contact;
    PyBodyObject *refBody, *incidBody;
    PyVector2 moment, bm;
    PyVector2 refR, incidR;
    PyShapeObject *refShape, *incidShape;

    contact = (PyContact*)joint;
    refBody = (PyBodyObject *)joint->body1;
    incidBody = (PyBodyObject *)joint->body2;
    refShape = (PyShapeObject*) refBody->shape;
    incidShape = (PyShapeObject*) incidBody->shape;

    moment = **(contact->ppAccMoment);
    bm = **(contact->ppSplitAccMoment);

    refR = c_diff(contact->pos, refBody->vecPosition);
    incidR = c_diff(contact->pos, incidBody->vecPosition);

    if(PyVector2_Dot(contact->dv, contact->normal) > 0) return;

    if(!refBody->bStatic)
    {
        refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
            PyVector2_DivideWithReal(moment, refBody->fMass));
        refBody->fAngleVelocity -= PyVector2_Cross(refR, moment)/refShape->rInertia;

        refBody->cBiasLV = c_diff(refBody->cBiasLV,
            PyVector2_DivideWithReal(bm, refBody->fMass));
        refBody->cBiasW -= PyVector2_Cross(refR, bm)/refShape->rInertia;
    }

    if(!incidBody->bStatic)
    {
        incidBody->vecLinearVelocity = c_sum(incidBody->vecLinearVelocity, 
            PyVector2_DivideWithReal(moment, incidBody->fMass));
        incidBody->fAngleVelocity += PyVector2_Cross(incidR, moment)/incidShape->rInertia;

        incidBody->cBiasLV = c_sum(incidBody->cBiasLV,
            PyVector2_DivideWithReal(bm, incidBody->fMass));
        incidBody->cBiasW += PyVector2_Cross(incidR, bm)/incidShape->rInertia;
    }
}

int Collision_LiangBarskey(AABBBox* box, PyVector2* p1, PyVector2* p2, 
    PyVector2* ans_p1, PyVector2* ans_p2)
{
    PyVector2 dp;
    double u1, u2;
	

    u1 = 0.f;
    u2 = 1.f;
    dp = c_diff(*p2, *p1);	//dp = p2 - p1

    if(!_LiangBarskey_Internal(-dp.real, p1->real - box->left, &u1, &u2)) return 0;
    if(!_LiangBarskey_Internal(dp.real, box->right - p1->real, &u1, &u2)) return 0;
    if(!_LiangBarskey_Internal(-dp.imag, p1->imag - box->bottom, &u1, &u2)) return 0;
    if(!_LiangBarskey_Internal(dp.imag, box->top - p1->imag, &u1, &u2)) return 0;

    if(u1 > u2) return 0;

    if(u1 == 0.f)
        *ans_p1 = *p1;
    else
        *ans_p1 = c_sum(*p1, PyVector2_MultiplyWithReal(dp, u1)); //ans_p1 = p1 + u1*dp
    if(u2 == 1.f)
        *ans_p2 = *p2;
    else
        *ans_p2 = c_sum(*p1, PyVector2_MultiplyWithReal(dp, u2)); //ans_p2 = p2 + u2*dp;

    return 1;
}

void Collision_ApplyContact(PyObject* contactObject, double step)
{
/**
 * MAX_C_DEP is the maximal permitted penetrating depth after collision reaction.
 * BIAS_FACTOR is a empirical factor. The two constants are used for position collision
 * after collision reaction is done.
 * for further learning you can read Erin Canto's GDC 2006 Slides, page 23.
 * (You can download it from www.gphysics.com)
 */
#define MAX_C_DEP 0.01
#define BIAS_FACTOR 0.2

    PyVector2 neg_dV, refV, incidV;
    PyVector2 refR, incidR;
    PyContact *contact;
    PyBodyObject *refBody, *incidBody;
    double moment_len;
    PyVector2 moment;
    PyVector2* p;

    double vbias;
    PyVector2 brefV, bincidV, bneg_dV;
    double bm_len;
    PyVector2 bm;

    contact = (PyContact*)contactObject;
    refBody = (PyBodyObject*)contact->joint.body1;
    incidBody = (PyBodyObject*)contact->joint.body2;

    contact->resist = sqrt(refBody->fRestitution*incidBody->fRestitution);

    /*
     * The algorithm below is an implementation of the empirical formula for
	 * impulse-based collision reaction. You can learn the formula thoroughly
	 * from Helmut Garstenauer's thesis, Page 60.
     */
    refR = c_diff(contact->pos, refBody->vecPosition);
    incidR = c_diff(contact->pos, incidBody->vecPosition);
    //dV = v2 + w2xr2 - (v1 + w1xr1)
    incidV = c_sum(incidBody->vecLinearVelocity, PyVector2_fCross(incidBody->fAngleVelocity,
            incidR));
    refV = c_sum(refBody->vecLinearVelocity, PyVector2_fCross(refBody->fAngleVelocity,
            refR));

    contact->dv = c_diff(incidV, refV);
    neg_dV = c_diff(refV, incidV);
	
    moment_len = PyVector2_Dot(PyVector2_MultiplyWithReal(neg_dV, (1 + contact->resist)), 
        contact->normal)/contact->kFactor;
    moment_len = MAX(0, moment_len);
    
    //finally we get the momentum(oh...)
    moment = PyVector2_MultiplyWithReal(contact->normal, moment_len);
    p = *(contact->ppAccMoment);
    p->real += moment.real/contact->weight;
    p->imag += moment.imag/contact->weight; 

    //split impulse
    vbias = BIAS_FACTOR*MAX(0, contact->depth - MAX_C_DEP)/step;
    //biased dv
    bincidV = c_sum(incidBody->cBiasLV, PyVector2_fCross(incidBody->cBiasW, incidR));
    brefV = c_sum(refBody->cBiasLV, PyVector2_fCross(refBody->cBiasW, refR));
    bneg_dV = c_diff(brefV, bincidV); 
    //biased moment
    bm_len = PyVector2_Dot(PyVector2_MultiplyWithReal(bneg_dV, 1.),
        contact->normal)/contact->kFactor;
    bm_len = MAX(0, bm_len + vbias/contact->kFactor);
    bm = PyVector2_MultiplyWithReal(contact->normal, bm_len);
    p = *(contact->ppSplitAccMoment);
    p->real += bm.real/contact->weight;
    p->imag += bm.imag/contact->weight;


#undef MAX_C_DEP
#undef BIAS_FACTOR
}


PyTypeObject PyContact_Type =
{
    PyObject_HEAD_INIT(NULL)
    0,
    "physics.Contact",		/* tp_name */
    sizeof(PyContact),		/* tp_basicsize */
    0,                          /* tp_itemsize */
    0,				/* tp_dealloc */
    0,                          /* tp_print */
    0,                          /* tp_getattr */
    0,                          /* tp_setattr */
    0,                          /* tp_compare */
    0,                          /* tp_repr */
    0,                          /* tp_as_number */
    0,                          /* tp_as_sequence */
    0,                          /* tp_as_mapping */
    0,                          /* tp_hash */
    0,                          /* tp_call */
    0,                          /* tp_str */
    0,                          /* tp_getattro */
    0,                          /* tp_setattro */
    0,                          /* tp_as_buffer */
    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, /*tp_flags*/
    "",                         /* tp_doc */
    0,                          /* tp_traverse */
    0,                          /* tp_clear */
    0,                          /* tp_richcompare */
    0,                          /* tp_weaklistoffset */
    0,                          /* tp_iter */
    0,                          /* tp_iternext */
    0,				/* tp_methods */
    0,				/* tp_members */
    0,				/* tp_getset */
    0,				/* tp_base */
    0,                          /* tp_dict */
    0,                          /* tp_descr_get */
    0,                          /* tp_descr_set */
    0,                          /* tp_dictoffset */
    0,				/* tp_init */
    0,				/* tp_alloc */
    _ContactNewInternal,	/* tp_new */
    0,                          /* tp_free */
    0,                          /* tp_is_gc */
    0,                          /* tp_bases */
    0,                          /* tp_mro */
    0,                          /* tp_cache */
    0,                          /* tp_subclasses */
    0,                          /* tp_weaklist */
    0                           /* tp_del */
};

/**
 * Deallocates the passed PyContact.
 *
 * @param contact The PyContact to deallocate
 */
static void _ContactDestroy(PyJointObject* contact)
{
    PyVector2 **p = ((PyContact*)contact)->ppAccMoment;
    if(p)
    {
        if(*p)
        {
            PyObject_Free(*p);
            *p = NULL;
        }
        PyObject_Free(p);
        p = NULL;
    }

    p = ((PyContact*)contact)->ppSplitAccMoment;
    if(p)
    {
        if(*p)
        {
            PyObject_Free(*p);
            *p = NULL;
        }
        PyObject_Free(p);
        p = NULL;
    }
}

/**
 * Creates a new PyContact object.
 */
static PyObject* _ContactNewInternal(PyTypeObject *type, PyObject *args,
    PyObject *kwds)
{
    PyContact* op;
    type->tp_base = &PyJoint_Type;
    op = (PyContact*)type->tp_alloc(type, 0);
    return (PyObject*)op;
}

/**
 * Creates a new PyContact object and initializes its internals.
 *
 * @param refBody
 * @param incidBody
 * @return A new PyContact.
 */
static PyJointObject* _ContactNew(PyBodyObject* refBody,PyBodyObject* incidBody)
{
    PyContact* contact;

    contact = (PyContact*)_ContactNewInternal(&PyContact_Type, NULL, NULL);
    contact->joint.body1 = (PyObject*)refBody;
    contact->joint.body2 = (PyObject*)incidBody;
    contact->joint.SolveConstraints = _UpdateV;
    contact->joint.Destroy = _ContactDestroy;

    contact->ppAccMoment = NULL;
    contact->ppSplitAccMoment = NULL;

    return (PyJointObject*)contact;
}

/* Internally used PyContact functions */

PyObject* PyContact_New(PyBodyObject* refBody, PyBodyObject* incidBody)
{
    return (PyObject*)_ContactNew (refBody, incidBody);
}