btVector3 btKinematicCharacterController::stepDown( btCollisionWorld* collisionWorld, const btVector3& currentPosition, btScalar currentStepOffset )
{
btVector3 stepDrop = getUpAxisDirections()[ m_upAxis ] * currentStepOffset;
// Be sure we are falling from the last m_currentPosition
// It prevents some flickering
//
btVector3 targetPosition = currentPosition - stepDrop;
//if the no collisions mode is on, no need to go any further
if(!mCollision) return targetPosition;
btTransform start;
start.setIdentity();
start.setOrigin( currentPosition );
btTransform end;
end.setIdentity();
end.setOrigin( targetPosition );
btKinematicClosestNotMeConvexResultCallback callback( internalGhostObject, getUpAxisDirections()[ m_upAxis ], m_maxSlopeCosine );
callback.m_collisionFilterGroup = internalGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = internalGhostObject->getBroadphaseHandle()->m_collisionFilterMask;
// Retrieve the collision shape
//
btCollisionShape* collisionShape = internalGhostObject->getCollisionShape();
btAssert( collisionShape->isConvex() );
btConvexShape* convexShape = ( btConvexShape* )collisionShape;
if( m_useGhostObjectSweepTest )
externalGhostObject->convexSweepTest( convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );
else
collisionWorld->convexSweepTest( convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );
if( callback.hasHit() )
{
m_verticalVelocity = btScalar( 0.0 );
m_verticalOffset = btScalar( 0.0 );
m_wasJumping = false;
// We dropped a fraction of the height -> hit floor
//
return currentPosition.lerp( targetPosition, callback.m_closestHitFraction );
}
else
// We dropped the full height
//
return targetPosition;
}
/* PMDModel::smearAllBonesToDefault: smear all bone pos/rot into default value (rate 1.0 = keep, rate 0.0 = reset) */
void PMDModel::smearAllBonesToDefault(float rate)
{
unsigned short i;
const btVector3 v(0.0f, 0.0f, 0.0f);
const btQuaternion q(0.0f, 0.0f, 0.0f, 1.0f);
btVector3 tmpv;
btQuaternion tmpq;
for (i = 0; i < m_numBone; i++) {
m_boneList[i].getCurrentPosition(&tmpv);
tmpv = v.lerp(tmpv, rate);
m_boneList[i].setCurrentPosition(&tmpv);
m_boneList[i].getCurrentRotation(&tmpq);
tmpq = q.slerp(tmpq, rate);
m_boneList[i].setCurrentRotation(&tmpq);
}
for (i = 0; i < m_numFace; i++) {
m_faceList[i].setWeight(m_faceList[i].getWeight() * rate);
}
}
btVector3 btKinematicCharacterController::stepUp( btCollisionWorld* world, const btVector3& currentPosition, btScalar& currentStepOffset )
{
btVector3 targetPosition = currentPosition + getUpAxisDirections()[ m_upAxis ] * ( m_stepHeight + ( m_verticalOffset > btScalar( 0.0 ) ? m_verticalOffset : 0.0 ) );
//if the no collisions mode is on, no need to go any further
if(!mCollision)
{
currentStepOffset = m_stepHeight;
return targetPosition;
}
// Retrieve the collision shape
//
btCollisionShape* collisionShape = externalGhostObject->getCollisionShape();
btAssert( collisionShape->isConvex() );
btConvexShape* convexShape = ( btConvexShape* )collisionShape;
// FIXME: Handle penetration properly
//
btTransform start;
start.setIdentity();
start.setOrigin( currentPosition + getUpAxisDirections()[ m_upAxis ] * ( convexShape->getMargin() ) );
btTransform end;
end.setIdentity();
end.setOrigin( targetPosition );
btKinematicClosestNotMeConvexResultCallback callback( externalGhostObject, -getUpAxisDirections()[ m_upAxis ], m_maxSlopeCosine );
callback.m_collisionFilterGroup = externalGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = externalGhostObject->getBroadphaseHandle()->m_collisionFilterMask;
// Sweep test
//
if( m_useGhostObjectSweepTest )
externalGhostObject->convexSweepTest( convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration );
else
world->convexSweepTest( 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]) > btScalar( 0.0 ) )
{
// We moved up only a fraction of the step height
//
currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
return currentPosition.lerp( targetPosition, callback.m_closestHitFraction );
}
m_verticalVelocity = btScalar( 0.0 );
m_verticalOffset = btScalar( 0.0 );
return currentPosition;
}
else
{
currentStepOffset = m_stepHeight;
return targetPosition;
}
}