00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00033
00034
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
00043
00044
00045
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
00065
00066
00067
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
00076
00077
00078
00079
00080
00081
00082
00083
00084
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
00097
00098
00099
00100
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
00124
00125
00126
00127
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