From 2fad4153a9a333a17a73c3bf94627ff6b9be4710 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:34:27 -0700 Subject: [PATCH 1/8] remove unused Avatar::getBoundingRadius() --- interface/src/avatar/Avatar.cpp | 5 ----- interface/src/avatar/Avatar.h | 3 --- 2 files changed, 8 deletions(-) diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index a0fa21c674..75d77b780a 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -1026,11 +1026,6 @@ float Avatar::getHeadHeight() const { return DEFAULT_HEAD_HEIGHT; } -float Avatar::getBoundingRadius() const { - // TODO: also use head model when computing the avatar's bounding radius - return _skeletonModel.getBoundingRadius(); -} - float Avatar::getPelvisFloatingHeight() const { return -_skeletonModel.getBindExtents().minimum.y; } diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index e9a21af98e..0cde800be0 100644 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -133,9 +133,6 @@ public: virtual void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) { } - /// \return bounding radius of avatar - virtual float getBoundingRadius() const; - Q_INVOKABLE void setSkeletonOffset(const glm::vec3& offset); Q_INVOKABLE glm::vec3 getSkeletonOffset() { return _skeletonOffset; } virtual glm::vec3 getSkeletonPosition() const; From 1f3a267b16499a37d620c583ea7382db925b3670 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:34:47 -0700 Subject: [PATCH 2/8] compute correct bounding shape dimensions --- interface/src/avatar/SkeletonModel.cpp | 62 +++++++++++++------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index d083116ecd..093c4b1668 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -659,57 +659,57 @@ void SkeletonModel::buildShapes() { void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { // compute default joint transforms int numStates = _jointStates.size(); + assert(numStates == _shapes.size()); QVector transforms; transforms.fill(glm::mat4(), numStates); - // compute the default transforms - for (int i = 0; i < numStates; i++) { - JointState& state = _jointStates[i]; - const FBXJoint& joint = state.getFBXJoint(); - int parentIndex = joint.parentIndex; - if (parentIndex == -1) { - transforms[i] = _jointStates[i].getTransform(); - continue; - } - - glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation; - transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) - * joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform; - // TODO: Andrew to harvest transforms here to move shapes to correct positions so that - // bounding capsule calculations below are correct. - } - // compute bounding box that encloses all shapes Extents totalExtents; totalExtents.reset(); totalExtents.addPoint(glm::vec3(0.0f)); - for (int i = 0; i < _shapes.size(); i++) { + for (int i = 0; i < numStates; i++) { + // compute the default transform of this joint + JointState& state = _jointStates[i]; + const FBXJoint& joint = state.getFBXJoint(); + int parentIndex = joint.parentIndex; + if (parentIndex == -1) { + transforms[i] = _jointStates[i].getTransform(); + } else { + glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation; + transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) + * joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform; + } + Shape* shape = _shapes[i]; if (!shape) { continue; } + + // Each joint with a shape contributes to the totalExtents: a box + // that contains the sphere centered at the end of the joint with radius of the bone. + // TODO: skip hand and arm shapes for bounding box calculation - Extents shapeExtents; - shapeExtents.reset(); - glm::vec3 localPosition = shape->getTranslation(); + glm::vec3 jointPosition = extractTranslation(transforms[i]); + int type = shape->getType(); + float radius = 0.0f; if (type == CAPSULE_SHAPE) { // add the two furthest surface points of the capsule CapsuleShape* capsule = static_cast(shape); - glm::vec3 axis; - capsule->computeNormalizedAxis(axis); float radius = capsule->getRadius(); - float halfHeight = capsule->getHalfHeight(); - axis = halfHeight * axis + glm::vec3(radius); - - shapeExtents.addPoint(localPosition + axis); - shapeExtents.addPoint(localPosition - axis); + glm::vec3 axis(radius); + Extents shapeExtents; + shapeExtents.reset(); + shapeExtents.addPoint(jointPosition + axis); + shapeExtents.addPoint(jointPosition - axis); totalExtents.addExtents(shapeExtents); } else if (type == SPHERE_SHAPE) { float radius = shape->getBoundingRadius(); - glm::vec3 axis = glm::vec3(radius); - shapeExtents.addPoint(localPosition + axis); - shapeExtents.addPoint(localPosition - axis); + glm::vec3 axis(radius); + Extents shapeExtents; + shapeExtents.reset(); + shapeExtents.addPoint(jointPosition + axis); + shapeExtents.addPoint(jointPosition - axis); totalExtents.addExtents(shapeExtents); } } From 6bd857c4ea9ce6309dd91d6cdc3209940b7372fe Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:35:12 -0700 Subject: [PATCH 3/8] update Avatar in PhysicsEngine on load new body --- interface/src/Application.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index a7a6c8c094..9979c54f11 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4091,5 +4091,7 @@ void Application::checkSkeleton() { _myAvatar->setSkeletonModelURL(DEFAULT_BODY_MODEL_URL); _myAvatar->sendIdentityPacket(); + } else { + _physicsEngine.setAvatarData(_myAvatar); } } From d1b977ec9eea25a21dfd3fbc01dd4a766f160d1c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:36:08 -0700 Subject: [PATCH 4/8] remove warnings about unused variables and remind us to impelement unit test for tetrahedron mass props --- tests/physics/src/MeshInfoTests.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/tests/physics/src/MeshInfoTests.cpp b/tests/physics/src/MeshInfoTests.cpp index fa841f3930..221ffa117c 100644 --- a/tests/physics/src/MeshInfoTests.cpp +++ b/tests/physics/src/MeshInfoTests.cpp @@ -104,15 +104,16 @@ void MeshInfoTests::testWithTetrahedronAsMesh(){ glm::vec3 p2(52.61236, 5.00000, -5.38580); glm::vec3 p3(2.00000, 5.00000, 3.00000); glm::vec3 centroid(15.92492, 0.782813, 3.72962); - float volume = 1873.233236f; - float inertia_a = 43520.33257f; - //actual should be 194711.28938f. But for some reason it becomes 194711.296875 during + /* TODO: actually test inertia/volume calculations here + //float volume = 1873.233236f; //runtime due to how floating points are stored. + float inertia_a = 43520.33257f; float inertia_b = 194711.289f; float inertia_c = 191168.76173f; float inertia_aa = 4417.66150f; float inertia_bb = -46343.16662f; float inertia_cc = 11996.20119f; + */ std::cout << std::setprecision(12); vector vertices = { p0, p1, p2, p3 }; vector triangles = { 0, 2, 1, 0, 3, 2, 0, 1, 3, 1, 2, 3 }; @@ -129,6 +130,7 @@ void MeshInfoTests::testWithTetrahedronAsMesh(){ p2 -= centroid; p3 -= centroid; } + void MeshInfoTests::testWithCube(){ glm::vec3 p0(1.0, -1.0, -1.0); glm::vec3 p1(1.0, -1.0, 1.0); @@ -232,4 +234,4 @@ void MeshInfoTests::runAllTests(){ testWithTetrahedronAsMesh(); testWithUnitCube(); testWithCube(); -} \ No newline at end of file +} From a1a12465da8e270961337f4c76bf86083cfc3147 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 12 Mar 2015 16:36:41 -0700 Subject: [PATCH 5/8] able to update avatar controller in PhysicsEngine --- libraries/avatars/src/AvatarData.cpp | 7 +- libraries/avatars/src/AvatarData.h | 5 +- libraries/physics/src/CharacterController.cpp | 706 ++++++++++++++++++ libraries/physics/src/CharacterController.h | 172 +++++ libraries/physics/src/PhysicsEngine.cpp | 106 ++- libraries/physics/src/PhysicsEngine.h | 16 +- 6 files changed, 971 insertions(+), 41 deletions(-) create mode 100644 libraries/physics/src/CharacterController.cpp create mode 100644 libraries/physics/src/CharacterController.h diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 3ab8f38fe6..6d7395754c 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -32,6 +32,9 @@ quint64 DEFAULT_FILTERED_LOG_EXPIRY = 2 * USECS_PER_SECOND; using namespace std; +const glm::vec3 DEFAULT_LOCAL_AABOX_CORNER(-0.5f); +const glm::vec3 DEFAULT_LOCAL_AABOX_SCALE(1.0f); + AvatarData::AvatarData() : _sessionUUID(), _position(0.0f), @@ -55,9 +58,9 @@ AvatarData::AvatarData() : _errorLogExpiry(0), _owningAvatarMixer(), _lastUpdateTimer(), - _velocity(0.0f) + _velocity(0.0f), + _localAABox(DEFAULT_LOCAL_AABOX_CORNER, DEFAULT_LOCAL_AABOX_SCALE) { - } AvatarData::~AvatarData() { diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 02b2c364c6..5490938e30 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -50,6 +50,7 @@ typedef unsigned long long quint64; #include +#include "AABox.h" #include "HandData.h" #include "HeadData.h" #include "Player.h" @@ -296,8 +297,7 @@ public: QElapsedTimer& getLastUpdateTimer() { return _lastUpdateTimer; } - virtual float getBoundingRadius() const { return 1.0f; } - + const AABox& getLocalAABox() const { return _localAABox; } const Referential* getReferential() const { return _referential; } void togglePhysicsEnabled() { _enablePhysics = !_enablePhysics; } @@ -410,6 +410,7 @@ private: QReadWriteLock _lock; bool _enablePhysics = false; + AABox _localAABox; }; Q_DECLARE_METATYPE(AvatarData*) diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp new file mode 100644 index 0000000000..5173b368c1 --- /dev/null +++ b/libraries/physics/src/CharacterController.cpp @@ -0,0 +1,706 @@ +/* +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 + +#include "BulletCollision/CollisionDispatch/btGhostObject.h" + +#include "CharacterController.h" + + +// static helper method +static btVector3 getNormalizedVector(const btVector3& v) { + // NOTE: check the length first, then normalize + // --> avoids assert when trying to normalize zero-length vectors + btScalar vLength = v.length(); + if (vLength < FLT_EPSILON) { + return btVector3(0.0f, 0.0f, 0.0f); + } + btVector3 n = v; + n /= vLength; + 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 + +/* This callback is unused, but we're keeping it around just in case we figure out how to use it. +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 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) +{ + btScalar magnitude = direction.dot(normal); + return normal * magnitude; +} + +/* + * Returns the portion of 'direction' that is perpindicular to '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; + 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; + m_interpolateUp = true; + setMaxSlope(btRadians(45.0)); + m_currentStepOffset = 0; + + // internal state data members + full_drop = false; + bounce_fix = false; +} + +CharacterController::~CharacterController() { +} + +btPairCachingGhostObject* CharacterController::getGhostObject() { + return m_ghostObject; +} + +bool CharacterController::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(collisionPair->m_pProxy0->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + + if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) { + continue; + } + + if (collisionPair->m_algorithm) { + collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); + } + + for (int j=0;jgetBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); + for (int p=0;pgetNumContacts();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;//?? + + } + 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 + 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)); + end.setOrigin(m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -getUpAxisDirections()[m_upAxis], 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); + } + + if (callback.hasHit()) { + // 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; + if (m_interpolateUp == true) { + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } else { + m_currentPosition = m_targetPosition; + } + } + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + } else { + m_currentStepOffset = m_stepHeight; + m_currentPosition = m_targetPosition; + } +} + +void CharacterController::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 (tangentMag != 0.0) { + if (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 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; + + start.setIdentity(); + end.setIdentity(); + + btScalar fraction = 1.0; + btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); + //printf("distance2=%f\n", distance2); + + if (m_touchingContact) { + if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) { + //interferes with step movement + //updateTargetPositionBasedOnCollision(m_touchingNormal); + } + } + + int maxIter = 10; + + while (fraction > btScalar(0.01) && maxIter-- > 0) { + start.setOrigin(m_currentPosition); + end.setOrigin(m_targetPosition); + 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; + + if (callback.hasHit()) { + // 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 { + // we moved whole way + m_currentPosition = m_targetPosition; + } + + //if (callback.m_closestHitFraction == 0.f) { + // break; + //} + + } +} + +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);*/ + + 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; + } + + btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + m_targetPosition -= step_drop; + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + 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); + + //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); + } + } 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); + } + } + + 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); + m_velocityTimeInterval += timeInterval; +} + +void CharacterController::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 CharacterController::warp(const btVector3& origin) { + btTransform xform; + xform.setIdentity(); + xform.setOrigin(origin); + m_ghostObject->setWorldTransform(xform); +} + + +void CharacterController::preStep( btCollisionWorld* collisionWorld) { + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(collisionWorld)) { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; + //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"); + 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(); + + //printf("walkDirection(%f,%f,%f)\n", walkDirection[0], walkDirection[1], walkDirection[2]); + //printf("walkSpeed=%f\n", walkSpeed); + + stepUp (collisionWorld); + 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); + m_ghostObject->setWorldTransform(xform); +} + +void CharacterController::setFallSpeed(btScalar fallSpeed) { + m_fallSpeed = fallSpeed; +} + +void CharacterController::setJumpSpeed(btScalar jumpSpeed) { + m_jumpSpeed = jumpSpeed; +} + +void CharacterController::setMaxJumpHeight(btScalar maxJumpHeight) { + m_maxJumpHeight = maxJumpHeight; +} + +bool CharacterController::canJump() const { + return onGround(); +} + +void CharacterController::jump() { + if (!canJump()) { + return; + } + + m_verticalVelocity = m_jumpSpeed; + m_wasJumping = true; + +#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 CharacterController::setGravity(btScalar gravity) { + m_gravity = gravity; +} + +btScalar CharacterController::getGravity() const { + return m_gravity; +} + +void CharacterController::setMaxSlope(btScalar slopeRadians) { + m_maxSlopeRadians = slopeRadians; + m_maxSlopeCosine = btCos(slopeRadians); +} + +btScalar CharacterController::getMaxSlope() const { + return m_maxSlopeRadians; +} + +bool CharacterController::onGround() const { + return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; +} + +btVector3* CharacterController::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 CharacterController::debugDraw(btIDebugDraw* debugDrawer) { +} + +void CharacterController::setUpInterpolate(bool value) { + m_interpolateUp = value; +} diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h new file mode 100644 index 0000000000..301253b2bd --- /dev/null +++ b/libraries/physics/src/CharacterController.h @@ -0,0 +1,172 @@ +/* +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. +*/ + + +#ifndef hifi_CharacterController_h +#define hifi_CharacterController_h + +#include +#include +#include + + +class btConvexShape; +class btCollisionWorld; +class btCollisionDispatcher; +class btPairCachingGhostObject; + +///CharacterController is a custom version of btKinematicCharacterController + +///btKinematicCharacterController is an object that supports a sliding motion in a world. +///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. +///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. + +ATTRIBUTE_ALIGNED16(class) CharacterController : public btCharacterControllerInterface +{ +protected: + + 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_jumpSpeed; + btScalar m_maxJumpHeight; + btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) + btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) + btScalar m_gravity; + + btScalar m_turnAngle; + + btScalar m_stepHeight; + + btScalar m_addedMargin;//@todo: remove this and fix the code + + ///this is the desired walk direction, set by the user + btVector3 m_walkDirection; + btVector3 m_normalizedDirection; + + //some internal variables + btVector3 m_currentPosition; + btScalar m_currentStepOffset; + btVector3 m_targetPosition; + + ///keep track of the contact manifolds + btManifoldArray m_manifoldArray; + + bool m_touchingContact; + btVector3 m_touchingNormal; + + bool m_wasOnGround; + bool m_wasJumping; + bool m_useGhostObjectSweepTest; + bool m_useWalkDirection; + btScalar m_velocityTimeInterval; + int m_upAxis; + + static btVector3* getUpAxisDirections(); + bool m_interpolateUp; + bool full_drop; + bool bounce_fix; + + btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal); + btVector3 parallelComponent(const btVector3& direction, const btVector3& normal); + btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal); + + 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 stepDown(btCollisionWorld* collisionWorld, btScalar dt); +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + CharacterController( + btPairCachingGhostObject* ghostObject, + btConvexShape* convexShape, + btScalar stepHeight, + int upAxis = 1); + ~CharacterController(); + + + ///btActionInterface interface + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { + preStep(collisionWorld); + playerStep(collisionWorld, deltaTime); + } + + ///btActionInterface interface + void debugDraw(btIDebugDraw* debugDrawer); + + void setUpAxis(int axis) { + if (axis < 0) + axis = 0; + if (axis > 2) + axis = 2; + m_upAxis = axis; + } + + /// This should probably be called setPositionIncrementPerSimulatorStep. + /// This is neither a direction nor a velocity, but the amount to + /// increment the position each simulation iteration, regardless + /// of dt. + /// This call will reset any velocity set by setVelocityForTimeInterval(). + virtual void setWalkDirection(const btVector3& walkDirection); + + /// Caller provides a velocity with which the character should move for + /// the given time period. After the time period, velocity is reset + /// to zero. + /// This call will reset any walk direction set by setWalkDirection(). + /// Negative time intervals will result in no motion. + virtual void setVelocityForTimeInterval(const btVector3& velocity, + btScalar timeInterval); + + void reset(btCollisionWorld* collisionWorld ); + void warp(const btVector3& origin); + + void preStep(btCollisionWorld* collisionWorld); + void playerStep(btCollisionWorld* collisionWorld, btScalar dt); + + void setFallSpeed(btScalar fallSpeed); + void setJumpSpeed(btScalar jumpSpeed); + void setMaxJumpHeight(btScalar maxJumpHeight); + bool canJump() const; + + void jump(); + + void setGravity(btScalar gravity); + btScalar getGravity() const; + + /// The max slope determines the maximum angle that the controller can walk up. + /// The slope angle is measured in radians. + void setMaxSlope(btScalar slopeRadians); + btScalar getMaxSlope() const; + + btPairCachingGhostObject* getGhostObject(); + void setUseGhostSweepTest(bool useGhostObjectSweepTest) { + m_useGhostObjectSweepTest = useGhostObjectSweepTest; + } + + bool onGround() const; + void setUpInterpolate(bool value); +}; + +#endif // hifi_CharacterController_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 634c3707a8..a46095ee0c 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -9,11 +9,12 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include + #include "PhysicsEngine.h" #include "ShapeInfoUtil.h" #include "PhysicsHelpers.h" #include "ThreadSafeDynamicsWorld.h" -#include "AvatarData.h" static uint32_t _numSubsteps; @@ -23,10 +24,12 @@ uint32_t PhysicsEngine::getNumSubsteps() { } PhysicsEngine::PhysicsEngine(const glm::vec3& offset) - : _originOffset(offset) { + : _originOffset(offset), + _avatarShapeLocalOffset(0.0f) { } PhysicsEngine::~PhysicsEngine() { + // TODO: delete engine components... if we ever plan to create more than one instance } // begin EntitySimulation overrides @@ -260,6 +263,9 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) { _constraintSolver = new btSequentialImpulseConstraintSolver; _dynamicsWorld = new ThreadSafeDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig); + _ghostPairCallback = new btGhostPairCallback(); + _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback); + // default gravity of the world is zero, so each object must specify its own gravity // TODO: set up gravity zones _dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, 0.0f)); @@ -271,6 +277,9 @@ 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 @@ -288,17 +297,13 @@ void PhysicsEngine::stepSimulation() { float timeStep = btMin(dt, MAX_TIMESTEP); if (_avatarData->isPhysicsEnabled()) { - _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), - glmToBullet(_avatarData->getPosition()))); - // WORKAROUND: there is a bug in the debug Bullet-2.82 libs where a zero length walk velocity will trigger - // an assert when the getNormalizedVector() helper function in btKinematicCharacterController.cpp tries to - // first normalize a vector before checking its length. Here we workaround the problem by checking the - // length first. NOTE: the character's velocity is reset to zero after each step, so when we DON'T set - // the velocity for this time interval it is the same thing as setting its velocity to zero. + // 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()); - if (walkVelocity.length2() > FLT_EPSILON * FLT_EPSILON) { - _characterController->setVelocityForTimeInterval(walkVelocity, timeStep); - } + _characterController->setVelocityForTimeInterval(walkVelocity, timeStep); } // This is step (2). @@ -323,8 +328,10 @@ void PhysicsEngine::stepSimulation() { if (_avatarData->isPhysicsEnabled()) { const btTransform& avatarTransform = _avatarGhostObject->getWorldTransform(); - _avatarData->setOrientation(bulletToGLM(avatarTransform.getRotation())); - _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin())); + glm::quat rotation = bulletToGLM(avatarTransform.getRotation()); + glm::vec3 offset = rotation * _avatarShapeLocalOffset; + _avatarData->setOrientation(rotation); + _avatarData->setPosition(bulletToGLM(avatarTransform.getOrigin()) - offset); } unlock(); @@ -610,28 +617,69 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio void PhysicsEngine::setAvatarData(AvatarData *avatarData) { - _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; + 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(_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; + } + + // delete old controller and friends + _dynamicsWorld->removeCollisionObject(_avatarGhostObject); + _dynamicsWorld->removeAction(_characterController); + delete _characterController; + _characterController = NULL; + delete _avatarGhostObject; + _avatarGhostObject = NULL; + delete capsule; + } + + // set offset + _avatarShapeLocalOffset = offset; + + // build ghost, shape, and controller _avatarGhostObject = new btPairCachingGhostObject(); _avatarGhostObject->setWorldTransform(btTransform(glmToBullet(_avatarData->getOrientation()), - glmToBullet(_avatarData->getPosition()))); + glmToBullet(_avatarData->getPosition()))); + // ?TODO: use ShapeManager to get avatar's shape? + btCapsuleShape* capsule = new btCapsuleShape(radius, 2.0f * halfHeight); - // XXX these values should be computed from the character model. - btScalar characterRadius = 0.3f; - btScalar characterHeight = 1.75 - 2.0f * characterRadius; - btScalar stepHeight = btScalar(0.35); - - btConvexShape* capsule = new btCapsuleShape(characterRadius, characterHeight); _avatarGhostObject->setCollisionShape(capsule); _avatarGhostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); - _characterController = new btKinematicCharacterController(_avatarGhostObject, capsule, stepHeight); + const float MIN_STEP_HEIGHT = 0.35f; + btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, 0.6f * halfHeight); + _characterController = new CharacterController(_avatarGhostObject, capsule, stepHeight); _dynamicsWorld->addCollisionObject(_avatarGhostObject, btBroadphaseProxy::CharacterFilter, - btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); + btBroadphaseProxy::StaticFilter | btBroadphaseProxy::DefaultFilter); _dynamicsWorld->addAction(_characterController); - _characterController->reset (_dynamicsWorld); - // _characterController->warp (btVector3(10.210001,-2.0306311,16.576973)); - - btGhostPairCallback* ghostPairCallback = new btGhostPairCallback(); - _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(ghostPairCallback); + _characterController->reset(_dynamicsWorld); } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 649aec2755..acf1617b16 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -16,21 +16,19 @@ #include #include -#include #include -#include -#include -#include +//#include +#include #include #include #include "BulletUtil.h" +#include "CharacterController.h" #include "ContactInfo.h" #include "EntityMotionState.h" #include "ShapeManager.h" #include "ThreadSafeDynamicsWorld.h" -#include "AvatarData.h" const float HALF_SIMULATION_EXTENT = 512.0f; // meters @@ -106,6 +104,7 @@ private: btBroadphaseInterface* _broadphaseFilter = NULL; btSequentialImpulseConstraintSolver* _constraintSolver = NULL; ThreadSafeDynamicsWorld* _dynamicsWorld = NULL; + btGhostPairCallback* _ghostPairCallback = NULL; ShapeManager _shapeManager; glm::vec3 _originOffset; @@ -123,9 +122,10 @@ private: uint32_t _lastNumSubstepsAtUpdateInternal = 0; /// character collisions - btCharacterControllerInterface* _characterController = 0; - class btPairCachingGhostObject* _avatarGhostObject = 0; - AvatarData *_avatarData = 0; + CharacterController* _characterController = NULL; + class btPairCachingGhostObject* _avatarGhostObject = NULL; + AvatarData* _avatarData = NULL; + glm::vec3 _avatarShapeLocalOffset; }; #endif // hifi_PhysicsEngine_h From 645fc7dbc9c9594cd4c45c18985446c482c29090 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 13 Mar 2015 09:39:06 -0700 Subject: [PATCH 6/8] remove unused variable --- interface/src/avatar/SkeletonModel.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 5dad5a0a67..4fdebd5f6f 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -692,7 +692,6 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { glm::vec3 jointPosition = extractTranslation(transforms[i]); int type = shape->getType(); - float radius = 0.0f; if (type == CAPSULE_SHAPE) { // add the two furthest surface points of the capsule CapsuleShape* capsule = static_cast(shape); From 924ebe1e54d0ba2a67f9f8062bc1669d037a8b24 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 13 Mar 2015 09:39:47 -0700 Subject: [PATCH 7/8] propagate skeleton's bounding box into AvatarData --- interface/src/Application.cpp | 1 + interface/src/avatar/MyAvatar.cpp | 11 +++++++++++ interface/src/avatar/MyAvatar.h | 1 + libraries/avatars/src/AvatarData.h | 3 ++- 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 437ef3374c..9b2bec079f 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4094,6 +4094,7 @@ void Application::checkSkeleton() { _myAvatar->setSkeletonModelURL(DEFAULT_BODY_MODEL_URL); _myAvatar->sendIdentityPacket(); } else { + _myAvatar->updateLocalAABox(); _physicsEngine.setAvatarData(_myAvatar); } } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index d9c9ff3ad1..bdb0877cda 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -954,6 +954,17 @@ glm::vec3 MyAvatar::getSkeletonPosition() const { return Avatar::getPosition(); } +void MyAvatar::updateLocalAABox() { + const CapsuleShape& capsule = _skeletonModel.getBoundingShape(); + float radius = capsule.getRadius(); + float height = 2.0f * (capsule.getHalfHeight() + radius); + glm::vec3 offset = _skeletonModel.getBoundingShapeOffset(); + glm::vec3 corner(-radius, -0.5f * height, -radius); + corner += offset; + glm::vec3 scale(2.0f * radius, height, 2.0f * radius); + _localAABox.setBox(corner, scale); +} + QString MyAvatar::getScriptedMotorFrame() const { QString frame = "avatar"; if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 7376acfdcb..08c0228f1e 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -121,6 +121,7 @@ public: virtual void setAttachmentData(const QVector& attachmentData); virtual glm::vec3 getSkeletonPosition() const; + void updateLocalAABox(); void clearJointAnimationPriorities(); diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 5490938e30..28123124a0 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -403,6 +403,8 @@ protected: glm::vec3 _velocity; + AABox _localAABox; + private: // privatize the copy constructor and assignment operator so they cannot be called AvatarData(const AvatarData&); @@ -410,7 +412,6 @@ private: QReadWriteLock _lock; bool _enablePhysics = false; - AABox _localAABox; }; Q_DECLARE_METATYPE(AvatarData*) From 99e2d799c525d72bb344178dfa327608be4c225d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 13 Mar 2015 09:40:32 -0700 Subject: [PATCH 8/8] correct math for capsule halfHeight --- libraries/physics/src/PhysicsEngine.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index a46095ee0c..3e2fabfd89 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -623,7 +623,11 @@ void PhysicsEngine::setAvatarData(AvatarData *avatarData) { 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; + 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) { @@ -675,7 +679,7 @@ void PhysicsEngine::setAvatarData(AvatarData *avatarData) { _avatarGhostObject->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT); const float MIN_STEP_HEIGHT = 0.35f; - btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, 0.6f * halfHeight); + btScalar stepHeight = glm::max(MIN_STEP_HEIGHT, radius + 0.5f * halfHeight); _characterController = new CharacterController(_avatarGhostObject, capsule, stepHeight); _dynamicsWorld->addCollisionObject(_avatarGhostObject, btBroadphaseProxy::CharacterFilter,