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; } }