void StepForwardAndStrafe(CollisionWorld collisionWorld)
{
Matrix start = Matrix.Identity, end = Matrix.Identity;
float fraction = 1.0f;
int maxIter = maximumIterations;
while (fraction > smallestFraction && maxIter-- > 0)
{
start.Origin = currentPosition;
end.Origin = targetPosition;
Vector3 sweepDirNegative = currentPosition - targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(collisionObject, sweepDirNegative, 0f);
callback.CollisionFilterGroup = collisionObjectCastToPairCache.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = collisionObjectCastToPairCache.BroadphaseHandle.CollisionFilterMask;
float margin = collisionShape.Margin;
collisionObjectCastToPairCache.ConvexSweepTestRef(collisionShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
collisionShape.Margin = margin;
//The decimal percent of we can move along targetPosition - currentPosition before collision.
fraction -= callback.ClosestHitFraction;
if (callback.HasHit)
{
//We moved only a fraction of target-current.
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref hitNormalWorld);
Vector3 currentDir = targetPosition - currentPosition;
if (Mathf.Approximately(currentDir.LengthSquared, 0))
{
break;
}
//TODO: Break if velocity is against original velocity, stop early to avoid tiny oscilations in sloping corners.
}
else
{
//We can move the requested to the requested position. Therefore fraction > smallestFraction.
currentPosition = targetPosition;
//But I don't trust that so imma break.
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 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 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;
}
}
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
{
Matrix start = Matrix.Identity, end = Matrix.Identity;
targetPosition = currentPosition + walkMove;
float fraction = 1.0f;
float distance2 = (currentPosition - targetPosition).LengthSquared;
// printf("distance2=%f\n",distance2);
if (touchingContact)
{
float dot;
Vector3.Dot(ref normalizedDirection, ref m_touchingNormal, out dot);
if (dot > 0.0f)
{
//interferes with step movement
//UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f);
}
}
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0)
{
start.Origin = (currentPosition);
end.Origin = (targetPosition);
Vector3 sweepDirNegative = currentPosition - targetPosition;
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, sweepDirNegative, 0f);
callback.CollisionFilterGroup = ghostObject.BroadphaseHandle.CollisionFilterGroup;
callback.CollisionFilterMask = ghostObject.BroadphaseHandle.CollisionFilterMask;
float margin = convexShape.Margin;
convexShape.Margin = margin + .02f;
ghostObject.ConvexSweepTestRef(convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
convexShape.Margin = margin;
fraction -= callback.ClosestHitFraction;
if (callback.HasHit)
{
// We moved only a fraction
float hitDistance = (callback.HitPointWorld - currentPosition).Length;
Vector3 hitNormalWorld = callback.HitNormalWorld;
UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f);
Vector3 currentDir = targetPosition - 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." */
float dot;
Vector3.Dot(ref currentDir, ref normalizedDirection, out dot);
if (dot <= 0.0f)
{
break;
}
}
else
{
break;
}
}
else
{
// we moved whole way
currentPosition = targetPosition;
}
}
}
