Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

The kinematic character controller with various fixes and a few new features like, being able to set any vector for gravity/up, jumping in a certain direction, possibility to use collision masks, angular & linear velocity, angular & linear damping.

Browse Source
This commit is contained in:
MiCroN3000
2016-07-28 20:15:38 +02:00
parent 4cbc741f66
commit 626a913866
3 changed files with 395 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual void jump(const btVector3& dir = btVector3()) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;

461
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
return direction - parallelComponent(direction, normal);
}
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
m_upAxis = upAxis;
m_up.setValue(0.0f, 0.0f, 1.0f);
m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
setUp(up);
setStepHeight(stepHeight);
m_addedMargin = 0.02;
m_walkDirection.setValue(0,0,0);
m_walkDirection.setValue(0.0,0.0,0.0);
m_AngVel.setValue(0.0, 0.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_gravity = 9.8 * 3.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
m_SetjumpSpeed = m_jumpSpeed;
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
m_currentStepOffset = 0;
m_currentStepOffset = 0.0;
m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
m_linearDamping = btScalar(0.0);
m_angularDamping = btScalar(0.0);
}
btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0);
// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
if (!needsCollision(obj0, obj1))
continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
if (dist < 0.0)
if (dist < -m_maxPenetrationDepth)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
// TODO: cause problems on slopes, not sure if it is needed
//if (dist < maxPen)
//{
// maxPen = dist;
// m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
}
//}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
start.setOrigin(m_currentPosition);
m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
m_currentPosition = m_targetPosition;
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
world->convexSweepTest (m_convexShape, start, end, callback);
world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// 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 (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
btTransform& xform = m_ghostObject->getWorldTransform();
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
// fix penetration if we hit a ceiling for example
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(world))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
m_currentPosition = m_targetPosition;
if (m_verticalOffset > 0)
{
m_verticalOffset = 0.0;
m_verticalVelocity = 0.0;
m_currentStepOffset = m_stepHeight;
}
} else {
m_currentStepOffset = m_stepHeight;
m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
{
bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
return collides;
}
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
{
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
if (!(start == end))
{
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_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())
if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
//btScalar hitDistance;
@@ -404,9 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
// if (!m_wasJumping)
// we moved whole way
m_currentPosition = m_targetPosition;
}
m_currentPosition = m_targetPosition;
// if (callback.m_closestHitFraction == 0.f)
// break;
@@ -421,27 +473,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
if (m_verticalVelocity > 0.0)
return;
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -455,6 +510,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
start.setRotation(m_currentOrientation);
end.setRotation(m_targetOrientation);
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@@ -462,7 +520,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -471,30 +529,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
if (!callback.hasHit())
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (!callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
bool has_hit = false;
if (bounce_fix == true)
has_hit = callback.hasHit() || callback2.hasHit();
has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
has_hit = callback2.hasHit();
has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
btScalar stepHeight = 0.0f;
if (m_verticalVelocity < 0.0)
stepHeight = m_stepHeight;
if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
downVelocity = m_stepHeight;
downVelocity = stepHeight;
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@@ -502,10 +564,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
if (callback.hasHit() || runonce == true)
if ((callback.hasHit() || runonce == true) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we dropped a fraction of the height -> hit floor
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,10 +574,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -528,7 +589,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_wasJumping = false;
} else {
// we dropped the full height
full_drop = true;
if (bounce_fix == true)
@@ -538,7 +599,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@@ -579,21 +640,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
{
m_AngVel = velocity;
}
const btVector3& btKinematicCharacterController::getAngularVelocity() const
{
return m_AngVel;
}
void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
{
m_walkDirection = velocity;
// HACK: if we are moving in the direction of the up, treat it as a jump :(
if (m_walkDirection.length2() > 0)
{
btVector3 w = velocity.normalized();
btScalar c = w.dot(m_up);
if (c != 0)
{
//there is a component in walkdirection for vertical velocity
btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
m_walkDirection -= upComponent;
m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
if (c > 0.0f)
{
m_wasJumping = true;
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
}
}
}
else
m_verticalVelocity = 0.0f;
}
btVector3 btKinematicCharacterController::getLinearVelocity() const
{
return m_walkDirection + (m_verticalVelocity * m_up);
}
void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
{
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
m_wasOnGround = false;
m_wasJumping = false;
m_walkDirection.setValue(0,0,0);
m_velocityTimeInterval = 0.0;
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
//clear pair cache
btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
while (cache->getOverlappingPairArray().size() > 0)
{
cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
}
}
void btKinematicCharacterController::warp (const btVector3& origin)
@@ -607,62 +710,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration (collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
#include <stdio.h>
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
if (m_AngVel.length2() > 0.0f)
{
m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
}
// integrate for angular velocity
if (m_AngVel.length2() > 0.0f)
{
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
btQuaternion orn = rot * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
m_targetOrientation = m_currentOrientation;
}
// quick check...
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
//btVector3 lvel = m_walkDirection;
btScalar c = 0.0f;
if (m_walkDirection.length2() > 0)
{
// apply damping
m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
}
m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
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 ();
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp (collisionWorld);
stepUp(collisionWorld);
//todo: Experimenting with behavior of controller when it hits a ceiling..
//bool hitUp = stepUp (collisionWorld);
//if (hitUp)
//{
// 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;
// xform = m_ghostObject->getWorldTransform();
//}
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@@ -682,10 +822,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
//todo: Experimenting with max jump height
//if (m_wasJumping)
//{
// btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
// if (ds > m_maxJumpHeight)
// {
// // substract the overshoot
// m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
// // max height was reached, so potential energy is at max
// // and kinematic energy is 0, thus velocity is 0.
// if (m_verticalVelocity > 0.0)
// m_verticalVelocity = 0.0;
// }
//}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
int numPenetrationLoops = 0;
m_touchingContact = false;
while (recoverFromPenetration(collisionWorld))
{
numPenetrationLoops++;
m_touchingContact = true;
if (numPenetrationLoops > 4)
{
//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +864,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +877,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
void btKinematicCharacterController::jump ()
void btKinematicCharacterController::jump(const btVector3& v)
{
if (!canJump())
return;
m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
#if 0
currently no jumping.
btTransform xform;
@@ -727,14 +898,16 @@ void btKinematicCharacterController::jump ()
#endif
}
void btKinematicCharacterController::setGravity(btScalar gravity)
void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
m_gravity = gravity;
if (gravity.length2() > 0) setUpVector(-gravity);
m_gravity = gravity.length();
}
btScalar btKinematicCharacterController::getGravity() const
btVector3 btKinematicCharacterController::getGravity() const
{
return m_gravity;
return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +921,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
bool btKinematicCharacterController::onGround () const
void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
{
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
m_maxPenetrationDepth = d;
}
btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
{
return m_maxPenetrationDepth;
}
bool btKinematicCharacterController::onGround () const
{
return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
void btKinematicCharacterController::setStepHeight(btScalar h)
{
m_stepHeight = h;
}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@@ -769,3 +956,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
void btKinematicCharacterController::setUp(const btVector3& up)
{
if (up.length2() > 0 && m_gravity > 0.0f)
{
setGravity(-m_gravity * up.normalized());
return;
}
setUpVector(up);
}
void btKinematicCharacterController::setUpVector(const btVector3& up)
{
if (m_up == up)
return;
btVector3 u = m_up;
if (up.length2() > 0)
m_up = up.normalized();
else
m_up = btVector3(0.0, 0.0, 0.0);
if (!m_ghostObject) return;
btQuaternion rot = getRotation(m_up, u);
//set orientation with new up
btTransform xform;
xform = m_ghostObject->getWorldTransform();
btQuaternion orn = rot.inverse() * xform.getRotation();
xform.setRotation(orn);
m_ghostObject->setWorldTransform(xform);
}
btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
{
if (v0.length2() == 0.0f || v1.length2() == 0.0f)
{
btQuaternion q;
return q;
}
return shortestArcQuatNormalize2(v0, v1);
}

60
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@@ -43,10 +43,12 @@ protected:
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
btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_SetjumpSpeed;
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)
@@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
btVector3 m_AngVel;
btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
btQuaternion m_currentOrientation;
btQuaternion m_targetOrientation;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;
btVector3 m_up;
btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
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);
virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
void setUpVector(const btVector3& up);
btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
void setUp(const btVector3& up);
const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
virtual void setAngularVelocity(const btVector3& velocity);
virtual const btVector3& getAngularVelocity() const;
virtual void setLinearVelocity(const btVector3& velocity);
virtual btVector3 getLinearVelocity() const;
void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getLinearDamping() const { return m_linearDamping; }
void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
btScalar getAngularDamping() const { return m_angularDamping; }
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void setStepHeight(btScalar h);
btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
void jump(const btVector3& v = btVector3());
void setGravity(btScalar gravity);
btScalar getGravity() const;
void applyImpulse(const btVector3& v) { jump(v); }
void setGravity(const btVector3& gravity);
btVector3 getGravity() const;
/// 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;
void setMaxPenetrationDepth(btScalar d);
btScalar getMaxPenetrationDepth() const;
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{