Panda3D
|
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