Переглянути джерело

BulletCharacterController is now readable by a human being.

BtPhysTest now implements it with the BtKineCC logic -just modify that now-.
legacy
Benjamin ‘Touky’ Huet touky 12 роки тому
джерело
коміт
006c3c4649
7 змінених файлів з 425 додано та 632 видалено
  1. +3
    -3
      test/BtPhysTest.cpp
  2. +169
    -100
      test/Physics/Include/BulletCharacterController.h
  3. +3
    -1
      test/Physics/Include/EasyCharacterController.h
  4. +2
    -2
      test/Physics/Include/EasyPhysics.h
  5. +4
    -4
      test/Physics/Include/LolPhysics.h
  6. +240
    -519
      test/Physics/Src/BulletCharacterController.cpp
  7. +4
    -3
      test/Physics/Src/EasyCharacterController.cpp

+ 3
- 3
test/BtPhysTest.cpp Переглянути файл

@@ -104,9 +104,9 @@ BtPhysTest::BtPhysTest(bool editor)

if (idx != 1)
{
vec3 axis = vec3(.0f);
axis[2 - idx] = 1;
NewRotation = quat::rotate(90.f, axis);
vec3 NewAxis = vec3(.0f);
NewAxis[2 - idx] = 1;
NewRotation = quat::rotate(90.f, NewAxis);
}

NewPhyobj->SetTransform(NewPosition, NewRotation);


+ 169
- 100
test/Physics/Include/BulletCharacterController.h Переглянути файл

@@ -24,146 +24,215 @@
//#include "BulletDynamics\Character\btCharacterControllerInterface.h"
#endif

#define USE_LOL_CTRLR_CHARAC

namespace lol
{

namespace phys
{

#if 0
#ifdef USE_LOL_CTRLR_CHARAC
#ifdef HAVE_PHYS_USE_BULLET

///BulletKinematicCharacterController is an object that supports a sliding motion in a world.
///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.
///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
class BulletKinematicCharacterController : public btCharacterControllerInterface
class BulletKinematicCharacterController : public btActionInterface
{
protected:

btScalar m_halfHeight;

btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
public:
BulletKinematicCharacterController(btPairCachingGhostObject* NewGhostObject, btConvexShape* NewConvexShape, float NewStepHeight, int NewUpAxis=1)
{
m_convex_shape = NewConvexShape;
m_up_axis = NewUpAxis;
m_ghost_object = NewGhostObject;
m_step_height = NewStepHeight;

m_added_margin = 0.02f;
m_walk_direction = vec3(.0f, .0f, .0f);
m_do_gobject_sweep_test = true;
m_turn_angle = .0f;
m_use_walk_direction = false; // Should remove walk direction, this doesn't work correctly.
m_velocity_time_interval = .0f;
m_vertical_velocity = .0f;
m_vertical_offset = .0f;
m_gravity = 9.8f * 3.f; // 3G acceleration.
m_fall_speed = 55.f; // Terminal velocity of a sky diver in m/s.
m_jump_speed = 10.f; // ?
m_was_on_ground = false;
m_was_jumping = false;
SetMaxSlope(45.f);
}
~BulletKinematicCharacterController() { }

btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
btScalar m_gravity;
protected:

btScalar m_turnAngle;
static vec3* GetUpAxisDirections()
{
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) };
return sUpAxisDirection;
}

btScalar m_stepHeight;
//--------------------------
//CONVENIENCE FUNCTIONS
//--

btScalar m_addedMargin;//@todo: remove this and fix the code
//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
vec3 GetReflectedDir(const vec3& Direction, const vec3& Normal)
{
return Direction - (2.f * dot(Direction, Normal) * Normal);
}
//Returns the portion of 'direction' that is parallel to 'normal'
vec3 ProjectDirOnNorm(const vec3& Direction, const vec3& Normal)
{
return Normal * dot(Direction, Normal);
}
//Returns the portion of 'Direction' that is perpindicular to 'Normal'
vec3 ProjectDirOnNormPerpindicular(const vec3& Direction, const vec3& Normal)
{
return Direction - ProjectDirOnNorm(Direction, Normal);
}
//Returns Ghost Object. -duh-
btPairCachingGhostObject* GetGhostObject()
{
return m_ghost_object;
}

///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
//"Real" war functions
bool RecoverFromPenetration(btCollisionWorld* CollisionWorld);
void UpdateTargetOnHit(const vec3& hit_normal, float TangentMag = .0f, float NormalMag = 1.f);
void StepUp(btCollisionWorld* CollisionWorld);
void StepForwardAndStrafe(btCollisionWorld* CollisionWorld, const vec3& MoveStep);
void StepDown(btCollisionWorld* CollisionWorld, float DeltaTime);

//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;

///keep track of the contact manifolds
btManifoldArray m_manifoldArray;

bool m_touchingContact;
btVector3 m_touchingNormal;

bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;

static btVector3* getUpAxisDirections();

btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);

bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public:
BulletKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~BulletKinematicCharacterController ();
///btActionInterface interface : KEEP IN camelCase
virtual void updateAction(btCollisionWorld* CollisionWorld, float deltaTime)
{
PreStep(CollisionWorld);
PlayerStep(CollisionWorld, deltaTime);
}

//not in the interface, but called above
void PreStep(btCollisionWorld* CollisionWorld);
void PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime);

///btActionInterface interface : KEEP IN camelCase
void debugDraw(btIDebugDraw* debugDrawer) { }
void SetUpAxis(int NewAxis)
{
if (NewAxis < 0)
NewAxis = 0;
if (NewAxis > 2)
NewAxis = 2;
m_up_axis = NewAxis;
}

///btActionInterface interface
virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
//!!!!!! SHOULD DITCH THAT !!!!!!
//This should probably be called setPositionIncrementPerSimulatorStep.
//This is neither a Direction nor a velocity, but the amount to
//increment the position each simulation iteration, regardless
//of DeltaTime.
//This call will Reset any velocity set by SetVelocityForTimeInterval().
virtual void SetWalkDirection(const vec3& walkDirection)
{
preStep ( collisionWorld);
playerStep (collisionWorld, deltaTime);
m_use_walk_direction = true;
m_walk_direction = walkDirection;
m_normalized_direction = normalize(m_walk_direction);
}

///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
//Caller provides a velocity with which the character should MoveStep for
//the given time period. After the time period, velocity is Reset
//to zero.
//This call will Reset any walk Direction set by SetWalkDirection().
//Negative time intervals will result in no motion.
virtual void SetVelocityForTimeInterval(const vec3& velocity, float timeInterval)
{
m_use_walk_direction = false;
m_walk_direction = velocity;
m_normalized_direction = normalize(m_walk_direction);
m_velocity_time_interval = timeInterval;
}

void setUpAxis (int axis)
//Usefulness ?
void Reset() { }
void Warp(const vec3& NewOrigin)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
btTransform NewTransform;
NewTransform.setIdentity();
NewTransform.setOrigin(LOL2BTU_VEC3(NewOrigin));
m_ghost_object->setWorldTransform(NewTransform);
}

/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
/// increment the position each simulation iteration, regardless
/// of dt.
/// This call will reset any velocity set by setVelocityForTimeInterval().
virtual void setWalkDirection(const btVector3& walkDirection);
//External Setup
//--

/// Caller provides a velocity with which the character should move for
/// the given time period. After the time period, velocity is reset
/// to zero.
/// This call will reset any walk direction set by setWalkDirection().
/// Negative time intervals will result in no motion.
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
void SetFallSpeed(float NewFallSpeed) { m_fall_speed = NewFallSpeed; }
void SetJumpSpeed(float NewJumpSpeed) { m_jump_speed = NewJumpSpeed; }
void SetMaxJumpHeight(float NewMaxJumpHeight) { m_max_jump_height = NewMaxJumpHeight; }

void reset ();
void warp (const btVector3& origin);
//Jump logic will go in EasyCC
bool CanJump() const { return OnGround(); }
void Jump();

void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
//NewGravity functions
void SetGravity(float NewGravity) { m_gravity = NewGravity; }
float GetGravity() const { return m_gravity; }

void setFallSpeed (btScalar fallSpeed);
void setJumpSpeed (btScalar jumpSpeed);
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
//The max slope determines the maximum angle that the controller can walk up.
//The slope angle is measured in radians.
void SetMaxSlope(float NewSlopeRadians) { m_max_slope_radians = NewSlopeRadians; m_max_slope_cosine = lol::cos(NewSlopeRadians); }
float GetMaxSlope() const { return m_max_slope_radians; }

void jump ();
void SetUseGhostSweepTest(bool UseGObjectSweepTest) { m_do_gobject_sweep_test = UseGObjectSweepTest; }

void setGravity(btScalar gravity);
btScalar getGravity() const;
bool OnGround() const { return m_vertical_velocity == .0f && m_vertical_offset == .0f; }

/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
private:

btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
m_useGhostObjectSweepTest = useGhostObjectSweepTest;
}
btPairCachingGhostObject* m_ghost_object;
btConvexShape* m_convex_shape; //is also in m_ghost_object, but it needs to be convex, so we store it here to avoid upcast

bool onGround () const;
//keep track of the contact manifolds
btManifoldArray m_manifold_array;

float m_half_height;
float m_velocity_time_interval;
float m_vertical_velocity;
float m_vertical_offset;
float m_fall_speed;
float m_jump_speed;
float m_max_jump_height;
float m_max_slope_radians; // Slope angle that is set (used for returning the exact value)
float m_max_slope_cosine; // Cosine equivalent of m_max_slope_radians (calculated once when set, for optimization)
float m_gravity;
float m_turn_angle;
float m_step_height;
float m_added_margin;//@todo: remove this and fix the code

///this is the desired walk Direction, set by the user
vec3 m_walk_direction;
vec3 m_normalized_direction;

//some internal variables
vec3 m_current_position;
float m_current_step_offset;
vec3 m_target_position;

vec3 m_touching_normal;
bool m_touching_contact;

bool m_was_on_ground;
bool m_was_jumping;
bool m_do_gobject_sweep_test;
bool m_use_walk_direction;
int m_up_axis;
};

#endif // HAVE_PHYS_USE_BULLET
#endif // 0
#endif // USE_LOL_CTRLR_CHARAC

} /* namespace phys */



+ 3
- 1
test/Physics/Include/EasyCharacterController.h Переглянути файл

@@ -23,6 +23,7 @@
#ifdef HAVE_PHYS_USE_BULLET
#include "core.h"
#include "EasyPhysics.h"
#include "BulletCharacterController.h"
#include <BulletDynamics/Character/btKinematicCharacterController.h>
#endif

@@ -82,7 +83,8 @@ protected:
virtual btGhostObject* GetGhostObjectInstance();

btPairCachingGhostObject* m_pair_caching_object;
btKinematicCharacterController* m_character;
//btKinematicCharacterController* m_character;
BulletKinematicCharacterController* m_character;

float m_step_height;
int m_up_axis;


+ 2
- 2
test/Physics/Include/EasyPhysics.h Переглянути файл

@@ -22,7 +22,7 @@
#include <bullet/btBulletDynamicsCommon.h>
#include <bullet/btBulletCollisionCommon.h>
#include <bullet/BulletCollision/CollisionDispatch/btGhostObject.h>
#endif
#endif //HAVE_PHYS_USE_BULLET

namespace lol
{
@@ -156,7 +156,7 @@ protected:
Simulation* m_owner_simulation;

//Base/Attachment logic
Array<EasyPhysic*> m_based_physic_list; //List of objects based on this : this object moves, its based object move with it.
Array<EasyPhysic*> m_based_physic_list; //List of objects based on this : this object moves, its based object MoveStep with it.
EasyPhysic* m_base_physic; //Base for this object : The base moves, the object moves with it.
bool m_base_lock_location; //when this is TRUE, location moves with rotation change.
bool m_base_lock_rotation; //when this is TRUE, rotation moves with rotation change.


+ 4
- 4
test/Physics/Include/LolPhysics.h Переглянути файл

@@ -118,11 +118,11 @@ public:
}
}

//Reap-Off of the btKinematicClosestNotMeRayResultCallback
class LolClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
//Rip-Off of the btKinematicClosestNotMeRayResultCallback
class ClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
public:
LolClosestNotMeRayResultCallback(btCollisionObject* Me, const btVector3& rayFromWorld, const btVector3& rayToWorld) :
ClosestNotMeRayResultCallback(btCollisionObject* Me, const btVector3& rayFromWorld, const btVector3& rayToWorld) :
btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld)
{
m_me = Me;
@@ -168,7 +168,7 @@ public:
case ERT_Closest:
{
if (SourceCaster)
BtRayResult_Closest = new LolClosestNotMeRayResultCallback(SourceCaster->m_collision_object, LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));
BtRayResult_Closest = new ClosestNotMeRayResultCallback(SourceCaster->m_collision_object, LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));
else
BtRayResult_Closest = new btCollisionWorld::ClosestRayResultCallback(LOL2BTU_VEC3(RayFrom), LOL2BTU_VEC3(RayTo));
BtRayResult = BtRayResult_Closest;


+ 240
- 519
test/Physics/Src/BulletCharacterController.cpp Переглянути файл

@@ -14,6 +14,11 @@
# include "config.h"
#endif

#define USE_LOL_CTRLR_CHARAC

#ifdef HAVE_PHYS_USE_BULLET
#include "core.h"
#include <stdio.h>
#include "../Include/LolBtPhysicsIntegration.h"
#include "../Include/LolPhysics.h"
#include "../Include/EasyCharacterController.h"
@@ -25,636 +30,352 @@
//#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
//#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
//#include "LinearMath/btDefaultMotionState.h"
#endif //HAVE_PHYS_USE_BULLET


namespace lol
{

namespace phys
{

#if 0
#ifdef HAVE_PHYS_USE_BULLET

// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
namespace phys
{
btVector3 n = v.normalized();
if (n.length() < SIMD_EPSILON) {
n.setValue(0, 0, 0);
}
return n;
}

#ifdef USE_LOL_CTRLR_CHARAC
#ifdef HAVE_PHYS_USE_BULLET

///@todo Interact with dynamic objects,
///Ride kinematicly animated platforms properly
///More realistic (or maybe just a config option) falling
/// -> Should integrate falling velocity manually and use that in stepDown()
///Support jumping
///Support ducking
class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
//SweepCallback used for Swweep Tests.
class ClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{
public:
btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
{
m_me = me;
}
ClosestNotMeConvexResultCallback(btCollisionObject* NewMe, const vec3& NewUp, float MinSlopeDot) :
btCollisionWorld::ClosestConvexResultCallback(LOL2BTU_VEC3(vec3(.0f)), LOL2BTU_VEC3(vec3(.0f))),
m_me(NewMe),
m_up(NewUp),
m_min_slope_dot(MinSlopeDot) { }

virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& ConvexResult, bool NormalInWorld)
{
if (rayResult.m_collisionObject == m_me)
return 1.0;
//We hit ourselves, FAIL
if (ConvexResult.m_hitCollisionObject == m_me)
return btScalar(1.f);

vec3 WorldHitNomal(.0f);
if (NormalInWorld)
WorldHitNomal = BT2LOL_VEC3(ConvexResult.m_hitNormalLocal);
else //need to transform Normal into worldspace
{
btVector3 TmpWorldHitNormal = ConvexResult.m_hitCollisionObject->getWorldTransform().getBasis() * ConvexResult.m_hitNormalLocal;
WorldHitNomal = BT2LOL_VEC3(TmpWorldHitNormal);
}

float DotUp = dot(m_up, WorldHitNomal);
//We hit below the accepted slope_dot, FAIL
if (DotUp < m_min_slope_dot)
return btScalar(1.f);

return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
//Continue to next.
return ClosestConvexResultCallback::addSingleResult(ConvexResult, NormalInWorld);
}
protected:
btCollisionObject* m_me;
btCollisionObject* m_me;
const vec3 m_up;
float m_min_slope_dot;
};

class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
//When called, will try to remove Character controller from its collision.
bool BulletKinematicCharacterController::RecoverFromPenetration(btCollisionWorld* CollisionWorld)
{
public:
btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
, m_me(me)
, m_up(up)
, m_minSlopeDot(minSlopeDot)
{
}
bool HasPenetration = false;

virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
//Retrieve all pair with us colliding.
CollisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghost_object->getOverlappingPairCache(), CollisionWorld->getDispatchInfo(), CollisionWorld->getDispatcher());
m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
float MaxPen = .0f;
for (int i = 0; i < m_ghost_object->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
if (convexResult.m_hitCollisionObject == m_me)
return btScalar(1.0);
m_manifold_array.resize(0);

btVector3 hitNormalWorld;
if (normalInWorldSpace)
{
hitNormalWorld = convexResult.m_hitNormalLocal;
} else
//this is the equivalent of the "Touch algorithm". Maybe refactor ?
btBroadphasePair* CollisionPair = &m_ghost_object->getOverlappingPairCache()->getOverlappingPairArray()[i];
if (CollisionPair->m_algorithm)
CollisionPair->m_algorithm->getAllContactManifolds(m_manifold_array);
for (int j = 0; j < m_manifold_array.size(); ++j)
{
///need to transform normal into worldspace
hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
}
btPersistentManifold* CurMfold = m_manifold_array[j];
//Normal direction differs if we're Body0
float DirSign = CurMfold->getBody0() == m_ghost_object ? -1.f : 1.f;

btScalar dotUp = m_up.dot(hitNormalWorld);
if (dotUp < m_minSlopeDot) {
return btScalar(1.0);
for (int k = 0; k < CurMfold->getNumContacts(); k++)
{
const btManifoldPoint& MfPoint = CurMfold->getContactPoint(k);
float Dist = MfPoint.getDistance();
if (Dist < .0f)
{
if (Dist < MaxPen)
{
MaxPen = Dist;
m_touching_normal = BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign;
}
m_current_position += BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign * Dist * .2f;
HasPenetration = true;
}
}
}

return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
}
protected:
btCollisionObject* m_me;
const btVector3 m_up;
btScalar m_minSlopeDot;
};

/*
* 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
*/
btVector3 BulletKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
{
return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
}
btTransform GObjMx = m_ghost_object->getWorldTransform();
GObjMx.setOrigin(LOL2BTU_VEC3(m_current_position));
m_ghost_object->setWorldTransform(GObjMx);

/*
* Returns the portion of 'direction' that is parallel to 'normal'
*/
btVector3 BulletKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
{
btScalar magnitude = direction.dot(normal);
return normal * magnitude;
return HasPenetration;
}

/*
* Returns the portion of 'direction' that is perpindicular to 'normal'
*/
btVector3 BulletKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
//When the Controller hits a wall, we modify the target so the controller will MoveStep along the wall.
void BulletKinematicCharacterController::UpdateTargetOnHit(const vec3& HitNormal, float TangentMag, float NormalMag)
{
return direction - parallelComponent(direction, normal);
}

BulletKinematicCharacterController::BulletKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
{
m_upAxis = upAxis;
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_gravity = 9.8 * 3 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_wasOnGround = false;
m_wasJumping = false;
setMaxSlope(btRadians(45.0));
}

BulletKinematicCharacterController::~BulletKinematicCharacterController ()
{
}

btPairCachingGhostObject* BulletKinematicCharacterController::getGhostObject()
{
return m_ghostObject;
}

bool BulletKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
{

bool penetration = false;
vec3 Movedir = m_target_position - m_current_position;
float MoveLength = (float)length(Movedir);

collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());

m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
if (MoveLength > SIMD_EPSILON)
{
m_manifoldArray.resize(0);

btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);

for (int j=0;j<m_manifoldArray.size();j++)
{
btPersistentManifold* manifold = m_manifoldArray[j];
btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
for (int p=0;p<manifold->getNumContacts();p++)
{
const btManifoldPoint&pt = manifold->getContactPoint(p);
Movedir = normalize(Movedir);

btScalar dist = pt.getDistance();
vec3 ReflectDir = normalize(GetReflectedDir(Movedir, HitNormal));
vec3 ParallelDir = ProjectDirOnNorm(ReflectDir, HitNormal);
vec3 PerpindicularDir = ProjectDirOnNormPerpindicular(ReflectDir, HitNormal);

if (dist < 0.0)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
m_target_position = m_current_position;

}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
if (NormalMag != .0f)
m_target_position += PerpindicularDir * NormalMag * MoveLength;
}
btTransform newTrans = m_ghostObject->getWorldTransform();
newTrans.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return penetration;
}

void BulletKinematicCharacterController::stepUp ( btCollisionWorld* world)
//Handles the Step-Up : Currently taking into account Stair step & Jump.
void BulletKinematicCharacterController::StepUp(btCollisionWorld* world)
{
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
vec3 UpDir = GetUpAxisDirections()[m_up_axis];
btTransform SweepStart, SweepEnd;
SweepStart.setIdentity();
SweepEnd.setIdentity();

start.setIdentity ();
end.setIdentity ();
m_target_position = m_current_position + UpDir * (m_step_height + (m_vertical_offset > 0.f ? m_vertical_offset : 0.f));

/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
end.setOrigin (m_targetPosition);
/* FIXME: Handle HasPenetration properly */
SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position + UpDir * (m_convex_shape->getMargin() + m_added_margin)));
SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));

btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, -UpDir, float(0.7071));
SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
if (m_do_gobject_sweep_test)
m_ghost_object->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, world->getDispatchInfo().m_allowedCcdPenetration);
else
{
world->convexSweepTest (m_convexShape, start, end, callback);
}
world->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback);
if (callback.hasHit())
if (SweepCallback.hasHit())
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
if(SweepCallback.m_hitNormalWorld.dot(LOL2BTU_VEC3(UpDir)) > .0f)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
// we moved up only a Fraction of the step height
m_current_step_offset = m_step_height * SweepCallback.m_closestHitFraction;
btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3)
InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction);
m_current_position = BT2LOLU_VEC3(InterpolPos);
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
} else {
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
m_vertical_velocity = .0f;
m_vertical_offset = .0f;
}
}

void BulletKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
btScalar movementLength = movementDirection.length();
if (movementLength>SIMD_EPSILON)
{
movementDirection.normalize();

btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
reflectDir.normalize();

btVector3 parallelDir, perpindicularDir;

parallelDir = parallelComponent (reflectDir, hitNormal);
perpindicularDir = perpindicularComponent (reflectDir, hitNormal);

m_targetPosition = m_currentPosition;
if (0)//tangentMag != 0.0)
{
btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
m_targetPosition += parComponent;
}

if (normalMag != 0.0)
{
btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
m_targetPosition += perpComponent;
}
} else
else
{
// printf("movementLength don't normalize a zero vector\n");
m_current_step_offset = m_step_height;
m_current_position = m_target_position;
}
}

void BulletKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
//Handles the actual Movement. It actually moves in the 3 dimensions, function name is confusing.
void BulletKinematicCharacterController::StepForwardAndStrafe(btCollisionWorld* CollisionWorld, const vec3& MoveStep)
{
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
m_target_position = m_current_position + MoveStep;
btTransform SweepStart, SweepEnd;
SweepStart.setIdentity();
SweepEnd.setIdentity();

start.setIdentity ();
end.setIdentity ();
btScalar fraction = 1.0;
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
float Fraction = 1.f;
float SqDist = .0f;

if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}

int maxIter = 10;
if (m_touching_contact && dot(m_normalized_direction, m_touching_normal) > .0f)
UpdateTargetOnHit(m_touching_normal);

while (fraction > btScalar(0.01) && maxIter-- > 0)
//Let's loop on movement, until Movement fraction if below 0.01, which means we've reached our destination.
//Or until we'tried 10 times.
int MaxMoveLoop = 10;
while (Fraction > .01f && MaxMoveLoop-- > 0)
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);

btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;


btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);


if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
m_convexShape->setMargin(margin);

fraction -= callback.m_closestHitFraction;

if (callback.hasHit())
SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position));
SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));
vec3 SweepDirNeg(m_current_position - m_target_position);

ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, SweepDirNeg, .0f);
SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;

//The sweep test is done with an added margin, so we use it and then discard it
float SavedMargin = m_convex_shape->getMargin();
m_convex_shape->setMargin(SavedMargin + m_added_margin); //Apply Added Margin
if (m_do_gobject_sweep_test)
m_ghost_object->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
else
CollisionWorld->convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
m_convex_shape->setMargin(SavedMargin); //Restore saved margin

Fraction -= SweepCallback.m_closestHitFraction;

if (SweepCallback.hasHit())
{
// we moved only a fraction
btScalar hitDistance;
hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
//We moved only a Fraction
float HitDist = (float)length(BT2LOLU_VEC3(SweepCallback.m_hitPointWorld) - m_current_position);

// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);

updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.length2();
if (distance2 > SIMD_EPSILON)
UpdateTargetOnHit(BT2LOL_VEC3(SweepCallback.m_hitNormalWorld));
vec3 NewDir = m_target_position - m_current_position;
SqDist = sqlength(NewDir);
if (SqDist > SIMD_EPSILON)
{
currentDir.normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
{
NewDir = normalize(NewDir);
//See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners."
if (dot(NewDir, m_normalized_direction) <= .0f)
break;
}
} else
{
// printf("currentDir: don't normalize a zero vector\n");
break;
}

} else {
// we moved whole way
m_currentPosition = m_targetPosition;
else
break;
}

// if (callback.m_closestHitFraction == 0.f)
// break;

else //We moved whole way
m_current_position = m_target_position;
}
}

void BulletKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
//Handles the Step-down : We go back on the ground at the end of the MoveStep.
void BulletKinematicCharacterController::StepDown(btCollisionWorld* CollisionWorld, float DeltaTime)
{
btTransform start, end;

// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/

btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if(downVelocity > 0.0 && downVelocity < m_stepHeight
&& (m_wasOnGround || !m_wasJumping))
{
downVelocity = m_stepHeight;
}
vec3 UpDir = GetUpAxisDirections()[m_up_axis];
btTransform SweepStart, SweepEnd;
SweepStart.setIdentity();
SweepEnd.setIdentity();

btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
float DownVel = (m_vertical_velocity < 0.f ? -m_vertical_velocity : 0.f) * DeltaTime;
if (DownVel > .0f && DownVel < m_step_height && (m_was_on_ground || !m_was_jumping))
DownVel = m_step_height;

start.setIdentity ();
end.setIdentity ();
vec3 StepDrop = UpDir * (m_current_step_offset + DownVel);
m_target_position -= StepDrop;

start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position));
SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position));

btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, UpDir, m_max_slope_cosine);
SweepCallback.m_collisionFilterGroup = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
SweepCallback.m_collisionFilterMask = GetGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (m_do_gobject_sweep_test)
m_ghost_object->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
else
CollisionWorld->convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld->getDispatchInfo().m_allowedCcdPenetration);

if (callback.hasHit())
if (SweepCallback.hasHit())
{
// we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasJumping = false;
} else {
// we dropped the full height
m_currentPosition = m_targetPosition;
// we dropped a Fraction of the height -> hit floor
btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3)
InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction);
m_current_position = BT2LOLU_VEC3(InterpolPos);
m_vertical_velocity = .0f;
m_vertical_offset = .0f;
m_was_jumping = false;
}
else // we dropped the full height
m_current_position = m_target_position;
}



void BulletKinematicCharacterController::setWalkDirection
(
const btVector3& walkDirection
)
//The PreStepis done in order to recover from any HasPenetration.
void BulletKinematicCharacterController::PreStep(btCollisionWorld* CollisionWorld)
{
m_useWalkDirection = true;
m_walkDirection = walkDirection;
m_normalizedDirection = getNormalizedVector(m_walkDirection);
}

int MaxPenetrationLoop = 0;
m_touching_contact = false;


void BulletKinematicCharacterController::setVelocityForTimeInterval
(
const btVector3& velocity,
btScalar timeInterval
)
{
// printf("setVelocity!\n");
// printf(" interval: %f\n", timeInterval);
// printf(" velocity: (%f, %f, %f)\n",
// velocity.x(), velocity.y(), velocity.z());

m_useWalkDirection = false;
m_walkDirection = velocity;
m_normalizedDirection = getNormalizedVector(m_walkDirection);
m_velocityTimeInterval = timeInterval;
}



void BulletKinematicCharacterController::reset ()
{
}

void BulletKinematicCharacterController::warp (const btVector3& origin)
{
btTransform xform;
xform.setIdentity();
xform.setOrigin (origin);
m_ghostObject->setWorldTransform (xform);
}


void BulletKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration (collisionWorld))
while (RecoverFromPenetration(CollisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
MaxPenetrationLoop++;
m_touching_contact = true;
if (MaxPenetrationLoop > 4)
break;
}
}

m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);

m_current_position = BT2LOLU_VEC3(m_ghost_object->getWorldTransform().getOrigin());
m_target_position = m_current_position;
}

#include <stdio.h>
void BulletKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
//And so we step :
//StepUpfirst, then movement, then StepDownon the ground.
void BulletKinematicCharacterController::PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);

// quick check...
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
// printf("\n");
if (!m_use_walk_direction && m_velocity_time_interval <= .0f)
return; // no motion
}

m_wasOnGround = onGround();
m_was_on_ground = OnGround();

// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;


btTransform xform;
xform = m_ghostObject->getWorldTransform ();
m_vertical_velocity -= m_gravity * DeltaTime;
if(m_vertical_velocity > .0f && m_vertical_velocity > m_jump_speed)
m_vertical_velocity = m_jump_speed;

// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
if(m_vertical_velocity < .0f && btFabs(m_vertical_velocity) > btFabs(m_fall_speed))
m_vertical_velocity = -btFabs(m_fall_speed);
m_vertical_offset = m_vertical_velocity * DeltaTime;

stepUp (collisionWorld);
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
btScalar dtMoving =
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
btTransform NewTransform;
NewTransform = m_ghost_object->getWorldTransform();

// how far will we move while we are moving?
btVector3 move = m_walkDirection * dtMoving;

//printf(" dtMoving: %f", dtMoving);
vec3 MoveStep(.0f);
if (m_use_walk_direction)
MoveStep = m_walk_direction;
else
{
//Still have some time left for moving!
float dtMoving = (DeltaTime < m_velocity_time_interval) ? DeltaTime : m_velocity_time_interval;
m_velocity_time_interval -= DeltaTime;

// okay, step
stepForwardAndStrafe(collisionWorld, move);
// how far will we MoveStep while we are moving?
MoveStep = m_walk_direction * dtMoving;
}
stepDown (collisionWorld, dt);

// printf("\n");
//Okay, step !
StepUp(CollisionWorld);
StepForwardAndStrafe(CollisionWorld, MoveStep);
StepDown(CollisionWorld, DeltaTime);

xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
}

void BulletKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
{
m_fallSpeed = fallSpeed;
//Movement finished, update World transform
NewTransform.setOrigin(LOL2BTU_VEC3(m_current_position));
m_ghost_object->setWorldTransform(NewTransform);
}

void BulletKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
//should MoveStep Jump logic in EasyCC
void BulletKinematicCharacterController::Jump()
{
m_jumpSpeed = jumpSpeed;
}

void BulletKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
{
m_maxJumpHeight = maxJumpHeight;
}

bool BulletKinematicCharacterController::canJump () const
{
return onGround();
}

void BulletKinematicCharacterController::jump ()
{
if (!canJump())
if (!CanJump())
return;

m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;

#if 0
currently no jumping.
btTransform xform;
m_rigidBody->getMotionState()->getWorldTransform (xform);
btVector3 up = xform.getBasis()[1];
up.normalize ();
btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
m_rigidBody->applyCentralImpulse (up * magnitude);
#endif
}

void BulletKinematicCharacterController::setGravity(btScalar gravity)
{
m_gravity = gravity;
}

btScalar BulletKinematicCharacterController::getGravity() const
{
return m_gravity;
}

void BulletKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
{
m_maxSlopeRadians = slopeRadians;
m_maxSlopeCosine = btCos(slopeRadians);
}

btScalar BulletKinematicCharacterController::getMaxSlope() const
{
return m_maxSlopeRadians;
m_vertical_velocity = m_jump_speed;
m_was_jumping = true;
}

bool BulletKinematicCharacterController::onGround () const
{
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
}


btVector3* BulletKinematicCharacterController::getUpAxisDirections()
{
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) };
return sUpAxisDirection;
}

void BulletKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
{
}


#endif // HAVE_PHYS_USE_BULLET
#endif // 0
#endif // USE_LOL_CTRLR_CHARAC

} /* namespace phys */



+ 4
- 3
test/Physics/Src/EasyCharacterController.cpp Переглянути файл

@@ -61,7 +61,8 @@ void EasyCharacterController::AddToSimulation(class Simulation* current_simulati
if (m_character)
delete m_character;

m_character = new btKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);
//m_character = new btKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);
m_character = new BulletKinematicCharacterController(m_pair_caching_object, m_convex_shape, m_step_height, m_up_axis);

//Deactivate Character controller basic behaviour.
//m_character->setGravity(.0f);
@@ -92,7 +93,7 @@ void EasyCharacterController::RemoveFromSimulation(class Simulation* current_sim

void EasyCharacterController::Jump()
{
m_character->jump();
m_character->Jump();
}

//Set movement for this frame
@@ -140,7 +141,7 @@ void EasyCharacterController::TickGame(float seconds)
{
int IterationsNb = (int)(seconds / m_owner_simulation->m_timestep);
float NewSeconds = IterationsNb * m_owner_simulation->m_timestep;
m_character->setVelocityForTimeInterval(LOL2BT_VEC3(LOL2BT_UNIT * (m_base_cached_movement + m_frame_cached_movement)) / NewSeconds, NewSeconds);
m_character->SetVelocityForTimeInterval((m_base_cached_movement + m_frame_cached_movement) / NewSeconds, NewSeconds);
m_base_cached_movement = vec3(.0f);
}
}


Завантаження…
Відмінити
Зберегти