source: trunk/test/Physics/Src/BulletCharacterController.cpp @ 2183

Last change on this file since 2183 was 2183, checked in by sam, 7 years ago

build: fix the WTFPL site URL in all code comments.

File size: 8.9 KB
Line 
1//
2// Lol Engine
3//
4// Copyright: (c) 2010-2012 Sam Hocevar <sam@hocevar.net>
5//            (c) 2009-2012 Cédric Lecacheur <jordx@free.fr>
6//            (c) 2009-2012 Benjamin Huet <huet.benjamin@gmail.com>
7//   This program is free software; you can redistribute it and/or
8//   modify it under the terms of the Do What The Fuck You Want To
9//   Public License, Version 2, as published by Sam Hocevar. See
10//   http://www.wtfpl.net/ for more details.
11//
12
13#if defined HAVE_CONFIG_H
14#   include "config.h"
15#endif
16
17#define USE_LOL_CTRLR_CHARAC
18
19#ifdef HAVE_PHYS_USE_BULLET
20#include "core.h"
21#include <stdio.h>
22#include "../Include/LolBtPhysicsIntegration.h"
23#include "../Include/LolPhysics.h"
24#include "../Include/EasyCharacterController.h"
25#include "../Include/BulletCharacterController.h"
26//#include "LinearMath/btIDebugDraw.h"
27//#include "BulletCollision/CollisionDispatch/btGhostObject.h"
28//#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
29//#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
30//#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
31//#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
32//#include "LinearMath/btDefaultMotionState.h"
33#endif //HAVE_PHYS_USE_BULLET
34
35
36namespace lol
37{
38
39namespace phys
40{
41
42#ifdef USE_LOL_CTRLR_CHARAC
43#ifdef HAVE_PHYS_USE_BULLET
44
45//When called, will try to remove Character controller from its collision.
46bool BulletKinematicCharacterController::RecoverFromPenetration(btCollisionWorld* CollisionWorld)
47{
48    bool HasPenetration = false;
49
50    //Retrieve all pair with us colliding.
51    CollisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghost_object->getOverlappingPairCache(), CollisionWorld->getDispatchInfo(), CollisionWorld->getDispatcher());
52    m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
53
54    float MaxPen = .0f;
55    for (int i = 0; i < m_ghost_object->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
56    {
57        m_manifold_array.resize(0);
58
59        //this is the equivalent of the "Touch algorithm". Maybe refactor ?
60        btBroadphasePair* CollisionPair = &m_ghost_object->getOverlappingPairCache()->getOverlappingPairArray()[i];
61        if (CollisionPair->m_algorithm)
62            CollisionPair->m_algorithm->getAllContactManifolds(m_manifold_array);
63
64        for (int j = 0; j < m_manifold_array.size(); ++j)
65        {
66            btPersistentManifold* CurMfold = m_manifold_array[j];
67            //Normal direction differs if we're Body0
68            float DirSign = CurMfold->getBody0() == m_ghost_object ? -1.f : 1.f;
69
70            for (int k = 0; k < CurMfold->getNumContacts(); k++)
71            {
72                const btManifoldPoint& MfPoint = CurMfold->getContactPoint(k);
73                float Dist = MfPoint.getDistance();
74                if (Dist < .0f)
75                {
76                    if (Dist < MaxPen)
77                    {
78                        MaxPen = Dist;
79                        m_touching_normal = BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign;
80                    }
81                    m_current_position += BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign * Dist * .2f;
82                    HasPenetration = true;
83                }
84            }
85        }
86    }
87
88    btTransform GObjMx = m_ghost_object->getWorldTransform();
89    GObjMx.setOrigin(LOL2BTU_VEC3(m_current_position));
90    m_ghost_object->setWorldTransform(GObjMx);
91
92    return HasPenetration;
93}
94
95//When the Controller hits a wall, we modify the target so the controller will MoveStep along the wall.
96void BulletKinematicCharacterController::UpdateTargetOnHit(const vec3& HitNormal, float TangentMag, float NormalMag)
97{
98    vec3 Movedir = m_target_position - m_current_position;
99    float MoveLength = (float)length(Movedir);
100
101    if (MoveLength > SIMD_EPSILON)
102    {
103        Movedir = normalize(Movedir);
104
105        vec3 ReflectDir = normalize(GetReflectedDir(Movedir, HitNormal));
106        vec3 ParallelDir = ProjectDirOnNorm(ReflectDir, HitNormal);
107        vec3 PerpindicularDir = ProjectDirOnNormPerpindicular(ReflectDir, HitNormal);
108
109        m_target_position = m_current_position;
110
111        if (NormalMag != .0f)
112            m_target_position += PerpindicularDir * NormalMag * MoveLength;
113    }
114}
115
116//Handles the actual Movement. It actually moves in the 3 dimensions, function name is confusing.
117void BulletKinematicCharacterController::DoMove(btCollisionWorld* CollisionWorld, const vec3& MoveStep, float DeltaTime)
118{
119    // phase 2: forward and strafe
120    m_target_position = m_current_position + MoveStep;
121    btTransform SweepStart, SweepEnd;
122    SweepStart.setIdentity();
123    SweepEnd.setIdentity();
124
125    float Fraction = 1.f;
126    float SqDist = .0f;
127
128    if (m_touching_contact && dot(m_normalized_direction, m_touching_normal) > .0f)
129        UpdateTargetOnHit(m_touching_normal);
130
131    //Let's loop on movement, until Movement fraction if below 0.01, which means we've reached our destination.
132    //Or until we'tried 10 times.
133    int MaxMoveLoop = 10;
134    while (Fraction > .01f && MaxMoveLoop-- > 0)
135    {
136        SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position));
137        SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));
138        vec3 SweepDirNeg(m_current_position - m_target_position);
139
140        ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, SweepDirNeg, .0f);
141        SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
142        SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
143
144        //The sweep test is done with an added margin, so we use it and then discard it
145        float SavedMargin = m_convex_shape->getMargin();
146        m_convex_shape->setMargin(SavedMargin + m_added_margin); //Apply Added Margin
147        if (m_do_gobject_sweep_test)
148            m_ghost_object->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
149        else
150            CollisionWorld->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
151        m_convex_shape->setMargin(SavedMargin); //Restore saved margin
152
153        Fraction -= SweepCallback.m_closestHitFraction;
154
155        if (SweepCallback.hasHit())
156        {
157            //We moved only a Fraction
158            float HitDist = (float)length(BT2LOLU_VEC3(SweepCallback.m_hitPointWorld) - m_current_position);
159
160            UpdateTargetOnHit(BT2LOL_VEC3(SweepCallback.m_hitNormalWorld));
161            vec3 NewDir = m_target_position - m_current_position;
162            SqDist = sqlength(NewDir);
163            if (SqDist > SIMD_EPSILON)
164            {
165                NewDir = normalize(NewDir);
166                //See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners."
167                if (dot(NewDir, m_normalized_direction) <= .0f)
168                    break;
169            }
170            else
171                break;
172        }
173        else //We moved whole way
174            m_current_position = m_target_position;
175    }
176}
177
178//The PreStep is done in order to recover from any HasPenetration.
179void BulletKinematicCharacterController::PreStep(btCollisionWorld* CollisionWorld)
180{
181    int MaxPenetrationLoop = 0;
182    m_touching_contact = false;
183
184    while (RecoverFromPenetration(CollisionWorld))
185    {
186        MaxPenetrationLoop++;
187        m_touching_contact = true;
188        if (MaxPenetrationLoop > 4)
189            break;
190    }
191
192    m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
193    m_target_position = m_current_position;
194}
195
196//And so we step :
197//StepUpfirst, then movement, then StepDownon the ground.
198void BulletKinematicCharacterController::PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime)
199{
200    // quick check...
201    if (!m_use_walk_direction && m_velocity_time_interval <= .0f)
202        return;        // no motion
203
204    // Update fall velocity.
205    //m_velocity -= m_gravity * DeltaTime;
206
207    btTransform NewTransform;
208    NewTransform = m_ghost_object->getWorldTransform();
209
210    vec3 MoveStep(.0f);
211    if (m_use_walk_direction)
212        MoveStep = m_walk_direction;
213    else
214    {
215        //Still have some time left for moving!
216        float dtMoving = (DeltaTime < m_velocity_time_interval) ? DeltaTime : m_velocity_time_interval;
217        m_velocity_time_interval -= DeltaTime;
218
219        // how far will we MoveStep while we are moving?
220        MoveStep = m_walk_direction * dtMoving;
221    }
222
223    //Okay, step !
224    DoMove(CollisionWorld, MoveStep, DeltaTime);
225
226    //Movement finished, update World transform
227    NewTransform.setOrigin(LOL2BTU_VEC3(m_current_position));
228    m_ghost_object->setWorldTransform(NewTransform);
229}
230
231//should MoveStep Jump logic in EasyCC
232void BulletKinematicCharacterController::Jump()
233{
234    if (!CanJump())
235        return;
236
237    m_vertical_velocity = m_jump_speed;
238    m_was_jumping = true;
239}
240
241#endif // HAVE_PHYS_USE_BULLET
242#endif // USE_LOL_CTRLR_CHARAC
243
244    } /* namespace phys */
245
246} /* namespace lol */
Note: See TracBrowser for help on using the repository browser.