Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #873 from erwincoumans/master

Browse Source 
fix pybullet:getClosestPoints, add pybullet:get/resetBaseVelocity
This commit is contained in:
erwincoumans
2016-11-28 16:18:30 -08:00
committed by GitHub
parent d4a18c5634 2d42c7963a
commit 5e52760fbd
10 changed files with 382 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -672,6 +672,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1;
command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0];
command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1];
command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2];
return 0;
}
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1;
command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0];
command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1];
command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2];
return 0;
}
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
return 0;
}
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -256,6 +256,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]);
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);

26
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2342,6 +2342,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
btVector3 baseLinVel(0, 0, 0);
btVector3 baseAngVel(0, 0, 0);
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
{
baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
clientCmd.m_initPoseArgs.m_initialStateQdot[1],
clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
mb->setBaseVel(baseLinVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
{
baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
clientCmd.m_initPoseArgs.m_initialStateQdot[4],
clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
mb->setBaseOmega(baseAngVel);
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
btVector3 zero(0,0,0);
@@ -2349,7 +2371,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
mb->setBaseVel(zero);
mb->setBaseVel(baseLinVel);
mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
clientCmd.m_initPoseArgs.m_initialStateQ[1],
@@ -2362,7 +2384,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0));
mb->setBaseOmega(baseAngVel);
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -109,7 +109,9 @@ enum EnumInitPoseFlags
{
INIT_POSE_HAS_INITIAL_POSITION=1,
INIT_POSE_HAS_INITIAL_ORIENTATION=2,
INIT_POSE_HAS_JOINT_STATE=4
INIT_POSE_HAS_JOINT_STATE=4,
INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8,
INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16,
};
@@ -122,6 +124,8 @@ struct InitPoseArgs
int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM];
};

2
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -107,6 +107,7 @@ m_objectIndex(-1)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
@@ -127,6 +128,7 @@ m_objectIndex(objectIndex)
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();

425
examples/pybullet/pybullet.c
View File

@@ -30,6 +30,123 @@ enum eCONNECT_METHOD {
static PyObject* SpamError;
static b3PhysicsClientHandle sm = 0;
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq = 0;
if (objVec == NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
// Step through one timestep of the simulation
static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
if (0 == sm) {
@@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
return PyLong_FromLong(bodyIndex);
}
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
double v = 0.0;
PyObject* item;
if (PyList_Check(seq)) {
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
} else {
item = PyTuple_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
return v;
}
@@ -768,11 +872,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
return Py_None;
}
static int pybullet_internalGetBaseVelocity(
int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) {
baseLinearVelocity[0] = 0.;
baseLinearVelocity[1] = 0.;
baseLinearVelocity[2] = 0.;
baseAngularVelocity[0] = 0.;
baseAngularVelocity[1] = 0.;
baseAngularVelocity[2] = 0.;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return 0;
}
{
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQdot;
// const double* jointReactionForces[];
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
PyErr_SetString(SpamError, "getBaseVelocity failed.");
return 0;
}
b3GetStatusActualState(
status_handle, 0 /* body_unique_id */,
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
0 /*root_local_inertial_frame*/, 0,
&actualStateQdot, 0 /* joint_reaction_forces */);
// printf("joint reaction forces=");
// for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
// printf("%f ", jointReactionForces[i]);
// }
// now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
// and orientation x,y,z,w =
// actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
baseLinearVelocity[0] = actualStateQdot[0];
baseLinearVelocity[1] = actualStateQdot[1];
baseLinearVelocity[2] = actualStateQdot[2];
baseAngularVelocity[0] = actualStateQdot[3];
baseAngularVelocity[1] = actualStateQdot[4];
baseAngularVelocity[2] = actualStateQdot[5];
}
}
return 1;
}
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
static int pybullet_internalGetBasePositionAndOrientation(
int bodyIndex, double basePosition[3], double baseOrientation[3]) {
int bodyIndex, double basePosition[3], double baseOrientation[4]) {
basePosition[0] = 0.;
basePosition[1] = 0.;
basePosition[2] = 0.;
@@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
if (0 == pybullet_internalGetBasePositionAndOrientation(
bodyIndex, basePosition, baseOrientation)) {
PyErr_SetString(SpamError,
"GetBasePositionAndOrientation failed (#joints/links "
"exceeds maximum?).");
"GetBasePositionAndOrientation failed.");
return NULL;
}
@@ -891,6 +1051,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
}
}
static PyObject* pybullet_getBaseVelocity(PyObject* self,
PyObject* args) {
int bodyIndex = -1;
double baseLinearVelocity[3];
double baseAngularVelocity[3];
PyObject* pylistLinVel=0;
PyObject* pylistAngVel=0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
PyErr_SetString(SpamError, "Expected a body index (integer).");
return NULL;
}
if (0 == pybullet_internalGetBaseVelocity(
bodyIndex, baseLinearVelocity, baseAngularVelocity)) {
PyErr_SetString(SpamError,
"getBaseVelocity failed.");
return NULL;
}
{
PyObject* item;
int i;
int num = 3;
pylistLinVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseLinearVelocity[i]);
PyTuple_SetItem(pylistLinVel, i, item);
}
}
{
PyObject* item;
int i;
int num = 3;
pylistAngVel = PyTuple_New(num);
for (i = 0; i < num; i++) {
item = PyFloat_FromDouble(baseAngularVelocity[i]);
PyTuple_SetItem(pylistAngVel, i, item);
}
}
{
PyObject* pylist;
pylist = PyTuple_New(2);
PyTuple_SetItem(pylist, 0, pylistLinVel);
PyTuple_SetItem(pylist, 1, pylistAngVel);
return pylist;
}
}
static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
{
if (0 == sm) {
@@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
return NULL;
}
static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds)
{
static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL };
if (0 == sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int bodyIndex=0;
PyObject* linVelObj=0;
PyObject* angVelObj=0;
double linVel[3] = { 0, 0, 0 };
double angVel[3] = { 0, 0, 0 };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj))
{
return NULL;
}
if (linVelObj || angVelObj)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
if (linVelObj)
{
pybullet_internalSetVectord(linVelObj, linVel);
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel);
}
if (angVelObj)
{
pybullet_internalSetVectord(angVelObj, angVel);
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
else
{
PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity.");
return NULL;
}
}
PyErr_SetString(SpamError, "error in resetJointState.");
return NULL;
}
// Reset the position and orientation of the base/root link, position [x,y,z]
// and orientation quaternion [x,y,z,w]
static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
@@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
return Py_None;
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
if (seq)
{
len = PySequence_Size(objMat);
if (len == 16) {
for (i = 0; i < len; i++) {
matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// internal function to set a float vector[3]
// used to initialize camera position with
// a view and projection matrix in renderImage()
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq=0;
if (objVec==NULL)
return 0;
seq = PySequence_Fast(objVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
{
@@ -3301,11 +3478,6 @@ static PyMethodDef SpamMethods[] = {
"Set the amount of time to proceed at each call to stepSimulation. (unit "
"is seconds, typically range is 0.01 or 0.001)"},
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
"Set the amount of time to proceed at each call to stepSimulation."
" (unit is seconds, typically range is 0.01 or 0.001)"},
{"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
"Set the amount of contact penetration Error Recovery Paramater "
"(ERP) in each time step. \
@@ -3366,6 +3538,19 @@ static PyMethodDef SpamMethods[] = {
"instantaneously, not through physics simulation. (x,y,z) position vector "
"and (x,y,z,w) quaternion orientation."},
{ "getBaseVelocity", pybullet_getBaseVelocity,
METH_VARARGS,
"Get the linear and angular velocity of the base of the object "
" in world space coordinates. "
"(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." },
{ "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS,
"Reset the linear and/or angular velocity of the base of the object "
" in world space coordinates. "
"linearVelocity (x,y,z) and angularVelocity (x,y,z)." },
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."},

3
examples/pybullet/testrender.py
View File

@@ -19,6 +19,7 @@ pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional argument
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
@@ -28,7 +29,7 @@ for pitch in range (0,360,10) :
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection)
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]

2
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -100,7 +100,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize();
normal.safeNormalize();
btVector3 p1ToCentre = sphereCenter - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal);

3
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
View File

@@ -292,6 +292,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
btTransform otherInCompoundSpace;
otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
localAabbMin -= extraExtends;
localAabbMax += extraExtends;
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//process all children, that overlap with the given AABB bounds

2
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
View File

@@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold);
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds