void btKinematicCharacterController::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))
{
//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)
{
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;
}
}
//-----------------------------------------------------------------------
// s t e p F o r w a r d A n d S t r a f e
//-----------------------------------------------------------------------
void TKinematicCharacterTest::stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove)
{
btVector3 originalDir = walkMove.normalized();
if (walkMove.length() < SIMD_EPSILON)
{
originalDir.setValue(0.f,0.f,0.f);
}
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[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 (originalDir.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);
TKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
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())
{
if(callback.m_hitCollisionObject->isStaticObject())
{
// we moved only a fraction
btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
if (hitDistance<0.f)
{
// printf("neg dist?\n");
}
/* 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(originalDir) <= 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;
}
}
