protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (m_currentPosition-m_targetPosition).LengthSquared();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (Vector3.Dot(m_normalizedDirection,m_touchingNormal) > 0.0f)
{
UpdateTargetPositionBasedOnCollision (ref m_touchingNormal,0.0f,1.0f);
}
}
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Translation = (m_currentPosition);
end.Translation = (m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
float margin = m_convexShape.Margin;
m_convexShape.Margin = margin + m_addedMargin;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
} else
{
collisionWorld.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
m_convexShape.Margin = margin;
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
{
// we moved only a fraction
float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length();
if (hitDistance<0.0f)
{
// 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 = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
UpdateTargetPositionBasedOnCollision (ref callback.m_hitNormalWorld,0f,1f);
Vector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.LengthSquared();
if (distance2 > MathUtil.SIMD_EPSILON)
{
currentDir.Normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (Vector3.Dot(currentDir, m_normalizedDirection) <= 0.0f)
{
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;
}
}
protected void StepDown(CollisionWorld collisionWorld, float dt)
{
Matrix start= Matrix.Identity, end = Matrix.Identity;
// phase 3: down
Vector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
Vector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop);
start.Translation = m_currentPosition;
end.Translation = m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
if (callback.hasHit())
{
// we dropped a fraction of the height -> hit floor
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
else
{
// we dropped the full height
m_currentPosition = m_targetPosition;
}
}
protected void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
Matrix start = Matrix.Identity, end = Matrix.Identity;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
/* FIXME: Handle penetration properly */
start.Translation = (m_currentPosition + upAxisDirection[m_upAxis] * 0.1f);
end.Translation = m_targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, ref start, ref end, callback,0f);
}
if (callback.hasHit())
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}