btBvhTriangleMeshShape strange bug

Post Reply
ddnsky
Posts: 1
Joined: Wed May 15, 2013 10:12 am

btBvhTriangleMeshShape strange bug

Post by ddnsky »

I use btBvhTriangleMeshShape with one triangle ABC=( -3, 2.1, -1 ), ( 7, 4.5, -1 ), ( 7, 3.2, 9 ) and boxes unit sized.
All boxes passing through triangle without any collision but from other side of triangle surface collision works. If I change one Y coord of B point from 4.5 to 4. collisions again works. This can be reproduced on x32 & x64.
modified to test case BasicDemo.cpp -

Code: Select all

#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 1
#define ARRAY_SIZE_Z 5

///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 0.5
#define START_POS_X  5
#define START_POS_Y -5
#define START_POS_Z  0

#include "BasicDemo.h"
#include "GlutStuff.h"
#include "btBulletDynamicsCommon.h"
#include <stdio.h> //printf debugging
#include "GLDebugDrawer.h"

static GLDebugDrawer gDebugDraw;

void BasicDemo::clientMoveAndDisplay() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	///step the simulation
	if (m_dynamicsWorld) {
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
	renderme(); 
	glFlush();
	swapBuffers();
}

void BasicDemo::displayCallback(void) {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	renderme();
	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();
	glFlush();
	swapBuffers();
}

void BasicDemo::initPhysics() {
	setTexturing(true);
	setShadows(true);
	setCameraDistance(btScalar(SCALING*25.));

	static bool g1stTime = true;
	if( g1stTime ) { g1stTime = false; m_azi = -30.f; m_ele = 60.f; }

	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();

	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
	m_dynamicsWorld->setGravity(btVector3(0,-2,0));

	///create triangle
	btTriangleMesh* data = new btTriangleMesh();
	btVector3 A(  -3,   2.1,   -1   );
	btVector3 B(   7,   4.5,   -1  );
	btVector3 C(   7,   3.2,    9  );
	data->addTriangle(A,B,C,false); // false, don’t remove duplicate vertices
	btBvhTriangleMeshShape* groundShape=new btBvhTriangleMeshShape(data,true,true);
	groundShape->setMargin( 0.1f );
	m_collisionShapes.push_back(groundShape);
	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,0,0));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
	{	btScalar mass(0.);
		btVector3 localInertia(0,0,0);
		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}

	{	//create a few dynamic rigidbodies
		// Re-using the same collision is better for memory usage and performance
		btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		m_collisionShapes.push_back(colShape);
		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();
		btScalar	mass(1.f);
		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);
		btVector3 localInertia(0,0,0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass,localInertia);

		float start_x = START_POS_X - ARRAY_SIZE_X/2;
		float start_y = 15+START_POS_Y;
		float start_z = START_POS_Z - ARRAY_SIZE_Z/2;

		for (int k=0;k<ARRAY_SIZE_Y;k++) {
			for (int i=0;i<ARRAY_SIZE_X;i++) {
				for(int j = 0;j<ARRAY_SIZE_Z;j++) {
					startTransform.setOrigin( SCALING*btVector3( btScalar(2.0*i + start_x), btScalar(2.0*k + start_y), btScalar(2.0*j + start_z)) );
			
					//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
					btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
					btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
					btRigidBody* body = new btRigidBody(rbInfo);
					m_dynamicsWorld->addRigidBody(body);
				}
			}
		}
	}
}

void BasicDemo::clientResetScene() {
	exitPhysics();
	initPhysics();
}

void BasicDemo::exitPhysics() {
	//cleanup in the reverse order of creation/initialization
	//remove the rigidbodies from the dynamics world and delete them
	for( int i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ; i-- ) {
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState()) {
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for( int j=0; j<m_collisionShapes.size(); j++ ) {
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();
	delete m_dynamicsWorld;
	delete m_solver;
	delete m_broadphase;
	delete m_dispatcher;
	delete m_collisionConfiguration;
}
laadams85
Posts: 18
Joined: Wed May 22, 2013 1:45 pm

Re: btBvhTriangleMeshShape strange bug

Post by laadams85 »

I'm just gonna take a guess at this, but I would imagine that for a TriangleMeshShape you need to have an enclosed volume. Chances are the collision detection is only checking against the outward normal as opposed to the inward normal. Try adding a second triangle to your mesh with the exact same points but the opposite normal, ( 7, 3.2, 9 ), ( 7, 4.5, -1 ), ( -3, 2.1, -1 ).
slithEToves
Posts: 24
Joined: Mon Mar 12, 2007 9:55 pm

Re: btBvhTriangleMeshShape strange bug

Post by slithEToves »

Sorry this reply is so slow in coming, but thanks for your post with such a clean example of this problem. I just updated from 2.76 to 2.82 and had the same issue, but noticed that it didn't happen if I ported your example to 2.83. Anyway, the problem is in btGjkPairDetector.cpp. If you set m_fixContactNormalDirection to 0 in both constructors you should be all fixed up.
Post Reply