diff --git a/Components/Bullet/include/OgreBullet.h b/Components/Bullet/include/OgreBullet.h index 55b2686dd8a..748f24d10e2 100644 --- a/Components/Bullet/include/OgreBullet.h +++ b/Components/Bullet/include/OgreBullet.h @@ -9,10 +9,10 @@ #include "OgreBulletExports.h" #include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "Ogre.h" #include "btBulletCollisionCommon.h" #include "btBulletDynamicsCommon.h" -#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" namespace Ogre { @@ -22,8 +22,8 @@ namespace Bullet { /** \addtogroup Optional -* @{ -*/ + * @{ + */ /** \defgroup Bullet Bullet * [Bullet-Physics](https://pybullet.org) to %Ogre connection * @{ @@ -71,13 +71,14 @@ class _OgreBulletExport RigidBodyState : public btMotionState }; /// height field data -struct _OgreBulletExport HeightFieldData { +struct _OgreBulletExport HeightFieldData +{ /** the position for a center of the shape, i.e. where to place btRigidBody * or a child of btCompoundShape */ Vector3 bodyPosition; /** a heightfield pointer to be freed when * btHeightfieldTerrainShape is freed */ - float *terrainHeights; + float* terrainHeights; }; /// create sphere collider using ogre provided data @@ -95,15 +96,16 @@ _OgreBulletExport btConvexHullShape* createConvexHullCollider(const Entity* ent) /// create compound shape _OgreBulletExport btCompoundShape* createCompoundShape(); /// create height field collider -_OgreBulletExport btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, struct HeightFieldData *data); +_OgreBulletExport btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, + struct HeightFieldData* data); struct _OgreBulletExport CollisionListener { virtual ~CollisionListener() {} /** Called when two objects collide - * @param other the other object - * @param manifoldPoint the collision point - */ + * @param other the other object + * @param manifoldPoint the collision point + */ virtual void contact(const MovableObject* other, const btManifoldPoint& manifoldPoint) = 0; }; @@ -111,9 +113,9 @@ struct _OgreBulletExport RayResultCallback { virtual ~RayResultCallback() {} /** Called for each object hit by the ray - * @param other the other object - * @param distance the distance from ray origin to hit point - */ + * @param other the other object + * @param distance the distance from ray origin to hit point + */ virtual void addSingleResult(const MovableObject* other, float distance) = 0; }; @@ -135,7 +137,7 @@ class _OgreBulletExport CollisionWorld btCollisionObject* addCollisionObject(Entity* ent, ColliderType ct, int group = 1, int mask = -1); void rayTest(const Ray& ray, RayResultCallback* callback, float maxDist = 1000); - void attachCollisionObject(btCollisionObject *collisionObject, Entity *ent, int group = 1, int mask = -1); + void attachCollisionObject(btCollisionObject* collisionObject, Entity* ent, int group = 1, int mask = -1); }; /// helper class for kinematic body motion @@ -145,6 +147,7 @@ class _OgreBulletExport KinematicMotionSimple : public btActionInterface std::vector mCollisionTransforms; btPairCachingGhostObject* mGhostObject; btVector3 mCurrentPosition; + btVector3 mPreviousPosition; btQuaternion mCurrentOrientation; btManifoldArray mManifoldArray; btScalar mMaxPenetrationDepth; @@ -153,10 +156,25 @@ class _OgreBulletExport KinematicMotionSimple : public btActionInterface bool mIsPenetrating; int mManifolds; bool mAllowManualNarrowPhase; + bool mVelocityMotion; + float mStepHeight; + float mCurrentStepOffset; + float mVerticalOffset; + btVector3 mUp; virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); void preStep(btCollisionWorld* collisionWorld); void playerStep(btCollisionWorld* collisionWorld, btScalar dt); void setupCollisionShapes(btCollisionObject* body); + btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal); + btVector3 parallelComponent(const btVector3& direction, const btVector3& normal); + btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal); + void sweepTest(btCollisionWorld* world, const btTransform& start, const btTransform& end, + btCollisionWorld::ClosestConvexResultCallback& callback, float margin = 0.0f); + void updateTargetPositionBasedOnCollision(bool current, btVector3& targetPosition, const btVector3& hitNormal, + btScalar tangentMag = 1.0f, btScalar normalMag = 1.0f); + void stepUp(btCollisionWorld* world, btScalar& verticalVelocity, bool interpolateUp); + void stepForwardAndStrafe(bool current, btCollisionWorld* world, const btVector3& motion, float margin = 0.0f, + int iterations = 10); public: KinematicMotionSimple(btPairCachingGhostObject* ghostObject, Node* node); @@ -169,11 +187,9 @@ class _OgreBulletExport KinematicMotionSimple : public btActionInterface int getManifolds() const { return mManifolds; } /** Enable manual narrow phase * @param enable if enabled - */ - void enableManualNarrowPhase(bool enable) - { - mAllowManualNarrowPhase = enable; - } + */ + void enableManualNarrowPhase(bool enable) { mAllowManualNarrowPhase = enable; } + /** Report manual narrow phase enabled status */ bool isManualNarrowPhaseEnabled() const { return mAllowManualNarrowPhase; } }; @@ -187,13 +203,13 @@ class _OgreBulletExport DynamicsWorld : public CollisionWorld DynamicsWorld(btDynamicsWorld* btWorld) : CollisionWorld(btWorld) {} /** Add an Entity as a rigid body to the DynamicsWorld - * @param mass the mass of the object - * @param ent the entity to control - * @param ct the collider type - * @param listener a listener to call on collision with other objects - * @param group the collision group - * @param mask the collision mask - */ + * @param mass the mass of the object + * @param ent the entity to control + * @param ct the collider type + * @param listener a listener to call on collision with other objects + * @param group the collision group + * @param mask the collision mask + */ btRigidBody* addRigidBody(float mass, Entity* ent, ColliderType ct, CollisionListener* listener = nullptr, int group = 1, int mask = -1); btRigidBody* addKinematicRigidBody(Entity* ent, ColliderType ct, int group = 1, int mask = -1); @@ -206,7 +222,8 @@ class _OgreBulletExport DynamicsWorld : public CollisionWorld * @param mask the collision mask * @param debugDraw allow debug drawing */ - btRigidBody* addTerrainRigidBody(TerrainGroup* terrainGroup, long x, long y, int group = 1, int mask = -1, bool debugDraw = false); + btRigidBody* addTerrainRigidBody(TerrainGroup* terrainGroup, long x, long y, int group = 1, int mask = -1, + bool debugDraw = false); /** Add static body for Ogre terrain * @param terrain the terrain * @param group the collision group @@ -215,8 +232,8 @@ class _OgreBulletExport DynamicsWorld : public CollisionWorld */ btRigidBody* addTerrainRigidBody(Terrain* terrain, int group = 1, int mask = -1, bool debugDraw = false); - void attachRigidBody(btRigidBody *rigidBody, Entity *ent, CollisionListener* listener = nullptr, - int group = 1, int mask = -1); + void attachRigidBody(btRigidBody* rigidBody, Entity* ent, CollisionListener* listener = nullptr, int group = 1, + int mask = -1); btDynamicsWorld* getBtWorld() const { return static_cast(mBtWorld); } }; @@ -252,7 +269,10 @@ class _OgreBulletExport DebugDrawer : public btIDebugDraw drawLine(PointOnB, PointOnB + normalOnB * distance * 20, color); } - void reportErrorWarning(const char* warningString) override { LogManager::getSingleton().logWarning(warningString); } + void reportErrorWarning(const char* warningString) override + { + LogManager::getSingleton().logWarning(warningString); + } void draw3dText(const btVector3& location, const char* textString) override {} diff --git a/Components/Bullet/src/OgreBullet.cpp b/Components/Bullet/src/OgreBullet.cpp index fa46f1d4892..3b9fd386009 100644 --- a/Components/Bullet/src/OgreBullet.cpp +++ b/Components/Bullet/src/OgreBullet.cpp @@ -96,26 +96,22 @@ btCylinderShape* createCylinderCollider(const MovableObject* mo) } /// create compound shape because we can -btCompoundShape* createCompoundShape() -{ - return new btCompoundShape; -} +btCompoundShape* createCompoundShape() { return new btCompoundShape; } /// create height field collider -btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, struct HeightFieldData *data) +btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, struct HeightFieldData* data) { #ifdef OGRE_BUILD_COMPONENT_TERRAIN OgreAssert(terrain && terrain->getHeightData() && terrain->isLoaded(), "invalid terrain supplied"); int i; uint16 size = terrain->getSize(); - float *heightData = terrain->getHeightData(); + float* heightData = terrain->getHeightData(); /* need to flip terrain data */ data->terrainHeights = new float[size * size]; - for (i = 0; i < size; i++) { - memcpy(data->terrainHeights + size * i, - heightData + size * (size - i - 1), - sizeof(float) * size); + for (i = 0; i < size; i++) + { + memcpy(data->terrainHeights + size * i, heightData + size * (size - i - 1), sizeof(float) * size); } Real minHeight = terrain->getMinHeight(); Real maxHeight = terrain->getMaxHeight(); @@ -123,9 +119,8 @@ btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, Real worldSize = terrain->getWorldSize(); float scaled = worldSize / (size - 1); btVector3 localScale(scaled, 1.0f, scaled); - btHeightfieldTerrainShape *shape = new btHeightfieldTerrainShape((int)size, (int)size, - data->terrainHeights, 1, minHeight, maxHeight, - 1, PHY_FLOAT, true); + btHeightfieldTerrainShape* shape = new btHeightfieldTerrainShape((int)size, (int)size, data->terrainHeights, 1, + minHeight, maxHeight, 1, PHY_FLOAT, true); data->bodyPosition.y += (maxHeight + minHeight) / 2.0f; shape->setUseDiamondSubdivision(true); shape->setLocalScaling(localScale); @@ -136,7 +131,6 @@ btHeightfieldTerrainShape* createHeightfieldTerrainShape(const Terrain* terrain, #endif } - struct EntityCollisionListener { const MovableObject* entity; @@ -209,16 +203,10 @@ class VertexIndexToShape }; /// create trimesh collider using ogre provided data -btBvhTriangleMeshShape* createTrimeshCollider(const Entity* ent) -{ - return VertexIndexToShape(ent).createTrimesh(); -} +btBvhTriangleMeshShape* createTrimeshCollider(const Entity* ent) { return VertexIndexToShape(ent).createTrimesh(); } /// create convex hull collider using ogre provided data -btConvexHullShape* createConvexHullCollider(const Entity* ent) -{ - return VertexIndexToShape(ent).createConvex(); -} +btConvexHullShape* createConvexHullCollider(const Entity* ent) { return VertexIndexToShape(ent).createConvex(); } /// wrapper with automatic memory management class CollisionObject @@ -248,7 +236,8 @@ class RigidBody : public CollisionObject } }; -DynamicsWorld::DynamicsWorld(const Vector3& gravity) : CollisionWorld(NULL) // prevent CollisionWorld from creating a world +DynamicsWorld::DynamicsWorld(const Vector3& gravity) + : CollisionWorld(NULL) // prevent CollisionWorld from creating a world { // Bullet initialisation. mCollisionConfig.reset(new btDefaultCollisionConfiguration()); @@ -256,7 +245,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity) : CollisionWorld(NULL) // p mSolver.reset(new btSequentialImpulseConstraintSolver()); mBroadphase.reset(new btDbvtBroadphase()); - auto btworld = new btDiscreteDynamicsWorld(mDispatcher.get(), mBroadphase.get(), mSolver.get(), mCollisionConfig.get()); + auto btworld = + new btDiscreteDynamicsWorld(mDispatcher.get(), mBroadphase.get(), mSolver.get(), mCollisionConfig.get()); btworld->setGravity(convert(gravity)); btworld->setInternalTickCallback(onTick); mGhostPairCallback = new btGhostPairCallback(); @@ -332,26 +322,25 @@ btRigidBody* DynamicsWorld::addRigidBody(float mass, Entity* ent, ColliderType c btRigidBody* DynamicsWorld::addKinematicRigidBody(Entity* ent, ColliderType ct, int group, int mask) { btRigidBody* rb = addRigidBody(0, ent, ct, nullptr, group, mask); - rb->setCollisionFlags(rb->getCollisionFlags() - | btCollisionObject::CF_KINEMATIC_OBJECT - | btCollisionObject::CF_NO_CONTACT_RESPONSE - ); + rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT | + btCollisionObject::CF_NO_CONTACT_RESPONSE); rb->setActivationState(DISABLE_DEACTIVATION); return rb; } -class TerrainRigidBody: public RigidBody +class TerrainRigidBody : public RigidBody { HeightFieldData hfdata; + public: - TerrainRigidBody(btCollisionObject* btBody, btCollisionWorld* btWorld, const HeightFieldData &data) - : RigidBody(btBody, btWorld), hfdata(data) {} - ~TerrainRigidBody() + TerrainRigidBody(btCollisionObject* btBody, btCollisionWorld* btWorld, const HeightFieldData& data) + : RigidBody(btBody, btWorld), hfdata(data) { - delete hfdata.terrainHeights; } + ~TerrainRigidBody() { delete hfdata.terrainHeights; } }; -btRigidBody* DynamicsWorld::addTerrainRigidBody(TerrainGroup* terrainGroup, long x, long y, int group, int mask, bool debugDraw) +btRigidBody* DynamicsWorld::addTerrainRigidBody(TerrainGroup* terrainGroup, long x, long y, int group, int mask, + bool debugDraw) { Terrain* terrain = nullptr; #ifdef OGRE_BUILD_COMPONENT_TERRAIN @@ -407,17 +396,18 @@ btCollisionObject* CollisionWorld::addCollisionObject(Entity* ent, ColliderType return co; } -void DynamicsWorld::attachRigidBody(btRigidBody *rigidBody, Entity* ent, CollisionListener* listener, - int group, int mask) +void DynamicsWorld::attachRigidBody(btRigidBody* rigidBody, Entity* ent, CollisionListener* listener, int group, + int mask) { auto node = ent->getParentSceneNode(); OgreAssert(node, "entity must be attached"); /* If the body has incorrect btMotionState and is in world * we will crash or corrupt some memory. Hope the user * will know what he/she is doing */ - if (!rigidBody->isInWorld()) { + if (!rigidBody->isInWorld()) + { RigidBodyState* state = new RigidBodyState(node); - rigidBody->setMotionState(state); + rigidBody->setMotionState(state); getBtWorld()->addRigidBody(rigidBody, group, mask); } rigidBody->setUserPointer(new EntityCollisionListener{ent, listener}); @@ -752,7 +742,7 @@ VertexIndexToShape::~VertexIndexToShape() if (mBoneIndex) { - for (auto & i : *mBoneIndex) + for (auto& i : *mBoneIndex) { delete i.second; } @@ -965,8 +955,6 @@ bool KinematicMotionSimple::recoverFromPenetration(btCollisionWorld* collisionWo dispatch->dispatchAllCollisionPairs(mGhostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), dispatch); - mCurrentPosition = mGhostObject->getWorldTransform().getOrigin(); - mIsOnFloor = false; mManifolds = 0; @@ -1070,6 +1058,15 @@ void KinematicMotionSimple::preStep(btCollisionWorld* collisionWorld) void KinematicMotionSimple::playerStep(btCollisionWorld* collisionWorld, btScalar dt) { int numPenetrationLoops = 0; + if (!mVelocityMotion) + { + btVector3 currentPosition = convert(mNode->getPosition()); + btVector3 motion = currentPosition - mPreviousPosition; + float verticalVelocity = motion.y() * dt; + stepUp(collisionWorld, verticalVelocity, true); + motion.setY(0); + stepForwardAndStrafe(false, collisionWorld, motion); + } while (recoverFromPenetration(collisionWorld)) { numPenetrationLoops++; @@ -1085,6 +1082,7 @@ void KinematicMotionSimple::updateAction(btCollisionWorld* collisionWorld, btSca btTransform xform = mGhostObject->getWorldTransform(); mNode->setPosition(convert(xform.getOrigin())); mNode->setOrientation(convert(xform.getRotation())); + mPreviousPosition = xform.getOrigin(); } void KinematicMotionSimple::debugDraw(btIDebugDraw* debugDrawer) {} void KinematicMotionSimple::setupCollisionShapes(btCollisionObject* body) @@ -1136,16 +1134,300 @@ void KinematicMotionSimple::setupCollisionShapes(btCollisionObject* body) } OgreAssert(mCollisionShapes.size() > 0, "No collision shapes"); } +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)), mMe(me), + mUp(up), mMinSlopeDot(minSlopeDot) + { + } + + btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) override + { + if (convexResult.m_hitCollisionObject == mMe) + 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 = mUp.dot(hitNormalWorld); + if (dotUp < mMinSlopeDot) + { + return btScalar(1.0); + } + + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); + } + +protected: + btCollisionObject* mMe; + const btVector3 mUp; + btScalar mMinSlopeDot; +}; + +void KinematicMotionSimple::sweepTest(btCollisionWorld* world, const btTransform& start, const btTransform& end, + btCollisionWorld::ClosestConvexResultCallback& callback, float margin) +{ + int i; + + bool updateMargin = false; + float originalMargin; + if (margin > 0.0f) + updateMargin = true; + /* loop over shapes */ + btCollisionShape* shape = mGhostObject->getCollisionShape(); + if (shape->isCompound()) + { + btCompoundShape* compoundShape = static_cast(shape); + for (i = 0; i < compoundShape->getNumChildShapes(); i++) + { + btCollisionShape* childShape = compoundShape->getChildShape(i); + if (childShape->isConvex()) + { + btConvexShape* convexShape = static_cast(childShape); + if (updateMargin) + { + originalMargin = convexShape->getMargin(); + convexShape->setMargin(margin); + } + const btTransform& shapeXform = compoundShape->getChildTransform(i); + mGhostObject->convexSweepTest(convexShape, start * shapeXform, end * shapeXform, callback, + world->getDispatchInfo().m_allowedCcdPenetration); + if (updateMargin) + convexShape->setMargin(originalMargin); + } + } + } +} +btVector3 KinematicMotionSimple::computeReflectionDirection(const btVector3& direction, const btVector3& normal) +{ + return direction - (btScalar(2.0) * direction.dot(normal)) * normal; +} +btVector3 KinematicMotionSimple::parallelComponent(const btVector3& direction, const btVector3& normal) +{ + btScalar magnitude = direction.dot(normal); + return normal * magnitude; +} +btVector3 KinematicMotionSimple::perpindicularComponent(const btVector3& direction, const btVector3& normal) +{ + return direction - parallelComponent(direction, normal); +} +void KinematicMotionSimple::updateTargetPositionBasedOnCollision(bool current, btVector3& targetPosition, + const btVector3& hitNormal, btScalar tangentMag, + btScalar normalMag) +{ + btVector3 currentPosition = (current ? mCurrentPosition : mPreviousPosition); + btVector3 movementDirection = targetPosition - 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); + + targetPosition = currentPosition; + if (0) + { + btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength); + targetPosition += parComponent; + } + + if (normalMag != 0.0) + { + btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength); + targetPosition += perpComponent; + } + } + else + { + } +} + +void KinematicMotionSimple::stepUp(btCollisionWorld* world, btScalar& verticalVelocity, bool interpolateUp) +{ + btScalar stepHeight = 0.0f; + if (verticalVelocity < 0.0) + stepHeight = mStepHeight; + + // phase 1: up + btTransform start, end; + + start.setIdentity(); + end.setIdentity(); + + /* FIXME: Handle penetration properly */ + start.setOrigin(mCurrentPosition); + + btVector3 targetPosition = mCurrentPosition + mUp * stepHeight; + mCurrentPosition = targetPosition; + + end.setOrigin(targetPosition); + + start.setRotation(mCurrentOrientation); + end.setRotation(mCurrentOrientation); + + btKinematicClosestNotMeConvexResultCallback callback(mGhostObject, -mUp, + Ogre::Math::Cos(Ogre::Math::DegreesToRadians(30))); + callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask; + + sweepTest(world, start, end, callback); + +#if 0 + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, + world->getDispatchInfo().m_allowedCcdPenetration); + } + else + { + world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); + } +#endif + + if (callback.hasHit() && mGhostObject->hasContactResponse() && + needsCollision(mGhostObject, callback.m_hitCollisionObject)) + { + // Only modify the position if the hit was a slope and not a wall or ceiling. + if (callback.m_hitNormalWorld.dot(mUp) > 0.0) + { + // we moved up only a fraction of the step height + mCurrentStepOffset = stepHeight * callback.m_closestHitFraction; + if (interpolateUp == true) + mCurrentPosition.setInterpolate3(mCurrentPosition, targetPosition, callback.m_closestHitFraction); + else + mCurrentPosition = targetPosition; + } + + btTransform& xform = mGhostObject->getWorldTransform(); + xform.setOrigin(mCurrentPosition); + mGhostObject->setWorldTransform(xform); + + // fix penetration if we hit a ceiling for example + int numPenetrationLoops = 0; + /* TODO: bool touchingContact = false; */ + while (recoverFromPenetration(world)) + { + numPenetrationLoops++; + /* TODO: touchingContact = true; */ + if (numPenetrationLoops > 4) + { + // printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + targetPosition = mGhostObject->getWorldTransform().getOrigin(); + mCurrentPosition = targetPosition; + + if (mVerticalOffset > 0) + { + mVerticalOffset = 0.0; + verticalVelocity = 0.0; + mCurrentStepOffset = mStepHeight; + } + } + else + { + mCurrentStepOffset = stepHeight; + mCurrentPosition = targetPosition; + } +} + +void KinematicMotionSimple::stepForwardAndStrafe(bool current, btCollisionWorld* world, const btVector3& motion, + float margin, int iterations) +{ + + btTransform start, end; + + btVector3 currentPosition = (current ? mCurrentPosition : mPreviousPosition); + + btVector3 targetPosition = currentPosition + motion; + btQuaternion currentOrientation = mGhostObject->getWorldTransform().getRotation(); + btQuaternion targetOrientation = currentOrientation; + + start.setIdentity(); + end.setIdentity(); + + btScalar fraction = 1.0; + btScalar distance2 = (currentPosition - targetPosition).length2(); + + int maxIter = iterations; + + while (fraction > btScalar(0.01) && maxIter-- > 0) + { + start.setOrigin(currentPosition); + end.setOrigin(targetPosition); + btVector3 sweepDirNegative(currentPosition - targetPosition); + + start.setRotation(currentOrientation); + end.setRotation(targetOrientation); + + btKinematicClosestNotMeConvexResultCallback callback(mGhostObject, sweepDirNegative, btScalar(0.0)); + callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask; + + if (!(start == end)) + sweepTest(world, start, end, callback, margin); + + fraction -= callback.m_closestHitFraction; + + if (callback.hasHit() && needsCollision(mGhostObject, callback.m_hitCollisionObject)) + { + updateTargetPositionBasedOnCollision(current, targetPosition, callback.m_hitNormalWorld); + btVector3 currentDir = targetPosition - currentPosition; + distance2 = currentDir.length2(); + if (distance2 > SIMD_EPSILON) + { + currentDir.normalize(); + btVector3 normalizedDirection = motion.normalized(); + if (currentDir.dot(normalizedDirection) <= btScalar(0.0)) + { + break; + } + } + else + { + break; + } + } + else + { + currentPosition = targetPosition; + } + } + mCurrentPosition = currentPosition; +} KinematicMotionSimple::KinematicMotionSimple(btPairCachingGhostObject* ghostObject, Node* node) - : btActionInterface(), mGhostObject(ghostObject), mMaxPenetrationDepth(0.0f), - mNode(node), mIsOnFloor(false), mIsPenetrating(false), mManifolds(0), - mAllowManualNarrowPhase(false) + : btActionInterface(), mGhostObject(ghostObject), mMaxPenetrationDepth(0.0f), mNode(node), mIsOnFloor(false), + mIsPenetrating(false), mManifolds(0), mAllowManualNarrowPhase(false), mVelocityMotion(false), mStepHeight(0.0f), + mCurrentStepOffset(0.0f), mVerticalOffset(0.0f), mUp(0.0f, 1.0f, 0.0f) { btTransform nodeXform; + nodeXform.setIdentity(); nodeXform.setRotation(convert(node->getOrientation())); nodeXform.setOrigin(convert(node->getPosition())); ghostObject->setWorldTransform(nodeXform); setupCollisionShapes(ghostObject); + mPreviousPosition = convert(node->getPosition()); } KinematicMotionSimple::~KinematicMotionSimple() {}