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

Merge pull request #1749 from erwincoumans/master

Browse Source 
remove getLinkState from API, it automatically calculated forwardKine…
This commit is contained in:
erwincoumans
2018-06-13 03:38:36 +00:00
committed by GitHub
parent ce47fcc7f2 e24da97140
commit 6376197e5e
20 changed files with 384 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
data/cube_concave.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision concave="yes">
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

4
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -513,6 +513,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
convexHull->initializePolyhedralFeatures();
compound->addChildShape(identity,convexHull);
}
@@ -694,6 +695,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
boxShape->initializePolyhedralFeatures();
shape = boxShape;
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
@@ -905,7 +907,7 @@ upAxisMat.setIdentity();
BT_PROFILE("convert btConvexHullShape");
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->initializePolyhedralFeatures();
convexHull->setMargin(gUrdfDefaultCollisionMargin);
convexHull->recalcLocalAabb();
//convexHull->setLocalScaling(collision->m_geometry.m_meshScale);

5
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -502,7 +502,10 @@ public:
}
// compute body position and orientation
b3LinkState linkState;
m_robotSim.getLinkState(0, 6, &linkState);
bool computeVelocity=true;
bool computeForwardKinematics=true;
m_robotSim.getLinkState(0, 6, computeVelocity,computeForwardKinematics, &linkState);
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);

4
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -151,7 +151,9 @@ public:
}
// compute body position and orientation
b3LinkState linkState;
m_robotSim.getLinkState(0, 6, &linkState);
bool computeVelocity=true;
bool computeForwardKinematics=true;
m_robotSim.getLinkState(0, 6, computeVelocity,computeForwardKinematics, &linkState);
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -611,8 +611,14 @@ B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle comma
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_enableSAT = enableSAT;
command->m_updateFlags |= SIM_PARAM_ENABLE_SAT;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -317,6 +317,7 @@ B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryComma
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode);
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold);
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);

64
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1582,9 +1582,11 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<b3MouseEvent> m_mouseEvents;
CommandLogger* m_commandLogger;
int m_commandLoggingUid;
CommandLogPlayback* m_logPlayback;
int m_logPlaybackUid;
btScalar m_physicsDeltaTime;
btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
@@ -1661,7 +1663,9 @@ struct PhysicsServerCommandProcessorInternalData
:m_pluginManager(proc),
m_useRealTimeSimulation(false),
m_commandLogger(0),
m_commandLoggingUid(-1),
m_logPlayback(0),
m_logPlaybackUid(-1),
m_physicsDeltaTime(1./240.),
m_numSimulationSubSteps(0),
m_userConstraintUIDGenerator(1),
@@ -3090,6 +3094,31 @@ bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct Shar
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
{
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_ALL_COMMANDS)
{
if(m_data->m_commandLogger==0)
{
enableCommandLogging(true, clientCmd.m_stateLoggingArguments.m_fileName);
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
int loggerUid = m_data->m_stateLoggersUniqueId++;
m_data->m_commandLoggingUid = loggerUid;
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
}
}
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_REPLAY_ALL_COMMANDS)
{
if(m_data->m_logPlayback==0)
{
replayFromLogFile(clientCmd.m_stateLoggingArguments.m_fileName);
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
int loggerUid = m_data->m_stateLoggersUniqueId++;
m_data->m_logPlaybackUid = loggerUid;
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
}
}
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS)
{
if (m_data->m_profileTimingLoggingUid<0)
@@ -3168,6 +3197,9 @@ bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct Shar
}
}
}
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT)
{
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
@@ -3245,6 +3277,27 @@ bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct Shar
}
if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
{
if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_logPlaybackUid)
{
if(m_data->m_logPlayback)
{
delete m_data->m_logPlayback;
m_data->m_logPlayback = 0;
m_data->m_logPlaybackUid = -1;
}
}
if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_commandLoggingUid)
{
if(m_data->m_commandLogger)
{
enableCommandLogging(false,0);
serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
m_data->m_commandLoggingUid = -1;
}
}
if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid)
{
serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
@@ -6865,6 +6918,8 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
@@ -6988,6 +7043,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
}
if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
{
m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -455,6 +455,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152,
SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
SIM_PARAM_ENABLE_SAT = 8388608,
};
enum EnumLoadSoftBodyUpdateFlags

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -533,6 +533,8 @@ enum b3StateLoggingType
STATE_LOGGING_COMMANDS = 4,
STATE_LOGGING_CONTACT_POINTS = 5,
STATE_LOGGING_PROFILE_TIMINGS = 6,
STATE_LOGGING_ALL_COMMANDS=7,
STATE_REPLAY_ALL_COMMANDS=8,
};
@@ -797,6 +799,7 @@ struct b3PhysicsSimulationParameters
int m_jointFeedbackMode;
double m_solverResidualThreshold;
double m_contactSlop;
int m_enableSAT;
};

6
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -896,13 +896,7 @@ bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int l
}
bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
{
bool computeLinkVelocity = true;
bool computeForwardKinematics = true;
return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState);
}
bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
{

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -555,8 +555,6 @@ public:
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos);

15
examples/pybullet/examples/commandLogAndPlayback.py Normal file
View File

@@ -0,0 +1,15 @@
import pybullet as p
import time
p.connect(p.GUI)
logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS,"commandLog.bin")
p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf",[0,0,1])
p.stopStateLogging(logId)
p.resetSimulation();
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS,"commandLog.bin")
while(p.isConnected()):
time.sleep(1./240.)

14
examples/pybullet/examples/satCollision.py Normal file
View File

@@ -0,0 +1,14 @@
import pybullet as p
import time
p.connect(p.GUI)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(enableSAT=1)
p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True)
p.loadURDF("cube.urdf",[0,0,1], globalScaling=1)
p.loadURDF("duck_vhacd.urdf",[1,0,1], globalScaling=1)
while (p.isConnected()):
p.stepSimulation()
time.sleep(1./240.)

2
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
View File

@@ -271,7 +271,7 @@ class MinitaurGymEnv(gym.Env):
"%s/plane.urdf" % self._urdf_root)
if (self._reflection):
self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8])
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,0)
#self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,1)
self._pybullet_client.setGravity(0, 0, -10)
acc_motor = self._accurate_motor_model_enabled
motor_protect = self._motor_overheat_protection

19
examples/pybullet/pybullet.c
View File

@@ -1434,12 +1434,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int jointFeedbackMode =-1;
double solverResidualThreshold = -1;
double contactSlop = -1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "physicsClientId", NULL};
int enableSAT = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &physicsClientId))
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &physicsClientId))
{
return NULL;
}
@@ -1540,6 +1541,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParameterSetJointFeedbackMode(command,jointFeedbackMode);
}
if (enableSAT>=0)
{
b3PhysicsParameterSetEnableSAT(command, enableSAT);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
@@ -9571,7 +9576,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4);
PyModule_AddIntConstant(m, "STATE_LOGGING_CONTACT_POINTS", STATE_LOGGING_CONTACT_POINTS);
PyModule_AddIntConstant(m, "STATE_LOGGING_PROFILE_TIMINGS", STATE_LOGGING_PROFILE_TIMINGS);
PyModule_AddIntConstant(m, "STATE_LOGGING_ALL_COMMANDS", STATE_LOGGING_ALL_COMMANDS);
PyModule_AddIntConstant(m, "STATE_REPLAY_ALL_COMMANDS", STATE_REPLAY_ALL_COMMANDS);
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);
PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME);

146
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
@@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
{
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasContact;
btDummyResult()
: m_hasContact(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0){}
virtual void setShapeIdentifiersB(int partId1,int index1){}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
m_hasContact = true;
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
}
};
@@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
} else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
{
btVertexArray vertices;
btVertexArray worldSpaceVertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
@@ -579,17 +599,97 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
btScalar maxDist = threshold;
bool foundSepAxis = false;
if (0)
bool useSatSepNormal = true;
if (useSatSepNormal)
{
polyhedronB->initializePolyhedralFeatures();
if (0)
{
//initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle
polyhedronB->initializePolyhedralFeatures();
} else
{
btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0],
tri->m_vertices1[2]-tri->m_vertices1[1],
tri->m_vertices1[0]-tri->m_vertices1[2]};
uniqueEdges[0].normalize();
uniqueEdges[1].normalize();
uniqueEdges[2].normalize();
btConvexPolyhedron polyhedron;
polyhedron.m_vertices.push_back(tri->m_vertices1[2]);
polyhedron.m_vertices.push_back(tri->m_vertices1[0]);
polyhedron.m_vertices.push_back(tri->m_vertices1[1]);
{
btFace combinedFaceA;
combinedFaceA.m_indices.push_back(0);
combinedFaceA.m_indices.push_back(1);
combinedFaceA.m_indices.push_back(2);
btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceA.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceA.m_plane[0] = faceNormal[0];
combinedFaceA.m_plane[1] = faceNormal[1];
combinedFaceA.m_plane[2] = faceNormal[2];
combinedFaceA.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceA);
}
{
btFace combinedFaceB;
combinedFaceB.m_indices.push_back(0);
combinedFaceB.m_indices.push_back(2);
combinedFaceB.m_indices.push_back(1);
btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFaceB.m_indices.size();v++)
{
btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFaceB.m_plane[0] = faceNormal[0];
combinedFaceB.m_plane[1] = faceNormal[1];
combinedFaceB.m_plane[2] = faceNormal[2];
combinedFaceB.m_plane[3] = -planeEq;
polyhedron.m_faces.push_back(combinedFaceB);
}
polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]);
polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]);
polyhedron.initialize2();
polyhedronB->setPolyhedralFeatures(polyhedron);
}
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(),
sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
}
else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
@@ -598,6 +698,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
if (dummy.m_hasContact && dummy.m_depth<0)
{
if (foundSepAxis)
{
if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99)
{
printf("?\n");
}
} else
{
printf("!\n");
}
sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld;
//minDist = dummy.m_depth;
foundSepAxis = true;
}
#if 0
btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if (l2>SIMD_EPSILON)
{
@@ -607,6 +725,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
foundSepAxis = true;
}
#endif
}
@@ -614,7 +733,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
{
worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
}
@@ -625,6 +744,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
return;
}
}

12
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
View File

@@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize()
btHashMap<btInternalVertexPair,btInternalEdge> edges;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -172,6 +172,13 @@ void btConvexPolyhedron::initialize()
}
#endif//USE_CONNECTED_FACES
initialize2();
}
void btConvexPolyhedron::initialize2()
{
m_localCenter.setValue(0, 0, 0);
btScalar TotalArea = 0.0f;
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
@@ -274,7 +281,6 @@ void btConvexPolyhedron::initialize()
}
#endif
}
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
minProj = FLT_MAX;

1
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
View File

@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
btVector3 mE;
void initialize();
void initialize2();
bool testContainment() const;
void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;

79
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
View File

@@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
}
}
void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron)
{
if (m_polyhedron)
{
*m_polyhedron = polyhedron;
} else
{
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron(polyhedron);
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
{
@@ -87,7 +98,71 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
}
#ifndef BT_RECONSTRUCT_FACES
int numVertices = conv.vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = conv.vertices[p];
}
int v0, v1;
for (int j = 0; j < conv.faces.size(); j++)
{
btVector3 edges[3];
int numEdges = 0;
btFace combinedFace;
const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
v0 = edge->getSourceVertex();
int prevVertex=v0;
combinedFace.m_indices.push_back(v0);
v1 = edge->getTargetVertex();
while (v1 != v0)
{
btVector3 wa = conv.vertices[prevVertex];
btVector3 wb = conv.vertices[v1];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
//face->addIndex(v1);
combinedFace.m_indices.push_back(v1);
edge = edge->getNextEdgeOfFace();
prevVertex = v1;
int v01 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
}
btAssert(combinedFace.m_indices.size() > 2);
btVector3 faceNormal = edges[0].cross(edges[1]);
faceNormal.normalize();
btScalar planeEq=1e30f;
for (int v=0;v<combinedFace.m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal);
if (planeEq>eq)
{
planeEq=eq;
}
}
combinedFace.m_plane[0] = faceNormal.getX();
combinedFace.m_plane[1] = faceNormal.getY();
combinedFace.m_plane[2] = faceNormal.getZ();
combinedFace.m_plane[3] = -planeEq;
m_polyhedron->m_faces.push_back(combinedFace);
}
#else//BT_RECONSTRUCT_FACES
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
@@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
}
#endif //BT_RECONSTRUCT_FACES
m_polyhedron->initialize();
return true;

3
src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
View File

@@ -43,6 +43,9 @@ public:
///experimental/work-in-progress
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0);
virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron);
const btConvexPolyhedron* getConvexPolyhedron() const
{
return m_polyhedron;