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

Last change on this file since 1819 was 1819, checked in by touky, 8 years ago

BulletCharacterController is now readable by a human being.
BtPhysTest now implements it with the BtKineCC logic -just modify that now-.

File size: 13.5 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://sam.zoy.org/projects/COPYING.WTFPL 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//SweepCallback used for Swweep Tests.
46class ClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
47{
48public:
49        ClosestNotMeConvexResultCallback(btCollisionObject* NewMe, const vec3& NewUp, float MinSlopeDot) :
50                                        btCollisionWorld::ClosestConvexResultCallback(LOL2BTU_VEC3(vec3(.0f)), LOL2BTU_VEC3(vec3(.0f))),
51                                        m_me(NewMe),
52                                        m_up(NewUp),
53                                        m_min_slope_dot(MinSlopeDot) { }
54
55        virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& ConvexResult, bool NormalInWorld)
56        {
57                //We hit ourselves, FAIL
58                if (ConvexResult.m_hitCollisionObject == m_me)
59                        return btScalar(1.f);
60
61                vec3 WorldHitNomal(.0f);
62                if (NormalInWorld)
63                        WorldHitNomal = BT2LOL_VEC3(ConvexResult.m_hitNormalLocal);
64                else //need to transform Normal into worldspace
65                {
66                        btVector3 TmpWorldHitNormal = ConvexResult.m_hitCollisionObject->getWorldTransform().getBasis() * ConvexResult.m_hitNormalLocal;
67                        WorldHitNomal = BT2LOL_VEC3(TmpWorldHitNormal);
68                }
69
70                float DotUp = dot(m_up, WorldHitNomal);
71                //We hit below the accepted slope_dot, FAIL
72                if (DotUp < m_min_slope_dot)
73                        return btScalar(1.f);
74
75                //Continue to next.
76                return ClosestConvexResultCallback::addSingleResult(ConvexResult, NormalInWorld);
77        }
78protected:
79        btCollisionObject*      m_me;
80        const vec3                      m_up;
81        float                           m_min_slope_dot;
82};
83
84//When called, will try to remove Character controller from its collision.
85bool BulletKinematicCharacterController::RecoverFromPenetration(btCollisionWorld* CollisionWorld)
86{
87        bool HasPenetration = false;
88
89        //Retrieve all pair with us colliding.
90        CollisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghost_object->getOverlappingPairCache(), CollisionWorld->getDispatchInfo(), CollisionWorld->getDispatcher());
91        m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
92       
93        float MaxPen = .0f;
94        for (int i = 0; i < m_ghost_object->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
95        {
96                m_manifold_array.resize(0);
97
98                //this is the equivalent of the "Touch algorithm". Maybe refactor ?
99                btBroadphasePair* CollisionPair = &m_ghost_object->getOverlappingPairCache()->getOverlappingPairArray()[i];
100                if (CollisionPair->m_algorithm)
101                        CollisionPair->m_algorithm->getAllContactManifolds(m_manifold_array);
102               
103                for (int j = 0; j < m_manifold_array.size(); ++j)
104                {
105                        btPersistentManifold* CurMfold = m_manifold_array[j];
106                        //Normal direction differs if we're Body0
107                        float DirSign = CurMfold->getBody0() == m_ghost_object ? -1.f : 1.f;
108
109                        for (int k = 0; k < CurMfold->getNumContacts(); k++)
110                        {
111                                const btManifoldPoint& MfPoint = CurMfold->getContactPoint(k);
112                                float Dist = MfPoint.getDistance();
113                                if (Dist < .0f)
114                                {
115                                        if (Dist < MaxPen)
116                                        {
117                                                MaxPen = Dist;
118                                                m_touching_normal = BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign;
119                                        }
120                                        m_current_position += BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign * Dist * .2f;
121                                        HasPenetration = true;
122                                }
123                        }
124                }
125        }
126
127        btTransform GObjMx = m_ghost_object->getWorldTransform();
128        GObjMx.setOrigin(LOL2BTU_VEC3(m_current_position));
129        m_ghost_object->setWorldTransform(GObjMx);
130
131        return HasPenetration;
132}
133
134//When the Controller hits a wall, we modify the target so the controller will MoveStep along the wall.
135void BulletKinematicCharacterController::UpdateTargetOnHit(const vec3& HitNormal, float TangentMag, float NormalMag)
136{
137        vec3 Movedir = m_target_position - m_current_position;
138        float MoveLength = (float)length(Movedir);
139
140        if (MoveLength > SIMD_EPSILON)
141        {
142                Movedir = normalize(Movedir);
143
144                vec3 ReflectDir = normalize(GetReflectedDir(Movedir, HitNormal));
145                vec3 ParallelDir = ProjectDirOnNorm(ReflectDir, HitNormal);
146                vec3 PerpindicularDir = ProjectDirOnNormPerpindicular(ReflectDir, HitNormal);
147
148                m_target_position = m_current_position;
149
150                if (NormalMag != .0f)
151                        m_target_position += PerpindicularDir * NormalMag * MoveLength;
152        }
153}
154
155//Handles the Step-Up : Currently taking into account Stair step & Jump.
156void BulletKinematicCharacterController::StepUp(btCollisionWorld* world)
157{
158        // phase 1: up
159        vec3 UpDir = GetUpAxisDirections()[m_up_axis];
160        btTransform SweepStart, SweepEnd;
161        SweepStart.setIdentity();
162        SweepEnd.setIdentity();
163
164        m_target_position = m_current_position + UpDir * (m_step_height + (m_vertical_offset > 0.f ? m_vertical_offset : 0.f));
165
166        /* FIXME: Handle HasPenetration properly */
167        SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position + UpDir * (m_convex_shape->getMargin() + m_added_margin)));
168        SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));
169
170        ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, -UpDir, float(0.7071));
171        SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
172        SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
173       
174        if (m_do_gobject_sweep_test)
175                m_ghost_object->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, world->getDispatchInfo().m_allowedCcdPenetration);
176        else
177                world->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback);
178       
179        if (SweepCallback.hasHit())
180        {
181                // Only modify the position if the hit was a slope and not a wall or ceiling.
182                if(SweepCallback.m_hitNormalWorld.dot(LOL2BTU_VEC3(UpDir)) > .0f)
183                {
184                        // we moved up only a Fraction of the step height
185                        m_current_step_offset = m_step_height * SweepCallback.m_closestHitFraction;
186                        btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3)
187                        InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction);
188                        m_current_position = BT2LOLU_VEC3(InterpolPos);
189                }
190                m_vertical_velocity = .0f;
191                m_vertical_offset = .0f;
192        }
193        else
194        {
195                m_current_step_offset = m_step_height;
196                m_current_position = m_target_position;
197        }
198}
199
200//Handles the actual Movement. It actually moves in the 3 dimensions, function name is confusing.
201void BulletKinematicCharacterController::StepForwardAndStrafe(btCollisionWorld* CollisionWorld, const vec3& MoveStep)
202{
203        // phase 2: forward and strafe
204        m_target_position = m_current_position + MoveStep;
205        btTransform SweepStart, SweepEnd;
206        SweepStart.setIdentity();
207        SweepEnd.setIdentity();
208
209        float Fraction = 1.f;
210        float SqDist = .0f;
211
212        if (m_touching_contact && dot(m_normalized_direction, m_touching_normal) > .0f)
213                UpdateTargetOnHit(m_touching_normal);
214
215        //Let's loop on movement, until Movement fraction if below 0.01, which means we've reached our destination.
216        //Or until we'tried 10 times.
217        int MaxMoveLoop = 10;
218        while (Fraction > .01f && MaxMoveLoop-- > 0)
219        {
220                SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position));
221                SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));
222                vec3 SweepDirNeg(m_current_position - m_target_position);
223
224                ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, SweepDirNeg, .0f);
225                SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
226                SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
227
228                //The sweep test is done with an added margin, so we use it and then discard it
229                float SavedMargin = m_convex_shape->getMargin();
230                m_convex_shape->setMargin(SavedMargin + m_added_margin); //Apply Added Margin
231                if (m_do_gobject_sweep_test)
232                        m_ghost_object->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
233                else
234                        CollisionWorld->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
235                m_convex_shape->setMargin(SavedMargin); //Restore saved margin
236
237                Fraction -= SweepCallback.m_closestHitFraction;
238
239                if (SweepCallback.hasHit())
240                {       
241                        //We moved only a Fraction
242                        float HitDist = (float)length(BT2LOLU_VEC3(SweepCallback.m_hitPointWorld) - m_current_position);
243
244                        UpdateTargetOnHit(BT2LOL_VEC3(SweepCallback.m_hitNormalWorld));
245                        vec3 NewDir = m_target_position - m_current_position;
246                        SqDist = sqlength(NewDir);
247                        if (SqDist > SIMD_EPSILON)
248                        {
249                                NewDir = normalize(NewDir);
250                                //See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners."
251                                if (dot(NewDir, m_normalized_direction) <= .0f)
252                                        break;
253                        }
254                        else
255                                break;
256                }
257                else //We moved whole way
258                        m_current_position = m_target_position;
259        }
260}
261
262//Handles the Step-down : We go back on the ground at the end of the MoveStep.
263void BulletKinematicCharacterController::StepDown(btCollisionWorld* CollisionWorld, float DeltaTime)
264{
265        // phase 3: down
266        vec3 UpDir = GetUpAxisDirections()[m_up_axis];
267        btTransform SweepStart, SweepEnd;
268        SweepStart.setIdentity();
269        SweepEnd.setIdentity();
270
271        float DownVel = (m_vertical_velocity < 0.f ? -m_vertical_velocity : 0.f) * DeltaTime;
272        if (DownVel > .0f && DownVel < m_step_height && (m_was_on_ground || !m_was_jumping))
273                DownVel = m_step_height;
274
275        vec3 StepDrop = UpDir * (m_current_step_offset + DownVel);
276        m_target_position -= StepDrop;
277
278        SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position));
279        SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));
280
281        ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, UpDir, m_max_slope_cosine);
282        SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
283        SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
284       
285        if (m_do_gobject_sweep_test)
286                m_ghost_object->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
287        else
288                CollisionWorld->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
289
290        if (SweepCallback.hasHit())
291        {
292                // we dropped a Fraction of the height -> hit floor
293                btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3)
294                InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction);
295                m_current_position = BT2LOLU_VEC3(InterpolPos);
296                m_vertical_velocity = .0f;
297                m_vertical_offset = .0f;
298                m_was_jumping = false;
299        }
300        else // we dropped the full height
301                m_current_position = m_target_position;
302}
303
304//The PreStepis done in order to recover from any HasPenetration.
305void BulletKinematicCharacterController::PreStep(btCollisionWorld* CollisionWorld)
306{
307        int MaxPenetrationLoop = 0;
308        m_touching_contact = false;
309
310        while (RecoverFromPenetration(CollisionWorld))
311        {
312                MaxPenetrationLoop++;
313                m_touching_contact = true;
314                if (MaxPenetrationLoop > 4)
315                        break;
316        }
317
318        m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
319        m_target_position = m_current_position;
320}
321
322//And so we step :
323//StepUpfirst, then movement, then StepDownon the ground.
324void BulletKinematicCharacterController::PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime)
325{
326        // quick check...
327        if (!m_use_walk_direction && m_velocity_time_interval <= .0f)
328                return;         // no motion
329
330        m_was_on_ground = OnGround();
331
332        // Update fall velocity.
333        m_vertical_velocity -= m_gravity * DeltaTime;
334        if(m_vertical_velocity > .0f && m_vertical_velocity > m_jump_speed)
335                m_vertical_velocity = m_jump_speed;
336
337        if(m_vertical_velocity < .0f && btFabs(m_vertical_velocity) > btFabs(m_fall_speed))
338                m_vertical_velocity = -btFabs(m_fall_speed);
339        m_vertical_offset = m_vertical_velocity * DeltaTime;
340
341        btTransform NewTransform;
342        NewTransform = m_ghost_object->getWorldTransform();
343
344        vec3 MoveStep(.0f);
345        if (m_use_walk_direction)
346                MoveStep = m_walk_direction;
347        else
348        {
349                //Still have some time left for moving!
350                float dtMoving = (DeltaTime < m_velocity_time_interval) ? DeltaTime : m_velocity_time_interval;
351                m_velocity_time_interval -= DeltaTime;
352
353                // how far will we MoveStep while we are moving?
354                MoveStep = m_walk_direction * dtMoving;
355        }
356
357        //Okay, step !
358        StepUp(CollisionWorld);
359        StepForwardAndStrafe(CollisionWorld, MoveStep);
360        StepDown(CollisionWorld, DeltaTime);
361
362        //Movement finished, update World transform
363        NewTransform.setOrigin(LOL2BTU_VEC3(m_current_position));
364        m_ghost_object->setWorldTransform(NewTransform);
365}
366
367//should MoveStep Jump logic in EasyCC
368void BulletKinematicCharacterController::Jump()
369{
370        if (!CanJump())
371                return;
372
373        m_vertical_velocity = m_jump_speed;
374        m_was_jumping = true;
375}
376
377#endif // HAVE_PHYS_USE_BULLET
378#endif // USE_LOL_CTRLR_CHARAC
379
380        } /* namespace phys */
381
382} /* namespace lol */
Note: See TracBrowser for help on using the repository browser.