virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth)
{
btVector3 endPt, startPt;
btScalar newDepth;
btVector3 newNormal;
if (m_perturbA)
{
btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
startPt = endPt+normalOnBInWorld*newDepth;
} else
{
endPt = pointInWorld + normalOnBInWorld*orgDepth;
startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
newDepth = (endPt - startPt).dot(normalOnBInWorld);
}
#ifdef DEBUG_CONTACTS
m_debugDrawer->drawLine(startPt, endPt, btVector3(1,0,0));
m_debugDrawer->drawSphere(startPt,0.05, btVector3(0,1,0));
m_debugDrawer->drawSphere(endPt,0.05, btVector3(0,0,1));
#endif //DEBUG_CONTACTS
m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth);
}
virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
{
(void)partId;
(void)triangleIndex;
btVector3 wv0,wv1,wv2;
wv0 = m_worldTrans*triangle[0];
wv1 = m_worldTrans*triangle[1];
wv2 = m_worldTrans*triangle[2];
m_debugDrawer->drawLine(wv0,wv1,m_color);
m_debugDrawer->drawLine(wv1,wv2,m_color);
m_debugDrawer->drawLine(wv2,wv0,m_color);
}
virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
{
(void)partId;
(void)triangleIndex;
btVector3 wv0,wv1,wv2;
wv0 = m_worldTrans*triangle[0];
wv1 = m_worldTrans*triangle[1];
wv2 = m_worldTrans*triangle[2];
btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
btVector3 normal = (wv1-wv0).cross(wv2-wv0);
normal.normalize();
btVector3 normalColor(1,1,0);
m_debugDrawer->drawLine(center,center+normal,normalColor);
m_debugDrawer->drawLine(wv0,wv1,m_color);
m_debugDrawer->drawLine(wv1,wv2,m_color);
m_debugDrawer->drawLine(wv2,wv0,m_color);
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
( void )partId;
( void )triangleIndex;
btVector3 vert0 = m_worldTrans * triangle[ 0 ];
btVector3 vert1 = m_worldTrans * triangle[ 1 ];
btVector3 vert2 = m_worldTrans * triangle[ 2 ];
btVector3 v10; v10 = vert1 - vert0 ;
btVector3 v20; v20 = vert2 - vert0 ;
btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
triangleNormal.normalize();
btVector3 pt = ( vert0 + vert1 + vert2 ) / 3.0f;
m_debugDraw->drawLine( pt, pt + triangleNormal * 10.0f, btVector3( 1.0f, 1.0f, 1.0f ) );
}
Physics::Physics(ActorList const & actors, btIDebugDraw & debugger) :
actorList(actors),
debugger(debugger),
dispatcher(&collisionConfiguration),
dynamicsWorld(&dispatcher, &broadphase, &solver, &collisionConfiguration)
{
dynamicsWorld.setGravity(btVector3(0,LoadFloat("config/world.xml", "gravity"),0));
newActors(actors);
/*turn on debugging*/
#ifdef DEBUG_RENDERING
debugger.setDebugMode(btIDebugDraw::DBG_DrawWireframe);
dynamicsWorld.setDebugDrawer(&debugger);
#endif
// HeightMap * m = new HeightMap("esDc1.png");
HeightMap const * m = HeightMapManager::GetHeightMap();
btHeightfieldTerrainShape * heightfieldShape = new btHeightfieldTerrainShape(m->width, m->height,
m->map,
LoadFloat("config/world.xml","height_map_scale_y"),
-300, 300,
1, PHY_UCHAR, false);
btTransform tr;
tr.setIdentity();
// tr.setOrigin(btVector3(-32*14, 0, -32*14));
btVector3 localInertia(0,0,0);
heightfieldShape->setLocalScaling(btVector3(LoadFloat("config/world.xml","height_map_scale_x"), 1, LoadFloat("config/world.xml","height_map_scale_z")));
btRigidBody* body = new btRigidBody(0,0,heightfieldShape,localInertia);
body->setWorldTransform(tr);
dynamicsWorld.addRigidBody(body);
}