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