Changeset 1819


Ignore:
Timestamp:
Aug 25, 2012, 7:21:55 PM (11 years ago)
Author:
touky
Message:

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

Location:
trunk/test
Files:
7 edited

Legend:

Unmodified
Added
Removed

  • trunk/test/BtPhysTest.cpp

    r1803 r1819  
    105105                        if (idx != 1)
    106106                        {
    107                                 vec3 axis = vec3(.0f);
    108                                 axis[2 - idx] = 1;
    109                                 NewRotation = quat::rotate(90.f, axis);
     107                                vec3 NewAxis = vec3(.0f);
     108                                NewAxis[2 - idx] = 1;
     109                                NewRotation = quat::rotate(90.f, NewAxis);
    110110                        }
    111111

  • trunk/test/Physics/Include/BulletCharacterController.h

    r1802 r1819  
    2525#endif
    2626
     27#define USE_LOL_CTRLR_CHARAC
     28
    2729namespace lol
    2830{
    … …  
    3133        {
    3234
    33 #if 0
     35#ifdef USE_LOL_CTRLR_CHARAC
    3436#ifdef HAVE_PHYS_USE_BULLET
    3537
    … …  
    3739                ///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
    3840                ///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
    39                 class BulletKinematicCharacterController : public btCharacterControllerInterface
     41                class BulletKinematicCharacterController : public btActionInterface
    4042                {
     43                public:
     44                        BulletKinematicCharacterController(btPairCachingGhostObject* NewGhostObject, btConvexShape* NewConvexShape, float NewStepHeight, int NewUpAxis=1)
     45                        {
     46                                m_convex_shape = NewConvexShape;       
     47                                m_up_axis = NewUpAxis;
     48                                m_ghost_object = NewGhostObject;
     49                                m_step_height = NewStepHeight;
     50
     51                                m_added_margin = 0.02f;
     52                                m_walk_direction = vec3(.0f, .0f, .0f);
     53                                m_do_gobject_sweep_test = true;
     54                                m_turn_angle = .0f;
     55                                m_use_walk_direction = false; // Should remove walk direction, this doesn't work correctly.
     56                                m_velocity_time_interval = .0f;
     57                                m_vertical_velocity = .0f;
     58                                m_vertical_offset = .0f;
     59                                m_gravity = 9.8f * 3.f; // 3G acceleration.
     60                                m_fall_speed = 55.f; // Terminal velocity of a sky diver in m/s.
     61                                m_jump_speed = 10.f; // ?
     62                                m_was_on_ground = false;
     63                                m_was_jumping = false;
     64                                SetMaxSlope(45.f);
     65                        }
     66                        ~BulletKinematicCharacterController() { }
     67
    4168                protected:
    4269
    43                         btScalar m_halfHeight;
    44 
    45                         btPairCachingGhostObject* m_ghostObject;
    46                         btConvexShape*  m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
    47 
    48                         btScalar m_verticalVelocity;
    49                         btScalar m_verticalOffset;
    50                         btScalar m_fallSpeed;
    51                         btScalar m_jumpSpeed;
    52                         btScalar m_maxJumpHeight;
    53                         btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
    54                         btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
    55                         btScalar m_gravity;
    56 
    57                         btScalar m_turnAngle;
    58 
    59                         btScalar m_stepHeight;
    60 
    61                         btScalar        m_addedMargin;//@todo: remove this and fix the code
    62 
    63                         ///this is the desired walk direction, set by the user
    64                         btVector3       m_walkDirection;
    65                         btVector3       m_normalizedDirection;
     70                        static vec3* GetUpAxisDirections()
     71                        {
     72                                static vec3 sUpAxisDirection[3] = { vec3(1.0f, 0.0f, 0.0f), vec3(0.0f, 1.0f, 0.0f), vec3(0.0f, 0.0f, 1.0f) };
     73       
     74                                return sUpAxisDirection;
     75                        }
     76
     77                        //--------------------------
     78                        //CONVENIENCE FUNCTIONS
     79                        //--
     80
     81                        //Returns the reflection Direction of a ray going 'Direction' hitting a surface with Normal 'Normal' from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
     82                        vec3 GetReflectedDir(const vec3& Direction, const vec3& Normal)
     83                        {
     84                                return Direction - (2.f * dot(Direction, Normal) * Normal);
     85                        }
     86                        //Returns the portion of 'direction' that is parallel to 'normal'
     87                        vec3 ProjectDirOnNorm(const vec3& Direction, const vec3& Normal)
     88                        {
     89                                return Normal * dot(Direction, Normal);
     90                        }
     91                        //Returns the portion of 'Direction' that is perpindicular to 'Normal'
     92                        vec3 ProjectDirOnNormPerpindicular(const vec3& Direction, const vec3& Normal)
     93                        {
     94                                return Direction - ProjectDirOnNorm(Direction, Normal);
     95                        }
     96                        //Returns Ghost Object. -duh-
     97                        btPairCachingGhostObject* GetGhostObject()
     98                        {
     99                                return m_ghost_object;
     100                        }
     101
     102                        //"Real" war functions
     103                        bool RecoverFromPenetration(btCollisionWorld* CollisionWorld);
     104                        void UpdateTargetOnHit(const vec3& hit_normal, float TangentMag = .0f, float NormalMag = 1.f);
     105                        void StepUp(btCollisionWorld* CollisionWorld);
     106                        void StepForwardAndStrafe(btCollisionWorld* CollisionWorld, const vec3& MoveStep);
     107                        void StepDown(btCollisionWorld* CollisionWorld, float DeltaTime);
     108
     109                public:
     110                        ///btActionInterface interface : KEEP IN camelCase
     111                        virtual void updateAction(btCollisionWorld* CollisionWorld, float deltaTime)
     112                        {
     113                                PreStep(CollisionWorld);
     114                                PlayerStep(CollisionWorld, deltaTime);
     115                        }
     116
     117                        //not in the interface, but called above
     118                        void PreStep(btCollisionWorld* CollisionWorld);
     119                        void PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime);
     120
     121                        ///btActionInterface interface : KEEP IN camelCase
     122                        void debugDraw(btIDebugDraw* debugDrawer) { }
     123                       
     124                        void SetUpAxis(int NewAxis)
     125                        {
     126                                if (NewAxis < 0)
     127                                        NewAxis = 0;
     128                                if (NewAxis > 2)
     129                                        NewAxis = 2;
     130                                m_up_axis = NewAxis;
     131                        }
     132
     133                        //!!!!!! SHOULD DITCH THAT !!!!!!
     134                        //This should probably be called setPositionIncrementPerSimulatorStep.
     135                        //This is neither a Direction nor a velocity, but the amount to
     136                        //increment the position each simulation iteration, regardless
     137                        //of DeltaTime.
     138                        //This call will Reset any velocity set by SetVelocityForTimeInterval().
     139                        virtual void SetWalkDirection(const vec3& walkDirection)
     140                        {
     141                                m_use_walk_direction = true;
     142                                m_walk_direction = walkDirection;
     143                                m_normalized_direction = normalize(m_walk_direction);
     144                        }
     145
     146                        //Caller provides a velocity with which the character should MoveStep for
     147                        //the given time period.  After the time period, velocity is Reset
     148                        //to zero.
     149                        //This call will Reset any walk Direction set by SetWalkDirection().
     150                        //Negative time intervals will result in no motion.
     151                        virtual void SetVelocityForTimeInterval(const vec3& velocity, float timeInterval)
     152                        {
     153                                m_use_walk_direction = false;
     154                                m_walk_direction = velocity;
     155                                m_normalized_direction = normalize(m_walk_direction);
     156                                m_velocity_time_interval = timeInterval;
     157                        }
     158
     159                        //Usefulness ?
     160                        void Reset() { }
     161                        void Warp(const vec3& NewOrigin)
     162                        {
     163                                btTransform NewTransform;
     164                                NewTransform.setIdentity();
     165                                NewTransform.setOrigin(LOL2BTU_VEC3(NewOrigin));
     166                                m_ghost_object->setWorldTransform(NewTransform);
     167                        }
     168
     169                        //External Setup
     170                        //--
     171
     172                        void SetFallSpeed(float NewFallSpeed)                   { m_fall_speed = NewFallSpeed; }
     173                        void SetJumpSpeed(float NewJumpSpeed)                   { m_jump_speed = NewJumpSpeed; }
     174                        void SetMaxJumpHeight(float NewMaxJumpHeight)   { m_max_jump_height = NewMaxJumpHeight; }
     175
     176                        //Jump logic will go in EasyCC
     177                        bool CanJump() const                                                    { return OnGround(); }
     178                        void Jump();
     179
     180                        //NewGravity functions
     181                        void SetGravity(float NewGravity)                               { m_gravity = NewGravity; }
     182                        float GetGravity() const                                                { return m_gravity; }
     183
     184                        //The max slope determines the maximum angle that the controller can walk up.
     185                        //The slope angle is measured in radians.
     186                        void SetMaxSlope(float NewSlopeRadians)                 { m_max_slope_radians = NewSlopeRadians; m_max_slope_cosine = lol::cos(NewSlopeRadians); }
     187                        float GetMaxSlope() const                                               { return m_max_slope_radians; }
     188
     189                        void SetUseGhostSweepTest(bool UseGObjectSweepTest) { m_do_gobject_sweep_test = UseGObjectSweepTest; }
     190
     191                        bool OnGround() const                                                   { return m_vertical_velocity == .0f && m_vertical_offset == .0f; }
     192
     193                private:
     194
     195                        btPairCachingGhostObject*       m_ghost_object;
     196                        btConvexShape*                          m_convex_shape; //is also in m_ghost_object, but it needs to be convex, so we store it here to avoid upcast
     197
     198                        //keep track of the contact manifolds
     199                        btManifoldArray                         m_manifold_array;
     200
     201                        float                                           m_half_height;
     202                        float                                           m_velocity_time_interval;
     203                        float                                           m_vertical_velocity;
     204                        float                                           m_vertical_offset;
     205                        float                                           m_fall_speed;
     206                        float                                           m_jump_speed;
     207                        float                                           m_max_jump_height;
     208                        float                                           m_max_slope_radians; // Slope angle that is set (used for returning the exact value)
     209                        float                                           m_max_slope_cosine;  // Cosine equivalent of m_max_slope_radians (calculated once when set, for optimization)
     210                        float                                           m_gravity;
     211                        float                                           m_turn_angle;
     212                        float                                           m_step_height;
     213                        float                                           m_added_margin;//@todo: remove this and fix the code
     214
     215                        ///this is the desired walk Direction, set by the user
     216                        vec3                                            m_walk_direction;
     217                        vec3                                            m_normalized_direction;
    66218
    67219                        //some internal variables
    68                         btVector3 m_currentPosition;
    69                         btScalar  m_currentStepOffset;
    70                         btVector3 m_targetPosition;
    71 
    72                         ///keep track of the contact manifolds
    73                         btManifoldArray m_manifoldArray;
    74 
    75                         bool m_touchingContact;
    76                         btVector3 m_touchingNormal;
    77 
    78                         bool  m_wasOnGround;
    79                         bool  m_wasJumping;
    80                         bool    m_useGhostObjectSweepTest;
    81                         bool    m_useWalkDirection;
    82                         btScalar        m_velocityTimeInterval;
    83                         int m_upAxis;
    84 
    85                         static btVector3* getUpAxisDirections();
    86 
    87                         btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
    88                         btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
    89                         btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
    90 
    91                         bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
    92                         void stepUp (btCollisionWorld* collisionWorld);
    93                         void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
    94                         void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
    95                         void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
    96                 public:
    97                         BulletKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
    98                         ~BulletKinematicCharacterController ();
    99 
    100 
    101                         ///btActionInterface interface
    102                         virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
    103                         {
    104                                 preStep ( collisionWorld);
    105                                 playerStep (collisionWorld, deltaTime);
    106                         }
    107 
    108                         ///btActionInterface interface
    109                         void    debugDraw(btIDebugDraw* debugDrawer);
    110 
    111                         void setUpAxis (int axis)
    112                         {
    113                                 if (axis < 0)
    114                                         axis = 0;
    115                                 if (axis > 2)
    116                                         axis = 2;
    117                                 m_upAxis = axis;
    118                         }
    119 
    120                         /// This should probably be called setPositionIncrementPerSimulatorStep.
    121                         /// This is neither a direction nor a velocity, but the amount to
    122                         ///     increment the position each simulation iteration, regardless
    123                         ///     of dt.
    124                         /// This call will reset any velocity set by setVelocityForTimeInterval().
    125                         virtual void    setWalkDirection(const btVector3& walkDirection);
    126 
    127                         /// Caller provides a velocity with which the character should move for
    128                         ///     the given time period.  After the time period, velocity is reset
    129                         ///     to zero.
    130                         /// This call will reset any walk direction set by setWalkDirection().
    131                         /// Negative time intervals will result in no motion.
    132                         virtual void setVelocityForTimeInterval(const btVector3& velocity,
    133                                 btScalar timeInterval);
    134 
    135                         void reset ();
    136                         void warp (const btVector3& origin);
    137 
    138                         void preStep (  btCollisionWorld* collisionWorld);
    139                         void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
    140 
    141                         void setFallSpeed (btScalar fallSpeed);
    142                         void setJumpSpeed (btScalar jumpSpeed);
    143                         void setMaxJumpHeight (btScalar maxJumpHeight);
    144                         bool canJump () const;
    145 
    146                         void jump ();
    147 
    148                         void setGravity(btScalar gravity);
    149                         btScalar getGravity() const;
    150 
    151                         /// The max slope determines the maximum angle that the controller can walk up.
    152                         /// The slope angle is measured in radians.
    153                         void setMaxSlope(btScalar slopeRadians);
    154                         btScalar getMaxSlope() const;
    155 
    156                         btPairCachingGhostObject* getGhostObject();
    157                         void    setUseGhostSweepTest(bool useGhostObjectSweepTest)
    158                         {
    159                                 m_useGhostObjectSweepTest = useGhostObjectSweepTest;
    160                         }
    161 
    162                         bool onGround () const;
     220                        vec3                                            m_current_position;
     221                        float                                           m_current_step_offset;
     222                        vec3                                            m_target_position;
     223
     224                        vec3                                            m_touching_normal;
     225                        bool                                            m_touching_contact;
     226
     227                        bool                                            m_was_on_ground;
     228                        bool                                            m_was_jumping;
     229                        bool                                            m_do_gobject_sweep_test;
     230                        bool                                            m_use_walk_direction;
     231                        int                                                     m_up_axis;
    163232                };
    164233
    165234#endif // HAVE_PHYS_USE_BULLET
    166 #endif // 0
     235#endif // USE_LOL_CTRLR_CHARAC
    167236
    168237        } /* namespace phys */

  • trunk/test/Physics/Include/EasyCharacterController.h

    r1795 r1819  
    2424#include "core.h"
    2525#include "EasyPhysics.h"
     26#include "BulletCharacterController.h"
    2627#include <BulletDynamics/Character/btKinematicCharacterController.h>
    2728#endif
    … …  
    8384
    8485        btPairCachingGhostObject*               m_pair_caching_object;
    85         btKinematicCharacterController* m_character;
     86        //btKinematicCharacterController*       m_character;
     87        BulletKinematicCharacterController* m_character;
    8688
    8789        float                                                   m_step_height;

  • trunk/test/Physics/Include/EasyPhysics.h

    r1782 r1819  
    2323#include <bullet/btBulletCollisionCommon.h>
    2424#include <bullet/BulletCollision/CollisionDispatch/btGhostObject.h>
    25 #endif
     25#endif //HAVE_PHYS_USE_BULLET
    2626
    2727namespace lol
    … …  
    157157
    158158        //Base/Attachment logic
    159         Array<EasyPhysic*>                                                      m_based_physic_list;    //List of objects based on this : this object moves, its based object move with it.
     159        Array<EasyPhysic*>                                                      m_based_physic_list;    //List of objects based on this : this object moves, its based object MoveStep with it.
    160160        EasyPhysic*                                                                     m_base_physic;                  //Base for this object : The base moves, the object moves with it.
    161161        bool                                                                            m_base_lock_location;   //when this is TRUE, location moves with rotation change.

  • trunk/test/Physics/Include/LolPhysics.h

    r1782 r1819  
    119119        }
    120120
    121         //Reap-Off of the btKinematicClosestNotMeRayResultCallback
    122         class LolClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
     121        //Rip-Off of the btKinematicClosestNotMeRayResultCallback
     122        class ClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
    123123        {
    124124        public:
    125                 LolClosestNotMeRayResultCallback(btCollisionObject* Me, const btVector3& rayFromWorld, const btVector3& rayToWorld) :
     125                ClosestNotMeRayResultCallback(btCollisionObject* Me, const btVector3& rayFromWorld, const btVector3& rayToWorld) :
    126126                  btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld)
    127127                {
    … …  
    169169                        {
    170170                                if (SourceCaster)
    171                                         BtRayResult_Closest = new LolClosestNotMeRayResultCallback(SourceCaster->m_collision_object, LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));
     171                                        BtRayResult_Closest = new ClosestNotMeRayResultCallback(SourceCaster->m_collision_object, LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));
    172172                                else
    173173                                        BtRayResult_Closest = new btCollisionWorld::ClosestRayResultCallback(LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));

  • trunk/test/Physics/Src/BulletCharacterController.cpp

    r1802 r1819  
    1515#endif
    1616
     17#define USE_LOL_CTRLR_CHARAC
     18
     19#ifdef HAVE_PHYS_USE_BULLET
     20#include "core.h"
     21#include <stdio.h>
    1722#include "../Include/LolBtPhysicsIntegration.h"
    1823#include "../Include/LolPhysics.h"
    … …  
    2631//#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
    2732//#include "LinearMath/btDefaultMotionState.h"
     33#endif //HAVE_PHYS_USE_BULLET
    2834
    2935
    … …  
    3137{
    3238
    33         namespace phys
    34         {
    35 
    36 #if 0
     39namespace phys
     40{
     41
     42#ifdef USE_LOL_CTRLR_CHARAC
    3743#ifdef HAVE_PHYS_USE_BULLET
    3844
    39 // static helper method
    40 static btVector3
    41 getNormalizedVector(const btVector3& v)
    42 {
    43         btVector3 n = v.normalized();
    44         if (n.length() < SIMD_EPSILON) {
    45                 n.setValue(0, 0, 0);
    46         }
    47         return n;
    48 }
    49 
    50 
    51 ///@todo Interact with dynamic objects,
    52 ///Ride kinematicly animated platforms properly
    53 ///More realistic (or maybe just a config option) falling
    54 /// -> Should integrate falling velocity manually and use that in stepDown()
    55 ///Support jumping
    56 ///Support ducking
    57 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
     45//SweepCallback used for Swweep Tests.
     46class ClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
    5847{
    5948public:
    60         btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
    61         {
    62                 m_me = me;
    63         }
    64 
    65         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
    66         {
    67                 if (rayResult.m_collisionObject == m_me)
    68                         return 1.0;
    69 
    70                 return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
     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);
    7177        }
    7278protected:
    73         btCollisionObject* m_me;
     79        btCollisionObject*      m_me;
     80        const vec3                      m_up;
     81        float                           m_min_slope_dot;
    7482};
    7583
    76 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
    77 {
    78 public:
    79         btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
    80         : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
    81         , m_me(me)
    82         , m_up(up)
    83         , m_minSlopeDot(minSlopeDot)
    84         {
    85         }
    86 
    87         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
    88         {
    89                 if (convexResult.m_hitCollisionObject == m_me)
    90                         return btScalar(1.0);
    91 
    92                 btVector3 hitNormalWorld;
    93                 if (normalInWorldSpace)
     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)
    94104                {
    95                         hitNormalWorld = convexResult.m_hitNormalLocal;
    96                 } else
    97                 {
    98                         ///need to transform normal into worldspace
    99                         hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
    100                 }
    101 
    102                 btScalar dotUp = m_up.dot(hitNormalWorld);
    103                 if (dotUp < m_minSlopeDot) {
    104                         return btScalar(1.0);
    105                 }
    106 
    107                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
    108         }
    109 protected:
    110         btCollisionObject* m_me;
    111         const btVector3 m_up;
    112         btScalar m_minSlopeDot;
    113 };
    114 
    115 /*
    116  * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
    117  *
    118  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
    119  */
    120 btVector3 BulletKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
    121 {
    122         return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
    123 }
    124 
    125 /*
    126  * Returns the portion of 'direction' that is parallel to 'normal'
    127  */
    128 btVector3 BulletKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
    129 {
    130         btScalar magnitude = direction.dot(normal);
    131         return normal * magnitude;
    132 }
    133 
    134 /*
    135  * Returns the portion of 'direction' that is perpindicular to 'normal'
    136  */
    137 btVector3 BulletKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
    138 {
    139         return direction - parallelComponent(direction, normal);
    140 }
    141 
    142 BulletKinematicCharacterController::BulletKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
    143 {
    144         m_upAxis = upAxis;
    145         m_addedMargin = 0.02;
    146         m_walkDirection.setValue(0,0,0);
    147         m_useGhostObjectSweepTest = true;
    148         m_ghostObject = ghostObject;
    149         m_stepHeight = stepHeight;
    150         m_turnAngle = btScalar(0.0);
    151         m_convexShape=convexShape;     
    152         m_useWalkDirection = true;      // use walk direction by default, legacy behavior
    153         m_velocityTimeInterval = 0.0;
    154         m_verticalVelocity = 0.0;
    155         m_verticalOffset = 0.0;
    156         m_gravity = 9.8 * 3 ; // 3G acceleration.
    157         m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
    158         m_jumpSpeed = 10.0; // ?
    159         m_wasOnGround = false;
    160         m_wasJumping = false;
    161         setMaxSlope(btRadians(45.0));
    162 }
    163 
    164 BulletKinematicCharacterController::~BulletKinematicCharacterController ()
    165 {
    166 }
    167 
    168 btPairCachingGhostObject* BulletKinematicCharacterController::getGhostObject()
    169 {
    170         return m_ghostObject;
    171 }
    172 
    173 bool BulletKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
    174 {
    175 
    176         bool penetration = false;
    177 
    178         collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
    179 
    180         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
    181        
    182         btScalar maxPen = btScalar(0.0);
    183         for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
    184         {
    185                 m_manifoldArray.resize(0);
    186 
    187                 btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
    188                
    189                 if (collisionPair->m_algorithm)
    190                         collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
    191 
    192                
    193                 for (int j=0;j<m_manifoldArray.size();j++)
    194                 {
    195                         btPersistentManifold* manifold = m_manifoldArray[j];
    196                         btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
    197                         for (int p=0;p<manifold->getNumContacts();p++)
     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++)
    198110                        {
    199                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
    200 
    201                                 btScalar dist = pt.getDistance();
    202 
    203                                 if (dist < 0.0)
     111                                const btManifoldPoint& MfPoint = CurMfold->getContactPoint(k);
     112                                float Dist = MfPoint.getDistance();
     113                                if (Dist < .0f)
    204114                                {
    205                                         if (dist < maxPen)
     115                                        if (Dist < MaxPen)
    206116                                        {
    207                                                 maxPen = dist;
    208                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
    209 
     117                                                MaxPen = Dist;
     118                                                m_touching_normal = BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign;
    210119                                        }
    211                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
    212                                         penetration = true;
    213                                 } else {
    214                                         //printf("touching %f\n", dist);
     120                                        m_current_position += BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign * Dist * .2f;
     121                                        HasPenetration = true;
    215122                                }
    216123                        }
    217                        
    218                         //manifold->clearManifold();
    219124                }
    220125        }
    221         btTransform newTrans = m_ghostObject->getWorldTransform();
    222         newTrans.setOrigin(m_currentPosition);
    223         m_ghostObject->setWorldTransform(newTrans);
    224 //      printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
    225         return penetration;
    226 }
    227 
    228 void BulletKinematicCharacterController::stepUp ( btCollisionWorld* world)
     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)
    229157{
    230158        // phase 1: up
    231         btTransform start, end;
    232         m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
    233 
    234         start.setIdentity ();
    235         end.setIdentity ();
    236 
    237         /* FIXME: Handle penetration properly */
    238         start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
    239         end.setOrigin (m_targetPosition);
    240 
    241         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
    242         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
    243         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
     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;
    244173       
    245         if (m_useGhostObjectSweepTest)
    246         {
    247                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
    248         }
     174        if (m_do_gobject_sweep_test)
     175                m_ghost_object->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, world->getDispatchInfo().m_allowedCcdPenetration);
    249176        else
    250         {
    251                 world->convexSweepTest (m_convexShape, start, end, callback);
    252         }
     177                world->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback);
    253178       
    254         if (callback.hasHit())
     179        if (SweepCallback.hasHit())
    255180        {
    256181                // Only modify the position if the hit was a slope and not a wall or ceiling.
    257                 if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
     182                if(SweepCallback.m_hitNormalWorld.dot(LOL2BTU_VEC3(UpDir)) > .0f)
    258183                {
    259                         // we moved up only a fraction of the step height
    260                         m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
    261                         m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
     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);
    262189                }
    263                 m_verticalVelocity = 0.0;
    264                 m_verticalOffset = 0.0;
    265         } else {
    266                 m_currentStepOffset = m_stepHeight;
    267                 m_currentPosition = m_targetPosition;
    268         }
    269 }
    270 
    271 void BulletKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
    272 {
    273         btVector3 movementDirection = m_targetPosition - m_currentPosition;
    274         btScalar movementLength = movementDirection.length();
    275         if (movementLength>SIMD_EPSILON)
    276         {
    277                 movementDirection.normalize();
    278 
    279                 btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
    280                 reflectDir.normalize();
    281 
    282                 btVector3 parallelDir, perpindicularDir;
    283 
    284                 parallelDir = parallelComponent (reflectDir, hitNormal);
    285                 perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
    286 
    287                 m_targetPosition = m_currentPosition;
    288                 if (0)//tangentMag != 0.0)
    289                 {
    290                         btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
    291 //                      printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
    292                         m_targetPosition +=  parComponent;
     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;
    293256                }
    294 
    295                 if (normalMag != 0.0)
    296                 {
    297                         btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
    298 //                      printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
    299                         m_targetPosition += perpComponent;
    300                 }
    301         } else
    302         {
    303 //              printf("movementLength don't normalize a zero vector\n");
    304         }
    305 }
    306 
    307 void BulletKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
    308 {
    309         // printf("m_normalizedDirection=%f,%f,%f\n",
    310         //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    311         // phase 2: forward and strafe
    312         btTransform start, end;
    313         m_targetPosition = m_currentPosition + walkMove;
    314 
    315         start.setIdentity ();
    316         end.setIdentity ();
     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;
    317284       
    318         btScalar fraction = 1.0;
    319         btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
    320 //      printf("distance2=%f\n",distance2);
    321 
    322         if (m_touchingContact)
    323         {
    324                 if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
    325                 {
    326                         updateTargetPositionBasedOnCollision (m_touchingNormal);
    327                 }
    328         }
    329 
    330         int maxIter = 10;
    331 
    332         while (fraction > btScalar(0.01) && maxIter-- > 0)
    333         {
    334                 start.setOrigin (m_currentPosition);
    335                 end.setOrigin (m_targetPosition);
    336                 btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
    337 
    338                 btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
    339                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
    340                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
    341 
    342 
    343                 btScalar margin = m_convexShape->getMargin();
    344                 m_convexShape->setMargin(margin + m_addedMargin);
    345 
    346 
    347                 if (m_useGhostObjectSweepTest)
    348                 {
    349                         m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
    350                 } else
    351                 {
    352                         collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
    353                 }
    354                
    355                 m_convexShape->setMargin(margin);
    356 
    357                
    358                 fraction -= callback.m_closestHitFraction;
    359 
    360                 if (callback.hasHit())
    361                 {       
    362                         // we moved only a fraction
    363                         btScalar hitDistance;
    364                         hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
    365 
    366 //                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
    367 
    368                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
    369                         btVector3 currentDir = m_targetPosition - m_currentPosition;
    370                         distance2 = currentDir.length2();
    371                         if (distance2 > SIMD_EPSILON)
    372                         {
    373                                 currentDir.normalize();
    374                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
    375                                 if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
    376                                 {
    377                                         break;
    378                                 }
    379                         } else
    380                         {
    381 //                              printf("currentDir: don't normalize a zero vector\n");
    382                                 break;
    383                         }
    384 
    385                 } else {
    386                         // we moved whole way
    387                         m_currentPosition = m_targetPosition;
    388                 }
    389 
    390         //      if (callback.m_closestHitFraction == 0.f)
    391         //              break;
    392 
    393         }
    394 }
    395 
    396 void BulletKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
    397 {
    398         btTransform start, end;
    399 
    400         // phase 3: down
    401         /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
    402         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
    403         btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
    404         btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
    405         m_targetPosition -= (step_drop + gravity_drop);*/
    406 
    407         btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
    408         if(downVelocity > 0.0 && downVelocity < m_stepHeight
    409                 && (m_wasOnGround || !m_wasJumping))
    410         {
    411                 downVelocity = m_stepHeight;
    412         }
    413 
    414         btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
    415         m_targetPosition -= step_drop;
    416 
    417         start.setIdentity ();
    418         end.setIdentity ();
    419 
    420         start.setOrigin (m_currentPosition);
    421         end.setOrigin (m_targetPosition);
    422 
    423         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
    424         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
    425         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
    426        
    427         if (m_useGhostObjectSweepTest)
    428         {
    429                 m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
    430         } else
    431         {
    432                 collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
    433         }
    434 
    435         if (callback.hasHit())
    436         {
    437                 // we dropped a fraction of the height -> hit floor
    438                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
    439                 m_verticalVelocity = 0.0;
    440                 m_verticalOffset = 0.0;
    441                 m_wasJumping = false;
    442         } else {
    443                 // we dropped the full height
    444                
    445                 m_currentPosition = m_targetPosition;
    446         }
    447 }
    448 
    449 
    450 
    451 void BulletKinematicCharacterController::setWalkDirection
    452 (
    453 const btVector3& walkDirection
    454 )
    455 {
    456         m_useWalkDirection = true;
    457         m_walkDirection = walkDirection;
    458         m_normalizedDirection = getNormalizedVector(m_walkDirection);
    459 }
    460 
    461 
    462 
    463 void BulletKinematicCharacterController::setVelocityForTimeInterval
    464 (
    465 const btVector3& velocity,
    466 btScalar timeInterval
    467 )
    468 {
    469 //      printf("setVelocity!\n");
    470 //      printf("  interval: %f\n", timeInterval);
    471 //      printf("  velocity: (%f, %f, %f)\n",
    472 //               velocity.x(), velocity.y(), velocity.z());
    473 
    474         m_useWalkDirection = false;
    475         m_walkDirection = velocity;
    476         m_normalizedDirection = getNormalizedVector(m_walkDirection);
    477         m_velocityTimeInterval = timeInterval;
    478 }
    479 
    480 
    481 
    482 void BulletKinematicCharacterController::reset ()
    483 {
    484 }
    485 
    486 void BulletKinematicCharacterController::warp (const btVector3& origin)
    487 {
    488         btTransform xform;
    489         xform.setIdentity();
    490         xform.setOrigin (origin);
    491         m_ghostObject->setWorldTransform (xform);
    492 }
    493 
    494 
    495 void BulletKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
    496 {
    497        
    498         int numPenetrationLoops = 0;
    499         m_touchingContact = false;
    500         while (recoverFromPenetration (collisionWorld))
    501         {
    502                 numPenetrationLoops++;
    503                 m_touchingContact = true;
    504                 if (numPenetrationLoops > 4)
    505                 {
    506                         //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
     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)
    507315                        break;
    508                 }
    509         }
    510 
    511         m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
    512         m_targetPosition = m_currentPosition;
    513 //      printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
    514 
    515        
    516 }
    517 
    518 #include <stdio.h>
    519 
    520 void BulletKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
    521 {
    522 //      printf("playerStep(): ");
    523 //      printf("  dt = %f", dt);
    524 
     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{
    525326        // quick check...
    526         if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
    527 //              printf("\n");
     327        if (!m_use_walk_direction && m_velocity_time_interval <= .0f)
    528328                return;         // no motion
    529         }
    530 
    531         m_wasOnGround = onGround();
     329
     330        m_was_on_ground = OnGround();
    532331
    533332        // Update fall velocity.
    534         m_verticalVelocity -= m_gravity * dt;
    535         if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
    536         {
    537                 m_verticalVelocity = m_jumpSpeed;
    538         }
    539         if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
    540         {
    541                 m_verticalVelocity = -btFabs(m_fallSpeed);
    542         }
    543         m_verticalOffset = m_verticalVelocity * dt;
    544 
    545 
    546         btTransform xform;
    547         xform = m_ghostObject->getWorldTransform ();
    548 
    549 //      printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
    550 //      printf("walkSpeed=%f\n",walkSpeed);
    551 
    552         stepUp (collisionWorld);
    553         if (m_useWalkDirection) {
    554                 stepForwardAndStrafe (collisionWorld, m_walkDirection);
    555         } else {
    556                 //printf("  time: %f", m_velocityTimeInterval);
    557                 // still have some time left for moving!
    558                 btScalar dtMoving =
    559                         (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
    560                 m_velocityTimeInterval -= dt;
    561 
    562                 // how far will we move while we are moving?
    563                 btVector3 move = m_walkDirection * dtMoving;
    564 
    565                 //printf("  dtMoving: %f", dtMoving);
    566 
    567                 // okay, step
    568                 stepForwardAndStrafe(collisionWorld, move);
    569         }
    570         stepDown (collisionWorld, dt);
    571 
    572         // printf("\n");
    573 
    574         xform.setOrigin (m_currentPosition);
    575         m_ghostObject->setWorldTransform (xform);
    576 }
    577 
    578 void BulletKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
    579 {
    580         m_fallSpeed = fallSpeed;
    581 }
    582 
    583 void BulletKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
    584 {
    585         m_jumpSpeed = jumpSpeed;
    586 }
    587 
    588 void BulletKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
    589 {
    590         m_maxJumpHeight = maxJumpHeight;
    591 }
    592 
    593 bool BulletKinematicCharacterController::canJump () const
    594 {
    595         return onGround();
    596 }
    597 
    598 void BulletKinematicCharacterController::jump ()
    599 {
    600         if (!canJump())
     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())
    601371                return;
    602372
    603         m_verticalVelocity = m_jumpSpeed;
    604         m_wasJumping = true;
    605 
    606 #if 0
    607         currently no jumping.
    608         btTransform xform;
    609         m_rigidBody->getMotionState()->getWorldTransform (xform);
    610         btVector3 up = xform.getBasis()[1];
    611         up.normalize ();
    612         btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
    613         m_rigidBody->applyCentralImpulse (up * magnitude);
    614 #endif
    615 }
    616 
    617 void BulletKinematicCharacterController::setGravity(btScalar gravity)
    618 {
    619         m_gravity = gravity;
    620 }
    621 
    622 btScalar BulletKinematicCharacterController::getGravity() const
    623 {
    624         return m_gravity;
    625 }
    626 
    627 void BulletKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
    628 {
    629         m_maxSlopeRadians = slopeRadians;
    630         m_maxSlopeCosine = btCos(slopeRadians);
    631 }
    632 
    633 btScalar BulletKinematicCharacterController::getMaxSlope() const
    634 {
    635         return m_maxSlopeRadians;
    636 }
    637 
    638 bool BulletKinematicCharacterController::onGround () const
    639 {
    640         return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
    641 }
    642 
    643 
    644 btVector3* BulletKinematicCharacterController::getUpAxisDirections()
    645 {
    646         static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
    647        
    648         return sUpAxisDirection;
    649 }
    650 
    651 void BulletKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
    652 {
    653 }
    654 
     373        m_vertical_velocity = m_jump_speed;
     374        m_was_jumping = true;
     375}
    655376
    656377#endif // HAVE_PHYS_USE_BULLET
    657 #endif // 0
     378#endif // USE_LOL_CTRLR_CHARAC
    658379
    659380        } /* namespace phys */

  • trunk/test/Physics/Src/EasyCharacterController.cpp

    r1795 r1819  
    6262                        delete m_character;
    6363
    64                 m_character = new btKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);
     64                //m_character = new btKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);
     65                m_character = new BulletKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);
    6566
    6667                //Deactivate Character controller basic behaviour.
    … …  
    9394void EasyCharacterController::Jump()
    9495{
    95         m_character->jump();
     96        m_character->Jump();
    9697}
    9798
    … …  
    141142                int IterationsNb = (int)(seconds / m_owner_simulation->m_timestep);
    142143                float NewSeconds = IterationsNb * m_owner_simulation->m_timestep;
    143                 m_character->setVelocityForTimeInterval(LOL2BT_VEC3(LOL2BT_UNIT * (m_base_cached_movement + m_frame_cached_movement)) / NewSeconds, NewSeconds);
     144                m_character->SetVelocityForTimeInterval((m_base_cached_movement + m_frame_cached_movement) / NewSeconds, NewSeconds);
    144145                m_base_cached_movement = vec3(.0f);
    145146        }
Note: See TracChangeset for help on using the changeset viewer.