void btKinematicCharacterController::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;
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
}
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
} else {
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
void bulletCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
{
btTransform start, end;
// phase 3: down
btScalar additionalDownStep = (m_wasOnGround /*&& !onGround()*/) ? m_stepHeight : btScalar(0.0);
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == btScalar(0.0) && m_verticalVelocity<btScalar(0.0)?-m_verticalVelocity:btScalar(0.0)) * dt;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);
start.setIdentity ();
end.setIdentity ();
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
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, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
{
// we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
} else {
// we dropped the full height
m_currentPosition = m_targetPosition;
}
}
void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
{
btTransform start, end;
// phase 3: down
btVector3 step_drop = getUpAxisDirections()[m_upAxis] * m_currentStepOffset;
btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop);
start.setIdentity ();
end.setIdentity ();
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
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, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
if (callback.hasHit())
{
// we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
} else {
// we dropped the full height
m_currentPosition = m_targetPosition;
}
}
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
// phase 1: up
btTransform start, end;
m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * m_stepHeight;
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * btScalar(0.1f));
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
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())
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
} else {
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
void btKinematicCharacterController::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;
}
}
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;
}
}
void bulletCharacterController::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))
updateTargetPositionBasedOnCollision (m_touchingNormal);
}*/
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], btScalar(-1.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 = (callback.m_hitPointWorld - m_currentPosition).length();
/* If the distance is farther than the collision margin, move */
// if (hitDistance > m_addedMargin)
// {
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
// 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;
}
}