Merge pull request #1749 from erwincoumans/master

remove getLinkState from API, it automatically calculated forwardKine…

data/cube_concave.urdf

@@ -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>
+

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -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);

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -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]);

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -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]);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -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)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -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);
 
 
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1582,8 +1582,10 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<b3MouseEvent> m_mouseEvents;
 
 	CommandLogger* m_commandLogger;
-	CommandLogPlayback* m_logPlayback;
+	int m_commandLoggingUid;
 
+	CommandLogPlayback* m_logPlayback;
+	int m_logPlaybackUid;
 	
 	btScalar m_physicsDeltaTime;
     btScalar m_numSimulationSubSteps;
@@ -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)
 	{

examples/SharedMemory/SharedMemoryCommands.h

@@ -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

examples/SharedMemory/SharedMemoryPublic.h

@@ -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;
 };
 
 

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -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)
 {

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -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);
 

examples/pybullet/examples/commandLogAndPlayback.py

@@ -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.)
+

examples/pybullet/examples/satCollision.py

@@ -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.)
+	

examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py

@@ -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

examples/pybullet/pybullet.c

@@ -1434,12 +1434,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	int jointFeedbackMode =-1;
 	double solverResidualThreshold = -1;
 	double contactSlop = -1;
+	int enableSAT = -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};
+	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, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	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, &physicsClientId))
+									 &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,6 +9576,8 @@ 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);

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -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]);
+				worldSpaceVertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
-				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
+				worldSpaceVertices.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[2]);
 				
 				//tri->initializePolyhedralFeatures();
 
@@ -579,9 +599,88 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				btScalar maxDist = threshold;
 				
 				bool foundSepAxis = false;
+				bool useSatSepNormal = true;
+
+				if (useSatSepNormal)
+				{
 					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(), 
@@ -589,7 +688,8 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 						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);
 			}
 				
 				
@@ -626,6 +745,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
 				return;
 			}
 
+			
 		}
 
 

src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -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;

src/BulletCollision/CollisionShapes/btConvexPolyhedron.h

@@ -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;

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

@@ -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();
@@ -312,6 +387,8 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa
 
 	}
 
+#endif //BT_RECONSTRUCT_FACES
+
 	m_polyhedron->initialize();
 
 	return true;

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h

@@ -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;