Panda3D
 All Classes Functions Variables Enumerations
odeUtil.cxx
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 
 All Classes Functions Variables Enumerations