protected void StepDown(CollisionWorld collisionWorld, float dt) { Matrix start, end, end_double; bool runonce = false; // phase 3: down /*float additionalDownStep = (m_wasOnGround && !OnGround) ? m_stepHeight : 0; * Vector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep); * float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity < 0 ? -m_verticalVelocity : 0) * dt; * Vector3 gravity_drop = m_up * downVelocity; * m_targetPosition -= (step_drop + gravity_drop);*/ Vector3 orig_position = m_targetPosition; float downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt; if (m_verticalVelocity > 0.0) { return; } if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { downVelocity = m_fallSpeed; } Vector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine)) using (KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine)) { callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; while (true) { Matrix.Translation(ref m_currentPosition, out start); Matrix.Translation(ref m_targetPosition, out end); start.SetRotation(m_currentOrientation, out start); end.SetRotation(m_targetOrientation, out end); //set double test for 2x the step drop, to check for a large drop vs small drop end_double = Matrix.Translation(m_targetPosition - step_drop); if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit && m_ghostObject.HasContactResponse) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } else { collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit && m_ghostObject.HasContactResponse) { //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) collisionWorld.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } float downVelocity2 = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt; bool has_hit; if (bounce_fix == true) { has_hit = (callback.HasHit || callback2.HasHit) && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject); } else { has_hit = callback2.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback2.HitCollisionObject); } float stepHeight = 0.0f; if (m_verticalVelocity < 0.0) { stepHeight = m_stepHeight; } if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; downVelocity = stepHeight; step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests } break; } if ((m_ghostObject.HasContactResponse && (callback.HasHit && NeedsCollision(m_ghostObject, callback.HitCollisionObject))) || runonce == true) { // we dropped a fraction of the height -> hit floor float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2; //System.Console.WriteLine("hitpoint: {0} - pos {1}", callback.HitPointWorld.Y, m_currentPosition.Y); if (bounce_fix == true) { if (full_drop == true) { m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction); } else { //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, fraction); } } else { m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction); } full_drop = false; m_verticalVelocity = 0.0f; m_verticalOffset = 0.0f; m_wasJumping = false; } else { // we dropped the full height full_drop = true; if (bounce_fix == true) { downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } //System.Console.WriteLine("full drop - {0}, {1}", m_currentPosition.Y, m_targetPosition.Y); m_currentPosition = m_targetPosition; } } }
protected void StepUp(CollisionWorld world) { float stepHeight = 0.0f; if (m_verticalVelocity < 0.0f) { stepHeight = m_stepHeight; } // phase 1: up Matrix start, end; start = Matrix.Identity; end = Matrix.Identity; /* FIX ME: Handle penetration properly */ start.Origin = m_currentPosition; m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0f ? m_verticalOffset : 0f)); m_currentPosition = m_targetPosition; end.Origin = m_targetPosition; start.SetRotation(m_currentOrientation, out start); end.SetRotation(m_targetOrientation, out end); using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -m_up, m_maxSlopeCosine)) { callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration); } else { world.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration); } if (callback.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject)) { // Only modify the position if the hit was a slope and not a wall or ceiling. if (callback.HitNormalWorld.Dot(m_up) > 0.0) { // we moved up only a fraction of the step height m_currentStepOffset = stepHeight * callback.ClosestHitFraction; if (m_interpolateUp == true) { m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction); } else { m_currentPosition = m_targetPosition; } } Matrix xform = m_ghostObject.WorldTransform; xform.Origin = m_currentPosition; m_ghostObject.WorldTransform = xform; // fix penetration if we hit a ceiling for example int numPenetrationLoops = 0; m_touchingContact = false; while (RecoverFromPenetration(world)) { numPenetrationLoops++; m_touchingContact = true; if (numPenetrationLoops > 4) { //System.Console.WriteLine("character could not recover from penetration = " + numPenetrationLoops); break; } } m_targetPosition = m_ghostObject.WorldTransform.Origin; m_currentPosition = m_targetPosition; if (m_verticalOffset > 0) { m_verticalOffset = 0.0f; m_verticalVelocity = 0.0f; m_currentStepOffset = m_stepHeight; } } else { m_currentStepOffset = stepHeight; m_currentPosition = m_targetPosition; } } }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { //System.Console.WriteLine("m_normalizedDirection=" + m_normalizedDirection); // phase 2: forward and strafe Matrix start = Matrix.Identity; Matrix end = Matrix.Identity; m_targetPosition = m_currentPosition + walkMove; float fraction = 1.0f; float distance2 = (m_currentPosition - m_targetPosition).LengthSquared; //System.Console.WriteLine("distance2=" + distance2); int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = m_currentPosition; end.Origin = m_targetPosition; Vector3 sweepDirNegative = m_currentPosition - m_targetPosition; start.SetRotation(m_currentOrientation, out start); end.SetRotation(m_targetOrientation, out end); using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0.0f)) { callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (start != end) { if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } m_convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit && GhostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject)) { // we moved only a fraction //float hitDistance = (callback.HitPointWorld - m_currentPosition).Length; //Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld); 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 (currentDir.Dot(m_normalizedDirection) <= 0.0f) { break; } } else { //System.Console.WriteLine("currentDir: don't normalize a zero vector"); break; } } else { m_currentPosition = m_targetPosition; } } } }
public void StepForwardAndStrafe(CollisionWorld collisionWorld, ref IndexedVector3 walkMove) { // printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); // phase 2: forward and strafe IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.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 (IndexedVector3.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._origin = (m_currentPosition); end._origin = (m_targetPosition); IndexedVector3 sweepDirNegative = m_currentPosition - m_targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f); callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup; callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask; float margin = m_convexShape.GetMargin(); m_convexShape.SetMargin(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.SetMargin(margin); fraction -= callback.m_closestHitFraction; if (callback.HasHit()) { // we moved only a fraction float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length(); UpdateTargetPositionBasedOnCollision(ref callback.m_hitNormalWorld, 0f, 1f); IndexedVector3 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 (IndexedVector3.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; } }