public void Cast(CollisionWorld cw, float frameDelta) { using (var cb = new ClosestConvexResultCallback()) { for (int i = 0; i < NumRays; i++) { cb.ClosestHitFraction = 1.0f; cb.ConvexFromWorld = _source[i]; cb.ConvexToWorld = _destination[i]; Matrix from = _fromRotation * Matrix.Translation(_source[i]); Matrix to = _toRotation * Matrix.Translation(_destination[i]); cw.ConvexSweepTestRef(_boxShape, ref from, ref to, cb); if (cb.HasHit) { _hitPoint[i] = cb.HitPointWorld; Vector3.Lerp(ref _source[i], ref _destination[i], cb.ClosestHitFraction, out _hitCenterOfMass[i]); _hitFraction[i] = cb.ClosestHitFraction; _normal[i] = cb.HitNormalWorld; _normal[i].Normalize(); } else { _hitCenterOfMass[i] = _destination[i]; _hitPoint[i] = _destination[i]; _hitFraction[i] = 1.0f; _normal[i] = new Vector3(1.0f, 0.0f, 0.0f); } } } _time += frameDelta; _frameCount++; if (_frameCount > 50) { if (_time < _timeMin) { _timeMin = _time; } if (_time > _timeMax) { _timeMax = _time; } _timeTotal += _time; _sampleCount++; float timeMean = _timeTotal / _sampleCount; Console.WriteLine("{0} rays in {1} s, min {2}, max {3}, mean {4}", NumRays * _frameCount, _time.ToString("0.000", CultureInfo.InvariantCulture), _timeMin.ToString("0.000", CultureInfo.InvariantCulture), _timeMax.ToString("0.000", CultureInfo.InvariantCulture), timeMean.ToString("0.000", CultureInfo.InvariantCulture)); _time = 0; _frameCount = 0; } }
protected void StepUp(CollisionWorld collisionWorld) { // phase 1: up Matrix start, end; m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f)); /* FIXME: Handle penetration properly */ start = Matrix.Translation(m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.Margin + m_addedMargin)); end = Matrix.Translation(m_targetPosition); KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f); callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, 0f); } if (callback.HasHit) { // Only modify the position if the hit was a slope and not a wall or ceiling. if (Vector3.Dot(callback.HitNormalWorld, upAxisDirection[m_upAxis]) > 0.0) { // we moved up only a fraction of the step height m_currentStepOffset = m_stepHeight * callback.ClosestHitFraction; if (m_interpolateUp) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); } else { m_currentPosition = m_targetPosition; } } m_verticalVelocity = 0.0f; m_verticalOffset = 0.0f; } else { m_currentStepOffset = m_stepHeight; m_currentPosition = m_targetPosition; } }
public void Cast(CollisionWorld cw, ClosestConvexResultCallback callback, float frameDelta) { foreach (var ray in _rays) { callback.ClosestHitFraction = 1.0f; callback.ConvexFromWorld = ray.Source; callback.ConvexToWorld = ray.Destination; Matrix from = _fromRotation * Matrix.Translation(ray.Source); Matrix to = _toRotation * Matrix.Translation(ray.Destination); cw.ConvexSweepTestRef(_boxShape, ref from, ref to, callback); if (callback.HasHit) { ray.HitPoint = callback.HitPointWorld; Vector3.Lerp(ref ray.Source, ref ray.Destination, callback.ClosestHitFraction, out ray.HitCenterOfMass); ray.HitFraction = callback.ClosestHitFraction; ray.Normal = callback.HitNormalWorld; ray.Normal.Normalize(); } else { ray.HitCenterOfMass = ray.Destination; ray.HitPoint = ray.Destination; ray.HitFraction = 1.0f; ray.Normal = new Vector3(1.0f, 0.0f, 0.0f); } } _time += frameDelta; _frameCount++; if (_frameCount > 50) { if (_time < _timeMin) { _timeMin = _time; } if (_time > _timeMax) { _timeMax = _time; } _timeTotal += _time; _sampleCount++; PrintStats(); _time = 0; _frameCount = 0; } }
protected void StepUp(CollisionWorld collisionWorld) { Matrix start, end; m_targetPosition = m_currentPosition + Vector3.UnitY * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f)); start = Matrix.Translation(m_currentPosition + Vector3.UnitY * (m_convexShape.Margin + m_addedMargin)); end = Matrix.Translation(m_targetPosition); _callback.Me = m_ghostObject; _callback.Up = -Vector3.UnitY; _callback.MinSlopeDot = 0.7071f; _callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; _callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; _callback.ClosestHitFraction = 1; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, 0f); } if (_callback.HasHit) { if (Vector3.Dot(_callback.HitNormalWorld, Vector3.UnitY) > 0.0) { m_currentStepOffset = m_stepHeight * _callback.ClosestHitFraction; if (m_interpolateUp) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, _callback.ClosestHitFraction, out m_currentPosition); } else { m_currentPosition = m_targetPosition; } } m_verticalVelocity = 0.0f; m_verticalOffset = 0.0f; } else { m_currentStepOffset = m_stepHeight; m_currentPosition = m_targetPosition; } }
public void Cast(CollisionWorld cw) { for (int i = 0; i < NUMRAYS_IN_BAR; i++) { using (var cb = new ClosestConvexResultCallback(ref source[i], ref dest[i])) { Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f); Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f); Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]); Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]); cw.ConvexSweepTestRef(boxShape, ref from, ref to, cb); if (cb.HasHit) { hit_surface[i] = cb.HitPointWorld; hit_com[i] = Vector3.Lerp(source[i], dest[i], cb.ClosestHitFraction); hit_fraction[i] = cb.ClosestHitFraction; normal[i] = cb.HitNormalWorld; normal[i].Normalize(); } else { hit_com[i] = dest[i]; hit_surface[i] = dest[i]; hit_fraction[i] = 1.0f; normal[i] = new Vector3(1.0f, 0.0f, 0.0f); } } } frame_counter++; if (frame_counter > 50) { min_ms = ms < min_ms ? ms : min_ms; max_ms = ms > max_ms ? ms : max_ms; sum_ms += ms; sum_ms_samples++; float mean_ms = (float)sum_ms / (float)sum_ms_samples; Console.WriteLine("{0} rays in {1} ms {2} {3} {4}", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms); ms = 0; frame_counter = 0; } }
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.0; * btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); * float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; * btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; * m_targetPosition -= (step_drop + gravity_drop);*/ Vector3 orig_position = m_targetPosition; float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { downVelocity = m_fallSpeed; } Vector3 step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine); callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, upAxisDirection[m_upAxis], m_maxSlopeCosine); callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback2.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; while (true) { start = Matrix.Translation(m_currentPosition); end = Matrix.Translation(m_targetPosition); //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.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit) { //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 { // this works.... collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); if (!callback.HasHit) { //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration); } } float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; bool has_hit = false; if (bounce_fix == true) { has_hit = callback.HasHit || callback2.HasHit; } else { has_hit = callback2.HasHit; } if (downVelocity2 > 0.0f && downVelocity2 < m_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 = m_stepHeight; Vector3 step_drop2 = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop2; runonce = true; continue; //re-run previous tests } break; } if (callback.HasHit || runonce == true) { // we dropped a fraction of the height -> hit floor float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2; //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); if (bounce_fix == true) { if (full_drop == true) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); } else { //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition); } } else { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition); } 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 < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = upAxisDirection[m_upAxis] * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY()); 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) { float dot; Vector3.Dot(ref m_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 = (m_currentPosition); end.Origin = (m_targetPosition); Vector3 sweepDirNegative = m_currentPosition - m_targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f); callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } m_convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit) { // we moved only a fraction float hitDistance = (callback.HitPointWorld - m_currentPosition).Length; Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref 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." */ float dot; Vector3.Dot(ref currentDir, ref m_normalizedDirection, out dot); if (dot <= 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 StepDown(CollisionWorld collisionWorld, float dt) { Matrix start, end, end_double; bool runonce = false; Vector3 orig_position = m_targetPosition; float downVelocity = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { downVelocity = m_fallSpeed; } Vector3 step_drop = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; _callback.MinSlopeDot = m_maxSlopeCosine; _callback.Up = Vector3.UnitY; _callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; _callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; bool hasFirstHit; float hitFraction; Vector3 pointWorld; while (true) { start = Matrix.Translation(m_currentPosition); end = Matrix.Translation(m_targetPosition); //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); bool has_hit_first = false; bool has_hit_second = false; if (m_useGhostObjectSweepTest) { _callback.ClosestHitFraction = 1; m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); pointWorld = _callback.HitPointWorld; has_hit_first = _callback.HasHit; hitFraction = _callback.ClosestHitFraction; if (!has_hit_first) { _callback.ClosestHitFraction = 1; //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, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); has_hit_second = _callback.HasHit; } } else { _callback.ClosestHitFraction = 1; // this works.... collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); pointWorld = _callback.HitPointWorld; has_hit_first = _callback.HasHit; hitFraction = _callback.ClosestHitFraction; if (!has_hit_first) { _callback.ClosestHitFraction = 1; //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); has_hit_second = _callback.HasHit; } } hasFirstHit = has_hit_first; float downVelocity2 = (m_verticalVelocity < 0.0f ? -m_verticalVelocity : 0.0f) * dt; bool has_hit; if (bounce_fix == true) { has_hit = has_hit_first || has_hit_second; } else { has_hit = has_hit_second; } if (downVelocity2 > 0.0f && downVelocity2 < m_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 = m_stepHeight; Vector3 step_drop2 = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop2; runonce = true; continue; //re-run previous tests } break; } if (hasFirstHit || runonce) { // we dropped a fraction of the height -> hit floor float fraction = (m_currentPosition.Y - pointWorld.Y) / 2; if (bounce_fix) { if (full_drop) { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, hitFraction, out m_currentPosition); } else { //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, fraction, out m_currentPosition); } } else { Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, hitFraction, out m_currentPosition); } 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 < 0.0f ? -m_verticalVelocity : 0.0f) * dt; if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; step_drop = Vector3.UnitY * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } m_currentPosition = m_targetPosition; } }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { Matrix start = Matrix.Identity, end = Matrix.Identity; m_targetPosition = m_currentPosition + walkMove; float fraction = 1.0f; float distance2 = (m_currentPosition - m_targetPosition).LengthSquared; int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (m_currentPosition); end.Origin = (m_targetPosition); Vector3 sweepDirNegative = m_currentPosition - m_targetPosition; _callback.MinSlopeDot = 0; _callback.Up = sweepDirNegative; _callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup; _callback.CollisionFilterMask = GhostObject.BroadphaseHandle.CollisionFilterMask; _callback.ClosestHitFraction = 1; float margin = m_convexShape.Margin; m_convexShape.Margin = margin + m_addedMargin; if (m_useGhostObjectSweepTest) { m_ghostObject.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } else { collisionWorld.ConvexSweepTestRef(m_convexShape, ref start, ref end, _callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); } m_convexShape.Margin = margin; fraction -= _callback.ClosestHitFraction; if (_callback.HasHit) { Vector3 hitNormalWorld = _callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref 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." */ Vector3.Dot(ref currentDir, ref m_normalizedDirection, out var dot); if (dot <= 0.0f) { break; } } else { break; } } else { m_currentPosition = m_targetPosition; } } }