Webcam/include/modules/physic_bullet/bullet_world.cpp

/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "physic_bullet/bullet_world.h"
		#include "BulletCollision/CollisionDispatch/btGhostObject.h"
		#include "physic_bullet/bullet_item.h"
		#include "physic_bullet/bullet_constraint.h"
		#include "physic_bullet/bullet_debug.h"
		#include "core/geometry.h"
		#include "metafile/nml_object.h"
		#include "log/log.h"

		using namespace GS;
		using namespace GS::S3D;


//------------------------------------------------------------------------------
PhysicItem			*BulletWorld::NewItem()
{	return new BulletPhysicItem(world);	}
PhysicConstraint	*BulletWorld::NewConstraint()
{	return new BulletConstraint(world);	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
BulletConvex		*BulletWorld::LoadConvex(const char *_name)
{
	String name(_name);

	// Check cache.
	ListForeachPtr(BulletConvex *, convex, convex_cache)
		if	(convex->name == name)
			return convex;

	// Load geometry.
	AutoPtr <Core::Geometry> g(new Core::Geometry);
	if	(!NML::LoadFromFile(*g, name))
		return NULL;

	// Setup convex.
	BulletConvex *bullet_convex = new BulletConvex;
	if	(!bullet_convex)
		__ERR__(__LOG_E__ << "Failed to allocate bullet convex.\n", NULL)

	Array <btScalar> bt_vtx(g->vtx.GetCount() * 3);
	btScalar *p_bt_vtx = bt_vtx.c_ptr();

	Vector4	gcenter(0, 0, 0);
	for (uint n = 0; n < g->vtx.GetCount(); ++n)
	{
		gcenter += g->vtx[n];
		*p_bt_vtx++ = g->vtx[n].x;
		*p_bt_vtx++ = g->vtx[n].y;
		*p_bt_vtx++ = g->vtx[n].z;
	}

	bullet_convex->name = name;
	bullet_convex->center = (gcenter / (float)g->vtx.GetCount());
	bullet_convex->convex = new btConvexHullShape(bt_vtx.c_ptr(), g->vtx.GetCount(), 3 * sizeof(btScalar));
	convex_cache.Add(bullet_convex);

	return bullet_convex;
}
BulletMesh			*BulletWorld::LoadMesh(const char *_name, const char *_suffix)
{
	String name(_name), suffix(_suffix);

	// Check cache.
	ListForeachPtr(BulletMesh *, mesh, mesh_cache)
		if	((mesh->name == name) && (mesh->suffix == suffix))
		{
			__LOG_V__ << "Reusing cached Bullet btMesh for " << _name << " (suffix: " << _suffix << ").\n";
			return mesh;
		}

	// Load bullet mesh.
	AutoPtr <Core::Geometry> g(new Core::Geometry);
	if	(g.IsNull())
		return NULL;

	g->name = name;
	if	(!NML::LoadFromFile(*g, name))
		return NULL;

	if	(!g->vtx.GetCount() || !g->pol.GetCount())
		__ERR__(__LOG_E__ << "No geometry data in '" << g->name << "' to build collision shape.\n", NULL)

	BulletMesh *bullet_mesh = new BulletMesh;
	if	(!bullet_mesh)
		__ERR__(__LOG_E__ << "Failed to allocate bullet mesh.\n", NULL)

	int	triangle_count = g->GetTriangleCount();

	bullet_mesh->bt_vtx.Allocate(g->vtx.GetCount() * 3);
	btScalar *p_bt_vtx = bullet_mesh->bt_vtx;
	bullet_mesh->bt_idx.Allocate(triangle_count * 3);
	int *p_bt_idx = bullet_mesh->bt_idx;

	Vector4 gcenter(0, 0, 0);
	for (uint n = 0; n < g->vtx.GetCount(); ++n)
	{
		gcenter += g->vtx[n];
		*p_bt_vtx++ = g->vtx[n].x;
		*p_bt_vtx++ = g->vtx[n].y;
		*p_bt_vtx++ = g->vtx[n].z;
	}
	bullet_mesh->center = gcenter / (float)g->vtx.GetCount();

	// Triangulate geometry on the fly, transfer material indices.
	bullet_mesh->bt_mat.Allocate(g->material_table.GetCount());
	for (uint n = 0; n < g->material_table.GetCount(); ++n)
		bullet_mesh->bt_mat[n] = g->material_table[n].name;

	bullet_mesh->bt_id_mat.Allocate(triangle_count);
	ushort *p_bt_id_mat = bullet_mesh->bt_id_mat;

	for (uint n = 0; n < g->pol.GetCount(); ++n)
		for (int p = 1; p < (g->pol[n].vtx_count - 1); ++p)
		{
			*p_bt_idx++ = g->pol[n].binding[0];
			*p_bt_idx++ = g->pol[n].binding[p];
			*p_bt_idx++ = g->pol[n].binding[p + 1];
			*p_bt_id_mat++ = g->pol[n].material;
		}

	bullet_mesh->name = name;
	bullet_mesh->suffix = suffix;
	bullet_mesh->mesh_interface = new btTriangleIndexVertexArray(triangle_count, bullet_mesh->bt_idx, 3 * sizeof(int), g->vtx.GetCount(), bullet_mesh->bt_vtx, 3 * sizeof(btScalar));
	bullet_mesh->mesh = new btBvhTriangleMeshShape(bullet_mesh->mesh_interface, true);

	mesh_cache.Add(bullet_mesh);

	return bullet_mesh;
}
void				BulletWorld::ClearConvexMeshCache()
{
	convex_cache.Clear();
	mesh_cache.Clear();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				BulletWorld::HasDebugger() const
{	return debug_draw.IsValid();	}
void				BulletWorld::CreateDebugger(Renderer *renderer)
{
	debug_draw = renderer ? new BulletDebugDraw(*renderer) : NULL;
	if	(world)
		world->setDebugDrawer(debug_draw);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
static void			bullet_pretick_callback(btDynamicsWorld *world, btScalar timeStep)
{
	BulletWorld *physic_world = (BulletWorld *)world->getWorldUserInfo();
	if	(physic_world->GetWorldInterface())
		physic_world->GetWorldInterface()->PhysicStep(timeStep, true);
}
static void			bullet_posttick_callback(btDynamicsWorld *world, btScalar timeStep)
{
	BulletWorld *physic_world = (BulletWorld *)world->getWorldUserInfo();
	if	(physic_world->GetWorldInterface())
		physic_world->GetWorldInterface()->PhysicStep(timeStep, false);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				BulletWorld::DrawDebug(Renderer &, Camera *c, RasterFont *f, bool xray_first_pass)
{
	if	(BulletDebugDraw *dd = (BulletDebugDraw *)world->getDebugDrawer())
	{
		dd->camera = c;
		dd->raster_font = f;
		dd->SetDebugMode(btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits | btIDebugDraw::DBG_DrawContactPoints);
//		dd->SetDebugMode(btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_FastWireframe);
		dd->SetXRayFirstPass(xray_first_pass);

		world->debugDrawWorld();
		dd->Flush();
	}
}
uint				BulletWorld::GetCollisionPairCount()
{	return world->getDispatcher()->getNumManifolds();	}
bool				BulletWorld::GetCollisionPair(uint n, CollisionPair &pair)
{
	btPersistentManifold *manifold = world->getDispatcher()->getInternalManifoldPointer()[n];
	if	(!manifold || !manifold->getNumContacts())	// Manifolds are valid as long as the bodies overlap in the broadphase.
		return false;

	pair.a = (PhysicItem *)((btRigidBody *)manifold->getBody0())->getUserPointer();
	pair.b = (PhysicItem *)((btRigidBody *)manifold->getBody1())->getUserPointer();

	pair.contact_count = 0;
	for	(int i = 0; (i < manifold->getNumContacts()) && (i < 4); ++i)
	{
		btVector3 p = manifold->getContactPoint(i).getPositionWorldOnB();
		pair.contact[i].Set(p.x(), p.y(), p.z());
		btVector3 n = manifold->getContactPoint(i).m_normalWorldOnB;
		pair.normal[i].Set(n.x(), n.y(), n.z());

		pair.contact_count++;
	}
	return true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				BulletWorld::Create()
{
	collision_config = new btDefaultCollisionConfiguration();

#if	__ENABLE_BULLET_MULTITHREAD__
	thread_support_collision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo("Bullet Collision", processCollisionTask, createCollisionLocalStoreMemory, __BULLET_THREAD_COUNT__));
	dispatcher = new SpuGatheringCollisionDispatcher(thread_support_collision, __BULLET_THREAD_COUNT__, collision_config);
#else
	dispatcher = new btCollisionDispatcher(collision_config);
#endif

	broadphase = new btDbvtBroadphase();
	broadphase->getOverlappingPairCache()->setInternalGhostPairCallback(pair_callback = new btGhostPairCallback);

	solver = new btSequentialImpulseConstraintSolver;
	world = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collision_config);
	world->setInternalTickCallback(bullet_pretick_callback, (void *)this, true);
	world->setInternalTickCallback(bullet_posttick_callback, (void *)this, false);

//	world->getSolverInfo().m_numIterations = 10;
//	world->getDispatchInfo().m_enableSPU = true;
	world->getSolverInfo().m_solverMode = SOLVER_SIMD + SOLVER_USE_WARMSTARTING;// + SOLVER_RANDMIZE_ORDER;
//	world->getSolverInfo().m_splitImpulse = 1;
//	world->getSolverInfo().m_splitImpulsePenetrationThreshold = 0.2;

	world->setDebugDrawer(debug_draw);
	return true;
}
void				BulletWorld::Delete()
{
	ClearConvexMeshCache();

	collision_config = NULL;
	dispatcher = NULL;
	broadphase = NULL;
	solver = NULL;
	world = NULL;
#if	__ENABLE_BULLET_MULTITHREAD__
	thread_support_collision = NULL;
#endif
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				BulletWorld::Step(const GS::Time &dt)
{
	ScopedBenchmark bench(bench_step);

#if	1
	substep_dt -= dt.toSec();

	int limit = 4;
	while (substep_dt < 0)
	{
		world->stepSimulation(GetTimestep(), 0, GetTimestep());
		substep_dt += GetTimestep();

		if	(--limit <= 0)
		{
			substep_dt = 0;
			break;
		}
	}
#else
	world->stepSimulation(dt, 12, GetTimestep());
#endif
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool				BulletWorld::Raytrace(const Vector4 &s, const Vector4 &d, PhysicTrace &hit, int collision_mask, int shape_mask, float max_distance)
{
	struct ClosestRayResultWithTriangleIndexCallback : public btCollisionWorld::ClosestRayResultCallback
	{
		ClosestRayResultWithTriangleIndexCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld) : ClosestRayResultCallback(rayFromWorld, rayToWorld) {}

		int m_TriangleIndex;
		int	m_shapePart;

		virtual	btScalar addSingleResult(btCollisionWorld::LocalRayResult &rayResult, bool normalInWorldSpace)
		{
			if	(rayResult.m_localShapeInfo)
			{
				m_TriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
				m_shapePart = rayResult.m_localShapeInfo->m_shapePart;
			}
			else
			{
				m_TriangleIndex = -1;
				m_shapePart = -1;
			}
			return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
		}
	};

	Vector4 e = s + d * (max_distance > 0 ? max_distance : 5000.f);
	btVector3 from(s.x, s.y, s.z), to(e.x, e.y, e.z);

	ClosestRayResultWithTriangleIndexCallback trace(from, to);
	trace.m_collisionFilterGroup = btBroadphaseProxy::AllFilter;
	trace.m_collisionFilterMask = (short)collision_mask;
	world->rayTest(from, to, trace);
	if	(!trace.hasHit())
		return false;

	hit.p.Set(trace.m_hitPointWorld.x(), trace.m_hitPointWorld.y(), trace.m_hitPointWorld.z());
	hit.n.Set(trace.m_hitNormalWorld.x(), trace.m_hitNormalWorld.y(), trace.m_hitNormalWorld.z());
	hit.i = (PhysicItem *)trace.m_collisionObject->getUserPointer();

	if	(BulletPhysicItem *bi = (BulletPhysicItem *)hit.i)
		if	((trace.m_shapePart >= 0) && ((uint)trace.m_shapePart < bi->shapes.GetCount()))
			if	(BulletMesh *mesh = bi->shapes[trace.m_shapePart].mesh.c_ptr())
				if	(uint(trace.m_TriangleIndex) < mesh->bt_id_mat.GetCount())
					hit.m = mesh->bt_mat[mesh->bt_id_mat[trace.m_TriangleIndex]];

	return true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
static void *bulletAlloc(size_t s) { return MemAllocPhysics::Alloc(s, Alloc::Physics); }
static void bulletFree(void *p) { MemAllocPhysics::Delete(p, Alloc::Physics); }
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
BulletWorld::BulletWorld()
{
	btAlignedAllocSetCustom(bulletAlloc, bulletFree);
	substep_dt = 0;
}
BulletWorld::~BulletWorld()
{
	Delete();
}
//------------------------------------------------------------------------------