Panda3D
|
00001 // Filename: odeUtil.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 "odeUtil.h" 00016 00017 #ifdef HAVE_PYTHON 00018 #include "py_panda.h" 00019 #include "typedReferenceCount.h" 00020 #ifndef CPPPARSER 00021 extern EXPCL_PANDAODE Dtool_PyTypedObject Dtool_OdeGeom; 00022 #endif 00023 #endif 00024 00025 dReal OdeUtil::OC_infinity = dInfinity; 00026 00027 #ifdef HAVE_PYTHON 00028 PyObject* OdeUtil::_python_callback = NULL; 00029 #endif 00030 00031 //////////////////////////////////////////////////////////////////// 00032 // Function: OdeUtil::get_connecting_joint 00033 // Access: Public, Static 00034 // Description: Returns the joint that connects the given bodies. 00035 //////////////////////////////////////////////////////////////////// 00036 OdeJoint OdeUtil:: 00037 get_connecting_joint(const OdeBody &body1, const OdeBody &body2) { 00038 return OdeJoint(dConnectingJoint(body1.get_id(),body2.get_id())); 00039 } 00040 00041 //////////////////////////////////////////////////////////////////// 00042 // Function: OdeUtil::get_connecting_joint_list 00043 // Access: Public, Static 00044 // Description: Returns a collection of joints connecting the 00045 // specified bodies. 00046 //////////////////////////////////////////////////////////////////// 00047 OdeJointCollection OdeUtil:: 00048 get_connecting_joint_list(const OdeBody &body1, const OdeBody &body2) { 00049 const int max_possible_joints = min(body1.get_num_joints(), body1.get_num_joints()); 00050 00051 dJointID *joint_list = (dJointID *)PANDA_MALLOC_ARRAY(max_possible_joints * sizeof(dJointID)); 00052 int num_joints = dConnectingJointList(body1.get_id(), body2.get_id(), 00053 joint_list); 00054 OdeJointCollection joints; 00055 for (int i = 0; i < num_joints; i++) { 00056 joints.add_joint(OdeJoint(joint_list[i])); 00057 } 00058 00059 PANDA_FREE_ARRAY(joint_list); 00060 return joints; 00061 } 00062 00063 //////////////////////////////////////////////////////////////////// 00064 // Function: OdeUtil::are_connected 00065 // Access: Public, Static 00066 // Description: Returns 1 if the given bodies are connected 00067 // by a joint, returns 0 otherwise. 00068 //////////////////////////////////////////////////////////////////// 00069 int OdeUtil:: 00070 are_connected(const OdeBody &body1, const OdeBody &body2) { 00071 return dAreConnected(body1.get_id(),body2.get_id()); 00072 } 00073 00074 //////////////////////////////////////////////////////////////////// 00075 // Function: OdeUtil::are_connected_excluding 00076 // Access: Public, Static 00077 // Description: Returns 1 if the given bodies are connected 00078 // by a joint that does not match the given 00079 // joint_type, returns 0 otherwise. This is useful 00080 // for deciding whether to add contact joints between 00081 // two bodies: if they are already connected by 00082 // non-contact joints then it may not be appropriate 00083 // to add contacts, however it is okay to add more 00084 // contact between bodies that already have contacts. 00085 //////////////////////////////////////////////////////////////////// 00086 int OdeUtil:: 00087 are_connected_excluding(const OdeBody &body1, 00088 const OdeBody &body2, 00089 const int joint_type) { 00090 return dAreConnectedExcluding(body1.get_id(), 00091 body2.get_id(), 00092 joint_type); 00093 } 00094 00095 //////////////////////////////////////////////////////////////////// 00096 // Function: OdeUtil::collide 00097 // Access: Public, Static 00098 // Description: Given two geometry objects that potentially touch 00099 // (geom1 and geom2), generate contact information 00100 // for them. Returns an OdeCollisionEntry. 00101 //////////////////////////////////////////////////////////////////// 00102 PT(OdeCollisionEntry) OdeUtil:: 00103 collide(const OdeGeom &geom1, const OdeGeom &geom2, const short int max_contacts) { 00104 dContactGeom *contact_list = (dContactGeom *)PANDA_MALLOC_ARRAY(max_contacts * sizeof(dContactGeom)); 00105 int num_contacts = dCollide(geom1.get_id(), geom2.get_id(), max_contacts, contact_list, sizeof(dContactGeom)); 00106 PT(OdeCollisionEntry) entry = new OdeCollisionEntry(); 00107 entry->_geom1 = geom1.get_id(); 00108 entry->_geom2 = geom2.get_id(); 00109 entry->_body1 = dGeomGetBody(geom1.get_id()); 00110 entry->_body2 = dGeomGetBody(geom2.get_id()); 00111 entry->_num_contacts = num_contacts; 00112 entry->_contact_geoms = new OdeContactGeom[num_contacts]; 00113 for (int i = 0; i < num_contacts; i++) { 00114 entry->_contact_geoms[i] = contact_list[i]; 00115 } 00116 00117 PANDA_FREE_ARRAY(contact_list); 00118 return entry; 00119 } 00120 00121 #ifdef HAVE_PYTHON 00122 //////////////////////////////////////////////////////////////////// 00123 // Function: OdeUtil::collide2 00124 // Access: Public, Static 00125 // Description: Calls the callback for all potentially intersecting 00126 // pairs that contain one geom from geom1 and one geom 00127 // from geom2. 00128 //////////////////////////////////////////////////////////////////// 00129 int OdeUtil:: 00130 collide2(const OdeGeom &geom1, const OdeGeom &geom2, PyObject* arg, PyObject* callback) { 00131 nassertr(callback != NULL, -1); 00132 if (!PyCallable_Check(callback)) { 00133 PyErr_Format(PyExc_TypeError, "'%s' object is not callable", callback->ob_type->tp_name); 00134 return -1; 00135 } else { 00136 _python_callback = (PyObject*) callback; 00137 Py_XINCREF(_python_callback); 00138 dSpaceCollide2(geom1.get_id(), geom2.get_id(), (void*) arg, &near_callback); 00139 Py_XDECREF(_python_callback); 00140 return 0; 00141 } 00142 } 00143 00144 void OdeUtil:: 00145 near_callback(void *data, dGeomID o1, dGeomID o2) { 00146 ode_cat.spam() << "near_callback called, data: " << data << ", dGeomID1: " << o1 << ", dGeomID2: " << o2 << "\n"; 00147 OdeGeom g1 (o1); 00148 OdeGeom g2 (o2); 00149 PyObject* p1 = DTool_CreatePyInstanceTyped(&g1, Dtool_OdeGeom, true, false, g1.get_type_index()); 00150 PyObject* p2 = DTool_CreatePyInstanceTyped(&g2, Dtool_OdeGeom, true, false, g2.get_type_index()); 00151 PyObject* result = PyEval_CallFunction(_python_callback, "OOO", (PyObject*) data, p1, p2); 00152 if (!result) { 00153 ode_cat.error() << "An error occurred while calling python function!\n"; 00154 PyErr_Print(); 00155 } 00156 } 00157 #endif 00158 00159 OdeGeom OdeUtil:: 00160 space_to_geom(const OdeSpace &space) { 00161 return OdeGeom((dGeomID)space.get_id()); 00162 } 00163