bulletml/_collision.c

   1 #include "Python.h"
   2
   3 #define STR_AND_SIZE(s) s, sizeof(s) - 1
   4 #define DOT(x1, y1, x2, y2) ((x1) * (x2) + (y1) * (y2))
   5 #define NEARZERO(d) ((d) < 0.0001 && (d) > -0.0001)
   6
   7 static const char *s_pchModDoc = "Optimized collision detection functions.";
   8
   9 static PyObject *s_ppykX;
  10 static PyObject *s_ppykY;
  11 static PyObject *s_ppykPX;
  12 static PyObject *s_ppykPY;
  13 static PyObject *s_ppykRadius;
  14
  15 static const char s_achOverlapsDoc[] = (
  16     "Return true if two circles are overlapping.\n\n"
  17     "Usually, you\'ll want to use the \'collides\' method instead, but\n"
  18     "this one can be useful for just checking to see if the player has\n"
  19     "entered an area or hit a stationary oject.\n\n"
  20     "(This function is optimized.)\n\n");
  21
  22 static const char s_achCollidesDoc[] = (
  23     "Return true if the two moving circles collide.\n\n"
  24     "The circles should have the following attributes:\n\n"
  25     "    x, y - required, current position\n"
  26     "    px, py - not required, defaults to x, y, previous frame position\n"
  27     "    radius - not required, defaults to 0.5\n\n"
  28     "(This function is optimized.)\n\n");
  29
  30 static const char s_achCollidesAllDoc[] = (
  31     "Filter the second argument to those that collide with the first.\n\n"
  32     "This is equivalent to filter(lambda o: collides(a, o), others),\n"
  33     "but is much faster when the compiled extension is available (which\n"
  34     "it is currently).\n\n");
  35
  36 // Get the attributes from a Python moving circle object.
  37 static int GetCircle(PyObject *ppy, double *pdX, double *pdY,
  38                      double *pdPX, double *pdPY, double *pdR)
  39 {
  40     PyObject *ppyf;
  41
  42     if (!ppy)
  43         return 0;
  44
  45     ppyf = PyObject_GetAttr(ppy, s_ppykX);
  46     if (ppyf)
  47     {
  48         *pdX = PyFloat_AsDouble(ppyf);
  49         Py_DECREF(ppyf);
  50     }
  51
  52     ppyf = PyObject_GetAttr(ppy, s_ppykY);
  53     if (ppyf)
  54     {
  55         *pdY = PyFloat_AsDouble(ppyf);
  56         Py_DECREF(ppyf);
  57     }
  58
  59     // Catch X or Y or failure to convert, any one of the four cases
  60     // is equally fatal. We don't need to check after each one.
  61     if (PyErr_Occurred())
  62         return 0;
  63
  64     ppyf = PyObject_GetAttr(ppy, s_ppykPX);
  65     if (ppyf)
  66     {
  67         *pdPX = PyFloat_AsDouble(ppyf);
  68         Py_DECREF(ppyf);
  69         if (PyErr_Occurred())
  70             return 0;
  71     }
  72     else
  73     {
  74         PyErr_Clear();
  75         *pdPX = *pdX;
  76     }
  77
  78     ppyf = PyObject_GetAttr(ppy, s_ppykPY);
  79     if (ppyf)
  80     {
  81         *pdPY = PyFloat_AsDouble(ppyf);
  82         Py_DECREF(ppyf);
  83         if (PyErr_Occurred())
  84             return 0;
  85     }
  86     else
  87     {
  88         PyErr_Clear();
  89         *pdPY = *pdY;
  90     }
  91
  92     ppyf = PyObject_GetAttr(ppy, s_ppykRadius);
  93     if (ppyf)
  94     {
  95         *pdR = PyFloat_AsDouble(ppyf);
  96         Py_DECREF(ppyf);
  97         if (PyErr_Occurred())
  98             return 0;
  99     }
 100     else
 101     {
 102         PyErr_Clear();
 103         *pdR = 0.5;
 104     }
 105
 106     return 1;
 107 }
 108
 109 static int Collides(double dXA, double dXB, double dYA, double dYB,
 110                     double dPXA, double dPXB, double dPYA, double dPYB,
 111                     double dRA, double dRB)
 112 {
 113     // Translate B's position to be relative to A's start.
 114     double dDirX = dPXA + (dXB - dXA) - dPXB;
 115     double dDirY = dPYA + (dYB - dYA) - dPYB;
 116     // Now A doesn't move. Treat B as a point by summing the radii.
 117     double dR = dRA + dRB;
 118     // Now the problem is just circle/line collision.
 119
 120     double dDiffX = dPXA - dPXB;
 121     double dDiffY = dPYA - dPYB;
 122
 123     // B didn't move relative to A, so early-out by doing point/circle.
 124     if (NEARZERO(dDirX) && NEARZERO(dDirY))
 125         return dDiffX * dDiffX + dDiffY * dDiffY <= dR * dR;
 126     else
 127     {
 128         double dT = (DOT(dDiffX, dDiffY, dDirX, dDirY)
 129                      / DOT(dDirX, dDirY, dDirX, dDirY));
 130         double dDistX;
 131         double dDistY;
 132         if (dT < 0.0) dT = 0.0;
 133         else if (dT > 1.0) dT = 1.0;
 134
 135         dDistX = dPXA - (dPXB + dDirX * dT);
 136         dDistY = dPYA - (dPYB + dDirY * dT);
 137
 138         return dDistX * dDistX + dDistY * dDistY <= dR * dR;
 139     }
 140 }
 141
 142 static PyObject *py_overlaps(PyObject *ppySelf, PyObject *ppyArgs) {
 143     double dXA, dYA, dPXA, dPYA, dRA;
 144     double dXB, dYB, dPXB, dPYB, dRB;
 145     PyObject *ppyA, *ppyB;
 146     if (PyArg_ParseTuple(ppyArgs, "OO", &ppyA, &ppyB)
 147         && GetCircle(ppyA, &dXA, &dYA, &dPXA, &dPYA, &dRA)
 148         && GetCircle(ppyB, &dXB, &dYB, &dPXB, &dPYB, &dRB))
 149     {
 150         double dX = dXA - dXB;
 151         double dY = dYA - dYB;
 152         double dR = dRA + dRB;
 153
 154         if (dX * dX + dY * dY <= dR * dR)
 155         {
 156             Py_RETURN_TRUE;
 157         }
 158         else
 159         {
 160             Py_RETURN_FALSE;
 161         }
 162     }
 163     else
 164         return NULL;
 165 }
 166
 167 static PyObject *py_collides(PyObject *ppySelf, PyObject *ppyArgs)
 168 {
 169     double dXA, dYA, dPXA, dPYA, dRA;
 170     double dXB, dYB, dPXB, dPYB, dRB;
 171     PyObject *ppyA, *ppyB;
 172     if (PyArg_ParseTuple(ppyArgs, "OO", &ppyA, &ppyB)
 173         && GetCircle(ppyA, &dXA, &dYA, &dPXA, &dPYA, &dRA)
 174         && GetCircle(ppyB, &dXB, &dYB, &dPXB, &dPYB, &dRB))
 175     {
 176         if (Collides(dXA, dXB, dYA, dYB, dPXA, dPXB, dPYA, dPYB, dRA, dRB))
 177         {
 178             Py_RETURN_TRUE;
 179         }
 180         else
 181         {
 182             Py_RETURN_FALSE;
 183         }
 184     }
 185     else
 186         return NULL;
 187 }
 188
 189 static PyObject *py_collides_all(PyObject *ppySelf, PyObject *ppyArgs)
 190 {
 191     double dXA, dYA, dPXA, dPYA, dRA;
 192     PyObject *ppyA, *ppyOthers;
 193     if (PyArg_ParseTuple(ppyArgs, "OO", &ppyA, &ppyOthers)
 194         && GetCircle(ppyA, &dXA, &dYA, &dPXA, &dPYA, &dRA))
 195     {
 196         PyObject *ppyRet = PyList_New(0);
 197         Py_ssize_t pyszLen = ppyRet ? PySequence_Length(ppyOthers) : -1;
 198         if (pyszLen >= 0)
 199         {
 200             Py_ssize_t sz;
 201             for (sz = 0; sz < pyszLen; sz++)
 202             {
 203                 double dXB, dYB, dPXB, dPYB, dRB;
 204                 PyObject *ppyB = PySequence_GetItem(ppyOthers, sz);
 205                 if (!GetCircle(ppyB, &dXB, &dYB, &dPXB, &dPYB, &dRB))
 206                 {
 207                     Py_XDECREF(ppyB);
 208                     return NULL;
 209                 }
 210                 else if (Collides(dXA, dXB, dYA, dYB, dPXA, dPXB, dPYA, dPYB,
 211                                   dRA, dRB))
 212                     PyList_Append(ppyRet, ppyB);
 213                 Py_DECREF(ppyB);
 214             }
 215             return ppyRet;
 216         }
 217         else
 218             return NULL;
 219     }
 220     else
 221         return NULL;
 222 }
 223
 224 static struct PyMethodDef s_apymeth[] = {
 225     {"overlaps", py_overlaps, METH_VARARGS, s_achOverlapsDoc },
 226     {"collides", py_collides, METH_VARARGS, s_achCollidesDoc },
 227     {"collides_all", py_collides_all, METH_VARARGS, s_achCollidesAllDoc },
 228     {NULL, NULL, 0, NULL}
 229 };
 230
 231 PyMODINIT_FUNC init_collision(void)
 232 {
 233     s_ppykX = PyString_FromStringAndSize(STR_AND_SIZE("x"));
 234     s_ppykY = PyString_FromStringAndSize(STR_AND_SIZE("y"));
 235     s_ppykPX = PyString_FromStringAndSize(STR_AND_SIZE("px"));
 236     s_ppykPY = PyString_FromStringAndSize(STR_AND_SIZE("py"));
 237     s_ppykRadius = PyString_FromStringAndSize(STR_AND_SIZE("radius"));
 238
 239     if (s_ppykX && s_ppykY && s_ppykPX && s_ppykPY && s_ppykRadius)
 240         Py_InitModule3("bulletml._collision", s_apymeth, s_pchModDoc);
 241 }