tak/physicengine.cpp at master · voidah/tak · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include "physicengine.h"
#include <cassert>

#ifdef TAK_USE_BULLET_PHYSICS

PhysicEngine::PhysicEngine() : m_isValid(false)
{
}

PhysicEngine::~PhysicEngine()
{
}

void PhysicEngine::AddRigidBody(RigidBody* rigidBody)
{
    assert(m_isValid);
    m_dynamicsWorld->addRigidBody(rigidBody->GetInternalHandle());
}

void PhysicEngine::RemoveRigidBody(RigidBody* rigidBody)
{
    assert(m_isValid);
    m_dynamicsWorld->removeRigidBody(rigidBody->GetInternalHandle());
}

void PhysicEngine::AddAction(btActionInterface* action)
{
    assert(m_isValid);
    m_dynamicsWorld->addAction(action);
}

void PhysicEngine::RemoveAction(btActionInterface* action)
{
    assert(m_isValid);
    m_dynamicsWorld->removeAction(action);
}

void PhysicEngine::DrawDebug() const
{
    if(m_isValid)
        m_dynamicsWorld->debugDrawWorld();
}

void PhysicEngine::Update(float elapsedTime)
{
    assert(m_isValid);
    m_dynamicsWorld->stepSimulation(elapsedTime, 10);
}

bool PhysicEngine::RayCastClosestCollisionPoint(const Vector3f& from, const Vector3f& to, Vector3f& hitPoint, SceneNode*& hitNode) const
{
    btVector3 btFrom = btVector3(from.x, from.y, from.z);
    btVector3 btTo = btVector3(to.x, to.y, to.z);
    btCollisionWorld::ClosestRayResultCallback rayCallback(btFrom, btTo);
    m_dynamicsWorld->rayTest(btFrom, btTo, rayCallback);

    if(rayCallback.hasHit())
    {
        const btRigidBody* pBody = btRigidBody::upcast(rayCallback.m_collisionObject);

        hitNode = 0;
        if(pBody)
        {
            RigidBody* hitBody = (RigidBody*)pBody->getUserPointer();
            hitNode = hitBody->GetLinkedNode();
        }

        btVector3& hit = rayCallback.m_hitPointWorld;
        hitPoint.x = hit.x();
        hitPoint.y = hit.y();
        hitPoint.z = hit.z();
        return true;
    }

    return false;
}

void PhysicEngine::Init()
{
    m_broadphase = new btDbvtBroadphase();
    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
    m_solver = new btSequentialImpulseConstraintSolver;

    m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
    m_dynamicsWorld->setGravity(btVector3(0,-10,0));

    // TODO
    //m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration=0.0001f;

    // TODO
    m_debugDrawer.setDebugMode(btIDebugDraw::DBG_NoDebug);
    //m_debugDrawer.setDebugMode(btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawContactPoints);
    m_dynamicsWorld->setDebugDrawer(&m_debugDrawer);

    m_isValid = true;
}

void PhysicEngine::DeInit()
{
    assert(m_isValid);
    delete m_dynamicsWorld;
    delete m_solver;
    delete m_dispatcher;
    delete m_collisionConfiguration;
    delete m_broadphase;
}

btCollisionWorld* PhysicEngine::GetCollisionWorld()
{
    return m_dynamicsWorld;
}

btDynamicsWorld* PhysicEngine::GetDynamicsWorld()
{
    return m_dynamicsWorld;
}

btBroadphaseInterface* PhysicEngine::GetBroadphase()
{
    return m_broadphase;
}

#endif