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;
    }
}