diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h index dffb06dfe..c3a3ac6c8 100644 --- a/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -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; diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 31faf1df5..60e49b750 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -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(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + btCollisionObject* obj1 = static_cast(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 - 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); +} + diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h index add6f30a6..3d677e647 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -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) {