protected void stepForwardAndStrafe(CollisionWorld collisionWorld, VInt3 walkMove)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
targetPosition = currentPosition + walkMove;
VFixedPoint fraction = VFixedPoint.One;
int maxIter = 10;
while (fraction > VFixedPoint.Create(0.01f) && maxIter-- > 0)
{
start.position = currentPosition;
end.position = targetPosition;
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = results[0].fraction;
VInt3 hitNormalWorld = results[0].normal;
for (int i = 1; i < results.Count; i++)
{
VFixedPoint afraction = results[i].fraction;
if (afraction <= closestHitFraction)
{
closestHitFraction = afraction;
hitNormalWorld = results[i].normal;
}
}
fraction -= closestHitFraction;
updateTargetPositionBasedOnCollision(hitNormalWorld);
VInt3 currentDir = targetPosition - currentPosition;
VFixedPoint distance2 = currentDir.sqrMagnitude;
if (distance2 > Globals.EPS2)
{
currentDir = currentDir.Normalize();
if (VInt3.Dot(currentDir, normalizedDirection) <= VFixedPoint.Zero)
{
break;
}
}
else
{
break;
}
}
else
{
currentPosition = targetPosition;
}
}
}
protected void stepDown(CollisionWorld collisionWorld, VFixedPoint dt)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
VFixedPoint additionalDownStep = wasOnGround ? stepHeight : VFixedPoint.Zero;
VInt3 stepDrop = upAxisDirection[upAxis] * (currentStepOffset + additionalDownStep);
VFixedPoint downVelocity = (additionalDownStep == VFixedPoint.Zero && verticalVelocity < VFixedPoint.Zero ? -verticalVelocity : VFixedPoint.Zero) * dt;
VInt3 gravityDrop = upAxisDirection[upAxis] * downVelocity;
targetPosition -= stepDrop;
targetPosition -= gravityDrop;
start.position = currentPosition; end.position = targetPosition;
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = VFixedPoint.One;
for (int i = 0; i < results.Count; i++)
{
VFixedPoint fraction = results[i].fraction;
if (VInt3.Dot(results[i].normal, upAxisDirection[upAxis]) < maxSlopeCosine)
{
continue;
}
if (fraction < closestHitFraction)
{
closestHitFraction = fraction;
}
}
// we dropped a fraction of the height -> hit floor
currentPosition = currentPosition * (VFixedPoint.One - closestHitFraction) + targetPosition * closestHitFraction;
verticalVelocity = VFixedPoint.Zero;
verticalOffset = VFixedPoint.Zero;
}
else
{
// we dropped the full height
currentPosition = targetPosition;
}
}
protected void stepUp(CollisionWorld collisionWorld)
{
VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity;
targetPosition = currentPosition + upAxisDirection[upAxis] * (stepHeight + (verticalOffset > VFixedPoint.Zero ? verticalOffset : VFixedPoint.Zero));
start.position = currentPosition + upAxisDirection[upAxis] * (me.getCollisionShape().getMargin() + addedMargin);
end.position = targetPosition;
VInt3 up = -upAxisDirection[upAxis];
List <CastResult> results = new List <CastResult>();
me.setWorldTransform(start);
collisionWorld.SweepTest(me, end.position, results);
if (results.Count > 0)
{
VFixedPoint closestHitFraction = VFixedPoint.One;
for (int i = 0; i < results.Count; i++)
{
VFixedPoint fraction = results[i].fraction;
if (VInt3.Dot(results[i].normal, up) < VFixedPoint.Zero)
{
fraction = VFixedPoint.One;
}
if (fraction < closestHitFraction)
{
closestHitFraction = fraction;
}
}
// we moved up only a fraction of the step height
currentStepOffset = stepHeight * closestHitFraction;
currentPosition = currentPosition * (VFixedPoint.One - closestHitFraction) + targetPosition * closestHitFraction;
verticalVelocity = VFixedPoint.Zero;
verticalOffset = VFixedPoint.Zero;
}
else
{
currentStepOffset = stepHeight;
currentPosition = targetPosition;
}
}
