Changeset 8284 for code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

Timestamp:

Apr 21, 2011, 6:58:23 PM (14 years ago)

Author:

rgrieder

Message:

Merged revisions 7978 - 8096 from kicklib to kicklib2.

Location:

code/branches/kicklib2

Files:

• . (modified) (1 prop)
• src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp (modified) (26 diffs)

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -13 +13 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
 
 #include "LinearMath/btIDebugDraw.h"
@@ -23 +24 @@
 #include "btKinematicCharacterController.h"
 
-static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+
+// static helper method
+static btVector3
+getNormalizedVector(const btVector3& v)
+{
+        btVector3 n = v.normalized();
+        if (n.length() < SIMD_EPSILON) {
+                n.setValue(0, 0, 0);
+        }
+        return n;
+}
+
 
 ///@todo Interact with dynamic objects,
@@ -53 +65 @@
 {
 public:
-        btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
-        {
-                m_me = me;
+        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)
+        {
         }
 
@@ -61 +76 @@
         {
                 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);
@@ -67 +97 @@
 protected:
         btCollisionObject* m_me;
+        const btVector3 m_up;
+        btScalar m_minSlopeDot;
 };
 
@@ -99 +131 @@
 {
         m_upAxis = upAxis;
-        m_addedMargin = 0.02f;
+        m_addedMargin = 0.02;
         m_walkDirection.setValue(0,0,0);
         m_useGhostObjectSweepTest = true;
@@ -106 +138 @@
         m_turnAngle = btScalar(0.0);
         m_convexShape=convexShape;      
+        m_useWalkDirection = true;      // use walk direction by default, legacy behavior
+        m_velocityTimeInterval = 0.0;
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+        m_gravity = 9.8 * 3 ; // 3G acceleration.
+        m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
+        m_jumpSpeed = 10.0; // ?
+        m_wasOnGround = false;
+        m_wasJumping = false;
+        setMaxSlope(btRadians(45.0));
 }
 
@@ -145 +187 @@
                                 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);
                                 }
                         }
@@ -174 +218 @@
         // phase 1: up
         btTransform start, end;
-        m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
+        m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
 
         start.setIdentity ();
@@ -180 +224 @@
 
         /* FIXME: Handle penetration properly */
-        start.setOrigin (m_currentPosition + upAxisDirection[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);
+        btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -198 +242 @@
         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);
+                // 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)
+                {
+                        // 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;
@@ -245 +295 @@
 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
 {
-
-        btVector3 originalDir = walkMove.normalized();
-        if (walkMove.length() < SIMD_EPSILON)
-        {
-                originalDir.setValue(0.f,0.f,0.f);
-        }
-//      printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
+        // printf("m_normalizedDirection=%f,%f,%f\n",
+        //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
         // phase 2: forward and strafe
         btTransform start, end;
         m_targetPosition = m_currentPosition + walkMove;
+
         start.setIdentity ();
         end.setIdentity ();
@@ -264 +310 @@
         if (m_touchingContact)
         {
-                if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
+                if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
+                {
                         updateTargetPositionBasedOnCollision (m_touchingNormal);
+                }
         }
 
@@ -274 +322 @@
                 start.setOrigin (m_currentPosition);
                 end.setOrigin (m_targetPosition);
-
-                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+                btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
+
+                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -300 +349 @@
                 {       
                         // 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)
-                        {
-//                              printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-                        }
+                        btScalar hitDistance;
+                        hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+
+//                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
@@ -320 +361 @@
                                 currentDir.normalize();
                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
-                                if (currentDir.dot(originalDir) <= btScalar(0.0))
+                                if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
                                 {
                                         break;
@@ -329 +370 @@
                                 break;
                         }
+
                 } else {
                         // we moved whole way
@@ -345 +387 @@
 
         // phase 3: down
-        btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
-        btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; 
-        m_targetPosition -= (step_drop + gravity_drop);
+        /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+        btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
+        btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
+        m_targetPosition -= (step_drop + gravity_drop);*/
+
+        btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+        if(downVelocity > 0.0 && downVelocity < m_stepHeight
+                && (m_wasOnGround || !m_wasJumping))
+        {
+                downVelocity = m_stepHeight;
+        }
+
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+        m_targetPosition -= step_drop;
 
         start.setIdentity ();
@@ -355 +409 @@
         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;
@@ -371 +425 @@
                 // we dropped a fraction of the height -> hit floor
                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                m_verticalVelocity = 0.0;
+                m_verticalOffset = 0.0;
+                m_wasJumping = false;
         } else {
                 // we dropped the full height
@@ -378 +435 @@
 }
 
+
+
+void btKinematicCharacterController::setWalkDirection
+(
+const btVector3& walkDirection
+)
+{
+        m_useWalkDirection = true;
+        m_walkDirection = walkDirection;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+}
+
+
+
+void btKinematicCharacterController::setVelocityForTimeInterval
+(
+const btVector3& velocity,
+btScalar timeInterval
+)
+{
+//      printf("setVelocity!\n");
+//      printf("  interval: %f\n", timeInterval);
+//      printf("  velocity: (%f, %f, %f)\n",
+//               velocity.x(), velocity.y(), velocity.z());
+
+        m_useWalkDirection = false;
+        m_walkDirection = velocity;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+        m_velocityTimeInterval = timeInterval;
+}
+
+
+
 void btKinematicCharacterController::reset ()
 {
@@ -402 +492 @@
                 if (numPenetrationLoops > 4)
                 {
-//                      printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+                        //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
                         break;
                 }
@@ -414 +504 @@
 }
 
+#include <stdio.h>
+
 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
 {
+//      printf("playerStep(): ");
+//      printf("  dt = %f", dt);
+
+        // quick check...
+        if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+//              printf("\n");
+                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 ();
@@ -423 +539 @@
 
         stepUp (collisionWorld);
-        stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        if (m_useWalkDirection) {
+                stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        } else {
+                //printf("  time: %f", m_velocityTimeInterval);
+                // still have some time left for moving!
+                btScalar dtMoving =
+                        (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
+                m_velocityTimeInterval -= dt;
+
+                // how far will we move while we are moving?
+                btVector3 move = m_walkDirection * dtMoving;
+
+                //printf("  dtMoving: %f", dtMoving);
+
+                // okay, step
+                stepForwardAndStrafe(collisionWorld, move);
+        }
         stepDown (collisionWorld, dt);
+
+        // printf("\n");
 
         xform.setOrigin (m_currentPosition);
@@ -454 +588 @@
         if (!canJump())
                 return;
+
+        m_verticalVelocity = m_jumpSpeed;
+        m_wasJumping = true;
 
 #if 0
@@ -466 +603 @@
 }
 
+void btKinematicCharacterController::setGravity(btScalar gravity)
+{
+        m_gravity = gravity;
+}
+
+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 true;
-}
-
-
-void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
-{
-}
+        return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+}
+
+
+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;
+}
+
+void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}