98 lines
3.1 KiB
C++
98 lines
3.1 KiB
C++
/**
|
|
* 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."
|
|
*
|
|
* @file odeUtil.cxx
|
|
* @author joswilso
|
|
* @date 2006-12-27
|
|
*/
|
|
|
|
#include "odeUtil.h"
|
|
|
|
dReal OdeUtil::OC_infinity = dInfinity;
|
|
|
|
/**
|
|
* 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()));
|
|
}
|
|
|
|
/**
|
|
* 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 = std::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;
|
|
}
|
|
|
|
/**
|
|
* 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());
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
OdeGeom OdeUtil::
|
|
space_to_geom(const OdeSpace &space) {
|
|
return OdeGeom((dGeomID)space.get_id());
|
|
}
|