/// <summary> /// Applies panning and volume reduction to an AudioInstance based on the audio's position relative to the camera. /// </summary> /// <param name="audioInstance">AudioInstance to apply panning.</param> public static void ApplySoundPanning(AudioInstance audioInstance) { Vector2 distance = audioInstance.Position; //- Screen.UnscaledViewCenter; float fade; if (distance.Length() < audioInstance.MinDistance) { fade = 1f; } else { fade = MathHelper.Clamp(1f - distance.Length() / audioInstance.MaxDistance, 0, 1); } audioInstance.Sound.Volume = fade * fade * audioInstance.BaseVolume; audioInstance.Sound.Pan = MathHelper.Clamp(distance.X / audioInstance.MaxDistance, -1, 1); }
/// <summary> /// Draws the sprite using the rotation, translation, and scale /// of the given transform /// </summary> /// <param name="transform"></param> public void Draw(Matrix3 transform) { //Finds the scale of the sprite float xMagnitude = (float)Math.Round(new Vector2(transform.m11, transform.m21).Magnitude); float yMagnitude = (float)Math.Round(new Vector2(transform.m12, transform.m22).Magnitude); Width = (int)xMagnitude; Height = (int)yMagnitude; //Sets the sprite center to the transform origin System.Numerics.Vector2 pos = new System.Numerics.Vector2(transform.m13, transform.m23); System.Numerics.Vector2 forward = new System.Numerics.Vector2(transform.m11, transform.m21); System.Numerics.Vector2 up = new System.Numerics.Vector2(transform.m12, transform.m22); if (pos.X < 0 && pos.Y > 0) { pos.X = Math.Abs(pos.X); } else if (pos.X > 0 && pos.Y < 0) { pos.Y = Math.Abs(pos.Y); } pos -= (forward / forward.Length()) * Width / 2; pos -= (up / up.Length()) * Height / 2; //Find the transform rotation in radians float rotation = (float)Math.Atan2(transform.m21, transform.m11); //Draw the sprite Raylib.DrawTextureEx(_texture, pos * 32, (float)(rotation * 180.0f / Math.PI), 32, Color.WHITE); }
public static bool Intersects(ref Circle a, ref RectangleF b, out float depth, out Vector2 normal) { normal = new Vector2(a.Center.X - (b.X + b.Width / 2), a.Center.Y - (b.Y + b.Height / 2)); #if NETSTANDARD2_1 Vector2 distance = new Vector2(MathF.Abs(normal.X), MathF.Abs(normal.Y)); #else Vector2 distance = new Vector2(Math.Abs(normal.X), Math.Abs(normal.Y)); #endif normal = Vector2.Normalize(normal); depth = distance.Length(); if (float.IsNaN(depth)) { normal = new Vector2(0, 0); depth = 0f; return(false); } if (distance.X > b.Width / 2 + a.Radius / 2 || distance.Y > b.Height / 2 + a.Radius / 2) { return(false); } if (distance.X <= b.Width / 2 || distance.Y <= b.Height / 2) { return(true); } #if NETSTANDARD2_1 float distanceSq = MathF.Pow(distance.X - b.Width / 2, 2) + MathF.Pow(distance.Y - b.Height / 2, 2); return(distanceSq <= MathF.Pow(a.Radius, 2)); #else double distanceSq = Math.Pow(distance.X - b.Width / 2, 2) + Math.Pow(distance.Y - b.Height / 2, 2); return(distanceSq <= Math.Pow(a.Radius, 2)); #endif }
/// <summary> /// 黑洞吸力 /// </summary> /// <param name="body"></param> /// <param name="attractPos"></param> /// <param name="proc"></param> public static void Attract(this Body body, Vector2 attractPos, float forceProc, float radius) { Vector2 tmp = body.GetPosition() - attractPos; float distance = radius - tmp.Length(); body.ApplyLinearImpulseToCenter(-tmp * distance * forceProc, true); }
public static float IncludedAngleCos(Vector2 v1, Vector2 v2) { v1.Normalize(); v2.Normalize(); float dot = Vector2.Dot(v1, v2); return(2 * dot / (v1.Length() + v2.Length())); }
private static V2 _PixelEvaluator(int x, int y, System.Numerics.Vector2 center) { var p = new V2((float)x + 0.5f, (float)y + 0.5f) - center; var angle = -Math.Atan2(p.X, p.Y); angle += Math.PI; angle /= Math.PI * 2; p /= center; var radius = p.Length(); return(new V2((float)angle.Clamp(0, 1), 1 - (float)radius.Clamp(0, 1))); }
public static bool Intersects(ref Circle a, ref Circle b, out float depth, out Vector2 normal) { Vector2 distance = a.Center - b.Center; depth = distance.Length(); if (float.IsNaN(depth)) { normal = new Vector2(0, 0); depth = 0f; return(false); } float radiusSum = a.Radius > b.Radius ? b.Radius : a.Radius; normal = Vector2.Normalize(distance); return(depth <= radiusSum); }
/// <summary> /// checks the result of a box being moved by deltaMovement with second /// </summary> /// <returns><c>true</c>, if to box cast was boxed, <c>false</c> otherwise.</returns> /// <param name="first">First.</param> /// <param name="second">Second.</param> /// <param name="deltaMovement">Delta movement.</param> /// <param name="hit">Hit.</param> public static bool BoxToBoxCast(Box first, Box second, System.Numerics.Vector2 movement, out RaycastHit hit) { // http://hamaluik.com/posts/swept-aabb-collision-using-minkowski-difference/ hit = new RaycastHit(); // first we check for an overlap. if we have an overlap we dont do the sweep test var minkowskiDiff = MinkowskiDifference(first, second); if (minkowskiDiff.Contains(0f, 0f)) { // calculate the MTV. if it is zero then we can just call this a non-collision var mtv = minkowskiDiff.GetClosestPointOnBoundsToOrigin(); if (mtv == System.Numerics.Vector2.Zero) { return(false); } hit.Normal = -mtv; hit.Normal.Normalize(); hit.Distance = 0f; hit.Fraction = 0f; return(true); } else { // ray-cast the movement vector against the Minkowski AABB var ray = new Ray2D(System.Numerics.Vector2.Zero, -movement); float fraction; if (minkowskiDiff.RayIntersects(ref ray, out fraction) && fraction <= 1.0f) { hit.Fraction = fraction; hit.Distance = movement.Length() * fraction; hit.Normal = -movement; hit.Normal.Normalize(); hit.Centroid = first.bounds.Center + movement * fraction; return(true); } } return(false); }
/// <summary> /// alters the minimumTranslationVector so that it removes the x-component of the translation if there was no movement in /// the same direction. /// </summary> /// <param name="deltaMovement">the original movement that caused the collision</param> public void RemoveHorizontalTranslation(System.Numerics.Vector2 deltaMovement) { // http://dev.yuanworks.com/2013/03/19/little-ninja-physics-and-collision-detection/ // fix is the vector that is only in the y-direction that we want. Projecting it on the normal gives us the // responseDistance that we already have (MTV). We know fix.x should be 0 so it simplifies to fix = r / normal.y // fix dot normal = responseDistance // check if the lateral motion is undesirable and if so remove it and fix the response if (Math.Sign(Normal.X) != Math.Sign(deltaMovement.X) || (deltaMovement.X == 0f && Normal.X != 0f)) { var responseDistance = MinimumTranslationVector.Length(); var fix = responseDistance / Normal.Y; // check some edge cases. make sure we dont have normal.x == 1 and a super small y which will result in a huge // fix value since we divide by normal if (Math.Abs(Normal.X) != 1f && Math.Abs(fix) < Math.Abs(deltaMovement.Y * 3f)) { MinimumTranslationVector = new System.Numerics.Vector2(0f, -fix); } } }
//Draws the sprites public void Draw(Matrix3 transform) { float xMagnitude = (float)Math.Round(new Vector2(transform.m11, transform.m21).Magnitude); float yMagnitude = (float)Math.Round(new Vector2(transform.m12, transform.m22).Magnitude); Width = (int)xMagnitude; Height = (int)yMagnitude; System.Numerics.Vector2 pos = new System.Numerics.Vector2(transform.m13, transform.m23); System.Numerics.Vector2 forward = new System.Numerics.Vector2(transform.m11, transform.m21); System.Numerics.Vector2 up = new System.Numerics.Vector2(transform.m12, transform.m22); pos -= (forward / forward.Length()) * Width / 2; pos -= (up / up.Length()) * Height / 2; float rotation = (float)Math.Atan2(transform.m21, transform.m11); //Draw the sprite Raylib.DrawTextureEx(_texture, pos * 32, (float)(rotation * 180.0f / Math.PI), 32, Color.WHITE); }
/// <summary> /// Constructor for PulleyJoint. /// </summary> /// <param name="bodyA">The first body.</param> /// <param name="bodyB">The second body.</param> /// <param name="anchorA">The anchor on the first body.</param> /// <param name="anchorB">The anchor on the second body.</param> /// <param name="worldAnchorA">The world anchor for the first body.</param> /// <param name="worldAnchorB">The world anchor for the second body.</param> /// <param name="ratio">The ratio.</param> /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param> public PulleyJoint(Body bodyA, Body bodyB, System.Numerics.Vector2 anchorA, System.Numerics.Vector2 anchorB, System.Numerics.Vector2 worldAnchorA, System.Numerics.Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) : base(bodyA, bodyB) { JointType = JointType.Pulley; this.WorldAnchorA = worldAnchorA; this.WorldAnchorB = worldAnchorB; if (useWorldCoordinates) { LocalAnchorA = base.BodyA.GetLocalPoint(anchorA); LocalAnchorB = base.BodyB.GetLocalPoint(anchorB); var dA = anchorA - worldAnchorA; LengthA = dA.Length(); var dB = anchorB - worldAnchorB; LengthB = dB.Length(); } else { LocalAnchorA = anchorA; LocalAnchorB = anchorB; System.Numerics.Vector2 dA = anchorA - base.BodyA.GetLocalPoint(worldAnchorA); LengthA = dA.Length(); System.Numerics.Vector2 dB = anchorB - base.BodyB.GetLocalPoint(worldAnchorB); LengthB = dB.Length(); } Debug.Assert(ratio != 0.0f); Debug.Assert(ratio > Settings.Epsilon); this.Ratio = ratio; constant = LengthA + ratio * LengthB; _impulse = 0.0f; }
public static void ProcessGaze() { if (zoomForm != null && zoomForm.Visible) { if (zoomStopwatch.ElapsedMilliseconds > ZOOM_CURSOR_DELAY) { int z = 4; bool gazeIsSteady = false; GazeZone gazeZone = GetGazeZone(z, z); if (!gazeZone.IsInside()) { // Do nothing if they look outside the blow-up picture rather then scroll indefinitely } else if (gazeZone.IsOnEdge()) { // Scroll the zoomed-in view when they look at the edge int k = 2; zoomBounds.source.X += gazeZone.GetHorizontalEdgeSign() * k; zoomBounds.source.Y += gazeZone.GetVerticalEdgeSign() * k; } else { // Check whether the gaze moved much recently by calculating the extent of the // bounding box of the recent few gaze positions zoomGazeHistory.Add(GetZoomGaze()); while (zoomGazeHistory.Count > 10) { zoomGazeHistory.RemoveAt(0); } int minX = Int32.MaxValue; int minY = Int32.MaxValue; int maxX = Int32.MinValue; int maxY = Int32.MinValue; foreach (Point p in zoomGazeHistory) { minX = Math.Min(minX, p.X); minY = Math.Min(minY, p.Y); maxX = Math.Max(maxX, p.X); maxY = Math.Max(maxY, p.Y); } int sizeX = maxX - minX; int sizeY = maxY - minY; if (sizeX <= 10 && sizeY <= 10) { gazeIsSteady = true; } } if (gazeIsSteady && zoomAutoClick) { int STEADY_THRESHOLD = 300; int CLICK_THRESHOLD = 1000; // First wait for the gaze to be steady for a while zoomSteadyStopwatch.Start(); long t = zoomSteadyStopwatch.ElapsedMilliseconds; if (t > STEADY_THRESHOLD) { // Then blink for a while before automatically clicking int BLINK_INTERVAL = 100; zoomHighlight = (((t - STEADY_THRESHOLD) / BLINK_INTERVAL) % 2 == 1); if (t > CLICK_THRESHOLD) { Unzoom(); state = State.None; // Click Mouse.Press(MouseButton.Left, GetZoomGaze()); Thread.Sleep(200); Mouse.Release(MouseButton.Left, GetZoomGaze()); } } } else { zoomSteadyStopwatch.Reset(); zoomHighlight = false; } } } if (state == State.Dragging) { float DISTANCE_THRESHOLD = 40.0f; long dt = dragStopwatch.ElapsedMilliseconds; dragStopwatch.Restart(); Vector2 gazePosition = new Vector2(gazeX, gazeY); Vector2 gazeDirection = gazePosition - dragPosition; float distance = gazeDirection.Length(); if (distance > DISTANCE_THRESHOLD) { // We need to update position in floating point to allow for sub-pixel frame to frame movement float speed = Math.Max(10.0f, (distance - DISTANCE_THRESHOLD) * 2.0f); float deltaPosition = (float)dt * speed / 1000.0f; dragPosition += gazeDirection * (deltaPosition / distance); // Converting back to integer loses sub pixel movement! Cursor.Position = new Point((int)dragPosition.X, (int)dragPosition.Y); } } else if (state == State.Strafing) { GazeZone gazeZone = GetGazeZone(3, 3); if (gazeZone.IsOnRightEdge()) { PressKey('D'); } else { ReleaseKey('D'); } if (gazeZone.IsOnLeftEdge()) { PressKey('A'); } else { ReleaseKey('A'); } if (gazeZone.IsOnBottomEdge()) { PressKey('S'); } else { ReleaseKey('S'); } if (gazeZone.IsOnTopEdge()) { PressKey('W'); } else { ReleaseKey('W'); } } else if (state == State.Orbiting) { GazeZone gazeZone = GetGazeZone(3, 3); Point center = ScreenCenter(); if (gazeZone.IsOnEdge()) { int k = 5; Point d = gazeZone.GetEdgeSign(); Mouse.Drag(MouseButton.Right, center, d.X * k, d.Y * k); } } }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; System.Numerics.Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; System.Numerics.Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; System.Numerics.Vector2 cB = data.Positions[_indexB].C; float aB = data.Positions[_indexB].A; System.Numerics.Vector2 vB = data.Velocities[_indexB].V; float wB = data.Velocities[_indexB].W; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; float lengthA = _uA.Length(); float lengthB = _uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = System.Numerics.Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = System.Numerics.Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(_rA, _uA); float ruB = MathUtils.Cross(_rB, _uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (Settings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.Step.DtRatio; // Warm starting. System.Numerics.Vector2 PA = -(_impulse) * _uA; System.Numerics.Vector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(_rA, PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(_rB, PB); } else { _impulse = 0.0f; } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
public static void FillRectangleVertices(this Span <POINT2> vertices, XFORM2 rect, float borderRadius, int arcVertexCount = 6) { var scaleX = new VECTOR2(rect.M11, rect.M12); var scaleY = new VECTOR2(rect.M21, rect.M22); var origin = new VECTOR2(rect.M31, rect.M32); if (vertices.Length == 4) { vertices[0] = origin; vertices[1] = origin + scaleX; vertices[2] = origin + scaleX + scaleY; vertices[3] = origin + scaleY; return; } int idx = 0; var sizeX = scaleX.Length(); var axisX = VECTOR2.Normalize(scaleX); var sizeY = scaleY.Length(); var axisY = VECTOR2.Normalize(scaleY); throw new NotImplementedException(); // top vertices[idx++] = origin + axisX * borderRadius; vertices[idx++] = origin + axisX * (sizeX - borderRadius); // top right var center = origin + axisX * (sizeX - borderRadius) + axisY * borderRadius; foreach (var p in _GetRectangleCornerVertices(arcVertexCount, PI * 0.5f, 0f)) { vertices[idx++] = center + (axisX * p.X + axisY * p.Y) * borderRadius; } // right vertices[idx++] = origin + axisX * (sizeX - borderRadius); vertices[idx++] = origin + new POINT2(sizeX, sizeY - borderRadius); // bottom right center = origin + new VECTOR2(sizeX - borderRadius, sizeY - borderRadius); foreach (var p in _GetRectangleCornerVertices(arcVertexCount, 0, -PI * 0.5f)) { vertices[idx++] = center + p * borderRadius; } // bottom vertices[idx++] = origin + new POINT2(sizeX - borderRadius, sizeY); vertices[idx++] = origin + new POINT2(borderRadius, sizeY); // bottom left center = origin + new VECTOR2(borderRadius, sizeY - borderRadius); foreach (var p in _GetRectangleCornerVertices(arcVertexCount, -PI * 0.5f, -PI)) { vertices[idx++] = center + p * borderRadius; } // left vertices[idx++] = origin + new POINT2(0, sizeY - borderRadius); vertices[idx++] = origin + new POINT2(0, borderRadius); // top left center = origin + new VECTOR2(borderRadius, borderRadius); foreach (var p in _GetRectangleCornerVertices(arcVertexCount, -PI, -PI * 1.5f)) { vertices[idx++] = center + p * borderRadius; } }
internal override bool SolvePositionConstraints(ref SolverData data) { System.Numerics.Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; System.Numerics.Vector2 cB = data.Positions[_indexB].C; float aB = data.Positions[_indexB].A; Rot qA = new Rot(aA), qB = new Rot(aB); System.Numerics.Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); System.Numerics.Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // Get the pulley axes. System.Numerics.Vector2 uA = cA + rA - WorldAnchorA; System.Numerics.Vector2 uB = cB + rB - WorldAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = System.Numerics.Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = System.Numerics.Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(rA, uA); float ruB = MathUtils.Cross(rB, uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; System.Numerics.Vector2 PA = -impulse * uA; System.Numerics.Vector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(rA, PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(rB, PB); data.Positions[_indexA].C = cA; data.Positions[_indexA].A = aA; data.Positions[_indexB].C = cB; data.Positions[_indexB].A = aB; return(linearError < Settings.LinearSlop); }
public void Solve(ref TimeStep step, ref System.Numerics.Vector2 gravity) { float h = step.Dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { var b = Bodies[i]; var c = b._sweep.C; float a = b._sweep.A; var v = b._linearVelocity; float w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { v += h * (b.GravityScale * gravity + b._invMass * b._force); } w += h * b._invI * b._torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].C = c; _positions[i].A = a; _velocities[i].V = v; _velocities[i].W = w; } // Solver data SolverData solverData = new SolverData(); solverData.Step = step; solverData.Positions = _positions; solverData.Velocities = _velocities; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.Inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { System.Numerics.Vector2 c = _positions[i].C; float a = _positions[i].A; System.Numerics.Vector2 v = _velocities[i].V; float w = _velocities[i].W; // Check for large velocities System.Numerics.Vector2 translation = h * v; if (System.Numerics.Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].C = c; _positions[i].A = a; _velocities[i].V = v; _velocities[i].W = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body._sweep.C = _positions[i].C; body._sweep.A = _positions[i].A; body._linearVelocity = _velocities[i].V; body._angularVelocity = _velocities[i].W; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.IsSleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || System.Numerics.Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.IsAwake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; _positions[i].C = b._sweep.C; _positions[i].A = b._sweep.A; _velocities[i].V = b._linearVelocity; _velocities[i].W = b._angularVelocity; } _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities); // Solve position constraints. for (int i = 0; i < Settings.ToiPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].C; Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].A; Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].C; Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].A; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < Settings.ToiVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.Dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { System.Numerics.Vector2 c = _positions[i].C; float a = _positions[i].A; System.Numerics.Vector2 v = _velocities[i].V; float w = _velocities[i].W; // Check for large velocities System.Numerics.Vector2 translation = h * v; if (System.Numerics.Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].C = c; _positions[i].A = a; _velocities[i].V = v; _velocities[i].W = w; // Sync bodies Body body = Bodies[i]; body._sweep.C = c; body._sweep.A = a; body._linearVelocity = v; body._angularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }