314 lines
8.8 KiB
C++
314 lines
8.8 KiB
C++
#include "engine/core.h"
|
|
#include "engine/log.h"
|
|
#include "engine/engine.h"
|
|
#include "engine/ientity.h"
|
|
#include "engine/camera.h"
|
|
#include "engine/physics/physicsworld.h"
|
|
#include "engine/physics/physicsdebugdraw.h"
|
|
#include "engine/physics/rigidbody.h"
|
|
|
|
#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
|
|
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
|
|
#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
|
|
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
|
|
|
|
PhysicsWorld* g_PhysicsWorld = nullptr;
|
|
|
|
static void InternalTickCallback(btDynamicsWorld* world, btScalar timeStep)
|
|
{
|
|
SDL_assert(world);
|
|
SDL_assert(world->getWorldUserInfo());
|
|
static_cast<PhysicsWorld*>(world->getWorldUserInfo())->InternalTick();
|
|
}
|
|
|
|
PhysicsWorld::PhysicsWorld()
|
|
{
|
|
m_debugDraw = false;
|
|
|
|
m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
|
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
|
|
m_overlappingPairCache = new btDbvtBroadphase();
|
|
m_solver = new btSequentialImpulseConstraintSolver();
|
|
m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);
|
|
m_btGhostPairCallback = new btGhostPairCallback();
|
|
|
|
m_world->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(m_btGhostPairCallback);
|
|
m_world->setInternalTickCallback(InternalTickCallback, this);
|
|
m_world->setDebugDrawer(&m_physicsDebugDraw);
|
|
|
|
m_accumulatedTime = 0.0f;
|
|
m_stepTime = (1.0f / 30.0f); // 30 fps
|
|
|
|
// set the standart gravity
|
|
//m_world->setGravity(btVector3(0, 0, 0));
|
|
//getWorld()->setGravity(btVector3(0.0, -15.0, 0.0));
|
|
}
|
|
|
|
PhysicsWorld::~PhysicsWorld()
|
|
{
|
|
m_world->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(nullptr);
|
|
|
|
// delete ghost pair callback
|
|
delete m_btGhostPairCallback;
|
|
|
|
// delete dynamics world
|
|
delete m_world;
|
|
|
|
// delete solver
|
|
delete m_solver;
|
|
|
|
// delete broadphase
|
|
delete m_overlappingPairCache;
|
|
|
|
// delete dispatcher
|
|
delete m_dispatcher;
|
|
|
|
// delete collision configuration
|
|
delete m_collisionConfiguration;
|
|
}
|
|
|
|
void PhysicsWorld::AddRigidBody(RigidBody* body)
|
|
{
|
|
SDL_assert(body->GetSDKBody() && "RigidBody is not properly initialized");
|
|
m_world->addRigidBody(body->GetSDKBody());
|
|
|
|
m_rigidbodies.push_back(body);
|
|
}
|
|
|
|
void PhysicsWorld::RemoveRigidBody(RigidBody* body)
|
|
{
|
|
SDL_assert(body->GetSDKBody() && "RigidBody is not properly initialized");
|
|
m_world->removeRigidBody(body->GetSDKBody());
|
|
|
|
auto it = std::find(m_rigidbodies.begin(), m_rigidbodies.end(), body);
|
|
if (it != m_rigidbodies.end())
|
|
m_rigidbodies.erase(it);
|
|
}
|
|
|
|
const std::vector<RigidBody*>& PhysicsWorld::GetRigidBodies()
|
|
{
|
|
return m_rigidbodies;
|
|
}
|
|
|
|
void PhysicsWorld::Step(float delta)
|
|
{
|
|
m_world->stepSimulation(delta);
|
|
// m_world->stepSimulation(delta, 12, m_stepTime);
|
|
#if 0
|
|
if (delta < 0.01f)
|
|
{
|
|
m_accumulatedTime += delta;
|
|
if (m_accumulatedTime > m_stepTime)
|
|
{
|
|
m_world->stepSimulation(m_stepTime);
|
|
m_accumulatedTime -= m_stepTime;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_world->stepSimulation(delta);
|
|
}
|
|
#endif
|
|
|
|
//m_debugDraw = true;
|
|
if (m_debugDraw)
|
|
{
|
|
//DebugDrawTriggers();
|
|
|
|
int debugDrawMode = 0;
|
|
debugDrawMode |= btIDebugDraw::DBG_DrawAabb;
|
|
debugDrawMode |= btIDebugDraw::DBG_DrawWireframe;
|
|
// debugDrawMode |= btIDebugDraw::DBG_DrawContactPoints;
|
|
m_physicsDebugDraw.setDebugMode(debugDrawMode);
|
|
|
|
m_world->debugDrawWorld();
|
|
}
|
|
|
|
/*if (m_world)
|
|
m_world->stepSimulation(1 / 60.0f);
|
|
|
|
int debugDrawMode = 0;
|
|
debugDrawMode |= btIDebugDraw::DBG_DrawAabb;
|
|
debugDrawMode |= btIDebugDraw::DBG_DrawWireframe;
|
|
debugDrawMode |= btIDebugDraw::DBG_DrawContactPoints;
|
|
m_physicsDebugDraw.setDebugMode(debugDrawMode);
|
|
|
|
m_world->debugDrawWorld();*/
|
|
}
|
|
|
|
bool PhysicsWorld::TraceRay(TraceRayResult& _result, const glm::vec3& _rayBegin, const glm::vec3& _rayEnd, IEntityBase* _pIgnoreEntity)
|
|
{
|
|
btVector3 rayStart = glmVectorToBt(_rayBegin);
|
|
btVector3 rayEnd = glmVectorToBt(_rayEnd);
|
|
|
|
ClosestRayResultCallback RayResultCallback(rayStart, rayEnd, _pIgnoreEntity);
|
|
g_PhysicsWorld->GetWorld()->rayTest(rayStart, rayEnd, RayResultCallback);
|
|
|
|
if (RayResultCallback.hasHit())
|
|
{
|
|
_result.hit = true;
|
|
|
|
_result.pEntity = (IEntityBase*)RayResultCallback.m_collisionObject->getUserPointer();
|
|
_result.position = btVectorToGlm(RayResultCallback.m_hitPointWorld);
|
|
_result.normal = btVectorToGlm(RayResultCallback.m_hitNormalWorld);
|
|
}
|
|
|
|
return _result.hit;
|
|
}
|
|
|
|
void PhysicsWorld::ToggleDebugDraw()
|
|
{
|
|
m_debugDraw = !m_debugDraw;
|
|
}
|
|
|
|
void PhysicsWorld::InternalTick()
|
|
{
|
|
int numManifolds = m_world->getDispatcher()->getNumManifolds();
|
|
for (int i = 0; i < numManifolds; i++)
|
|
{
|
|
btPersistentManifold* contactManifold = m_world->getDispatcher()->getManifoldByIndexInternal(i);
|
|
btCollisionObject* obA = const_cast<btCollisionObject*>(contactManifold->getBody0());
|
|
btCollisionObject* obB = const_cast<btCollisionObject*>(contactManifold->getBody1());
|
|
|
|
int numContacts = contactManifold->getNumContacts();
|
|
for (int j = 0; j < numContacts; j++)
|
|
{
|
|
btManifoldPoint& pt = contactManifold->getContactPoint(j);
|
|
if (pt.getDistance() < 0.f)
|
|
{
|
|
const btVector3& ptA = pt.getPositionWorldOnA();
|
|
const btVector3& ptB = pt.getPositionWorldOnB();
|
|
const btVector3& normalOnB = pt.m_normalWorldOnB;
|
|
|
|
//RigidBody* rbA = static_cast<RigidBody*>(obA->getUserPointer());
|
|
//RigidBody* rbB = static_cast<RigidBody*>(obB->getUserPointer());
|
|
//if (!rbA || !rbB)
|
|
// continue;
|
|
|
|
//SDL_assert(rbA);
|
|
//SDL_assert(rbB);
|
|
|
|
|
|
|
|
IEntityBase* entA = static_cast<IEntityBase*>(obA->getUserPointer());
|
|
IEntityBase* entB = static_cast<IEntityBase*>(obB->getUserPointer());
|
|
if (!entA || !entB)
|
|
continue;
|
|
|
|
if (entA)
|
|
entA->OnCollide(entB);
|
|
|
|
if (entB)
|
|
entB->OnCollide(entA);
|
|
|
|
|
|
/*if (TriggerComponent* trigger = rbA->GetEntity()->GetComponent<TriggerComponent>())
|
|
{
|
|
trigger->OnCollide(rbA, rbB);
|
|
}
|
|
else if (TriggerComponent* trigger = rbB->GetEntity()->GetComponent<TriggerComponent>())
|
|
{
|
|
trigger->OnCollide(rbB, rbA);
|
|
}*/
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PhysicsWorld::AddCollisionModel(StaticMeshVertex* vertices, size_t verticesCount, uint32_t* indices, size_t indicesCount)
|
|
{
|
|
int index = (int)m_collisionModels.size();
|
|
m_collisionModels.resize((size_t)index + 1);
|
|
|
|
// Check for is indices are 32 bits
|
|
bool is32Bits = true;// sizeof(index_t) == sizeof(uint32_t);
|
|
|
|
m_collisionModels[index].triangleMesh = new btTriangleMesh(is32Bits);
|
|
m_collisionModels[index].triangleMesh->preallocateVertices((int)verticesCount);
|
|
m_collisionModels[index].triangleMesh->preallocateIndices((int)indicesCount);
|
|
|
|
// Initialize triangle mesh
|
|
uint32_t trianglesCount = 0;
|
|
for (uint32_t n = 0; n < indicesCount; n += 3) {
|
|
m_collisionModels[index].triangleMesh->addTriangle(
|
|
glmVectorToBt(vertices[indices[n]].position),
|
|
glmVectorToBt(vertices[indices[n + 1]].position),
|
|
glmVectorToBt(vertices[indices[n + 2]].position),
|
|
true);
|
|
|
|
trianglesCount++;
|
|
}
|
|
|
|
// Create shape
|
|
m_collisionModels[index].shape = new btBvhTriangleMeshShape(m_collisionModels[index].triangleMesh, true);
|
|
|
|
// Create collision body
|
|
m_collisionModels[index].object = new btCollisionObject();
|
|
m_collisionModels[index].object->setCollisionShape(m_collisionModels[index].shape);
|
|
//m_collisionModels[index].object->setUserPointer(&m_collisionModels[index]);
|
|
|
|
// Add to scene
|
|
m_world->addCollisionObject(m_collisionModels[index].object);
|
|
|
|
// Report
|
|
Msg("PhysicsWorld::AddCollisionModel: %i triangles %i bytes", trianglesCount, trianglesCount * sizeof(btVector3));
|
|
}
|
|
|
|
void PhysicsWorld::Reset()
|
|
{
|
|
int numModels = (int)m_collisionModels.size();
|
|
for (int i = 0; i < numModels; i++)
|
|
{
|
|
if (m_collisionModels[i].object)
|
|
{
|
|
// remove object from world
|
|
m_world->removeCollisionObject(m_collisionModels[i].object);
|
|
|
|
// reset shape
|
|
m_collisionModels[i].object->setCollisionShape(nullptr);
|
|
|
|
// delete
|
|
delete m_collisionModels[i].object;
|
|
m_collisionModels[i].object = nullptr;
|
|
}
|
|
|
|
if (m_collisionModels[i].shape)
|
|
{
|
|
delete m_collisionModels[i].shape;
|
|
m_collisionModels[i].shape = nullptr;
|
|
}
|
|
|
|
if (m_collisionModels[i].triangleMesh)
|
|
{
|
|
delete m_collisionModels[i].triangleMesh;
|
|
m_collisionModels[i].triangleMesh = nullptr;
|
|
}
|
|
}
|
|
|
|
Msg("PhysicsWorld: Destroyed %d static collision models", numModels);
|
|
|
|
m_collisionModels.clear();
|
|
}
|
|
|
|
//
|
|
ClosestRayResultCallback::ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, IEntityBase* entity) :
|
|
btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
|
|
m_entity(entity)
|
|
{
|
|
}
|
|
|
|
ClosestRayResultCallback::~ClosestRayResultCallback()
|
|
{
|
|
m_entity = nullptr;
|
|
}
|
|
|
|
bool ClosestRayResultCallback::needsCollision(btBroadphaseProxy* proxy0) const
|
|
{
|
|
bool needsCollision = inherited::needsCollision(proxy0);
|
|
|
|
btCollisionObject* collisionObject = (btCollisionObject*)proxy0->m_clientObject;
|
|
if (m_entity && collisionObject->getUserPointer() == m_entity)
|
|
return false;
|
|
|
|
return needsCollision;
|
|
} |