mirror of
https://github.com/overte-org/overte.git
synced 2025-04-21 23:24:22 +02:00
Merge pull request #4482 from AndrewMeadows/bispinor
CharacterController can walk up ledges #2
This commit is contained in:
commit
dfd9b9958b
4 changed files with 453 additions and 394 deletions
libraries/physics/src
|
@ -16,10 +16,9 @@ subject to the following restrictions:
|
|||
*/
|
||||
|
||||
|
||||
//#include <stdio.h>
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
|
||||
|
||||
#include "BulletUtil.h"
|
||||
#include "CharacterController.h"
|
||||
|
||||
|
||||
|
@ -54,7 +53,7 @@ m_me = me;
|
|||
|
||||
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
|
||||
{
|
||||
if(rayResult.m_collisionObject == m_me)
|
||||
if (rayResult.m_collisionObject == m_me)
|
||||
return 1.0;
|
||||
|
||||
return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
|
||||
|
@ -64,8 +63,7 @@ btCollisionObject* m_me;
|
|||
};
|
||||
*/
|
||||
|
||||
class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
|
||||
{
|
||||
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))
|
||||
|
@ -92,6 +90,9 @@ class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::Clo
|
|||
hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
|
||||
}
|
||||
|
||||
// Note: hitNormalWorld points into character, away from object
|
||||
// and m_up points opposite to movement
|
||||
|
||||
btScalar dotUp = m_up.dot(hitNormalWorld);
|
||||
if (dotUp < m_minSlopeDot) {
|
||||
return btScalar(1.0);
|
||||
|
@ -106,21 +107,104 @@ protected:
|
|||
btScalar m_minSlopeDot;
|
||||
};
|
||||
|
||||
class StepDownConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
|
||||
// special convex sweep callback for character during the stepDown() phase
|
||||
public:
|
||||
StepDownConvexResultCallback(btCollisionObject* me,
|
||||
const btVector3& up,
|
||||
const btVector3& start,
|
||||
const btVector3& step,
|
||||
const btVector3& pushDirection,
|
||||
btScalar minSlopeDot,
|
||||
btScalar radius,
|
||||
btScalar halfHeight)
|
||||
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
|
||||
, m_me(me)
|
||||
, m_up(up)
|
||||
, m_start(start)
|
||||
, m_step(step)
|
||||
, m_pushDirection(pushDirection)
|
||||
, m_minSlopeDot(minSlopeDot)
|
||||
, m_radius(radius)
|
||||
, m_halfHeight(halfHeight)
|
||||
{
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// Note: hitNormalWorld points into character, away from object
|
||||
// and m_up points opposite to movement
|
||||
|
||||
btScalar dotUp = m_up.dot(hitNormalWorld);
|
||||
if (dotUp < m_minSlopeDot) {
|
||||
if (hitNormalWorld.dot(m_pushDirection) > 0.0f) {
|
||||
// ignore hits that push in same direction as character is moving
|
||||
// which helps character NOT snag when stepping off ledges
|
||||
return btScalar(1.0f);
|
||||
}
|
||||
|
||||
// compute the angle between "down" and the line from character center to "hit" point
|
||||
btVector3 fractionalStep = convexResult.m_hitFraction * m_step;
|
||||
btVector3 localHit = convexResult.m_hitPointLocal - m_start + fractionalStep;
|
||||
btScalar angle = localHit.angle(-m_up);
|
||||
|
||||
// compute a maxAngle based on size of m_step
|
||||
btVector3 side(m_radius, - (m_halfHeight - m_step.length() + fractionalStep.dot(m_up)), 0.0f);
|
||||
btScalar maxAngle = side.angle(-m_up);
|
||||
|
||||
// Ignore hits that are larger than maxAngle. Effectively what is happening here is:
|
||||
// we're ignoring hits at contacts that have non-vertical normals... if they hit higher
|
||||
// than the character's "feet". Ignoring the contact allows the character to slide down
|
||||
// for these hits. In other words, vertical walls against the character's torso will
|
||||
// not prevent them from "stepping down" to find the floor.
|
||||
if (angle > maxAngle) {
|
||||
return btScalar(1.0f);
|
||||
}
|
||||
}
|
||||
|
||||
btScalar fraction = ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
|
||||
return fraction;
|
||||
}
|
||||
|
||||
protected:
|
||||
btCollisionObject* m_me;
|
||||
const btVector3 m_up;
|
||||
btVector3 m_start;
|
||||
btVector3 m_step;
|
||||
btVector3 m_pushDirection;
|
||||
btScalar m_minSlopeDot;
|
||||
btScalar m_radius;
|
||||
btScalar m_halfHeight;
|
||||
};
|
||||
|
||||
/*
|
||||
* 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 CharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
btVector3 CharacterController::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 CharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
btVector3 CharacterController::parallelComponent(const btVector3& direction, const btVector3& normal) {
|
||||
btScalar magnitude = direction.dot(normal);
|
||||
return normal * magnitude;
|
||||
}
|
||||
|
@ -128,36 +212,38 @@ btVector3 CharacterController::parallelComponent(const btVector3& direction, con
|
|||
/*
|
||||
* Returns the portion of 'direction' that is perpindicular to 'normal'
|
||||
*/
|
||||
btVector3 CharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
|
||||
{
|
||||
btVector3 CharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal) {
|
||||
return direction - parallelComponent(direction, normal);
|
||||
}
|
||||
|
||||
CharacterController::CharacterController(
|
||||
btPairCachingGhostObject* ghostObject,
|
||||
btConvexShape* convexShape,
|
||||
btScalar stepHeight,
|
||||
int upAxis) {
|
||||
m_upAxis = upAxis;
|
||||
m_addedMargin = 0.02;
|
||||
m_walkDirection.setValue(0,0,0);
|
||||
m_useGhostObjectSweepTest = true;
|
||||
m_ghostObject = ghostObject;
|
||||
m_stepHeight = stepHeight;
|
||||
m_turnAngle = btScalar(0.0);
|
||||
m_convexShape = convexShape;
|
||||
CharacterController::CharacterController(AvatarData* avatarData) {
|
||||
assert(avatarData);
|
||||
m_avatarData = avatarData;
|
||||
|
||||
// cache the "PhysicsEnabled" state of m_avatarData
|
||||
m_avatarData->lockForRead();
|
||||
m_enabled = m_avatarData->isPhysicsEnabled();
|
||||
m_avatarData->unlock();
|
||||
|
||||
createShapeAndGhost();
|
||||
|
||||
m_upAxis = 1; // HACK: hard coded to be 1 for now (yAxis)
|
||||
|
||||
m_addedMargin = 0.02f;
|
||||
m_walkDirection.setValue(0.0f,0.0f,0.0f);
|
||||
m_turnAngle = btScalar(0.0f);
|
||||
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_velocityTimeInterval = 0.0f;
|
||||
m_verticalVelocity = 0.0f;
|
||||
m_verticalOffset = 0.0f;
|
||||
m_gravity = 9.8f;
|
||||
m_maxFallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
|
||||
m_jumpSpeed = 10.0f; // ?
|
||||
m_wasOnGround = false;
|
||||
m_wasJumping = false;
|
||||
m_interpolateUp = true;
|
||||
setMaxSlope(btRadians(45.0));
|
||||
m_currentStepOffset = 0;
|
||||
setMaxSlope(btRadians(45.0f));
|
||||
m_lastStepUp = 0.0f;
|
||||
|
||||
// internal state data members
|
||||
full_drop = false;
|
||||
|
@ -192,6 +278,9 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
|
|||
collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
|
||||
|
||||
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
|
||||
btVector3 up = getUpAxisDirections()[m_upAxis];
|
||||
|
||||
btVector3 currentPosition = m_currentPosition;
|
||||
|
||||
btScalar maxPen = btScalar(0.0);
|
||||
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) {
|
||||
|
@ -210,83 +299,109 @@ bool CharacterController::recoverFromPenetration(btCollisionWorld* collisionWorl
|
|||
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
|
||||
}
|
||||
|
||||
for (int j=0;j<m_manifoldArray.size();j++) {
|
||||
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++) {
|
||||
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 < 0.0) {
|
||||
if (dist < maxPen) {
|
||||
maxPen = dist;
|
||||
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
|
||||
bool useContact = true;
|
||||
btVector3 normal = pt.m_normalWorldOnB;
|
||||
normal *= directionSign; // always points from object to character
|
||||
|
||||
btScalar normalDotUp = normal.dot(up);
|
||||
if (normalDotUp < m_maxSlopeCosine) {
|
||||
// this contact has a non-vertical normal... might need to ignored
|
||||
btVector3 collisionPoint;
|
||||
if (directionSign > 0.0) {
|
||||
collisionPoint = pt.getPositionWorldOnB();
|
||||
} else {
|
||||
collisionPoint = pt.getPositionWorldOnA();
|
||||
}
|
||||
|
||||
// we do math in frame where character base is origin
|
||||
btVector3 characterBase = currentPosition - (m_radius + m_halfHeight) * up;
|
||||
collisionPoint -= characterBase;
|
||||
btScalar collisionHeight = collisionPoint.dot(up);
|
||||
|
||||
if (collisionHeight < m_lastStepUp) {
|
||||
// This contact is below the lastStepUp, so we ignore it for penetration resolution,
|
||||
// otherwise it may prevent the character from getting close enough to find any available
|
||||
// horizontal foothold that would allow it to climbe the ledge. In other words, we're
|
||||
// making the character's "feet" soft for collisions against steps, but not floors.
|
||||
useContact = false;
|
||||
}
|
||||
}
|
||||
if (useContact) {
|
||||
|
||||
if (dist < maxPen) {
|
||||
maxPen = dist;
|
||||
m_floorNormal = normal;
|
||||
}
|
||||
const btScalar INCREMENTAL_RESOLUTION_FACTOR = 0.2f;
|
||||
m_currentPosition += normal * (fabsf(dist) * INCREMENTAL_RESOLUTION_FACTOR);
|
||||
penetration = true;
|
||||
}
|
||||
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 CharacterController::stepUp( btCollisionWorld* world) {
|
||||
// phase 1: up
|
||||
|
||||
// compute start and end
|
||||
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));
|
||||
|
||||
//m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
|
||||
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight;
|
||||
end.setIdentity();
|
||||
end.setOrigin(m_targetPosition);
|
||||
|
||||
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
|
||||
// sweep up
|
||||
btVector3 sweepDirNegative = -getUpAxisDirections()[m_upAxis];
|
||||
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.7071));
|
||||
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);
|
||||
}
|
||||
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
|
||||
|
||||
if (callback.hasHit()) {
|
||||
// we hit something, so zero our vertical velocity
|
||||
m_verticalVelocity = 0.0;
|
||||
m_verticalOffset = 0.0;
|
||||
|
||||
// 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_lastStepUp = m_stepHeight * callback.m_closestHitFraction;
|
||||
if (m_interpolateUp == true) {
|
||||
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
|
||||
} else {
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
} else {
|
||||
m_lastStepUp = m_stepHeight;
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
m_verticalVelocity = 0.0;
|
||||
m_verticalOffset = 0.0;
|
||||
} else {
|
||||
m_currentStepOffset = m_stepHeight;
|
||||
m_currentPosition = m_targetPosition;
|
||||
m_lastStepUp = m_stepHeight;
|
||||
}
|
||||
}
|
||||
|
||||
void CharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) {
|
||||
btVector3 movementDirection = m_targetPosition - m_currentPosition;
|
||||
btScalar movementLength = movementDirection.length();
|
||||
if (movementLength>SIMD_EPSILON) {
|
||||
if (movementLength > SIMD_EPSILON) {
|
||||
movementDirection.normalize();
|
||||
|
||||
btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
|
||||
|
@ -300,241 +415,152 @@ void CharacterController::updateTargetPositionBasedOnCollision(const btVector3&
|
|||
m_targetPosition = m_currentPosition;
|
||||
//if (tangentMag != 0.0) {
|
||||
if (0) {
|
||||
btVector3 parComponent = parallelDir * btScalar(tangentMag*movementLength);
|
||||
//printf("parComponent=%f,%f,%f\n", parComponent[0], parComponent[1], parComponent[2]);
|
||||
btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
|
||||
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]);
|
||||
btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
|
||||
m_targetPosition += perpComponent;
|
||||
}
|
||||
} else {
|
||||
//printf("movementLength don't normalize a zero vector\n");
|
||||
}
|
||||
}
|
||||
|
||||
void CharacterController::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;
|
||||
void CharacterController::stepForward( btCollisionWorld* collisionWorld, const btVector3& movement) {
|
||||
// phase 2: forward
|
||||
m_targetPosition = m_currentPosition + movement;
|
||||
|
||||
btTransform start, end;
|
||||
start.setIdentity();
|
||||
end.setIdentity();
|
||||
|
||||
btScalar fraction = 1.0;
|
||||
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
|
||||
//printf("distance2=%f\n", distance2);
|
||||
|
||||
/* TODO: experiment with this to see if we can use this to help direct motion when a floor is available
|
||||
if (m_touchingContact) {
|
||||
if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) {
|
||||
//interferes with step movement
|
||||
//updateTargetPositionBasedOnCollision(m_touchingNormal);
|
||||
if (m_normalizedDirection.dot(m_floorNormal) < btScalar(0.0)) {
|
||||
updateTargetPositionBasedOnCollision(m_floorNormal, 1.0f, 1.0f);
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
// modify shape's margin for the sweeps
|
||||
btScalar margin = m_convexShape->getMargin();
|
||||
m_convexShape->setMargin(margin + m_addedMargin);
|
||||
|
||||
const btScalar MIN_STEP_DISTANCE = 0.0001f;
|
||||
btVector3 step = m_targetPosition - m_currentPosition;
|
||||
btScalar stepLength2 = step.length2();
|
||||
int maxIter = 10;
|
||||
|
||||
while (fraction > btScalar(0.01) && maxIter-- > 0) {
|
||||
while (stepLength2 > MIN_STEP_DISTANCE && maxIter-- > 0) {
|
||||
start.setOrigin(m_currentPosition);
|
||||
end.setOrigin(m_targetPosition);
|
||||
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
|
||||
|
||||
// sweep forward
|
||||
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;
|
||||
|
||||
|
||||
btScalar margin = m_convexShape->getMargin();
|
||||
m_convexShape->setMargin(margin + m_addedMargin);
|
||||
|
||||
|
||||
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;
|
||||
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
|
||||
|
||||
if (callback.hasHit()) {
|
||||
// we moved only a fraction
|
||||
//btScalar hitDistance;
|
||||
//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
|
||||
// we hit soemthing!
|
||||
// Compute new target position by removing portion cut-off by collision, which will produce a new target
|
||||
// that is the closest approach of the the obstacle plane to the original target.
|
||||
step = m_targetPosition - m_currentPosition;
|
||||
btScalar stepDotNormal = step.dot(callback.m_hitNormalWorld); // we expect this dot to be negative
|
||||
step += (stepDotNormal * (1.0f - callback.m_closestHitFraction)) * callback.m_hitNormalWorld;
|
||||
m_targetPosition = m_currentPosition + step;
|
||||
|
||||
//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;
|
||||
}
|
||||
stepLength2 = step.length2();
|
||||
} else {
|
||||
// we moved whole way
|
||||
// we swept to the end without hitting anything
|
||||
m_currentPosition = m_targetPosition;
|
||||
break;
|
||||
}
|
||||
|
||||
//if (callback.m_closestHitFraction == 0.f) {
|
||||
// break;
|
||||
//}
|
||||
|
||||
}
|
||||
|
||||
// restore shape's margin
|
||||
m_convexShape->setMargin(margin);
|
||||
}
|
||||
|
||||
void CharacterController::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 = 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);*/
|
||||
//
|
||||
// The "stepDown" phase first makes a normal sweep down that cancels the lift from the "stepUp" phase.
|
||||
// If it hits a ledge then it stops otherwise it makes another sweep down in search of a floor within
|
||||
// reach of the character's feet.
|
||||
|
||||
btVector3 orig_position = m_targetPosition;
|
||||
|
||||
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
|
||||
|
||||
if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) {
|
||||
downVelocity = m_fallSpeed;
|
||||
btScalar downSpeed = (m_verticalVelocity < 0.0f) ? -m_verticalVelocity : 0.0f;
|
||||
if (downSpeed > 0.0f && downSpeed > m_maxFallSpeed && (m_wasOnGround || !m_wasJumping)) {
|
||||
downSpeed = m_maxFallSpeed;
|
||||
}
|
||||
|
||||
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
|
||||
m_targetPosition -= step_drop;
|
||||
// first sweep for ledge
|
||||
btVector3 step = getUpAxisDirections()[m_upAxis] * (-(m_lastStepUp + downSpeed * dt));
|
||||
|
||||
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
|
||||
StepDownConvexResultCallback callback(m_ghostObject,
|
||||
getUpAxisDirections()[m_upAxis],
|
||||
m_currentPosition, step,
|
||||
m_walkDirection,
|
||||
m_maxSlopeCosine,
|
||||
m_radius, m_halfHeight);
|
||||
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
|
||||
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
|
||||
|
||||
btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
|
||||
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
|
||||
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
|
||||
btTransform start, end;
|
||||
start.setIdentity();
|
||||
end.setIdentity();
|
||||
|
||||
while (1) {
|
||||
start.setIdentity();
|
||||
end.setIdentity();
|
||||
start.setOrigin(m_currentPosition);
|
||||
m_targetPosition = m_currentPosition + step;
|
||||
end.setOrigin(m_targetPosition);
|
||||
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
|
||||
|
||||
end_double.setIdentity();
|
||||
if (callback.hasHit()) {
|
||||
m_currentPosition += callback.m_closestHitFraction * step;
|
||||
m_verticalVelocity = 0.0f;
|
||||
m_verticalOffset = 0.0f;
|
||||
m_wasJumping = false;
|
||||
} else {
|
||||
// sweep again for floor within downStep threshold
|
||||
StepDownConvexResultCallback callback2 (m_ghostObject,
|
||||
getUpAxisDirections()[m_upAxis],
|
||||
m_currentPosition, step,
|
||||
m_walkDirection,
|
||||
m_maxSlopeCosine,
|
||||
m_radius, m_halfHeight);
|
||||
|
||||
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
|
||||
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
|
||||
|
||||
m_currentPosition = m_targetPosition;
|
||||
btVector3 oldPosition = m_currentPosition;
|
||||
step = (- m_stepHeight) * getUpAxisDirections()[m_upAxis];
|
||||
m_targetPosition = m_currentPosition + step;
|
||||
|
||||
start.setOrigin(m_currentPosition);
|
||||
end.setOrigin(m_targetPosition);
|
||||
m_ghostObject->convexSweepTest(m_convexShape, start, end, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
|
||||
|
||||
//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()) {
|
||||
//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);
|
||||
}
|
||||
if (callback2.hasHit()) {
|
||||
m_currentPosition += callback2.m_closestHitFraction * step;
|
||||
m_verticalVelocity = 0.0f;
|
||||
m_verticalOffset = 0.0f;
|
||||
m_wasJumping = false;
|
||||
} else {
|
||||
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);
|
||||
}
|
||||
// nothing to step down on, so remove the stepUp effect
|
||||
m_currentPosition = oldPosition - m_lastStepUp * getUpAxisDirections()[m_upAxis];
|
||||
m_lastStepUp = 0.0f;
|
||||
}
|
||||
|
||||
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();
|
||||
} else {
|
||||
has_hit = callback2.hasHit();
|
||||
}
|
||||
|
||||
if(downVelocity2 > 0.0 && downVelocity2 < m_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;
|
||||
|
||||
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
|
||||
m_targetPosition -= step_drop;
|
||||
runonce = true;
|
||||
continue; //re-run previous tests
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (callback.hasHit() || 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 = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
|
||||
m_targetPosition -= step_drop;
|
||||
}
|
||||
}
|
||||
//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
|
||||
|
||||
m_currentPosition = m_targetPosition;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void CharacterController::setWalkDirection(const btVector3& walkDirection) {
|
||||
m_useWalkDirection = true;
|
||||
m_walkDirection = walkDirection;
|
||||
m_normalizedDirection = getNormalizedVector(m_walkDirection);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void CharacterController::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);
|
||||
|
@ -565,29 +591,25 @@ void CharacterController::warp(const btVector3& origin) {
|
|||
|
||||
|
||||
void CharacterController::preStep( btCollisionWorld* collisionWorld) {
|
||||
if (!m_enabled) {
|
||||
return;
|
||||
}
|
||||
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;
|
||||
//printf("m_targetPosition=%f,%f,%f\n", m_targetPosition[0], m_targetPosition[1], m_targetPosition[2]);
|
||||
}
|
||||
|
||||
void CharacterController::playerStep( btCollisionWorld* collisionWorld, btScalar dt) {
|
||||
//printf("playerStep(): ");
|
||||
//printf(" dt = %f", dt);
|
||||
|
||||
// quick check...
|
||||
if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
|
||||
//printf("\n");
|
||||
if (!m_enabled || (!m_useWalkDirection && m_velocityTimeInterval <= 0.0)) {
|
||||
return; // no motion
|
||||
}
|
||||
|
||||
|
@ -595,49 +617,41 @@ void CharacterController::playerStep( btCollisionWorld* collisionWorld, btScala
|
|||
|
||||
// Update fall velocity.
|
||||
m_verticalVelocity -= m_gravity * dt;
|
||||
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) {
|
||||
if (m_verticalVelocity > m_jumpSpeed) {
|
||||
m_verticalVelocity = m_jumpSpeed;
|
||||
}
|
||||
if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) {
|
||||
m_verticalVelocity = -btFabs(m_fallSpeed);
|
||||
} else if (m_verticalVelocity < -m_maxFallSpeed) {
|
||||
m_verticalVelocity = -m_maxFallSpeed;
|
||||
}
|
||||
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);
|
||||
// the algorithm is as follows:
|
||||
// (1) step the character up a little bit so that its forward step doesn't hit the floor
|
||||
// (2) step the character forward
|
||||
// (3) step the character down looking for new ledges, the original floor, or a floor one step below where we started
|
||||
|
||||
stepUp (collisionWorld);
|
||||
stepUp(collisionWorld);
|
||||
if (m_useWalkDirection) {
|
||||
stepForwardAndStrafe(collisionWorld, m_walkDirection);
|
||||
stepForward(collisionWorld, m_walkDirection);
|
||||
} else {
|
||||
//printf(" time: %f", m_velocityTimeInterval);
|
||||
// still have some time left for moving!
|
||||
btScalar dtMoving =
|
||||
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
|
||||
// compute substep and decrement total interval
|
||||
btScalar dtMoving = (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
|
||||
m_velocityTimeInterval -= dt;
|
||||
|
||||
// how far will we move while we are moving?
|
||||
// stepForward substep
|
||||
btVector3 move = m_walkDirection * dtMoving;
|
||||
|
||||
//printf(" dtMoving: %f", dtMoving);
|
||||
|
||||
// okay, step
|
||||
stepForwardAndStrafe(collisionWorld, move);
|
||||
stepForward(collisionWorld, move);
|
||||
}
|
||||
stepDown(collisionWorld, dt);
|
||||
|
||||
//printf("\n");
|
||||
|
||||
xform.setOrigin(m_currentPosition);
|
||||
m_ghostObject->setWorldTransform(xform);
|
||||
}
|
||||
|
||||
void CharacterController::setFallSpeed(btScalar fallSpeed) {
|
||||
m_fallSpeed = fallSpeed;
|
||||
void CharacterController::setMaxFallSpeed(btScalar speed) {
|
||||
m_maxFallSpeed = speed;
|
||||
}
|
||||
|
||||
void CharacterController::setJumpSpeed(btScalar jumpSpeed) {
|
||||
|
@ -662,7 +676,7 @@ void CharacterController::jump() {
|
|||
|
||||
#if 0
|
||||
currently no jumping.
|
||||
btTransform xform;
|
||||
btTransform xform;
|
||||
m_rigidBody->getMotionState()->getWorldTransform(xform);
|
||||
btVector3 up = xform.getBasis()[1];
|
||||
up.normalize();
|
||||
|
@ -689,7 +703,7 @@ btScalar CharacterController::getMaxSlope() const {
|
|||
}
|
||||
|
||||
bool CharacterController::onGround() const {
|
||||
return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
|
||||
return m_enabled && m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
|
||||
}
|
||||
|
||||
btVector3* CharacterController::getUpAxisDirections() {
|
||||
|
@ -704,3 +718,109 @@ void CharacterController::debugDraw(btIDebugDraw* debugDrawer) {
|
|||
void CharacterController::setUpInterpolate(bool value) {
|
||||
m_interpolateUp = value;
|
||||
}
|
||||
|
||||
// protected
|
||||
void CharacterController::createShapeAndGhost() {
|
||||
// get new dimensions from avatar
|
||||
m_avatarData->lockForRead();
|
||||
AABox box = m_avatarData->getLocalAABox();
|
||||
|
||||
// create new ghost
|
||||
m_ghostObject = new btPairCachingGhostObject();
|
||||
m_ghostObject->setWorldTransform(btTransform(glmToBullet(m_avatarData->getOrientation()),
|
||||
glmToBullet(m_avatarData->getPosition())));
|
||||
m_avatarData->unlock();
|
||||
|
||||
const glm::vec3& diagonal = box.getScale();
|
||||
m_radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
|
||||
m_halfHeight = 0.5f * diagonal.y - m_radius;
|
||||
float MIN_HALF_HEIGHT = 0.1f;
|
||||
if (m_halfHeight < MIN_HALF_HEIGHT) {
|
||||
m_halfHeight = MIN_HALF_HEIGHT;
|
||||
}
|
||||
glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
|
||||
m_shapeLocalOffset = offset;
|
||||
|
||||
// stepHeight affects the heights of ledges that the character can ascend
|
||||
// however the actual ledge height is some function of m_stepHeight
|
||||
// due to character shape and this CharacterController algorithm
|
||||
// (the function is approximately 2*m_stepHeight)
|
||||
m_stepHeight = 0.1f * (m_radius + m_halfHeight) + 0.1f;
|
||||
|
||||
// create new shape
|
||||
m_convexShape = new btCapsuleShape(m_radius, 2.0f * m_halfHeight);
|
||||
m_ghostObject->setCollisionShape(m_convexShape);
|
||||
m_ghostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
|
||||
}
|
||||
|
||||
bool CharacterController::needsShapeUpdate() {
|
||||
// get new dimensions from avatar
|
||||
m_avatarData->lockForRead();
|
||||
AABox box = m_avatarData->getLocalAABox();
|
||||
m_avatarData->unlock();
|
||||
|
||||
const glm::vec3& diagonal = box.getScale();
|
||||
float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
|
||||
float halfHeight = 0.5f * diagonal.y - radius;
|
||||
float MIN_HALF_HEIGHT = 0.1f;
|
||||
if (halfHeight < MIN_HALF_HEIGHT) {
|
||||
halfHeight = MIN_HALF_HEIGHT;
|
||||
}
|
||||
glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
|
||||
|
||||
// compare dimensions (and offset)
|
||||
float radiusDelta = glm::abs(radius - m_radius);
|
||||
float heightDelta = glm::abs(halfHeight - m_halfHeight);
|
||||
if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
|
||||
// shape hasn't changed --> nothing to do
|
||||
float offsetDelta = glm::distance(offset, m_shapeLocalOffset);
|
||||
if (offsetDelta > FLT_EPSILON) {
|
||||
// if only the offset changes then we can update it --> no need to rebuild shape
|
||||
m_shapeLocalOffset = offset;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void CharacterController::updateShape() {
|
||||
// DANGER: make sure this CharacterController and its GhostShape have been removed from
|
||||
// the PhysicsEngine before calling this.
|
||||
|
||||
// delete shape and GhostObject
|
||||
delete m_ghostObject;
|
||||
m_ghostObject = NULL;
|
||||
delete m_convexShape;
|
||||
m_convexShape = NULL;
|
||||
|
||||
createShapeAndGhost();
|
||||
}
|
||||
|
||||
void CharacterController::preSimulation(btScalar timeStep) {
|
||||
m_avatarData->lockForRead();
|
||||
|
||||
// cache the "PhysicsEnabled" state of m_avatarData here
|
||||
// and use the cached value for the rest of the simulation step
|
||||
m_enabled = m_avatarData->isPhysicsEnabled();
|
||||
|
||||
glm::quat rotation = m_avatarData->getOrientation();
|
||||
glm::vec3 position = m_avatarData->getPosition() + rotation * m_shapeLocalOffset;
|
||||
m_ghostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
|
||||
btVector3 walkVelocity = glmToBullet(m_avatarData->getVelocity());
|
||||
setVelocityForTimeInterval(walkVelocity, timeStep);
|
||||
|
||||
m_avatarData->unlock();
|
||||
}
|
||||
|
||||
void CharacterController::postSimulation() {
|
||||
if (m_enabled) {
|
||||
m_avatarData->lockForWrite();
|
||||
const btTransform& avatarTransform = m_ghostObject->getWorldTransform();
|
||||
glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
|
||||
glm::vec3 offset = rotation * m_shapeLocalOffset;
|
||||
m_avatarData->setOrientation(rotation);
|
||||
m_avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset);
|
||||
m_avatarData->unlock();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -19,6 +19,8 @@ subject to the following restrictions:
|
|||
#ifndef hifi_CharacterController_h
|
||||
#define hifi_CharacterController_h
|
||||
|
||||
#include <AvatarData.h>
|
||||
|
||||
#include <btBulletDynamicsCommon.h>
|
||||
#include <BulletDynamics/Character/btCharacterControllerInterface.h>
|
||||
#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h>
|
||||
|
@ -39,14 +41,17 @@ ATTRIBUTE_ALIGNED16(class) CharacterController : public btCharacterControllerInt
|
|||
{
|
||||
protected:
|
||||
|
||||
AvatarData* m_avatarData = NULL;
|
||||
btPairCachingGhostObject* m_ghostObject;
|
||||
glm::vec3 m_shapeLocalOffset;
|
||||
|
||||
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
|
||||
btScalar m_radius;
|
||||
btScalar m_halfHeight;
|
||||
|
||||
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_verticalOffset; // fall distance from velocity this frame
|
||||
btScalar m_maxFallSpeed;
|
||||
btScalar m_jumpSpeed;
|
||||
btScalar m_maxJumpHeight;
|
||||
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
|
||||
|
@ -55,7 +60,7 @@ protected:
|
|||
|
||||
btScalar m_turnAngle;
|
||||
|
||||
btScalar m_stepHeight;
|
||||
btScalar m_stepHeight; // height of stepUp prior to stepForward
|
||||
|
||||
btScalar m_addedMargin;//@todo: remove this and fix the code
|
||||
|
||||
|
@ -65,18 +70,18 @@ protected:
|
|||
|
||||
//some internal variables
|
||||
btVector3 m_currentPosition;
|
||||
btScalar m_currentStepOffset;
|
||||
btVector3 m_targetPosition;
|
||||
btScalar m_lastStepUp;
|
||||
|
||||
///keep track of the contact manifolds
|
||||
btManifoldArray m_manifoldArray;
|
||||
|
||||
bool m_touchingContact;
|
||||
btVector3 m_touchingNormal;
|
||||
btVector3 m_floorNormal; // points from object to character
|
||||
|
||||
bool m_wasOnGround;
|
||||
bool m_wasJumping;
|
||||
bool m_useGhostObjectSweepTest;
|
||||
bool m_enabled;
|
||||
bool m_wasOnGround;
|
||||
bool m_wasJumping;
|
||||
bool m_useWalkDirection;
|
||||
btScalar m_velocityTimeInterval;
|
||||
int m_upAxis;
|
||||
|
@ -93,17 +98,14 @@ protected:
|
|||
bool recoverFromPenetration(btCollisionWorld* collisionWorld);
|
||||
void stepUp(btCollisionWorld* collisionWorld);
|
||||
void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
|
||||
void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove);
|
||||
void stepForward(btCollisionWorld* collisionWorld, const btVector3& walkMove);
|
||||
void stepDown(btCollisionWorld* collisionWorld, btScalar dt);
|
||||
void createShapeAndGhost();
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
CharacterController(
|
||||
btPairCachingGhostObject* ghostObject,
|
||||
btConvexShape* convexShape,
|
||||
btScalar stepHeight,
|
||||
int upAxis = 1);
|
||||
CharacterController(AvatarData* avatarData);
|
||||
~CharacterController();
|
||||
|
||||
|
||||
|
@ -145,7 +147,7 @@ public:
|
|||
void preStep(btCollisionWorld* collisionWorld);
|
||||
void playerStep(btCollisionWorld* collisionWorld, btScalar dt);
|
||||
|
||||
void setFallSpeed(btScalar fallSpeed);
|
||||
void setMaxFallSpeed(btScalar speed);
|
||||
void setJumpSpeed(btScalar jumpSpeed);
|
||||
void setMaxJumpHeight(btScalar maxJumpHeight);
|
||||
bool canJump() const;
|
||||
|
@ -161,12 +163,15 @@ public:
|
|||
btScalar getMaxSlope() const;
|
||||
|
||||
btPairCachingGhostObject* getGhostObject();
|
||||
void setUseGhostSweepTest(bool useGhostObjectSweepTest) {
|
||||
m_useGhostObjectSweepTest = useGhostObjectSweepTest;
|
||||
}
|
||||
|
||||
bool onGround() const;
|
||||
void setUpInterpolate(bool value);
|
||||
|
||||
bool needsShapeUpdate();
|
||||
void updateShape();
|
||||
|
||||
void preSimulation(btScalar timeStep);
|
||||
void postSimulation();
|
||||
};
|
||||
|
||||
#endif // hifi_CharacterController_h
|
||||
|
|
|
@ -24,8 +24,7 @@ uint32_t PhysicsEngine::getNumSubsteps() {
|
|||
}
|
||||
|
||||
PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
|
||||
: _originOffset(offset),
|
||||
_avatarShapeLocalOffset(0.0f) {
|
||||
: _originOffset(offset) {
|
||||
}
|
||||
|
||||
PhysicsEngine::~PhysicsEngine() {
|
||||
|
@ -279,9 +278,6 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) {
|
|||
}
|
||||
|
||||
void PhysicsEngine::stepSimulation() {
|
||||
// expect the engine to have an avatar (and hence: a character controller)
|
||||
assert(_avatarData);
|
||||
|
||||
lock();
|
||||
// NOTE: the grand order of operations is:
|
||||
// (1) relay incoming changes
|
||||
|
@ -298,19 +294,11 @@ void PhysicsEngine::stepSimulation() {
|
|||
_clock.reset();
|
||||
float timeStep = btMin(dt, MAX_TIMESTEP);
|
||||
|
||||
_avatarData->lockForRead();
|
||||
if (_avatarData->isPhysicsEnabled()) {
|
||||
// update character controller
|
||||
glm::quat rotation = _avatarData->getOrientation();
|
||||
glm::vec3 position = _avatarData->getPosition() + rotation * _avatarShapeLocalOffset;
|
||||
_avatarGhostObject->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
|
||||
|
||||
btVector3 walkVelocity = glmToBullet(_avatarData->getVelocity());
|
||||
_characterController->setVelocityForTimeInterval(walkVelocity, timeStep);
|
||||
}
|
||||
_avatarData->unlock();
|
||||
|
||||
// This is step (2).
|
||||
if (_characterController) {
|
||||
_characterController->preSimulation(timeStep);
|
||||
}
|
||||
|
||||
int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
|
||||
_numSubsteps += (uint32_t)numSubsteps;
|
||||
stepNonPhysicalKinematics(usecTimestampNow());
|
||||
|
@ -326,21 +314,11 @@ void PhysicsEngine::stepSimulation() {
|
|||
//
|
||||
// TODO: untangle these lock sequences.
|
||||
_entityTree->lockForWrite();
|
||||
|
||||
lock();
|
||||
_dynamicsWorld->synchronizeMotionStates();
|
||||
|
||||
_avatarData->lockForRead();
|
||||
bool avatarHasPhysicsEnabled = _avatarData->isPhysicsEnabled();
|
||||
_avatarData->unlock();
|
||||
if (avatarHasPhysicsEnabled) {
|
||||
const btTransform& avatarTransform = _avatarGhostObject->getWorldTransform();
|
||||
glm::quat rotation = bulletToGLM(avatarTransform.getRotation());
|
||||
glm::vec3 offset = rotation * _avatarShapeLocalOffset;
|
||||
_avatarData->lockForWrite();
|
||||
_avatarData->setOrientation(rotation);
|
||||
_avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset);
|
||||
_avatarData->unlock();
|
||||
if (_characterController) {
|
||||
_characterController->postSimulation();
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
@ -620,76 +598,35 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PhysicsEngine::setAvatarData(AvatarData *avatarData) {
|
||||
assert(avatarData); // don't pass NULL argument
|
||||
|
||||
// compute capsule dimensions
|
||||
AABox box = avatarData->getLocalAABox();
|
||||
const glm::vec3& diagonal = box.getScale();
|
||||
float radius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
|
||||
float halfHeight = 0.5f * diagonal.y - radius;
|
||||
float MIN_HALF_HEIGHT = 0.1f;
|
||||
if (halfHeight < MIN_HALF_HEIGHT) {
|
||||
halfHeight = MIN_HALF_HEIGHT;
|
||||
}
|
||||
glm::vec3 offset = box.getCorner() + 0.5f * diagonal;
|
||||
|
||||
if (!_avatarData) {
|
||||
// _avatarData is being initialized
|
||||
_avatarData = avatarData;
|
||||
} else {
|
||||
// _avatarData is being updated
|
||||
assert(_avatarData == avatarData);
|
||||
|
||||
// get old dimensions from shape
|
||||
btCapsuleShape* capsule = static_cast<btCapsuleShape*>(_avatarGhostObject->getCollisionShape());
|
||||
btScalar oldRadius = capsule->getRadius();
|
||||
btScalar oldHalfHeight = capsule->getHalfHeight();
|
||||
|
||||
// compare dimensions (and offset)
|
||||
float radiusDelta = glm::abs(radius - oldRadius);
|
||||
float heightDelta = glm::abs(halfHeight - oldHalfHeight);
|
||||
if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
|
||||
// shape hasn't changed --> nothing to do
|
||||
float offsetDelta = glm::distance(offset, _avatarShapeLocalOffset);
|
||||
if (offsetDelta > FLT_EPSILON) {
|
||||
// if only the offset changes then we can update it --> no need to rebuild shape
|
||||
_avatarShapeLocalOffset = offset;
|
||||
}
|
||||
return;
|
||||
if (_characterController) {
|
||||
bool needsShapeUpdate = _characterController->needsShapeUpdate();
|
||||
if (needsShapeUpdate) {
|
||||
lock();
|
||||
// remove old info
|
||||
_dynamicsWorld->removeCollisionObject(_characterController->getGhostObject());
|
||||
_dynamicsWorld->removeAction(_characterController);
|
||||
// update shape
|
||||
_characterController->updateShape();
|
||||
// insert new info
|
||||
_dynamicsWorld->addCollisionObject(_characterController->getGhostObject(),
|
||||
btBroadphaseProxy::CharacterFilter,
|
||||
btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
|
||||
_dynamicsWorld->addAction(_characterController);
|
||||
_characterController->reset(_dynamicsWorld);
|
||||
unlock();
|
||||
}
|
||||
|
||||
// delete old controller and friends
|
||||
_dynamicsWorld->removeCollisionObject(_avatarGhostObject);
|
||||
_dynamicsWorld->removeAction(_characterController);
|
||||
delete _characterController;
|
||||
_characterController = NULL;
|
||||
delete _avatarGhostObject;
|
||||
_avatarGhostObject = NULL;
|
||||
delete capsule;
|
||||
} else {
|
||||
// initialize _characterController
|
||||
assert(avatarData); // don't pass NULL argument
|
||||
lock();
|
||||
_characterController = new CharacterController(avatarData);
|
||||
_dynamicsWorld->addCollisionObject(_characterController->getGhostObject(),
|
||||
btBroadphaseProxy::CharacterFilter,
|
||||
btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
|
||||
_dynamicsWorld->addAction(_characterController);
|
||||
_characterController->reset(_dynamicsWorld);
|
||||
unlock();
|
||||
}
|
||||
|
||||
// set offset
|
||||
_avatarShapeLocalOffset = offset;
|
||||
|
||||
// build ghost, shape, and controller
|
||||
_avatarGhostObject = new btPairCachingGhostObject();
|
||||
_avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()),
|
||||
glmToBullet(_avatarData->getPosition())));
|
||||
// ?TODO: use ShapeManager to get avatar's shape?
|
||||
btCapsuleShape* capsule = new btCapsuleShape(radius, 2.0f * halfHeight);
|
||||
|
||||
_avatarGhostObject->setCollisionShape(capsule);
|
||||
_avatarGhostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
|
||||
|
||||
const float MIN_STEP_HEIGHT = 0.35f;
|
||||
btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, radius + 0.5f * halfHeight);
|
||||
_characterController = new CharacterController(_avatarGhostObject, capsule, stepHeight);
|
||||
|
||||
_dynamicsWorld->addCollisionObject(_avatarGhostObject, btBroadphaseProxy::CharacterFilter,
|
||||
btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter);
|
||||
_dynamicsWorld->addAction(_characterController);
|
||||
_characterController->reset(_dynamicsWorld);
|
||||
}
|
||||
|
||||
|
|
|
@ -123,9 +123,6 @@ private:
|
|||
|
||||
/// character collisions
|
||||
CharacterController* _characterController = NULL;
|
||||
class btPairCachingGhostObject* _avatarGhostObject = NULL;
|
||||
AvatarData* _avatarData = NULL;
|
||||
glm::vec3 _avatarShapeLocalOffset;
|
||||
};
|
||||
|
||||
#endif // hifi_PhysicsEngine_h
|
||||
|
|
Loading…
Reference in a new issue