Panda3D
 All Classes Functions Variables Enumerations
odeSpace.cxx
00001 // Filename: odeSpace.cxx
00002 // Created by:  joswilso (27Dec06)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
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 // this data is used in auto_collide
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 // get the contact data it looks like so [x1,y1,z1,x2,y2,z2... x64,y64,z64]
00144 // use the return in from autoCollide to determine how much of the data is
00145 // valid. The data would be more straight forward but the callbacks have to be
00146 // static.
00147   return OdeSpace::contact_data[data_index];
00148 }
00149 
00150 int OdeSpace:: 
00151 get_contact_id(int data_index, int first) {
00152 // get the contact data it looks like so [x1,y1,z1,x2,y2,z2... x64,y64,z64]
00153 // use the return in from autoCollide to determine how much of the data is
00154 // valid. The data would be more straight forward but the callbacks have to be
00155 // static.
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 // uses data stored on the world to resolve collisions so you don't have to use near_callbacks in python
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       // this creates contact position data for python. It is useful for debugging only 64 points are stored
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     // Well, while we're in the mood of python exceptions, let's make this one too.
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     // odespace_cat.debug() << "get_default_surface_type the geomId =" << id <<" surfaceType=" << iter->second << "\n";
00313     return iter->second;
00314   }
00315   // odespace_cat.debug() << "get_default_surface_type not in map, returning 0" ;
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   //odespace_cat.debug() << "set_collide_id " << id << " " << _geom_collide_id_map[id] <<"\n";
00331     
00332 /*
00333   GeomCollideIdMap::iterator iter2 = _geom_collide_id_map.begin();
00334   while (iter2 != _geom_collide_id_map.end()) {
00335     odespace_cat.debug() << "set print get_collide_id the geomId =" << iter2->first <<" collide_id=" << iter2->second << "\n";
00336     iter2++;    
00337   }
00338   
00339   get_collide_id(id);
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   GeomCollideIdMap::iterator iter2 = _geom_collide_id_map.begin();
00361   while (iter2 != _geom_collide_id_map.end()) {
00362     odespace_cat.debug() << "get print get_collide_id the geomId =" << iter2->first <<" collide_id=" << iter2->second << "\n";
00363     iter2++;    
00364   }
00365 */
00366   
00367   GeomCollideIdMap::iterator iter = _geom_collide_id_map.find(id);
00368   if (iter != _geom_collide_id_map.end()) {
00369     //odespace_cat.debug() << "get_collide_id the geomId =" << id <<" collide_id=" << iter->second << "\n";
00370     return iter->second;
00371   }
00372 
00373   //odespace_cat.debug() << "get_collide_id not in map, returning 0 id =" << id << "\n" ;
00374   return 0;
00375 }
00376 
 All Classes Functions Variables Enumerations