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