487 lines
15 KiB
C++
487 lines
15 KiB
C++
|
|
|
|
#include "physic_bullet/bullet_character_controller.h"
|
|
#include "LinearMath/btIDebugDraw.h"
|
|
#include "nstring/nstring.h"
|
|
|
|
using namespace GS;
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::debugDraw(btIDebugDraw *idebug)
|
|
{
|
|
String output;
|
|
|
|
output << "Vertical Velocity: " << mVerticalVelocity << "\n";
|
|
output << "OnGround: " << (mGroundContact ? "Yes" : "No") << "\n";
|
|
output << "Ground.y: " << mGroundNormal.y() << "\n";
|
|
output << "Step high: " << dbg_step_high << "\n";
|
|
output << "Down sweep: " << dbg_down_sweep_hit << "\n";
|
|
|
|
idebug->draw3dText(mCurrentPosition, output.c_str());
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
btVector3 btCustomCharacterController::computeReflectionDirection(const btVector3 & direction, const btVector3 & normal)
|
|
{ return direction - (btScalar(2) * direction.dot(normal)) * normal; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
btCustomCharacterController::btCustomCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, btCollisionWorld * collisionWorld, int upAxis)
|
|
{
|
|
mUpAxis = upAxis;
|
|
mAddedMargin = 0.02;
|
|
mWalkDirection.setValue(0, 0, 0);
|
|
// mUseGhostObjectSweepTest = true;
|
|
mGhostObject = ghostObject;
|
|
mStepHeight = stepHeight;
|
|
mTurnAngle = 0;
|
|
mConvexShape = mStandingConvexShape = convexShape;
|
|
mUseWalkDirection = true;
|
|
mVelocityTimeInterval = 0;
|
|
mVerticalOffset = 0;
|
|
mVerticalVelocity = 0;
|
|
mGravity = 9.8 * 3.0;
|
|
mFallSpeed = 9.8;
|
|
mJumpSpeed = 10;
|
|
// mWasOnGround = false;
|
|
// mWasJumping = false;
|
|
setMaxSlope(btRadians(45));
|
|
mCollisionWorld = collisionWorld;
|
|
// mCanStand = true;
|
|
mCurrentPosition.setValue(0, 0, 0);
|
|
mMass = 20;
|
|
mGroundContact = false;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setDuckingConvexShape(btConvexShape * shape)
|
|
{ mDuckingConvexShape = shape; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setRBForceImpulseBasedOnCollision()
|
|
{
|
|
if (mWalkDirection.isZero())
|
|
return;
|
|
|
|
for (int i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); ++i)
|
|
{
|
|
btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
|
|
|
|
btRigidBody *rb = (btRigidBody*)collisionPair->m_pProxy1->m_clientObject;
|
|
|
|
if (mMass > rb->getInvMass())
|
|
{
|
|
btScalar resultMass = mMass - rb->getInvMass();
|
|
btVector3 reflection = computeReflectionDirection(mWalkDirection * resultMass, getNormalizedVector(mWalkDirection));
|
|
rb->applyCentralImpulse(reflection * -1);
|
|
}
|
|
}
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setVelocityForTimeInterval(const btVector3 & velocity, btScalar timeInterval)
|
|
{
|
|
mUseWalkDirection = false;
|
|
mWalkDirection = velocity;
|
|
mNormalizedDirection = getNormalizedVector(mWalkDirection);
|
|
mVelocityTimeInterval = timeInterval;
|
|
}
|
|
|
|
void btCustomCharacterController::warp(const btVector3 & origin)
|
|
{
|
|
btTransform xform;
|
|
xform.setIdentity();
|
|
xform.setOrigin(origin);
|
|
mGhostObject->setWorldTransform(xform);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool btCustomCharacterController::SweepTest(const btVector3 &src, const btVector3 &dst, btScalar &hitFraction, btVector3 *normal, btVector3 *hit)
|
|
{
|
|
btTransform start, end;
|
|
start.setIdentity(); end.setIdentity();
|
|
start.setOrigin(src); end.setOrigin(dst);
|
|
|
|
ClosestNotMeConvexResultCallback callback(mGhostObject, getUpAxisDirection(), 0);
|
|
callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
|
|
callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask;
|
|
|
|
mGhostObject->convexSweepTest(mConvexShape, start, end, callback, mCollisionWorld->getDispatchInfo().m_allowedCcdPenetration);
|
|
|
|
hitFraction = callback.hasHit() ? callback.m_closestHitFraction : btScalar(1);
|
|
if (normal)
|
|
*normal = callback.m_hitNormalWorld;
|
|
if (hit)
|
|
*hit = callback.m_hitPointWorld;
|
|
|
|
return callback.hasHit();
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool btCustomCharacterController::recoverFromPenetration(const btVector3 &step_direction)
|
|
{
|
|
return false;
|
|
mCollisionWorld->getDispatcher()->dispatchAllCollisionPairs(mGhostObject->getOverlappingPairCache(), mCollisionWorld->getDispatchInfo(), mCollisionWorld->getDispatcher());
|
|
|
|
bool penetration = false;
|
|
for (int i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); ++i)
|
|
{
|
|
btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
|
|
|
|
mManifoldArray.resize(0);
|
|
if (collisionPair->m_algorithm)
|
|
collisionPair->m_algorithm->getAllContactManifolds(mManifoldArray);
|
|
|
|
for (int j = 0; j < mManifoldArray.size(); ++j)
|
|
{
|
|
btPersistentManifold *manifold = mManifoldArray[j];
|
|
btScalar directionSign = manifold->getBody0() == mGhostObject ? btScalar(1) : btScalar(-1);
|
|
|
|
for (int p = 0; p < manifold->getNumContacts(); ++p)
|
|
{
|
|
const btManifoldPoint &pt = manifold->getContactPoint(p);
|
|
|
|
btScalar dist = pt.getDistance();
|
|
btVector3 normal = pt.m_normalWorldOnB * directionSign;
|
|
|
|
if (dist < 0.0)
|
|
{
|
|
penetration = true;
|
|
|
|
if (normal.y() > 0.9)
|
|
normal = btVector3(0, 1, 0); // prevent sliding down slopes
|
|
|
|
mCurrentPosition -= normal * dist * btScalar(0.1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return penetration;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
|
|
#include "log/log.h"
|
|
|
|
enum
|
|
{
|
|
UpSweep,
|
|
ForwardSweep,
|
|
DownSweep
|
|
};
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool btCustomCharacterController::SweepAndSlide(btVector3 &from, btVector3 &to, int sweep)
|
|
{
|
|
bool hit = false;
|
|
for (int it = 0; it < 4; ++it)
|
|
{
|
|
btScalar hit_fraction;
|
|
btVector3 hit_normal;
|
|
if (!SweepTest(from, to, hit_fraction, &hit_normal))
|
|
return hit;
|
|
|
|
switch (sweep)
|
|
{
|
|
case ForwardSweep:
|
|
if (hit_normal.y() < 0.75)
|
|
{
|
|
hit_normal.setY(0.0);
|
|
hit_normal.normalize();
|
|
}
|
|
else
|
|
hit = true;
|
|
break;
|
|
}
|
|
|
|
from += (to - from) * hit_fraction;
|
|
to -= hit_normal * (to - from).dot(hit_normal);
|
|
|
|
if ((to - from).length2() < btScalar(0.0001))
|
|
break;
|
|
}
|
|
return hit;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void btCustomCharacterController::performStep(btScalar dt)
|
|
{
|
|
btScalar hit_fraction;
|
|
btVector3 hit_normal, hit_point;
|
|
|
|
mStepHeight = 0.25;
|
|
|
|
// up sweep
|
|
btVector3 step_height = getUpAxisDirection() * mStepHeight;
|
|
|
|
btVector3 wpos = mGhostObject->getWorldTransform().getOrigin();
|
|
btVector3 tpos = wpos + step_height;
|
|
|
|
SweepTest(wpos, tpos, hit_fraction, &hit_normal);
|
|
tpos = wpos + (tpos - wpos) * hit_fraction;
|
|
|
|
// forward sweep
|
|
bool forward_hit = false;
|
|
|
|
if (mWalkDirection.length2() > 0.0)
|
|
{
|
|
wpos = tpos;
|
|
tpos += mWalkDirection;
|
|
|
|
forward_hit = SweepAndSlide(wpos, tpos, ForwardSweep);
|
|
}
|
|
|
|
// down sweep
|
|
btVector3 g = btVector3(0, -9, 0) * dt;
|
|
|
|
wpos = tpos;
|
|
tpos -= step_height;
|
|
tpos += g;
|
|
|
|
bool down_hit = SweepTest(wpos, tpos, hit_fraction, &hit_normal);
|
|
tpos = wpos + (tpos - wpos) * hit_fraction;
|
|
|
|
// landing on a steep slope higher than we starter, revert height change.
|
|
if (down_hit && (hit_normal.y() < 0.75))
|
|
{
|
|
tpos += hit_normal * 0.2;
|
|
wpos = tpos;
|
|
tpos = wpos - btVector3(0, 4, 0);
|
|
|
|
SweepTest(wpos, tpos, hit_fraction);
|
|
tpos = wpos + (tpos - wpos) * hit_fraction;
|
|
}
|
|
|
|
|
|
|
|
mCurrentPosition = tpos;
|
|
return;
|
|
// }
|
|
/*
|
|
// perform high sweep to step above small obstacle
|
|
btVector3 step_height = getUpAxisDirection() * mStepHeight;
|
|
|
|
wpos = mGhostObject->getWorldTransform().getOrigin() + step_height;
|
|
tpos = wpos + mWalkDirection;
|
|
|
|
if (!SweepAndSlide(wpos, tpos, HighSweep))
|
|
return; // high sweep is not cutting it either... drop its result entirely
|
|
|
|
// step down from the high sweep
|
|
wpos = tpos;
|
|
tpos -= step_height;
|
|
|
|
if (!SweepAndSlide(wpos, tpos, DownSweep))
|
|
{
|
|
mVerticalVelocity = 0;
|
|
return;
|
|
}
|
|
|
|
|
|
// else
|
|
{
|
|
mVerticalVelocity = btClamped(mVerticalVelocity - mGravity * dt, -mFallSpeed, mJumpSpeed);
|
|
|
|
wpos = tpos;
|
|
tpos += btVector3(0, mVerticalVelocity, 0) * dt;
|
|
|
|
SweepAndSlide(wpos, tpos, GravitySweep);
|
|
}
|
|
|
|
// commit
|
|
mCurrentPosition = tpos;
|
|
*/
|
|
}
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::preStep(btCollisionWorld *collisionWorld)
|
|
{}
|
|
void btCustomCharacterController::playerStep(btCollisionWorld * collisionWorld, btScalar dt)
|
|
{
|
|
if (!mUseWalkDirection && mVelocityTimeInterval <= 0)
|
|
return;
|
|
|
|
performStep(dt);
|
|
#if 0
|
|
mCurrentPosition = mGhostObject->getWorldTransform().getOrigin();
|
|
|
|
// Apply gravity.
|
|
mVerticalVelocity = btClamped(mVerticalVelocity - mGravity * dt, -mFallSpeed, mJumpSpeed);
|
|
|
|
// Compute total step velocity.
|
|
mTouchingContact = false;
|
|
for (int n = 0; (n < 4) && recoverFromPenetration(mWalkDirection + btVector3(0, -1, 0) * mVerticalVelocity); ++n)
|
|
mTouchingContact = true;
|
|
|
|
// Perform sweep tests.
|
|
btVector3 stepHeight = getUpAxisDirection() * mStepHeight;
|
|
btVector3 stepHigh = mCurrentPosition + stepHeight;
|
|
|
|
btScalar hitFraction;
|
|
|
|
mGroundContact = false;
|
|
if (SweepTest(stepHigh, stepHigh + mWalkDirection, hitFraction, &mGroundNormal))
|
|
{
|
|
btVector3 hit = stepHigh + mWalkDirection * hitFraction;
|
|
if (mGroundNormal.y() > 0.6) // on ground, take step
|
|
mCurrentPosition = hit;
|
|
}
|
|
else
|
|
{
|
|
stepHigh += mWalkDirection; // take the whole walk step
|
|
|
|
btVector3 g = stepHeight - mVerticalVelocity * getUpAxisDirection();
|
|
|
|
bool down_sweep_hit = SweepTest(stepHigh, stepHigh - g, hitFraction, &mGroundNormal);
|
|
|
|
if (!down_sweep_hit)
|
|
{
|
|
mCurrentPosition += mVerticalVelocity * getUpAxisDirection(); // free-falling
|
|
}
|
|
else
|
|
{
|
|
mGroundContact = mGroundNormal.y() > 0.6;
|
|
|
|
btVector3 dp = stepHigh - g * hitFraction;
|
|
|
|
if (dp.y() > mCurrentPosition.y()) // going up a slope
|
|
{
|
|
if (mGroundContact)
|
|
mCurrentPosition = dp; // ok if on ground
|
|
}
|
|
else
|
|
mCurrentPosition = dp;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
//
|
|
if (mGroundContact)
|
|
mVerticalVelocity = 0.0;
|
|
|
|
//
|
|
btTransform xform = mGhostObject->getWorldTransform();
|
|
xform.setOrigin(mCurrentPosition);
|
|
mGhostObject->setWorldTransform(xform);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setFallSpeed(btScalar fallSpeed)
|
|
{ mFallSpeed = fallSpeed; }
|
|
void btCustomCharacterController::setJumpSpeed(btScalar jumpSpeed)
|
|
{ mJumpSpeed = jumpSpeed; }
|
|
void btCustomCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
|
|
{ mMaxJumpHeight = maxJumpHeight; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool btCustomCharacterController::canJump() const
|
|
{ return onGround(); }
|
|
void btCustomCharacterController::jump()
|
|
{
|
|
if (!canJump())
|
|
return;
|
|
|
|
mVerticalVelocity = mJumpSpeed;
|
|
// mWasJumping = true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::duck()
|
|
{
|
|
mConvexShape = mDuckingConvexShape;
|
|
mGhostObject->setCollisionShape(mDuckingConvexShape);
|
|
|
|
btTransform xform;
|
|
xform.setIdentity();
|
|
xform.setOrigin(mCurrentPosition + btVector3(0, 0.1, 0));
|
|
mGhostObject->setWorldTransform(xform);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::stand()
|
|
{
|
|
mConvexShape = mStandingConvexShape;
|
|
mGhostObject->setCollisionShape(mStandingConvexShape);
|
|
}
|
|
bool btCustomCharacterController::canStand()
|
|
{
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setGravity(const btScalar gravity)
|
|
{ mGravity = gravity; }
|
|
btScalar btCustomCharacterController::getGravity() const
|
|
{ return mGravity; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setMaxSlope(btScalar slopeRadians)
|
|
{
|
|
mMaxSlopeRadians = slopeRadians;
|
|
mMaxSlopeCosine = btCos(slopeRadians);
|
|
}
|
|
btScalar btCustomCharacterController::getMaxSlope() const
|
|
{ return mMaxSlopeRadians; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool btCustomCharacterController::onGround() const
|
|
{ return mGroundContact; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setWalkDirection(const btVector3 & walkDirection)
|
|
{
|
|
mUseWalkDirection = true;
|
|
mWalkDirection = walkDirection;
|
|
mNormalizedDirection = getNormalizedVector(mWalkDirection);
|
|
}
|
|
void btCustomCharacterController::setWalkDirection(const btScalar x, const btScalar y, const btScalar z)
|
|
{
|
|
mUseWalkDirection = true;
|
|
mWalkDirection.setValue(x, y, z);
|
|
mNormalizedDirection = getNormalizedVector(mWalkDirection);
|
|
}
|
|
btVector3 btCustomCharacterController::getWalkDirection() const
|
|
{ return mWalkDirection; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
btVector3 btCustomCharacterController::getPosition() const
|
|
{ return mCurrentPosition; }
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::setOrientation(const btQuaternion &orientation)
|
|
{
|
|
btTransform xform;
|
|
xform = mGhostObject->getWorldTransform();
|
|
xform.setRotation(orientation);
|
|
mGhostObject->setWorldTransform(xform);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
//------------------------------------------------------------------------------
|
|
void btCustomCharacterController::updateAction(btCollisionWorld *collisionWorld, btScalar dt)
|
|
{
|
|
preStep(collisionWorld);
|
|
playerStep(collisionWorld, dt);
|
|
}
|
|
//------------------------------------------------------------------------------
|