diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index d55121827..48f4d3423 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -64,20 +64,40 @@ protected: class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { public: - btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + 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) { - m_me = me; } virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) - return 1.0; + return btScalar(1.0); + + btVector3 hitNormalWorld; + if (normalInWorldSpace) + { + hitNormalWorld = convexResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + } + + btScalar dotUp = m_up.dot(hitNormalWorld); + if (dotUp < m_minSlopeDot) { + return btScalar(1.0); + } return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); } protected: btCollisionObject* m_me; + const btVector3 m_up; + btScalar m_minSlopeDot; }; /* @@ -110,7 +130,7 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) { m_upAxis = upAxis; - m_addedMargin = 0.02f; + m_addedMargin = 0.02; m_walkDirection.setValue(0,0,0); m_useGhostObjectSweepTest = true; m_ghostObject = ghostObject; @@ -119,6 +139,13 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho 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; // 1G accelation. + m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. + m_jumpSpeed = 10.0; // ? + m_wasOnGround = false; + setMaxSlope(btRadians(50.0)); } btKinematicCharacterController::~btKinematicCharacterController () @@ -158,18 +185,20 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* { const btManifoldPoint&pt = manifold->getContactPoint(p); - if (pt.getDistance() < 0.0) + btScalar dist = pt.getDistance(); + + if (dist < 0.0) { - if (pt.getDistance() < maxPen) + if (dist < maxPen) { - maxPen = pt.getDistance(); + maxPen = dist; m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? } - m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); + m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); penetration = true; } else { - //printf("touching %f\n", pt.getDistance()); + //printf("touching %f\n", dist); } } @@ -187,16 +216,17 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) { // phase 1: up btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight; + m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0?m_verticalOffset:0.0)); start.setIdentity (); end.setIdentity (); /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * btScalar(0.1f)); + start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + // Find only sloped/flat surface hits, avoid wall and ceiling hits... + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -214,6 +244,8 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) // 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); + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; } else { m_currentStepOffset = m_stepHeight; m_currentPosition = m_targetPosition; @@ -270,11 +302,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); // printf("distance2=%f\n",distance2); - if (m_touchingContact) + /*if (m_touchingContact) { if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) updateTargetPositionBasedOnCollision (m_touchingNormal); - } + }*/ int maxIter = 10; @@ -283,7 +315,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co start.setOrigin (m_currentPosition); end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], btScalar(-1.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -309,17 +341,13 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co { // we moved only a fraction btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); - if (hitDistance<0.f) - { -// printf("neg dist?\n"); - } /* If the distance is farther than the collision margin, move */ - if (hitDistance > m_addedMargin) - { +// if (hitDistance > m_addedMargin) +// { // printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - } +// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); +// } updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); btVector3 currentDir = m_targetPosition - m_currentPosition; @@ -353,8 +381,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld btTransform start, end; // phase 3: down - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * m_currentStepOffset; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * m_stepHeight; + 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); start.setIdentity (); @@ -363,7 +393,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld start.setOrigin (m_currentPosition); end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -379,6 +409,8 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { // 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; } else { // we dropped the full height @@ -409,7 +441,7 @@ btScalar timeInterval // printf("setVelocity!\n"); // printf(" interval: %f\n", timeInterval); // printf(" velocity: (%f, %f, %f)\n", -// velocity.x(), velocity.y(), velocity.z()); +// velocity.x(), velocity.y(), velocity.z()); m_useWalkDirection = false; m_walkDirection = velocity; @@ -466,6 +498,21 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo return; // no motion } + m_wasOnGround = 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 (); @@ -479,7 +526,7 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo //printf(" time: %f", m_velocityTimeInterval); // still have some time left for moving! btScalar dtMoving = - (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; + (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; m_velocityTimeInterval -= dt; // how far will we move while we are moving? @@ -523,6 +570,8 @@ void btKinematicCharacterController::jump () if (!canJump()) return; + m_verticalVelocity = m_jumpSpeed; + #if 0 currently no jumping. btTransform xform; @@ -534,14 +583,30 @@ void btKinematicCharacterController::jump () #endif } -bool btKinematicCharacterController::onGround () const +void btKinematicCharacterController::setGravity(btScalar gravity) { - return true; + m_gravity = gravity; } - -void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) +btScalar btKinematicCharacterController::getGravity() const { + return m_gravity; +} + +void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) +{ + m_maxSlopeRadians = slopeRadians; + m_maxSlopeCosine = btCos(slopeRadians); +} + +btScalar btKinematicCharacterController::getMaxSlope() const +{ + return m_maxSlopeRadians; +} + +bool btKinematicCharacterController::onGround () const +{ + return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; } @@ -550,4 +615,8 @@ btVector3* btKinematicCharacterController::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; -} \ No newline at end of file +} + +void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) +{ +} diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h index 16bf5df7e..2e30894a9 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -41,9 +41,14 @@ 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_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; btScalar m_turnAngle; @@ -66,6 +71,7 @@ protected: bool m_touchingContact; btVector3 m_touchingNormal; + bool m_wasOnGround; bool m_useGhostObjectSweepTest; bool m_useWalkDirection; btScalar m_velocityTimeInterval; @@ -131,8 +137,17 @@ public: void setJumpSpeed (btScalar jumpSpeed); void setMaxJumpHeight (btScalar maxJumpHeight); bool canJump () const; + void jump (); + void setGravity(btScalar gravity); + btScalar 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; + btPairCachingGhostObject* getGhostObject(); void setUseGhostSweepTest(bool useGhostObjectSweepTest) {