#include "CBulletPhysics.h" #include "CEngine.h" #include "CConVarManager.h" CBulletWorld::CBulletWorld() { GhostPairCallback = std::make_unique(); Config = std::make_unique(); Dispatcher = std::make_unique(Config.get()); Broadphase = std::make_unique(); Solver = std::make_unique(); BulletWorld = std::make_unique(Dispatcher.get(), Broadphase.get(), Solver.get(), Config.get()); BulletWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(GhostPairCallback.get()); } void CBulletWorld::V_SetGravity(const glm::vec3& _gravity) { BulletWorld->setGravity(CBulletPhysics::GetVector(_gravity)); Log::Instance() << "Set gravity " << _gravity << Log::Endl; } glm::vec3 CBulletWorld::GetGravity() const { return CBulletPhysics::GetVector(BulletWorld->getGravity()); } void CBulletWorld::V_Step(float deltaTime) { int substeps = 10; COMPONENT_CALL_GET(substeps, CConVarManager, GetConVarValue("phys.substeps", 10)); int fixedTimeStepN = 60; COMPONENT_CALL_GET(fixedTimeStepN, CConVarManager, GetConVarValue("phys.fixedtimestep", 60)); float fixedTimeStep = 1.0f / 60.0f; if(fixedTimeStepN == 0) { fixedTimeStep = deltaTime; } else if(fixedTimeStep > 0) { fixedTimeStep = 1.0f / static_cast(fixedTimeStepN); } BulletWorld->stepSimulation(deltaTime, substeps, fixedTimeStep); } btVector3 CBulletPhysics::GetVector(const glm::vec3& vec) { return btVector3(vec.x, vec.y, vec.z); } glm::vec3 CBulletPhysics::GetVector(const btVector3& vec) { return glm::vec3(vec.x(), vec.y(), vec.z()); } btQuaternion CBulletPhysics::GetQuat(const glm::quat& qua) { btQuaternion ret; ret.setX(qua.x); ret.setY(qua.y); ret.setZ(qua.z); ret.setW(qua.w); return ret; } glm::quat CBulletPhysics::GetQuat(const btQuaternion& qua) { glm::quat ret; ret.x = qua.getX(); ret.y = qua.getY(); ret.z = qua.getZ(); ret.w = qua.getW(); return ret; } btTransform CBulletPhysics::GetTransform(const CTransformBase& trans) { btTransform ret; ret.setIdentity(); ret.setOrigin(GetVector(trans.GetPosition())); ret.setRotation(GetQuat(trans.GetRotation())); return ret; } void CBulletPhysics::GetTransform(CTransformBase& trans, const btTransform& btTrans) { trans.SetPosition(GetVector(btTrans.getOrigin())); trans.SetRotation(GetQuat(btTrans.getRotation())); } void BulletShapes::CTriMesh::V_Build() { Mesh = std::make_unique(); for(auto& tri : Triangles) { //auto& v1 = Vertices.at(tri.x - 1); auto& v1 = Vertices.at(tri.x); auto& v2 = Vertices.at(tri.y); auto& v3 = Vertices.at(tri.z); Mesh->addTriangle ( CBulletPhysics::GetVector(v1), CBulletPhysics::GetVector(v2), CBulletPhysics::GetVector(v3) ); } InternalShape = std::make_unique(Mesh.get(), true); } void BulletShapes::CConvex::V_Build() { Log::Instance() << "Convex vertices.size() is " << Vertices.size() << Log::Endl; InternalShape = std::make_unique((float*)Vertices.data(), Vertices.size(), sizeof(glm::vec3)); } void BulletShapes::CCompound::V_Build() { InternalShape = std::make_unique(); auto compound = dynamic_cast(InternalShape.get()); for(auto& ch : Childs) { auto bulletShape = std::dynamic_pointer_cast(ch.Shape); if(!bulletShape) { continue; } compound->addChildShape(CBulletPhysics::GetTransform(ch.Transform), bulletShape->InternalShape.get()); } } void BulletShapes::CCapsule::V_Build() { InternalShape = std::make_unique(Radius, Height); } void BulletShapes::CSphere::V_Build() { InternalShape = std::make_unique(Radius); } void BulletShapes::CBox::V_Build() { InternalShape = std::make_unique(CBulletPhysics::GetVector(HalfExtents)); } CBulletPhysics::CBulletPhysics() { ShapesFactory.replace("trimesh"); ShapesFactory.replace("convex"); ShapesFactory.replace("compound"); ShapesFactory.replace("capsule"); ShapesFactory.replace("sphere"); ShapesFactory.replace("box"); } std::unique_ptr CBulletPhysics::V_CreateWorld() { return std::make_unique(); } btCollisionShape* CBulletUnit::GetBulletCollision(std::shared_ptr _col) { Log::ErrInstance() << "CBulletUnit::GetCollision\n"; if(!_col) { Log::ErrInstance() << "NULL _col\n"; } auto __shape = _col->Shape.get(); if(!__shape) { Log::ErrInstance() << "NULL __shape\n"; } auto _shape = dynamic_cast(__shape); if(!_shape) { Log::ErrInstance() << "NULL _shape\n"; } auto& shape = _shape->InternalShape; if(!shape) { Log::ErrInstance() << "NULL shape\n"; } return shape.get(); } CBulletRigidBody::CBulletRigidBody(std::shared_ptr _col, const CTransform& _start_transform, float mass) { m_setCollisionPtr(_col); auto shape = CBulletUnit::GetBulletCollision(_col); if (mass != 0.0f) { shape->calculateLocalInertia(mass, LocalInertia); } auto start_transform = CBulletPhysics::GetTransform(_start_transform); MotionState = std::make_unique(start_transform); btRigidBody::btRigidBodyConstructionInfo rbInfo ( mass, MotionState.get(), shape, LocalInertia ); Log::Instance() << "CBulletRigidBody::CBulletRigidBody mass is " << mass << Log::Endl; RigidBody = std::make_unique(rbInfo); RigidBody->setUserPointer(static_cast(this)); Log::Instance() << "[WORLD] Setting user pointer " << this << Log::Endl; RigidBody->setWorldTransform(start_transform); //world->addRigidBody(body); } void CBulletRigidBody::V_SetMass(float mass) { btVector3 localInertia(0, 0, 0); RigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia); RigidBody->setMassProps(mass, localInertia); } void CBulletRigidBody::SetLinearVelocity(const glm::vec3& vel) { RigidBody->setLinearVelocity(CBulletPhysics::GetVector(vel)); } void CBulletRigidBody::SetAngularVelocity(const glm::vec3& ang_vel) { RigidBody->setAngularVelocity(CBulletPhysics::GetVector(ang_vel)); } glm::vec3 CBulletRigidBody::GetLinearVelocity() const { return CBulletPhysics::GetVector(RigidBody->getLinearVelocity()); } glm::vec3 CBulletRigidBody::GetAngularVelocity() const { return CBulletPhysics::GetVector(RigidBody->getAngularVelocity()); } void CBulletRigidBody::Activate() { return RigidBody->activate(); } void CBulletRigidBody::SetTransform(const CTransform& trans) { auto btTrans = CBulletPhysics::GetTransform(trans); RigidBody->getMotionState()->setWorldTransform(btTrans); RigidBody->setWorldTransform(btTrans); } CTransform CBulletRigidBody::GetInternalTransform() { CTransform ret; CBulletPhysics::GetTransform(ret, RigidBody->getWorldTransform()); return ret; } void CBulletKinematicBody::SetTransform(const CTransform& trans) { auto btTrans = CBulletPhysics::GetTransform(trans); RigidBody->getMotionState()->setWorldTransform(btTrans); RigidBody->setWorldTransform(btTrans); } CTransform CBulletKinematicBody::GetInternalTransform() { CTransform ret; CBulletPhysics::GetTransform(ret, RigidBody->getWorldTransform()); return ret; } void CBulletGhostBody::SetTransform(const CTransform& trans) { auto btTrans = CBulletPhysics::GetTransform(trans); GhostObject->setWorldTransform(btTrans); } CTransform CBulletGhostBody::GetInternalTransform() { CTransform ret; CBulletPhysics::GetTransform(ret, GhostObject->getWorldTransform()); return ret; } CBulletKinematicBody::CBulletKinematicBody(std::shared_ptr _col, const CTransform& _start_transform) { m_setCollisionPtr(_col); auto shape = CBulletUnit::GetBulletCollision(_col); auto start_transform = CBulletPhysics::GetTransform(_start_transform); MotionState = std::make_unique(start_transform); btRigidBody::btRigidBodyConstructionInfo rbInfo ( 0.0f, //mass MUST be zero MotionState.get(), shape ); RigidBody = std::make_unique(rbInfo); RigidBody->setUserPointer(static_cast(this)); RigidBody->setCollisionFlags(RigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); Log::Instance() << "[WORLD] Setting user pointer " << this << Log::Endl; RigidBody->setActivationState(DISABLE_DEACTIVATION); //world->addRigidBody(body); } CBulletBody::~CBulletBody() { if(m_addedToWorld && RigidBody) { Log::Instance() << "[WORLD] RemoveRigidBody " << GetCollision()->Name << Log::Endl; m_addedToWorld->BulletWorld->removeRigidBody(RigidBody.get()); } } CBulletGhostBody::~CBulletGhostBody() { if(m_addedToWorld && GhostObject) { Log::Instance() << "[WORLD] RemoveGhostBody " << GetCollision()->Name << Log::Endl; m_addedToWorld->BulletWorld->removeCollisionObject(GhostObject.get()); } } CBulletWorld::~CBulletWorld() { Units.clear(); //TODO manual deletion? } CBulletGhostBody::CBulletGhostBody(std::shared_ptr _col, const CTransform& _start_transform) { m_setCollisionPtr(_col); auto shape = CBulletUnit::GetBulletCollision(_col); auto start_transform = CBulletPhysics::GetTransform(_start_transform); /* btCollisionWorld::ClosestConvexResultCallback cb; cb.hasHit(); cb.m_closestHitFraction; cb.m_collisionFilterGroup; cb.m_collisionFilterMask; cb.m_convexFromWorld; cb.m_convexToWorld; cb.m_hitCollisionObject; cb.m_hitNormalWorld; cb.m_hitPointWorld; */ GhostObject = std::make_unique(); //TODO defaulting to btPairCachingGhostObject? GhostObject->setUserPointer(static_cast(this)); Log::Instance() << "[WORLD] Setting user pointer " << this << Log::Endl; GhostObject->setCollisionShape(shape); GhostObject->setWorldTransform(start_transform); GhostObject->setCollisionFlags(GhostObject->getCollisionFlags() & ~btCollisionObject::CF_NO_CONTACT_RESPONSE); //world->addCollisionObject(GhostObject, btBroadphaseProxy::SensorTrigger, btBroadphaseProxy::AllFilter); } void CBulletWorld::V_AddRigidBody(CRigidBody* rgBody) { Log::Instance() << "[WORLD] V_AddRigidBody " << rgBody->GetCollision()->Name << Log::Endl; auto btBody = dynamic_cast(rgBody); BulletWorld->addRigidBody(btBody->RigidBody.get()); btBody->m_addedToWorld = this; } void CBulletWorld::V_AddKinematicBody(CKinematicBody* knBody) { Log::Instance() << "[WORLD] V_AddKinematicBody " << knBody->GetCollision()->Name << Log::Endl; auto btBody = dynamic_cast(knBody); BulletWorld->addRigidBody(btBody->RigidBody.get()); btBody->m_addedToWorld = this; } void CBulletWorld::V_AddGhostBody(CGhostBody* ghBody) { Log::Instance() << "[WORLD] V_AddGhostBody " << ghBody->GetCollision()->Name << Log::Endl; auto btBody = dynamic_cast(ghBody); BulletWorld->addCollisionObject(btBody->GhostObject.get(), btBroadphaseProxy::SensorTrigger, btBroadphaseProxy::AllFilter); btBody->m_addedToWorld = this; } void CBulletRigidBody::V_SetCollision(std::shared_ptr _col) { auto shape = CBulletUnit::GetBulletCollision(_col); RigidBody->setCollisionShape(shape); } void CBulletKinematicBody::V_SetCollision(std::shared_ptr _col) { auto shape = CBulletUnit::GetBulletCollision(_col); RigidBody->setCollisionShape(shape); } void CBulletGhostBody::V_SetCollision(std::shared_ptr _col) { auto shape = CBulletUnit::GetBulletCollision(_col); GhostObject->setCollisionShape(shape); } std::unique_ptr CBulletWorld::V_CreateRigidBody(std::shared_ptr _collision, const CTransform& startTransform, float mass) { return std::make_unique(_collision, startTransform, mass); } std::unique_ptr CBulletWorld::V_CreateKinematicBody(std::shared_ptr _collision, const CTransform& startTransform) { return std::make_unique(_collision, startTransform); } std::unique_ptr CBulletWorld::V_CreateGhostBody(std::shared_ptr _collision, const CTransform& startTransform) { return std::make_unique(_collision, startTransform); } LINK_PHYSICS_ENGINE_TO_CLASS(CBulletPhysics, bulletphysics); BulletConvexSweepCallback::BulletConvexSweepCallback ( const btVector3& from, const btVector3& to, IConvexSweepCallback* userCb ) : btCollisionWorld::ClosestConvexResultCallback(from, to), m_UserCallback(userCb) {} bool BulletConvexSweepCallback::needsCollision(btBroadphaseProxy* proxy0) const { auto proxyPtr = proxy0->m_clientObject; auto it = std::find_if(ExcludeList.begin(), ExcludeList.end(), [proxyPtr](CWorldUnit* unit) -> bool { auto btbody = dynamic_cast(unit); auto btghost = dynamic_cast(unit); if(btbody) { return static_cast(btbody->RigidBody.get()) == proxyPtr; } else if(btghost) { return static_cast(btghost->GhostObject.get()) == proxyPtr; } return false; }); if (it != ExcludeList.end()) { return false; } return ClosestConvexResultCallback::needsCollision(proxy0); } btScalar BulletConvexSweepCallback::addSingleResult(btCollisionWorld::LocalConvexResult& r, bool normalInWorldSpace) { if (!m_UserCallback) { return ClosestConvexResultCallback::addSingleResult(r, normalInWorldSpace); } CSweepHit hit; hit.HitFraction = r.m_hitFraction; hit.HitPointWorld = CBulletPhysics::GetVector(r.m_hitPointLocal); hit.HitNormalWorld = CBulletPhysics::GetVector ( normalInWorldSpace ? r.m_hitNormalLocal : r.m_hitCollisionObject->getWorldTransform().getBasis() * r.m_hitNormalLocal ); hit.HitUnit = static_cast(r.m_hitCollisionObject->getUserPointer()); if (!m_UserCallback->OnHit(hit)) { return 1.0f; //ignore this hit } return ClosestConvexResultCallback::addSingleResult(r, normalInWorldSpace); } BulletConvexSweepResult::BulletConvexSweepResult(const btCollisionWorld::ClosestConvexResultCallback& cb) { if (cb.hasHit()) { m_HasHit = true; m_Hit.HitFraction = cb.m_closestHitFraction; m_Hit.HitPointWorld = CBulletPhysics::GetVector(cb.m_hitPointWorld); m_Hit.HitNormalWorld = CBulletPhysics::GetVector(cb.m_hitNormalWorld); m_Hit.HitUnit = static_cast(cb.m_hitCollisionObject->getUserPointer()); } } bool BulletConvexSweepResult::HasHit() const { return m_HasHit; } const CSweepHit& BulletConvexSweepResult::GetHit() const { return m_Hit; } BulletContactPoint::BulletContactPoint(const btManifoldPoint& pt) : m_Pt(pt) {} glm::vec3 BulletContactPoint::GetPositionWorldOnA() const { return CBulletPhysics::GetVector(m_Pt.getPositionWorldOnA()); } glm::vec3 BulletContactPoint::GetPositionWorldOnB() const { return CBulletPhysics::GetVector(m_Pt.getPositionWorldOnB()); } glm::vec3 BulletContactPoint::GetNormalWorldOnB() const { return CBulletPhysics::GetVector(m_Pt.m_normalWorldOnB); } float BulletContactPoint::GetDistance() const { return m_Pt.getDistance(); } BulletContactManifold::BulletContactManifold(btPersistentManifold* m) : m_Manifold(m) {} int BulletContactManifold::GetNumContacts() const { return m_Manifold->getNumContacts(); } std::unique_ptr BulletContactManifold::GetContact(int index) const { return std::make_unique(m_Manifold->getContactPoint(index)); } CWorldUnit* BulletContactManifold::GetUnitA() const { return static_cast(m_Manifold->getBody0()->getUserPointer()); } CWorldUnit* BulletContactManifold::GetUnitB() const { return static_cast(m_Manifold->getBody1()->getUserPointer()); } BulletGhostOverlapResult::BulletGhostOverlapResult ( const btGhostObject* ghost, btBroadphaseInterface* broadphase, btDispatcher* dispatcher, const CBulletWorld* world ) : m_Ghost(ghost), m_Broadphase(broadphase), m_Dispatcher(dispatcher), m_World(world) {} int BulletGhostOverlapResult::GetNumOverlappingUnits() const { return m_Ghost->getNumOverlappingObjects(); } CWorldUnit* BulletGhostOverlapResult::GetOverlappingUnit(int index) const { auto* obj = m_Ghost->getOverlappingObject(index); return static_cast(obj->getUserPointer()); } btCollisionObject* CBulletBody::GetBulletObject() const { return static_cast(RigidBody.get()); } btCollisionObject* CBulletGhostBody::GetBulletObject() const { return static_cast(GhostObject.get()); } std::vector> BulletGhostOverlapResult::GetContactManifolds(CWorldUnit* other) const { auto* ghost = static_cast ( const_cast(m_Ghost) ); btDispatcher* dispatcher = m_World->BulletWorld->getDispatcher(); const btDispatcherInfo& dispatchInfo = m_World->BulletWorld->getDispatchInfo(); dispatcher->dispatchAllCollisionPairs ( ghost->getOverlappingPairCache(), dispatchInfo, dispatcher ); std::vector> result; if (!m_Ghost || !other) { Log::ErrInstance() << "GetContactManifolds ERR#0\n"; return result; } auto* otherBullet = dynamic_cast(other); if (!otherBullet) { Log::ErrInstance() << "GetContactManifolds ERR#1\n"; return result; } btCollisionObject* otherObj = otherBullet->GetBulletObject(); if (!otherObj) { Log::ErrInstance() << "GetContactManifolds ERR#2\n"; return result; } auto* pairCache = static_cast(const_cast(m_Ghost))->getOverlappingPairCache(); const auto& pairs = pairCache->getOverlappingPairArray(); for (int i = 0; i < pairs.size(); ++i) { const btBroadphasePair& pair = pairs[i]; btCollisionObject* a = static_cast(pair.m_pProxy0->m_clientObject); btCollisionObject* b = static_cast(pair.m_pProxy1->m_clientObject); if (!((a == m_Ghost && b == otherObj) || (b == m_Ghost && a == otherObj))) { Log::ErrInstance() << "GetContactManifolds ERR#3:1\n"; continue; } if (!pair.m_algorithm) { Log::ErrInstance() << "GetContactManifolds ERR#3:2\n"; continue; } btManifoldArray manifolds; pair.m_algorithm->getAllContactManifolds(manifolds); for (int m = 0; m < manifolds.size(); ++m) { btPersistentManifold* manifold = manifolds[m]; if (manifold->getNumContacts() > 0) { result.push_back(std::make_unique(manifold)); } } } return result; } std::unique_ptr CBulletWorld::V_ConvexSweep ( std::shared_ptr shape, const CTransform& from, const CTransform& to, IConvexSweepCallback* callback, std::vector excludeList ) { btTransform btFrom = CBulletPhysics::GetTransform(from); btTransform btTo = CBulletPhysics::GetTransform(to); BulletConvexSweepCallback cb ( btFrom.getOrigin(), btTo.getOrigin(), callback ); cb.ExcludeList = std::move(excludeList); auto _btcol = dynamic_cast(shape->Shape.get()); if(!_btcol) { Log::ErrInstance() << "NULL _btcol typed " << shape->GetType() << " with collision " << shape->Shape->GetType() << Log::Endl; } auto btcol = _btcol->InternalShape.get(); BulletWorld->convexSweepTest ( static_cast(btcol), btFrom, btTo, cb ); return std::make_unique(cb); } std::unique_ptr CBulletWorld::V_GetGhostOverlaps(const CGhostBody* ghost) const { auto _btcol = dynamic_cast(ghost); auto btGhost = _btcol->GhostObject.get(); return std::make_unique ( btGhost, Broadphase.get(), Dispatcher.get(), this ); }