public void Solve(ref TimeStep step, ref TSVector2 gravity)
{
FP h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
TSVector2 c = b._sweep.C;
FP a = b._sweep.A;
TSVector2 v = b._linearVelocity;
FP 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);
v *= 1 / (1 + h * b.LinearDamping);
w *= 1 / (1 + h * b.AngularDamping);
}
_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();
}
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
TSVector2 c = _positions[i].c;
FP a = _positions[i].a;
TSVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
TSVector2 translation = h * v;
if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.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)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// 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)
{
FP minSleepTime = Settings.MaxFP;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || TSVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += h;
minSleepTime = SyncFrame.TSMath.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
/// <summary>
/// Attaches the bodies with revolute joints.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodies">The bodies.</param>
/// <param name="localAnchorA">The local anchor A.</param>
/// <param name="localAnchorB">The local anchor B.</param>
/// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
/// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
public static List <RevoluteJoint> AttachBodiesWithRevoluteJoint(World world, List <Body> bodies, TSVector2 localAnchorA, TSVector2 localAnchorB, bool connectFirstAndLast, bool collideConnected)
{
List <RevoluteJoint> joints = new List <RevoluteJoint>(bodies.Count + 1);
for (int i = 1; i < bodies.Count; i++)
{
RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB);
joint.CollideConnected = collideConnected;
world.AddJoint(joint);
joints.Add(joint);
}
if (connectFirstAndLast)
{
RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB);
lastjoint.CollideConnected = collideConnected;
world.AddJoint(lastjoint);
joints.Add(lastjoint);
}
return(joints);
}
public override bool TestPoint(ref Transform transform, ref TSVector2 point)
{
return(false);
}
/// <summary> /// Draw a line segment. /// </summary> /// <param name="start">The start.</param> /// <param name="end">The end.</param> /// <param name="red">The red value.</param> /// <param name="blue">The blue value.</param> /// <param name="green">The green value.</param> public abstract void DrawSegment(TSVector2 start, TSVector2 end, FP red, FP blue, FP green);
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;
FP aA = data.positions[_indexA].a;
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
TSVector2 vB = data.velocities[_indexB].v;
FP 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);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
K.ez.x = -_rA.y * iA - _rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
K.ez.y = _rA.x * iA + _rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
FP invM = iA + iB;
FP m = invM > 0.0f ? 1.0f / invM : 0.0f;
FP C = aB - aA - ReferenceAngle;
// Frequency
FP omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
FP d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
FP k = m * omega * omega;
// magic formulas
FP h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
TSVector2 P = new TSVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
}
else
{
_impulse = TSVector.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public BuoyancyController(AABB container, FP density, FP linearDragCoefficient, FP rotationalDragCoefficient, TSVector2 gravity) : base(ControllerType.BuoyancyController)
{
this.Container = container;
this._normal = new TSVector2(0, 1);
this.Density = density;
this.LinearDragCoefficient = linearDragCoefficient;
this.AngularDragCoefficient = rotationalDragCoefficient;
this._gravity = gravity;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
this._indexA = base.BodyA.IslandIndex;
this._indexB = base.BodyB.IslandIndex;
this._localCenterA = base.BodyA._sweep.LocalCenter;
this._localCenterB = base.BodyB._sweep.LocalCenter;
this._invMassA = base.BodyA._invMass;
this._invMassB = base.BodyB._invMass;
this._invIA = base.BodyA._invI;
this._invIB = base.BodyB._invI;
TSVector2 c = data.positions[this._indexA].c;
FP a = data.positions[this._indexA].a;
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 c2 = data.positions[this._indexB].c;
FP a2 = data.positions[this._indexB].a;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
Rot q = new Rot(a);
Rot q2 = new Rot(a2);
this._rA = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
this._rB = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
this._u = c2 + this._rB - c - this._rA;
this._length = this._u.magnitude;
FP x = this._length - this.MaxLength;
bool flag = x > 0f;
if (flag)
{
this.State = LimitState.AtUpper;
}
else
{
this.State = LimitState.Inactive;
}
bool flag2 = this._length > Settings.LinearSlop;
if (flag2)
{
this._u *= 1f / this._length;
FP y = MathUtils.Cross(this._rA, this._u);
FP y2 = MathUtils.Cross(this._rB, this._u);
FP fP3 = this._invMassA + this._invIA * y * y + this._invMassB + this._invIB * y2 * y2;
this._mass = ((fP3 != 0f) ? (1f / fP3) : 0f);
this._impulse *= data.step.dtRatio;
TSVector2 tSVector3 = this._impulse * this._u;
tSVector -= this._invMassA * tSVector3;
fP -= this._invIA * MathUtils.Cross(this._rA, tSVector3);
tSVector2 += this._invMassB * tSVector3;
fP2 += this._invIB * MathUtils.Cross(this._rB, tSVector3);
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
else
{
this._u = TSVector2.zero;
this._mass = 0f;
this._impulse = 0f;
}
}
public static BreakableBody CreateBreakableBody(World world, IEnumerable <Shape> shapes, TSVector2 position)
{
BreakableBody breakableBody = new BreakableBody(shapes, world);
breakableBody.MainBody.Position = position;
world.AddBreakableBody(breakableBody);
return(breakableBody);
}
public static Body CreateLineArc(World world, FP radians, int sides, FP radius, TSVector2 position, FP angle, bool closed)
{
Body body = CreateBody(world);
FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body);
return(body);
}
/// <summary>
/// Creates a rounded rectangle.
/// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
/// </summary>
/// <param name="world">The world.</param>
/// <param name="width">The width.</param>
/// <param name="height">The height.</param>
/// <param name="xRadius">The x radius.</param>
/// <param name="yRadius">The y radius.</param>
/// <param name="segments">The segments.</param>
/// <param name="density">The density.</param>
/// <param name="position">The position.</param>
/// <returns></returns>
public static Body CreateRoundedRectangle(World world, FP width, FP height, FP xRadius, FP yRadius, int segments, FP density, TSVector2 position, object userData = null)
{
Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);
//There are too many vertices in the capsule. We decompose it.
if (verts.Count >= Settings.MaxPolygonVertices)
{
List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3);
Body body = CreateCompoundPolygon(world, vertList, density, userData);
body.Position = position;
return(body);
}
return(CreatePolygon(world, verts, density, null));
}
/// <summary>
/// Creates a breakable body. You would want to remove collinear points before using this.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="vertices">The vertices.</param>
/// <param name="density">The density.</param>
/// <param name="position">The position.</param>
/// <returns></returns>
public static BreakableBody CreateBreakableBody(World world, Vertices vertices, FP density, TSVector2 position)
{
List <Vertices> triangles = Triangulate.ConvexPartition(vertices, TriangulationAlgorithm.Earclip, true, FP.EN3);
BreakableBody breakableBody = new BreakableBody(triangles, world, density);
breakableBody.MainBody.Position = position;
world.AddBreakableBody(breakableBody);
return(breakableBody);
}
/// <summary>
/// Creates a capsule.
/// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
/// </summary>
/// <returns></returns>
public static Body CreateCapsule(World world, FP height, FP topRadius, int topEdges, FP bottomRadius, int bottomEdges, FP density, TSVector2 position, object userData = null)
{
Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges);
Body body;
//There are too many vertices in the capsule. We decompose it.
if (verts.Count >= Settings.MaxPolygonVertices)
{
List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, FP.EN3);
body = CreateCompoundPolygon(world, vertList, density, userData);
body.Position = position;
return(body);
}
body = CreatePolygon(world, verts, density, userData);
body.Position = position;
return(body);
}
// TS - public static Body CreateBody(World world, Vector2 position, FP rotation = 0, object userData = null)
public static Body CreateBody(World world, TSVector2 position, FP rotation, object userData = null)
{
Body body = new Body(world, position, rotation, userData);
return(body);
}
public static Body CreateCompoundPolygon(World world, List <Vertices> list, FP density, TSVector2 position, object userData = null)
{
//We create a single body
Body polygonBody = CreateBody(world, position);
FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData);
return(polygonBody);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP angularError = 0.0f;
FP positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
FP angle = aB - aA - ReferenceAngle;
FP limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
FP C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = FP.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
FP C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
FP C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
TSVector2 C = cB + rB - cA - rA;
positionError = C.magnitude;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
TSVector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
public static Body CreateSolidArc(World world, FP density, FP radians, int sides, FP radius, TSVector2 position, FP angle)
{
Body body = CreateBody(world);
FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body);
return(body);
}
Vector3 ToXZ(TSVector2 target)
{
return(new Vector3(target.x.AsFloat(), 0, target.y.AsFloat()));
}
public static Body CreateRectangle(World world, FP width, FP height, FP density, TSVector2 position, object userData = null)
{
if (width <= 0)
{
throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters");
}
if (height <= 0)
{
throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters");
}
Body newBody = CreateBody(world, position);
newBody.UserData = userData;
Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
newBody.CreateFixture(rectangleShape);
return(newBody);
}
public override void Update(FP dt)
{
this._uniqueBodies.Clear();
this.World.QueryAABB(delegate(Fixture fixture)
{
bool flag3 = fixture.Body.IsStatic || !fixture.Body.Awake;
bool result;
if (flag3)
{
result = true;
}
else
{
bool flag4 = !this._uniqueBodies.ContainsKey(fixture.Body.BodyId);
if (flag4)
{
this._uniqueBodies.Add(fixture.Body.BodyId, fixture.Body);
}
result = true;
}
return(result);
}, ref this._container);
foreach (KeyValuePair <int, Body> current in this._uniqueBodies)
{
Body value = current.Value;
TSVector2 zero = TSVector2.zero;
TSVector2 zero2 = TSVector2.zero;
FP fP = 0;
FP fP2 = 0;
for (int i = 0; i < value.FixtureList.Count; i++)
{
Fixture fixture2 = value.FixtureList[i];
bool flag = fixture2.Shape.ShapeType != ShapeType.Polygon && fixture2.Shape.ShapeType > ShapeType.Circle;
if (!flag)
{
Shape shape = fixture2.Shape;
TSVector2 tSVector;
FP fP3 = shape.ComputeSubmergedArea(ref this._normal, this._offset, ref value._xf, out tSVector);
fP += fP3;
zero.x += fP3 * tSVector.x;
zero.y += fP3 * tSVector.y;
fP2 += fP3 * shape.Density;
zero2.x += fP3 * tSVector.x * shape.Density;
zero2.y += fP3 * tSVector.y * shape.Density;
}
}
zero.x /= fP;
zero.y /= fP;
zero2.x /= fP2;
zero2.y /= fP2;
bool flag2 = fP < Settings.Epsilon;
if (!flag2)
{
TSVector2 force = -this.Density * fP * this._gravity;
value.ApplyForce(force, zero2);
TSVector2 tSVector2 = value.GetLinearVelocityFromWorldPoint(zero) - this.Velocity;
tSVector2 *= -this.LinearDragCoefficient * fP;
value.ApplyForce(tSVector2, zero);
value.ApplyTorque(-value.Inertia / value.Mass * fP * value.AngularVelocity * this.AngularDragCoefficient);
}
}
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input)
{
TSVector2 p1 = input.Point1;
TSVector2 p2 = input.Point2;
TSVector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
TSVector2 absV = MathUtils.Abs(new TSVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FP maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
TSVector2 t = p1 + maxFraction * (p2 - p1);
TSVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
TSVector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_raycastStack.Clear();
_raycastStack.Push(_root);
while (_raycastStack.Count > 0)
{
int nodeId = _raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode <T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
TSVector2 c = node.AABB.Center;
TSVector2 h = node.AABB.Extents;
FP separation = FP.Abs(TSVector2.Dot(new TSVector2(-r.y, r.x), p1 - c)) - TSVector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
FP value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
TSVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = TSVector2.Min(p1, t);
segmentAABB.UpperBound = TSVector2.Max(p1, t);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
}
}
}
/// <summary>
/// Draw a solid circle.
/// </summary>
/// <param name="center">The center.</param>
/// <param name="radius">The radius.</param>
/// <param name="axis">The axis.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawSolidCircle(TSVector2 center, FP radius, TSVector2 axis, FP red, FP blue,
FP green);
/// <summary>
/// Constructor of RevoluteJoint.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="anchor">The shared anchor.</param>
/// <param name="useWorldCoordinates"></param>
public RevoluteJoint(Body bodyA, Body bodyB, TSVector2 anchor, bool useWorldCoordinates = false)
: this(bodyA, bodyB, anchor, anchor, useWorldCoordinates)
{
}
public FP SDValue(TSVector2 pos)
{
return(SDFUtils.SDCircle(pos, Center, Radius));
}
public override TSVector2 GetReactionForce(FP invDt)
{
TSVector2 p = new TSVector2(_impulse.x, _impulse.y);
return(invDt * p);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FP positionError, angularError;
Mat33 K = new Mat33();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
TSVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
TSVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
TSVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
TSVector C = new TSVector(C1.x, C1.y, C2);
TSVector impulse = K.Solve33(C) * -1;
TSVector2 P = new TSVector2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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;
FP aA = data.positions[_indexA].a;
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
TSVector2 vB = data.velocities[_indexB].v;
FP 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);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
_mass.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
_mass.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
_mass.ez.x = -_rA.y * iA - _rB.y * iB;
_mass.ex.y = _mass.ey.x;
_mass.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
_mass.ez.y = _rA.x * iA + _rB.x * iB;
_mass.ex.z = _mass.ez.x;
_mass.ey.z = _mass.ez.y;
_mass.ez.z = iA + iB;
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false || fixedRotation)
{
_motorImpulse = 0.0f;
}
if (_enableLimit && fixedRotation == false)
{
FP jointAngle = aB - aA - ReferenceAngle;
if (FP.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
TSVector2 P = new TSVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.z);
}
else
{
_impulse = TSVector.zero;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
private static List <Vertices> TriangulatePolygon(Vertices vertices, FP tolerance)
{
bool flag = vertices.Count < 3;
List <Vertices> result;
if (flag)
{
result = new List <Vertices>();
}
else
{
List <Vertices> list = new List <Vertices>();
Vertices pin = new Vertices(vertices);
Vertices vertices2;
Vertices vertices3;
bool flag2 = EarclipDecomposer.ResolvePinchPoint(pin, out vertices2, out vertices3, tolerance);
if (flag2)
{
List <Vertices> list2 = EarclipDecomposer.TriangulatePolygon(vertices2, tolerance);
List <Vertices> list3 = EarclipDecomposer.TriangulatePolygon(vertices3, tolerance);
bool flag3 = list2.Count == -1 || list3.Count == -1;
if (flag3)
{
throw new Exception("Can't triangulate your polygon.");
}
for (int i = 0; i < list2.Count; i++)
{
list.Add(new Vertices(list2[i]));
}
for (int j = 0; j < list3.Count; j++)
{
list.Add(new Vertices(list3[j]));
}
result = list;
}
else
{
Vertices[] array = new Vertices[vertices.Count - 2];
int num = 0;
FP[] array2 = new FP[vertices.Count];
FP[] array3 = new FP[vertices.Count];
for (int k = 0; k < vertices.Count; k++)
{
array2[k] = vertices[k].x;
array3[k] = vertices[k].y;
}
int l = vertices.Count;
while (l > 3)
{
int num2 = -1;
FP y = -10f;
for (int m = 0; m < l; m++)
{
bool flag4 = EarclipDecomposer.IsEar(m, array2, array3, l);
if (flag4)
{
int num3 = EarclipDecomposer.Remainder(m - 1, l);
int num4 = EarclipDecomposer.Remainder(m + 1, l);
TSVector2 tSVector = new TSVector2(array2[num4] - array2[m], array3[num4] - array3[m]);
TSVector2 tSVector2 = new TSVector2(array2[m] - array2[num3], array3[m] - array3[num3]);
TSVector2 tSVector3 = new TSVector2(array2[num3] - array2[num4], array3[num3] - array3[num4]);
tSVector.Normalize();
tSVector2.Normalize();
tSVector3.Normalize();
FP fP;
MathUtils.Cross(ref tSVector, ref tSVector2, out fP);
fP = FP.Abs(fP);
FP fP2;
MathUtils.Cross(ref tSVector2, ref tSVector3, out fP2);
fP2 = FP.Abs(fP2);
FP fP3;
MathUtils.Cross(ref tSVector3, ref tSVector, out fP3);
fP3 = FP.Abs(fP3);
FP fP4 = TSMath.Min(fP, TSMath.Min(fP2, fP3));
bool flag5 = fP4 > y;
if (flag5)
{
num2 = m;
y = fP4;
}
}
}
bool flag6 = num2 == -1;
if (flag6)
{
for (int n = 0; n < num; n++)
{
list.Add(array[n]);
}
result = list;
return(result);
}
l--;
FP[] array4 = new FP[l];
FP[] array5 = new FP[l];
int num5 = 0;
for (int num6 = 0; num6 < l; num6++)
{
bool flag7 = num5 == num2;
if (flag7)
{
num5++;
}
array4[num6] = array2[num5];
array5[num6] = array3[num5];
num5++;
}
int num7 = (num2 == 0) ? l : (num2 - 1);
int num8 = (num2 == l) ? 0 : (num2 + 1);
EarclipDecomposer.Triangle triangle = new EarclipDecomposer.Triangle(array2[num2], array3[num2], array2[num8], array3[num8], array2[num7], array3[num7]);
array[num] = triangle;
num++;
array2 = array4;
array3 = array5;
}
EarclipDecomposer.Triangle triangle2 = new EarclipDecomposer.Triangle(array2[1], array3[1], array2[2], array3[2], array2[0], array3[0]);
array[num] = triangle2;
num++;
for (int num9 = 0; num9 < num; num9++)
{
list.Add(new Vertices(array[num9]));
}
result = list;
}
}
return(result);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
TSVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
{
FP Cdot = wB - wA - _motorSpeed;
FP impulse = _motorMass * (-Cdot);
FP oldImpulse = _motorImpulse;
FP maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
TSVector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
FP Cdot2 = wB - wA;
TSVector Cdot = new TSVector(Cdot1.x, Cdot1.y, Cdot2);
TSVector impulse = _mass.Solve33(Cdot) * -1;
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
FP newImpulse = _impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
TSVector2 rhs = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y);
TSVector2 reduced = _mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
else
{
_impulse += impulse;
}
}
else if (_limitState == LimitState.AtUpper)
{
FP newImpulse = _impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
TSVector2 rhs = -Cdot1 + _impulse.z * new TSVector2(_mass.ez.x, _mass.ez.y);
TSVector2 reduced = _mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
else
{
_impulse += impulse;
}
}
TSVector2 P = new TSVector2(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
TSVector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
TSVector2 impulse = _mass.Solve22(-Cdot);
_impulse.x += impulse.x;
_impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
return(0);
}
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
{
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, warmstarting);
// 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.
FP h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
TSVector2 c = _positions[i].c;
FP a = _positions[i].a;
TSVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
TSVector2 translation = h * v;
if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.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);
}