Abstracted character controller interface
Renamed old character controller to DynamicCharacterController First start at KinematicCharacterController. Still has bugs.
This commit is contained in:
john.mccutchan
384
Demos/CharacterDemo/KinematicCharacterController.cpp
Normal file
384
Demos/CharacterDemo/KinematicCharacterController.cpp
Normal file
@@ -0,0 +1,384 @@
|
||||
#include <stdio.h>
|
||||
|
||||
#include "GLDebugDrawer.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
|
||||
#include "LinearMath/btDefaultMotionState.h"
|
||||
#include "KinematicCharacterController.h"
|
||||
|
||||
/* TODO:
|
||||
* Handle projecting/slide along surfaces
|
||||
* Deal with starting in penetration
|
||||
* Interact with dynamic objects
|
||||
* Ride kinematicly animated platforms properly
|
||||
* Step climbing
|
||||
*/
|
||||
class ClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
|
||||
{
|
||||
public:
|
||||
ClosestNotMeRayResultCallback (btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
|
||||
{
|
||||
m_me = me;
|
||||
}
|
||||
|
||||
virtual btScalar AddSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
|
||||
{
|
||||
if (rayResult.m_collisionObject == m_me)
|
||||
return 1.0;
|
||||
|
||||
return ClosestRayResultCallback::AddSingleResult (rayResult, normalInWorldSpace);
|
||||
}
|
||||
protected:
|
||||
btRigidBody* m_me;
|
||||
};
|
||||
|
||||
class ClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
|
||||
{
|
||||
public:
|
||||
ClosestNotMeConvexResultCallback (btRigidBody* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
|
||||
{
|
||||
m_me = me;
|
||||
}
|
||||
|
||||
virtual btScalar AddSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
|
||||
{
|
||||
if (convexResult.m_hitCollisionObject == m_me)
|
||||
return 1.0;
|
||||
|
||||
return ClosestConvexResultCallback::AddSingleResult (convexResult, normalInWorldSpace);
|
||||
}
|
||||
protected:
|
||||
btRigidBody* m_me;
|
||||
};
|
||||
|
||||
/*
|
||||
* 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 computeReflectionDirection (const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the portion of 'direction' that is parallel to 'normal'
|
||||
*/
|
||||
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
btScalar magnitude = direction.dot(normal);
|
||||
return normal * magnitude;
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the portion of 'direction' that is perpindicular to 'normal'
|
||||
*/
|
||||
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
return direction - parallelComponent(direction, normal);
|
||||
}
|
||||
|
||||
KinematicCharacterController::KinematicCharacterController ()
|
||||
{
|
||||
m_turnAngle = btScalar(0.0);
|
||||
m_walkVelocity = btScalar(1.1) * 4.0; // 4 km/h -> 1.1 m/s
|
||||
m_shape = NULL;
|
||||
m_pairCache = NULL;
|
||||
m_rigidBody = NULL;
|
||||
}
|
||||
|
||||
KinematicCharacterController::~KinematicCharacterController ()
|
||||
{
|
||||
}
|
||||
|
||||
void KinematicCharacterController::setup (btDynamicsWorld* dynamicsWorld, btScalar height, btScalar width, btScalar stepHeight)
|
||||
{
|
||||
btVector3 spherePositions[2];
|
||||
btScalar sphereRadii[2];
|
||||
|
||||
sphereRadii[0] = width;
|
||||
sphereRadii[1] = width;
|
||||
spherePositions[0] = btVector3 (0.0, (height/btScalar(2.0) - width), 0.0);
|
||||
spherePositions[1] = btVector3 (0.0, (-height/btScalar(2.0) + width), 0.0);
|
||||
|
||||
m_halfHeight = height/btScalar(2.0);
|
||||
|
||||
m_shape = new btMultiSphereShape (btVector3(width/btScalar(2.0), height/btScalar(2.0), width/btScalar(2.0)), &spherePositions[0], &sphereRadii[0], 2);
|
||||
m_stepHeight = stepHeight;
|
||||
m_height = height;
|
||||
m_width = width;
|
||||
btTransform startTransform;
|
||||
startTransform.setIdentity ();
|
||||
startTransform.setOrigin (btVector3(0.0, 4.0, 0.0));
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo cInfo(1.0, myMotionState, m_shape);
|
||||
m_rigidBody = new btRigidBody(cInfo);
|
||||
m_rigidBody->setCollisionFlags( m_rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_NO_CONTACT_RESPONSE);
|
||||
m_rigidBody->setSleepingThresholds (0.0, 0.0);
|
||||
m_rigidBody->setAngularFactor (0.0);
|
||||
dynamicsWorld->addRigidBody (m_rigidBody);
|
||||
}
|
||||
|
||||
void KinematicCharacterController::destroy (btDynamicsWorld* dynamicsWorld)
|
||||
{
|
||||
if (m_rigidBody)
|
||||
{
|
||||
dynamicsWorld->removeRigidBody (m_rigidBody);
|
||||
delete m_rigidBody;
|
||||
}
|
||||
|
||||
if (m_shape)
|
||||
{
|
||||
delete m_shape;
|
||||
}
|
||||
}
|
||||
|
||||
btRigidBody* KinematicCharacterController::getRigidBody ()
|
||||
{
|
||||
return m_rigidBody;
|
||||
}
|
||||
|
||||
void KinematicCharacterController::recoverFromPenetration (btDynamicsWorld* dynamicsWorld)
|
||||
{
|
||||
if (m_pairCache == NULL)
|
||||
return;
|
||||
|
||||
printf("%d\n", m_pairCache->getNumOverlappingPairs());
|
||||
dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs (m_pairCache, dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher());
|
||||
|
||||
btManifoldArray manifoldArray;
|
||||
for (int i = 0; i < m_pairCache->getNumOverlappingPairs(); i++)
|
||||
{
|
||||
printf("%d\n",i);
|
||||
manifoldArray.clear();
|
||||
|
||||
btBroadphasePair* collisionPair = &m_pairCache->getOverlappingPairArray()[i];
|
||||
|
||||
if (collisionPair->m_algorithm)
|
||||
collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
|
||||
|
||||
for (int j=0;j<manifoldArray.size();j++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldArray[j];
|
||||
for (int p=0;p<manifold->getNumContacts();p++)
|
||||
{
|
||||
const btManifoldPoint&pt = manifold->getContactPoint(p);
|
||||
|
||||
if (pt.getDistance() < 0.0)
|
||||
{
|
||||
printf("penetration %f\n", pt.getDistance());
|
||||
} else {
|
||||
printf("touching %f\n", pt.getDistance());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicCharacterController::stepUp (btDynamicsWorld* dynamicsWorld)
|
||||
{
|
||||
// phase 1: up
|
||||
btTransform start, end;
|
||||
m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
|
||||
|
||||
start.setIdentity ();
|
||||
end.setIdentity ();
|
||||
|
||||
/* FIXME: Handle penetration properly */
|
||||
start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
|
||||
end.setOrigin (m_targetPosition);
|
||||
|
||||
ClosestNotMeConvexResultCallback callback (m_rigidBody);
|
||||
|
||||
dynamicsWorld->convexSweepTest (m_shape, start, end, callback);
|
||||
|
||||
if (callback.HasHit())
|
||||
{
|
||||
// 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);
|
||||
} else {
|
||||
m_currentStepOffset = m_stepHeight;
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
|
||||
{
|
||||
btVector3 movementDirection = m_targetPosition - m_currentPosition;
|
||||
btScalar movementLength = movementDirection.length();
|
||||
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 (tangentMag != 0.0)
|
||||
{
|
||||
m_targetPosition += parallelDir * btScalar (tangentMag*movementLength);
|
||||
}
|
||||
|
||||
if (normalMag != 0.0)
|
||||
{
|
||||
m_targetPosition += perpindicularDir * btScalar (normalMag*movementLength);
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicCharacterController::stepForwardAndStrafe (btDynamicsWorld* dynamicsWorld, const btVector3& walkMove)
|
||||
{
|
||||
// phase 2: forward and strafe
|
||||
btTransform start, end;
|
||||
m_targetPosition = m_currentPosition + walkMove;
|
||||
start.setIdentity ();
|
||||
end.setIdentity ();
|
||||
|
||||
btScalar fraction = 1.0;
|
||||
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
|
||||
|
||||
while (fraction > btScalar(0.01))
|
||||
{
|
||||
start.setOrigin (m_currentPosition);
|
||||
end.setOrigin (m_targetPosition);
|
||||
|
||||
ClosestNotMeConvexResultCallback callback (m_rigidBody);
|
||||
dynamicsWorld->convexSweepTest (m_shape, start, end, callback);
|
||||
|
||||
fraction -= callback.m_closestHitFraction;
|
||||
|
||||
if (callback.HasHit())
|
||||
{
|
||||
// we moved only a fraction
|
||||
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
|
||||
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
|
||||
distance2 = (m_currentPosition-m_targetPosition).length2();
|
||||
} else {
|
||||
// we moved whole way
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicCharacterController::stepDown (btDynamicsWorld* dynamicsWorld, btScalar dt)
|
||||
{
|
||||
btTransform start, end;
|
||||
|
||||
// phase 3: down
|
||||
btVector3 step_drop = btVector3(btScalar(0.0), m_currentStepOffset, btScalar(0.0));
|
||||
btVector3 gravity_drop = btVector3(btScalar(0.0), m_stepHeight, btScalar(0.0));
|
||||
m_targetPosition -= (step_drop + gravity_drop);
|
||||
|
||||
start.setIdentity ();
|
||||
end.setIdentity ();
|
||||
|
||||
start.setOrigin (m_currentPosition);
|
||||
end.setOrigin (m_targetPosition);
|
||||
|
||||
ClosestNotMeConvexResultCallback callback (m_rigidBody);
|
||||
|
||||
dynamicsWorld->convexSweepTest (m_shape, start, end, callback);
|
||||
|
||||
if (callback.HasHit())
|
||||
{
|
||||
// we dropped a fraction of the height -> hit floor
|
||||
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
|
||||
} else {
|
||||
// we dropped the full height
|
||||
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
}
|
||||
|
||||
void KinematicCharacterController::registerPairCache (btOverlappingPairCache* pairCache)
|
||||
{
|
||||
m_pairCache = pairCache;
|
||||
}
|
||||
|
||||
void KinematicCharacterController::preStep (btDynamicsWorld* dynamicsWorld)
|
||||
{
|
||||
btTransform xform;
|
||||
m_rigidBody->getMotionState()->getWorldTransform (xform);
|
||||
|
||||
btVector3 forwardDir = xform.getBasis()[2];
|
||||
btVector3 upDir = xform.getBasis()[1];
|
||||
btVector3 strafeDir = xform.getBasis()[0];
|
||||
forwardDir.normalize ();
|
||||
upDir.normalize ();
|
||||
strafeDir.normalize ();
|
||||
|
||||
m_upDirection = upDir;
|
||||
m_forwardDirection = forwardDir;
|
||||
m_strafeDirection = strafeDir;
|
||||
|
||||
m_currentPosition = xform.getOrigin();
|
||||
m_targetPosition = m_currentPosition;
|
||||
recoverFromPenetration (dynamicsWorld);
|
||||
}
|
||||
|
||||
void KinematicCharacterController::playerStep (btDynamicsWorld* dynamicsWorld,
|
||||
btScalar dt,
|
||||
int forward,
|
||||
int backward,
|
||||
int left,
|
||||
int right)
|
||||
{
|
||||
btVector3 walkDirection = btVector3(0.0, 0.0, 0.0);
|
||||
btScalar walkSpeed = m_walkVelocity * dt;
|
||||
|
||||
if (left)
|
||||
walkDirection += m_strafeDirection;
|
||||
|
||||
if (right)
|
||||
walkDirection -= m_strafeDirection;
|
||||
|
||||
if (forward)
|
||||
walkDirection += m_forwardDirection;
|
||||
|
||||
if (backward)
|
||||
walkDirection -= m_forwardDirection;
|
||||
|
||||
btTransform xform;
|
||||
m_rigidBody->getMotionState()->getWorldTransform (xform);
|
||||
|
||||
stepUp (dynamicsWorld);
|
||||
stepForwardAndStrafe (dynamicsWorld, walkDirection * walkSpeed);
|
||||
stepDown (dynamicsWorld, dt);
|
||||
|
||||
xform.setOrigin (m_currentPosition);
|
||||
m_rigidBody->getMotionState()->setWorldTransform (xform);
|
||||
m_rigidBody->setCenterOfMassTransform (xform);
|
||||
}
|
||||
|
||||
bool KinematicCharacterController::canJump () const
|
||||
{
|
||||
return onGround();
|
||||
}
|
||||
|
||||
void KinematicCharacterController::jump ()
|
||||
{
|
||||
if (!canJump())
|
||||
return;
|
||||
|
||||
#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
|
||||
}
|
||||
|
||||
bool KinematicCharacterController::onGround () const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user