00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "config_ode.h"
00016 #include "odeSpace.h"
00017
00018 #include "throw_event.h"
00019
00020 #ifdef HAVE_PYTHON
00021 #include "py_panda.h"
00022 #include "typedReferenceCount.h"
00023 #ifndef CPPPARSER
00024 extern EXPCL_PANDAODE Dtool_PyTypedObject Dtool_OdeGeom;
00025 #endif
00026 #endif
00027
00028 TypeHandle OdeSpace::_type_handle;
00029
00030 const int OdeSpace::MAX_CONTACTS = 16;
00031 OdeWorld* OdeSpace::_collide_world;
00032 OdeSpace* OdeSpace::_collide_space;
00033 dJointGroupID OdeSpace::_collide_joint_group;
00034 int OdeSpace::contactCount = 0;
00035 double OdeSpace::contact_data[192];
00036 int OdeSpace::contact_ids[128];
00037 #ifdef HAVE_PYTHON
00038 PyObject* OdeSpace::_python_callback = NULL;
00039 #endif
00040
00041 OdeSpace::
00042 OdeSpace(dSpaceID id) :
00043 _id(id) {
00044 my_world = NULL;
00045 }
00046
00047 OdeSpace::
00048 ~OdeSpace() {
00049 }
00050
00051 void OdeSpace::
00052 destroy() {
00053 nassertv(_id);
00054 dSpaceDestroy(_id);
00055 }
00056
00057 int OdeSpace::
00058 query(const OdeGeom& geom) const {
00059 nassertr(_id, 0);
00060 return dSpaceQuery(_id, geom.get_id());
00061 }
00062
00063 int OdeSpace::
00064 query(const OdeSpace& space) const {
00065 nassertr(_id, 0);
00066 return dSpaceQuery(_id, (dGeomID)space.get_id());
00067 }
00068
00069 void OdeSpace::
00070 add(OdeSpace& space) {
00071 nassertv(_id);
00072 dSpaceAdd(_id, (dGeomID)space.get_id());
00073 }
00074
00075 void OdeSpace::
00076 remove(OdeSpace& space) {
00077 nassertv(_id);
00078 dSpaceRemove(_id, (dGeomID)space.get_id());
00079 }
00080
00081 void OdeSpace::
00082 add(OdeGeom& geom) {
00083 nassertv(_id);
00084 dSpaceAdd(_id, geom.get_id());
00085 }
00086
00087 void OdeSpace::
00088 remove(OdeGeom& geom) {
00089 nassertv(_id);
00090 dSpaceRemove(_id, geom.get_id());
00091 }
00092
00093 void OdeSpace::
00094 clean() {
00095 nassertv(_id);
00096 dSpaceClean(_id);
00097 }
00098
00099 OdeGeom OdeSpace::
00100 get_geom(int i) {
00101 nassertr(_id, OdeGeom(0));
00102 return OdeGeom(dSpaceGetGeom(_id, i));
00103 }
00104
00105
00106 void OdeSpace::
00107 write(ostream &out, unsigned int indent) const {
00108 out.width(indent); out << "" << get_type() << "(id = " << _id << ")";
00109 }
00110
00111 OdeSpace::
00112 operator bool () const {
00113 return (_id != NULL);
00114 }
00115
00116 void OdeSpace::
00117 set_auto_collide_world(OdeWorld &world) {
00118 my_world = &world;
00119 }
00120
00121 void OdeSpace::
00122 set_auto_collide_joint_group(OdeJointGroup &joint_group) {
00123 _collide_joint_group = joint_group.get_id();
00124 }
00125
00126 int OdeSpace::
00127 auto_collide() {
00128 if (my_world == NULL) {
00129 odespace_cat.error() << "No collide world has been set!\n";
00130 return 0;
00131 } else {
00132 nassertr(_id, 0);
00133 OdeSpace::contactCount = 0;
00134 _collide_space = this;
00135 _collide_world = my_world;
00136 dSpaceCollide(_id, this, &auto_callback);
00137 return OdeSpace::contactCount;
00138 }
00139 }
00140
00141 double OdeSpace::
00142 get_contact_data(int data_index) {
00143
00144
00145
00146
00147 return OdeSpace::contact_data[data_index];
00148 }
00149
00150 int OdeSpace::
00151 get_contact_id(int data_index, int first) {
00152
00153
00154
00155
00156 if (first == 0) {
00157 return OdeSpace::contact_ids[(data_index * 2) + 0];
00158 } else {
00159 return OdeSpace::contact_ids[(data_index * 2) + 1];
00160 }
00161 }
00162
00163 void OdeSpace::
00164 auto_callback(void *data, dGeomID o1, dGeomID o2) {
00165
00166 int i;
00167 static int autoCallbackCounter = 0;
00168 dBodyID b1 = dGeomGetBody(o1);
00169 dBodyID b2 = dGeomGetBody(o2);
00170
00171 dContact contact[OdeSpace::MAX_CONTACTS];
00172
00173 int surface1 = _collide_space->get_surface_type(o1);
00174 int surface2 = _collide_space->get_surface_type(o2);
00175
00176 nassertv(_collide_world != NULL);
00177 sSurfaceParams collide_params;
00178 collide_params = _collide_world->get_surface(surface1, surface2);
00179
00180 for (i=0; i < OdeSpace::MAX_CONTACTS; i++) {
00181 contact[i].surface.mode = collide_params.colparams.mode;
00182 contact[i].surface.mu = collide_params.colparams.mu;
00183 contact[i].surface.mu2 = collide_params.colparams.mu2;
00184 contact[i].surface.bounce = collide_params.colparams.bounce;
00185 contact[i].surface.bounce_vel = collide_params.colparams.bounce_vel;
00186 contact[i].surface.soft_cfm = collide_params.colparams.soft_cfm;
00187 }
00188
00189 static int numc = 0;
00190 numc = dCollide(o1, o2, OdeSpace::MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
00191
00192 if (numc) {
00193 if (odespace_cat.is_debug() && (autoCallbackCounter%30 == 0)) {
00194 odespace_cat.debug() << autoCallbackCounter <<" collision between geoms " << o1 << " and " << o2 << "\n";
00195 odespace_cat.debug() << "collision between body " << b1 << " and " << b2 << "\n";
00196 odespace_cat.debug() << "surface1= "<< surface1 << " surface2=" << surface2 << "\n";
00197 }
00198 autoCallbackCounter += 1;
00199
00200 PT(OdeCollisionEntry) entry;
00201 if (!_collide_space->_collision_event.empty()) {
00202 entry = new OdeCollisionEntry;
00203 entry->_geom1 = o1;
00204 entry->_geom2 = o2;
00205 entry->_body1 = b1;
00206 entry->_body2 = b2;
00207 entry->_num_contacts = numc;
00208 entry->_contact_geoms = new OdeContactGeom[numc];
00209 }
00210
00211 for(i=0; i < numc; i++) {
00212 dJointID c = dJointCreateContact(_collide_world->get_id(), _collide_joint_group, contact + i);
00213 if ((_collide_space->get_collide_id(o1) >= 0) && (_collide_space->get_collide_id(o2) >= 0)) {
00214 dJointAttach(c, b1, b2);
00215 }
00216 if (!_collide_space->_collision_event.empty()) {
00217 entry->_contact_geoms[i] = contact[i].geom;
00218 }
00219
00220 if(contactCount < 64) {
00221 OdeSpace::contact_data[0 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[0];
00222 OdeSpace::contact_data[1 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[1];
00223 OdeSpace::contact_data[2 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[2];
00224 OdeSpace::contact_ids[0 + (OdeSpace::contactCount * 2)] = _collide_space->get_collide_id(o1);
00225 OdeSpace::contact_ids[1 + (OdeSpace::contactCount * 2)] = _collide_space->get_collide_id(o2);
00226 OdeSpace::contactCount += 1;
00227 }
00228 }
00229 _collide_world->set_dampen_on_bodies(b1, b2, collide_params.dampen);
00230
00231 if (!_collide_space->_collision_event.empty()) {
00232 throw_event(_collide_space->_collision_event, EventParameter(entry));
00233 }
00234 }
00235 }
00236
00237 #ifdef HAVE_PYTHON
00238 int OdeSpace::
00239 collide(PyObject* arg, PyObject* callback) {
00240 nassertr(callback != NULL, -1);
00241 if (!PyCallable_Check(callback)) {
00242 PyErr_Format(PyExc_TypeError, "'%s' object is not callable", callback->ob_type->tp_name);
00243 return -1;
00244 } else if (_id == NULL) {
00245
00246 PyErr_Format(PyExc_TypeError, "OdeSpace is not valid!");
00247 return -1;
00248 } else {
00249 OdeSpace::_python_callback = (PyObject*) callback;
00250 Py_XINCREF(OdeSpace::_python_callback);
00251 dSpaceCollide(_id, (void*) arg, &near_callback);
00252 Py_XDECREF(OdeSpace::_python_callback);
00253 return 0;
00254 }
00255 }
00256
00257 void OdeSpace::
00258 near_callback(void *data, dGeomID o1, dGeomID o2) {
00259 OdeGeom *g1 = new OdeGeom(o1);
00260 OdeGeom *g2 = new OdeGeom(o2);
00261 PyObject *p1 = DTool_CreatePyInstanceTyped(g1, Dtool_OdeGeom, true, false, g1->get_type_index());
00262 PyObject *p2 = DTool_CreatePyInstanceTyped(g2, Dtool_OdeGeom, true, false, g2->get_type_index());
00263 PyObject *result = PyEval_CallFunction(_python_callback, "OOO", (PyObject*) data, p1, p2);
00264 if (!result) {
00265 odespace_cat.error() << "An error occurred while calling python function!\n";
00266 PyErr_Print();
00267 } else {
00268 Py_DECREF(result);
00269 }
00270 Py_XDECREF(p2);
00271 Py_XDECREF(p1);
00272 }
00273 #endif
00274
00275 OdeSimpleSpace OdeSpace::
00276 convert_to_simple_space() const {
00277 nassertr(_id != 0, OdeSimpleSpace((dSpaceID)0));
00278 nassertr(get_class() == OdeGeom::GC_simple_space, OdeSimpleSpace((dSpaceID)0));
00279 return OdeSimpleSpace(_id);
00280 }
00281
00282 OdeHashSpace OdeSpace::
00283 convert_to_hash_space() const {
00284 nassertr(_id != 0, OdeHashSpace((dSpaceID)0));
00285 nassertr(get_class() == OdeGeom::GC_hash_space, OdeHashSpace((dSpaceID)0));
00286 return OdeHashSpace(_id);
00287 }
00288
00289 OdeQuadTreeSpace OdeSpace::
00290 convert_to_quad_tree_space() const {
00291 nassertr(_id != 0, OdeQuadTreeSpace((dSpaceID)0));
00292 nassertr(get_class() == OdeGeom::GC_quad_tree_space, OdeQuadTreeSpace((dSpaceID)0));
00293 return OdeQuadTreeSpace(_id);
00294 }
00295
00296
00297 void OdeSpace::
00298 set_surface_type( int surfaceType, dGeomID id){
00299 _geom_surface_map[id]= surfaceType;
00300 }
00301
00302 void OdeSpace::
00303 set_surface_type(OdeGeom& geom, int surfaceType){
00304 dGeomID id = geom.get_id();
00305 set_surface_type(surfaceType, id);
00306 }
00307
00308 int OdeSpace::
00309 get_surface_type(dGeomID id) {
00310 GeomSurfaceMap::iterator iter = _geom_surface_map.find(id);
00311 if (iter != _geom_surface_map.end()) {
00312
00313 return iter->second;
00314 }
00315
00316 return 0;
00317 }
00318
00319 int OdeSpace::
00320 get_surface_type(OdeGeom& geom) {
00321 dGeomID id = geom.get_id();
00322 return get_surface_type(id);
00323 }
00324
00325
00326 int OdeSpace::
00327 set_collide_id( int collide_id, dGeomID id) {
00328 _geom_collide_id_map[id]= collide_id;
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341 return _geom_collide_id_map[id];
00342 }
00343
00344 int OdeSpace::
00345 set_collide_id( OdeGeom& geom, int collide_id) {
00346 dGeomID id = geom.get_id();
00347 return set_collide_id(collide_id, id);
00348 }
00349
00350 int OdeSpace::
00351 get_collide_id(OdeGeom& geom) {
00352 dGeomID id = geom.get_id();
00353 return get_collide_id(id);
00354 }
00355
00356
00357 int OdeSpace::
00358 get_collide_id(dGeomID id) {
00359
00360
00361
00362
00363
00364
00365
00366
00367 GeomCollideIdMap::iterator iter = _geom_collide_id_map.find(id);
00368 if (iter != _geom_collide_id_map.end()) {
00369
00370 return iter->second;
00371 }
00372
00373
00374 return 0;
00375 }
00376