void ComputeController(btVector3 ¤tSpeed, const btVector3 &delta, const btVector3 &maxSpeed, float scaleDelta, float damping) {
// Timestep scale
btVector3 acceleration = delta * scaleDelta;
if (currentSpeed.fuzzyZero() && !currentSpeed.isZero()) {
currentSpeed.setZero();
}
acceleration += currentSpeed * -damping;
// Clamp the acceleration to max speed
for(int i = 2; i >= 0; i--) {
if (fabs(acceleration[i]) < maxSpeed[i]) continue;
acceleration[i] = (acceleration[i] < 0) ? -maxSpeed[i] : maxSpeed[i];
}
currentSpeed += acceleration;
}
// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
/*btVector3 n(0, 0, 0);
if (v.fuzzyZero()) return n;
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;*/
btVector3 n;
if (v.fuzzyZero()) {
n.setValue(0, 0, 0);
}
else {
n = v.normalized();
}
return n;
}
void KinematicCharacterController::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;
if (walkMove.fuzzyZero()) {
return;
}
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))
{
//interferes with step movement
//updateTargetPositionBasedOnCollision (m_touchingNormal);
}
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
{
start.setOrigin(m_currentPosition);
end.setOrigin(m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.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) ///@todo if start and end are equal in debug mode, bullet asserts
{
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;
hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
// 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;
}
}