164 lines
6.2 KiB
C++
Executable File
164 lines
6.2 KiB
C++
Executable File
// Filename: odeUtil.cxx
|
|
// Created by: joswilso (27Dec06)
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
//
|
|
// PANDA 3D SOFTWARE
|
|
// Copyright (c) Carnegie Mellon University. All rights reserved.
|
|
//
|
|
// All use of this software is subject to the terms of the revised BSD
|
|
// license. You should have received a copy of this license along
|
|
// with this source code in a file named "LICENSE."
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
|
|
#include "odeUtil.h"
|
|
|
|
#ifdef HAVE_PYTHON
|
|
#include "py_panda.h"
|
|
#include "typedReferenceCount.h"
|
|
#ifndef CPPPARSER
|
|
extern EXPCL_PANDAODE Dtool_PyTypedObject Dtool_OdeGeom;
|
|
#endif
|
|
#endif
|
|
|
|
dReal OdeUtil::OC_infinity = dInfinity;
|
|
|
|
#ifdef HAVE_PYTHON
|
|
PyObject* OdeUtil::_python_callback = NULL;
|
|
#endif
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::get_connecting_joint
|
|
// Access: Public, Static
|
|
// Description: Returns the joint that connects the given bodies.
|
|
////////////////////////////////////////////////////////////////////
|
|
OdeJoint OdeUtil::
|
|
get_connecting_joint(const OdeBody &body1, const OdeBody &body2) {
|
|
return OdeJoint(dConnectingJoint(body1.get_id(),body2.get_id()));
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::get_connecting_joint_list
|
|
// Access: Public, Static
|
|
// Description: Returns a collection of joints connecting the
|
|
// specified bodies.
|
|
////////////////////////////////////////////////////////////////////
|
|
OdeJointCollection OdeUtil::
|
|
get_connecting_joint_list(const OdeBody &body1, const OdeBody &body2) {
|
|
const int max_possible_joints = min(body1.get_num_joints(), body1.get_num_joints());
|
|
|
|
dJointID *joint_list = (dJointID *)PANDA_MALLOC_ARRAY(max_possible_joints * sizeof(dJointID));
|
|
int num_joints = dConnectingJointList(body1.get_id(), body2.get_id(),
|
|
joint_list);
|
|
OdeJointCollection joints;
|
|
for (int i = 0; i < num_joints; i++) {
|
|
joints.add_joint(OdeJoint(joint_list[i]));
|
|
}
|
|
|
|
PANDA_FREE_ARRAY(joint_list);
|
|
return joints;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::are_connected
|
|
// Access: Public, Static
|
|
// Description: Returns 1 if the given bodies are connected
|
|
// by a joint, returns 0 otherwise.
|
|
////////////////////////////////////////////////////////////////////
|
|
int OdeUtil::
|
|
are_connected(const OdeBody &body1, const OdeBody &body2) {
|
|
return dAreConnected(body1.get_id(),body2.get_id());
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::are_connected_excluding
|
|
// Access: Public, Static
|
|
// Description: Returns 1 if the given bodies are connected
|
|
// by a joint that does not match the given
|
|
// joint_type, returns 0 otherwise. This is useful
|
|
// for deciding whether to add contact joints between
|
|
// two bodies: if they are already connected by
|
|
// non-contact joints then it may not be appropriate
|
|
// to add contacts, however it is okay to add more
|
|
// contact between bodies that already have contacts.
|
|
////////////////////////////////////////////////////////////////////
|
|
int OdeUtil::
|
|
are_connected_excluding(const OdeBody &body1,
|
|
const OdeBody &body2,
|
|
const int joint_type) {
|
|
return dAreConnectedExcluding(body1.get_id(),
|
|
body2.get_id(),
|
|
joint_type);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::collide
|
|
// Access: Public, Static
|
|
// Description: Given two geometry objects that potentially touch
|
|
// (geom1 and geom2), generate contact information
|
|
// for them. Returns an OdeCollisionEntry.
|
|
////////////////////////////////////////////////////////////////////
|
|
PT(OdeCollisionEntry) OdeUtil::
|
|
collide(const OdeGeom &geom1, const OdeGeom &geom2, const short int max_contacts) {
|
|
dContactGeom *contact_list = (dContactGeom *)PANDA_MALLOC_ARRAY(max_contacts * sizeof(dContactGeom));
|
|
int num_contacts = dCollide(geom1.get_id(), geom2.get_id(), max_contacts, contact_list, sizeof(dContactGeom));
|
|
PT(OdeCollisionEntry) entry = new OdeCollisionEntry();
|
|
entry->_geom1 = geom1.get_id();
|
|
entry->_geom2 = geom2.get_id();
|
|
entry->_body1 = dGeomGetBody(geom1.get_id());
|
|
entry->_body2 = dGeomGetBody(geom2.get_id());
|
|
entry->_num_contacts = num_contacts;
|
|
entry->_contact_geoms = new OdeContactGeom[num_contacts];
|
|
for (int i = 0; i < num_contacts; i++) {
|
|
entry->_contact_geoms[i] = contact_list[i];
|
|
}
|
|
|
|
PANDA_FREE_ARRAY(contact_list);
|
|
return entry;
|
|
}
|
|
|
|
#ifdef HAVE_PYTHON
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: OdeUtil::collide2
|
|
// Access: Public, Static
|
|
// Description: Calls the callback for all potentially intersecting
|
|
// pairs that contain one geom from geom1 and one geom
|
|
// from geom2.
|
|
////////////////////////////////////////////////////////////////////
|
|
int OdeUtil::
|
|
collide2(const OdeGeom &geom1, const OdeGeom &geom2, PyObject* arg, PyObject* callback) {
|
|
nassertr(callback != NULL, -1);
|
|
if (!PyCallable_Check(callback)) {
|
|
PyErr_Format(PyExc_TypeError, "'%s' object is not callable", callback->ob_type->tp_name);
|
|
return -1;
|
|
} else {
|
|
_python_callback = (PyObject*) callback;
|
|
Py_XINCREF(_python_callback);
|
|
dSpaceCollide2(geom1.get_id(), geom2.get_id(), (void*) arg, &near_callback);
|
|
Py_XDECREF(_python_callback);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void OdeUtil::
|
|
near_callback(void *data, dGeomID o1, dGeomID o2) {
|
|
ode_cat.spam() << "near_callback called, data: " << data << ", dGeomID1: " << o1 << ", dGeomID2: " << o2 << "\n";
|
|
OdeGeom g1 (o1);
|
|
OdeGeom g2 (o2);
|
|
PyObject* p1 = DTool_CreatePyInstanceTyped(&g1, Dtool_OdeGeom, true, false, g1.get_type_index());
|
|
PyObject* p2 = DTool_CreatePyInstanceTyped(&g2, Dtool_OdeGeom, true, false, g2.get_type_index());
|
|
PyObject* result = PyEval_CallFunction(_python_callback, "OOO", (PyObject*) data, p1, p2);
|
|
if (!result) {
|
|
ode_cat.error() << "An error occurred while calling python function!\n";
|
|
PyErr_Print();
|
|
}
|
|
}
|
|
#endif
|
|
|
|
OdeGeom OdeUtil::
|
|
space_to_geom(const OdeSpace &space) {
|
|
return OdeGeom((dGeomID)space.get_id());
|
|
}
|
|
|