summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
diff options
context:
space:
mode:
authorRémi Verschelde <rverschelde@gmail.com>2018-01-13 14:01:53 +0100
committerRémi Verschelde <rverschelde@gmail.com>2018-01-13 14:08:45 +0100
commite12c89e8c9896b2e5cdd70dbd2d2acb449ff4b94 (patch)
treeaf68e434545e20c538f896e28b73f2db7d626edd /thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
parent53c65ae7619ac9e80c89a321c70de64f3745e2aa (diff)
bullet: Streamline bundling, remove extraneous src/ folder
Document version and how to extract sources in thirdparty/README.md. Drop unnecessary CMake and Premake files. Simplify SCsub, drop unused one.
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp')
-rw-r--r--thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp1000
1 files changed, 0 insertions, 1000 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
deleted file mode 100644
index cb1aa71a14..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ /dev/null
@@ -1,1000 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include <stdio.h>
-#include "LinearMath/btIDebugDraw.h"
-#include "BulletCollision/CollisionDispatch/btGhostObject.h"
-#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
-#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
-#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
-#include "LinearMath/btDefaultMotionState.h"
-#include "btKinematicCharacterController.h"
-
-
-// static helper method
-static btVector3
-getNormalizedVector(const btVector3& v)
-{
- btVector3 n(0, 0, 0);
-
- if (v.length() > SIMD_EPSILON) {
- n = v.normalized();
- }
- return n;
-}
-
-
-///@todo Interact with dynamic objects,
-///Ride kinematicly animated platforms properly
-///More realistic (or maybe just a config option) falling
-/// -> Should integrate falling velocity manually and use that in stepDown()
-///Support jumping
-///Support ducking
-class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
-{
-public:
- btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
- {
- m_me = me;
- }
-
- virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
- {
- if (rayResult.m_collisionObject == m_me)
- return 1.0;
-
- return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
- }
-protected:
- btCollisionObject* m_me;
-};
-
-class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
-{
-public:
- 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)
- {
- }
-
- virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
- {
- if (convexResult.m_hitCollisionObject == m_me)
- return btScalar(1.0);
-
- if (!convexResult.m_hitCollisionObject->hasContactResponse())
- return btScalar(1.0);
-
- btVector3 hitNormalWorld;
- if (normalInWorldSpace)
- {
- hitNormalWorld = convexResult.m_hitNormalLocal;
- } else
- {
- ///need to transform normal into worldspace
- hitNormalWorld = convexResult.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;
-};
-
-/*
- * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
- *
- * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
- */
-btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
-{
- return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
-}
-
-/*
- * Returns the portion of 'direction' that is parallel to 'normal'
- */
-btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
-{
- btScalar magnitude = direction.dot(normal);
- return normal * magnitude;
-}
-
-/*
- * Returns the portion of 'direction' that is perpindicular to 'normal'
- */
-btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
-{
- return direction - parallelComponent(direction, normal);
-}
-
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
-{
- m_ghostObject = ghostObject;
- m_up.setValue(0.0f, 0.0f, 1.0f);
- m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
- m_addedMargin = 0.02;
- m_walkDirection.setValue(0.0,0.0,0.0);
- m_AngVel.setValue(0.0, 0.0, 0.0);
- m_useGhostObjectSweepTest = true;
- 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.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;
- m_currentStepOffset = 0.0;
- m_maxPenetrationDepth = 0.2;
- full_drop = false;
- bounce_fix = false;
- m_linearDamping = btScalar(0.0);
- m_angularDamping = btScalar(0.0);
-
- setUp(up);
- setStepHeight(stepHeight);
- setMaxSlope(btRadians(45.0));
-}
-
-btKinematicCharacterController::~btKinematicCharacterController ()
-{
-}
-
-btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
-{
- return m_ghostObject;
-}
-
-bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
-{
- // Here we must refresh the overlapping paircache as the penetrating movement itself or the
- // previous recovery iteration might have used setWorldTransform and pushed us into an object
- // that is not in the previous cache contents from the last timestep, as will happen if we
- // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
- //
- // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
- // paircache and the ghostobject's internal paircache at the same time. /BW
-
- btVector3 minAabb, maxAabb;
- m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
- collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
- minAabb,
- maxAabb,
- collisionWorld->getDispatcher());
-
- bool penetration = false;
-
- collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
-
- m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
-
-// btScalar maxPen = btScalar(0.0);
- for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
- {
- m_manifoldArray.resize(0);
-
- 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);
-
- if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
- continue;
-
- if (!needsCollision(obj0, obj1))
- continue;
-
- if (collisionPair->m_algorithm)
- collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
-
-
- for (int j=0;j<m_manifoldArray.size();j++)
- {
- btPersistentManifold* manifold = m_manifoldArray[j];
- btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
- for (int p=0;p<manifold->getNumContacts();p++)
- {
- const btManifoldPoint&pt = manifold->getContactPoint(p);
-
- btScalar dist = pt.getDistance();
-
- if (dist < -m_maxPenetrationDepth)
- {
- // 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 {
- //printf("touching %f\n", dist);
- }
- }
-
- //manifold->clearManifold();
- }
- }
- btTransform newTrans = m_ghostObject->getWorldTransform();
- newTrans.setOrigin(m_currentPosition);
- m_ghostObject->setWorldTransform(newTrans);
-// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
- return penetration;
-}
-
-void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
-{
- btScalar stepHeight = 0.0f;
- if (m_verticalVelocity < 0.0)
- stepHeight = m_stepHeight;
-
- // phase 1: up
- btTransform start, end;
-
- start.setIdentity ();
- end.setIdentity ();
-
- /* FIXME: Handle penetration properly */
- 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);
-
- 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;
-
- if (m_useGhostObjectSweepTest)
- {
- m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
- }
- else
- {
- world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
- }
-
- 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(m_up) > 0.0)
- {
- // we moved up only a fraction of the step height
- 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;
- }
-
- 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 = 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;
- btScalar movementLength = movementDirection.length();
- if (movementLength>SIMD_EPSILON)
- {
- movementDirection.normalize();
-
- btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
- reflectDir.normalize();
-
- btVector3 parallelDir, perpindicularDir;
-
- parallelDir = parallelComponent (reflectDir, hitNormal);
- perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
-
- m_targetPosition = m_currentPosition;
- if (0)//tangentMag != 0.0)
- {
- btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
-// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
- m_targetPosition += parComponent;
- }
-
- if (normalMag != 0.0)
- {
- btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
-// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
- m_targetPosition += perpComponent;
- }
- } else
- {
-// printf("movementLength don't normalize a zero vector\n");
- }
-}
-
-void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
-{
- // 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 ();
-
- btScalar fraction = 1.0;
- btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-// printf("distance2=%f\n",distance2);
-
- int maxIter = 10;
-
- while (fraction > btScalar(0.01) && maxIter-- > 0)
- {
- start.setOrigin (m_currentPosition);
- 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;
-
-
- btScalar margin = m_convexShape->getMargin();
- m_convexShape->setMargin(margin + m_addedMargin);
-
- if (!(start == end))
- {
- 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() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
- {
- // we moved only a fraction
- //btScalar hitDistance;
- //hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
-
-// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-
- updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
- btVector3 currentDir = m_targetPosition - m_currentPosition;
- distance2 = currentDir.length2();
- if (distance2 > SIMD_EPSILON)
- {
- currentDir.normalize();
- /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
- if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
- {
- break;
- }
- } else
- {
-// printf("currentDir: don't normalize a zero vector\n");
- break;
- }
-
- }
- else
- {
- m_currentPosition = m_targetPosition;
- }
- }
-}
-
-void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
-{
- btTransform start, end, end_double;
- bool runonce = false;
-
- // phase 3: down
- /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
- 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 = 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 = m_up * (m_currentStepOffset + downVelocity);
- m_targetPosition -= step_drop;
-
- 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, m_up, m_maxSlopeCosine);
- callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
- callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-
- while (1)
- {
- start.setIdentity ();
- end.setIdentity ();
-
- end_double.setIdentity ();
-
- 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);
-
- if (m_useGhostObjectSweepTest)
- {
- m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-
- if (!callback.hasHit() && m_ghostObject->hasContactResponse())
- {
- //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);
- }
- } else
- {
- collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-
- if (!callback.hasHit() && m_ghostObject->hasContactResponse())
- {
- //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;
- if (bounce_fix == true)
- has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
- else
- has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
-
- 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 = stepHeight;
-
- step_drop = m_up * (m_currentStepOffset + downVelocity);
- m_targetPosition -= step_drop;
- runonce = true;
- continue; //re-run previous tests
- }
- break;
- }
-
- if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true)
- {
- // 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());
-
- 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);
- }
- else
- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-
- full_drop = false;
-
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
- m_wasJumping = false;
- } else {
- // we dropped the full height
-
- full_drop = true;
-
- if (bounce_fix == true)
- {
- downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
- if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
- {
- m_targetPosition += step_drop; //undo previous target change
- downVelocity = m_fallSpeed;
- step_drop = m_up * (m_currentStepOffset + downVelocity);
- m_targetPosition -= step_drop;
- }
- }
- //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
-
- m_currentPosition = m_targetPosition;
- }
-}
-
-
-
-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::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 * (btSin(SIMD_HALF_PI - btAcos(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;
-
- //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)
-{
- btTransform xform;
- xform.setIdentity();
- xform.setOrigin (origin);
- m_ghostObject->setWorldTransform (xform);
-}
-
-
-void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
-{
- 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]);
-}
-
-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)) {
-// 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)
- {
- 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();
-
-// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
-// printf("walkSpeed=%f\n",walkSpeed);
-
- 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 {
- //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);
-
- //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)
-{
- m_fallSpeed = fallSpeed;
-}
-
-void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
-{
- m_jumpSpeed = jumpSpeed;
- m_SetjumpSpeed = m_jumpSpeed;
-}
-
-void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
-{
- m_maxJumpHeight = maxJumpHeight;
-}
-
-bool btKinematicCharacterController::canJump () const
-{
- return onGround();
-}
-
-void btKinematicCharacterController::jump(const btVector3& v)
-{
- 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;
- m_rigidBody->getMotionState()->getWorldTransform (xform);
- btVector3 up = xform.getBasis()[1];
- up.normalize ();
- btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
- m_rigidBody->applyCentralImpulse (up * magnitude);
-#endif
-}
-
-void btKinematicCharacterController::setGravity(const btVector3& gravity)
-{
- if (gravity.length2() > 0) setUpVector(-gravity);
-
- m_gravity = gravity.length();
-}
-
-btVector3 btKinematicCharacterController::getGravity() const
-{
- return -m_gravity * m_up;
-}
-
-void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
-{
- m_maxSlopeRadians = slopeRadians;
- m_maxSlopeCosine = btCos(slopeRadians);
-}
-
-btScalar btKinematicCharacterController::getMaxSlope() const
-{
- return m_maxSlopeRadians;
-}
-
-void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
-{
- 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()
-{
- 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)
-{
-}
-
-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);
-}
-