public static RectInt ToRect(this SDFRawData sdf, TSVector2 start, TSVector2 end)
{
TSVector2 min = new TSVector2(TSMath.Min(start.x, end.x), TSMath.Min(start.y, end.y));
TSVector2 max = new TSVector2(TSMath.Max(start.x, end.x), TSMath.Max(start.y, end.y));
return(new RectInt(sdf.FloorToGrid(min), sdf.CeilingToGrid(max)));
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
TSVector2 value = tSVector + MathUtils.Cross(fP, this._rA);
TSVector2 value2 = tSVector2 + MathUtils.Cross(fP2, this._rB);
FP fP3 = this._length - this.MaxLength;
FP fP4 = TSVector2.Dot(this._u, value2 - value);
bool flag = fP3 < 0f;
if (flag)
{
fP4 += data.step.inv_dt * fP3;
}
FP fP5 = -this._mass * fP4;
FP impulse = this._impulse;
this._impulse = TSMath.Min(0f, this._impulse + fP5);
fP5 = this._impulse - impulse;
TSVector2 tSVector3 = fP5 * 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;
}
/** Bias towards the right side of agents.
* Rotate desiredVelocity at most [value] number of radians. 1 radian ≈ 57°
* This breaks up symmetries.
*
* The desired velocity will only be rotated if it is inside a velocity obstacle (VO).
* If it is inside one, it will not be rotated further than to the edge of it
*
* The targetPointInVelocitySpace will be rotated by the same amount as the desired velocity
*
* \returns True if the desired velocity was inside any VO
*/
static bool BiasDesiredVelocity(VOBuffer vos, ref TSVector2 desiredVelocity, ref TSVector2 targetPointInVelocitySpace, FP maxBiasRadians)
{
var desiredVelocityMagn = desiredVelocity.magnitude;
FP maxValue = FP.Zero;
for (int i = 0; i < vos.length; i++)
{
FP value;
// The value is approximately the distance to the edge of the VO
// so taking the maximum will give us the distance to the edge of the VO
// which the desired velocity is furthest inside
vos.buffer[i].Gradient(desiredVelocity, out value);
maxValue = TSMath.Max(maxValue, value);
}
// Check if the agent was inside any VO
var inside = maxValue > 0;
// Avoid division by zero below
if (desiredVelocityMagn < FP.One / 1000)
{
return(inside);
}
// Rotate the desired velocity clockwise (to the right) at most maxBiasRadians number of radians
// Assuming maxBiasRadians is small, we can just move it instead and it will give approximately the same effect
// See https://en.wikipedia.org/wiki/Small-angle_approximation
var angle = TSMath.Min(maxBiasRadians, maxValue / desiredVelocityMagn);
desiredVelocity += new TSVector2(desiredVelocity.y, -desiredVelocity.x) * angle;
targetPointInVelocitySpace += new TSVector2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle;
return(inside);
}
internal void Extend(Envelope by)
{
X1 = TSMath.Min(X1, by.X1);
Y1 = TSMath.Min(Y1, by.Y1);
X2 = TSMath.Max(X2, by.X2);
Y2 = TSMath.Max(Y2, by.Y2);
}
/** \copydoc Pathfinding::RVO::IAgent::SetTarget */
public void SetTarget(TSVector2 targetPoint, FP desiredSpeed, FP maxSpeed)
{
maxSpeed = TSMath.Max(maxSpeed, 0);
desiredSpeed = TSMath.Min(TSMath.Max(desiredSpeed, 0), maxSpeed);
nextTargetPoint = targetPoint;
nextDesiredSpeed = desiredSpeed;
nextMaxSpeed = maxSpeed;
}
private static FP IntersectionArea(Envelope what, Envelope with)
{
var minX = TSMath.Max(what.X1, with.X1);
var minY = TSMath.Max(what.Y1, with.Y1);
var maxX = TSMath.Min(what.X2, with.X2);
var maxY = TSMath.Min(what.Y2, with.Y2);
return(TSMath.Max(0, maxX - minX) * TSMath.Max(0, maxY - minY));
}
private static FP CombinedArea(Envelope what, Envelope with)
{
var minX1 = TSMath.Max(what.X1, with.X1);
var minY1 = TSMath.Max(what.Y1, with.Y1);
var maxX2 = TSMath.Min(what.X2, with.X2);
var maxY2 = TSMath.Min(what.Y2, with.Y2);
return((maxX2 - minX1) * (maxY2 - minY1));
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref TSVector point1, ref TSVector point2, ref TSVector n,
FP penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
TSVector.Subtract(ref p1, ref body1.position, out relativePos1);
TSVector.Subtract(ref p2, ref body2.position, out relativePos2);
TSVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
TSVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
CBFrame.Utils.Logger.Debug("line812 body2.isStatic:" + body2.isStatic + ",body1.isStatic:" + body1.isStatic);
accumulatedNormalImpulse = FP.Zero;
accumulatedTangentImpulse = FP.Zero;
lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Max(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Min(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
restitution = (body1.restitution + body2.restitution) * FP.Half;
break;
}
}
this.settings = settings;
}
//旋转的box
public static FP SDOrientedBox(TSVector2 x, TSVector2 c, TSVector2 rot, TSVector2 b)
{
TSVector2 v = x - c;
FP px = TSMath.Abs(TSVector2.Dot(v, rot)); //在box的x轴的投影长度
FP py = TSMath.Abs(TSVector2.Dot(v, new TSVector2(-rot.y, rot.x))); //在box的y轴的投影长度
TSVector2 p = new TSVector2(px, py);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
//不旋转的box
public static FP SDBox(TSVector2 x, TSVector2 c, TSVector2 b)
{
TSVector2 p = x - c;
p.x = TSMath.Abs(p.x);
p.y = TSMath.Abs(p.y);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
public void QueryRec(int i, QTBound r)
{
var radius = TSMath.Min(TSMath.Max((nodes[i].maxSpeed + speed) * timeHorizon, TRadius), maxRadius); //+ TRadius,warning
if (nodes[i].childNode1 == i)
{
// Leaf node
for (T a = nodes[i].nextData; a != null; a = (T)a.next)
{
FP v = T.InsertNeighbour(a, radius * radius);
//
if (v < maxRadius * maxRadius)
{
maxRadius = TSMath.Sqrt(v);
}
}
}
else
{
TSVector min = TSVector.zero, max = TSVector.zero;
// Not a leaf node
TSVector c = r.center;
if (p.x - radius < c.x)
{
if (p.z - radius < c.z)
{
QueryRec(nodes[i].childNode1, QTBound.MinMaxQTBound(r.min, c));
radius = TSMath.Min(radius, maxRadius);
}
if (p.z + radius > c.z)
{
min.Set(r.min.x, 0, c.z);
max.Set(c.x, 0, r.max.z);
QueryRec(nodes[i].childNode2, QTBound.MinMaxQTBound(min, max));
radius = TSMath.Min(radius, maxRadius);
}
}
if (p.x + radius > c.x)
{
if (p.z - radius < c.z)
{
max.Set(r.max.x, 0, c.z);
min.Set(c.x, 0, r.min.z);
QueryRec(nodes[i].childNode3, QTBound.MinMaxQTBound(min, max));
radius = TSMath.Min(radius, maxRadius);
}
if (p.z + radius > c.z)
{
QueryRec(nodes[i].childNode4, QTBound.MinMaxQTBound(c, r.max));
}
}
}
}
/// <summary>
/// Passes a axis aligned bounding box to the shape where collision
/// could occour.
/// </summary>
/// <param name="box">The bounding box where collision could occur.</param>
/// <returns>The upper index with which <see cref="SetCurrentShape"/> can be
/// called.</returns>
public override int Prepare(ref TSBBox box)
{
// simple idea: the terrain is a grid. x and z is the position in the grid.
// y the height. we know compute the min and max grid-points. All quads
// between these points have to be checked.
// including overflow exception prevention
if (box.min.x < boundings.min.x)
{
minX = 0;
}
else
{
minX = (int)FP.Floor(((box.min.x - sphericalExpansion) / scaleX));
minX = (int)TSMath.Max(minX, 0);
}
if (box.max.x > boundings.max.x)
{
maxX = heightsLength0 - 1;
}
else
{
maxX = (int)FP.Ceiling(((box.max.x + sphericalExpansion) / scaleX));
maxX = (int)TSMath.Min(maxX, heightsLength0 - 1);
}
if (box.min.z < boundings.min.z)
{
minZ = 0;
}
else
{
minZ = (int)FP.Floor(((box.min.z - sphericalExpansion) / scaleZ));
minZ = (int)TSMath.Max(minZ, 0);
}
if (box.max.z > boundings.max.z)
{
maxZ = heightsLength1 - 1;
}
else
{
maxZ = (int)FP.Ceiling((FP)((box.max.z + sphericalExpansion) / scaleZ));
maxZ = (int)TSMath.Min(maxZ, heightsLength1 - 1);
}
numX = maxX - minX;
numZ = maxZ - minZ;
// since every quad contains two triangles we multiply by 2.
return(numX * numZ * 2);
}
public static bool rectsIntersect(TSVector2 S1, TSVector2 E1, TSVector2 S2, TSVector2 E2)
{
if (TSMath.Min(S1.y, E1.y) <= TSMath.Max(S2.y, E2.y) &&
TSMath.Max(S1.y, E1.y) >= TSMath.Min(S2.y, E2.y) &&
TSMath.Min(S1.x, E1.x) <= TSMath.Max(S2.x, E2.x) &&
TSMath.Max(S1.x, E1.x) >= TSMath.Min(S2.x, E2.x))
{
return(true);
}
return(false);
}
public bool SolvePositionConstraints()
{
FP fP = 0f;
for (int i = 0; i < this._count; i++)
{
ContactPositionConstraint contactPositionConstraint = this._positionConstraints[i];
int indexA = contactPositionConstraint.indexA;
int indexB = contactPositionConstraint.indexB;
TSVector2 localCenterA = contactPositionConstraint.localCenterA;
FP invMassA = contactPositionConstraint.invMassA;
FP invIA = contactPositionConstraint.invIA;
TSVector2 localCenterB = contactPositionConstraint.localCenterB;
FP invMassB = contactPositionConstraint.invMassB;
FP invIB = contactPositionConstraint.invIB;
int pointCount = contactPositionConstraint.pointCount;
TSVector2 tSVector = this._positions[indexA].c;
FP fP2 = this._positions[indexA].a;
TSVector2 tSVector2 = this._positions[indexB].c;
FP fP3 = this._positions[indexB].a;
for (int j = 0; j < pointCount; j++)
{
Transform transform = default(Transform);
Transform transform2 = default(Transform);
transform.q.Set(fP2);
transform2.q.Set(fP3);
transform.p = tSVector - MathUtils.Mul(transform.q, localCenterA);
transform2.p = tSVector2 - MathUtils.Mul(transform2.q, localCenterB);
TSVector2 tSVector3;
TSVector2 value;
FP fP4;
ContactSolver.PositionSolverManifold.Initialize(contactPositionConstraint, transform, transform2, j, out tSVector3, out value, out fP4);
TSVector2 a = value - tSVector;
TSVector2 a2 = value - tSVector2;
fP = TSMath.Min(fP, fP4);
FP x = MathUtils.Clamp(Settings.Baumgarte * (fP4 + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0f);
FP y = MathUtils.Cross(a, tSVector3);
FP y2 = MathUtils.Cross(a2, tSVector3);
FP fP5 = invMassA + invMassB + invIA * y * y + invIB * y2 * y2;
FP scaleFactor = (fP5 > 0f) ? (-x / fP5) : 0f;
TSVector2 tSVector4 = scaleFactor * tSVector3;
tSVector -= invMassA * tSVector4;
fP2 -= invIA * MathUtils.Cross(a, tSVector4);
tSVector2 += invMassB * tSVector4;
fP3 += invIB * MathUtils.Cross(a2, tSVector4);
}
this._positions[indexA].c = tSVector;
this._positions[indexA].a = fP2;
this._positions[indexB].c = tSVector2;
this._positions[indexB].a = fP3;
}
return(fP >= -3f * Settings.LinearSlop);
}
internal void CalculateVelocity(WorkerContext context, FP deltaTime)
{
if (manuallyControlled)
{
return;
}
if (locked)
{
calculatedSpeed = 0;
calculatedTargetPoint = position;
return;
}
// Buffer which will be filled up with velocity obstacles (VOs)
var vos = context.vos;
vos.Clear();
//GenerateObstacleVOs(vos);
#if USING_RVO
GenerateNeighbourAgentVOs(vos, deltaTime);
#endif
bool insideAnyVO = BiasDesiredVelocity(vos, ref desiredVelocity, ref desiredTargetPointInVelocitySpace, FP.One / 10);
if (!insideAnyVO)
{
// Desired velocity can be used directly since it was not inside any velocity obstacle.
// No need to run optimizer because this will be the global minima.
// This is also a special case in which we can set the
// calculated target point to the desired target point
// instead of calculating a point based on a calculated velocity
// which is an important difference when the agent is very close
// to the target point
// TODO: Not actually guaranteed to be global minima if desiredTargetPointInVelocitySpace.magnitude < desiredSpeed
// maybe do something different here?
calculatedTargetPoint = desiredTargetPointInVelocitySpace + position;
calculatedSpeed = desiredSpeed;
// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(calculatedTargetPoint), Color.white);
return;
}
TSVector2 result = TSVector2.zero;
result = GradientDescent(vos, currentVelocity, desiredVelocity);
// if (DebugDraw) Draw.Debug.CrossXZ(FromXZ(result + position), Color.white);
//Debug.DrawRay (To3D (position), To3D (result));
calculatedTargetPoint = position + result;
calculatedSpeed = TSMath.Min(result.magnitude, maxSpeed);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += TSVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
TSVector temp;
CBFrame.Utils.Logger.Debug("line195 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
TSVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
TSVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line201 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
TSVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
TSVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line206 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
CBFrame.Utils.Logger.Debug("line202 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += TSVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
body2.angularVelocity += TSVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line213 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(body1.linearVelocity, jacobian[0]);
jv += TSVector.Dot(body2.linearVelocity, jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
TSVector temp;
if (!body1.isStatic)
{
body1.ApplyImpulse(jacobian[0] * lambda);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(jacobian[1] * lambda);
}
}
public FP Distance(FP x, FP y)
{
FP distanceSquared = 0;
FP greatestMin = TSMath.Max(X1, x);
FP leastMax = TSMath.Min(X2, x);
if (greatestMin > leastMax)
{
distanceSquared += TSMath.Pow(greatestMin - leastMax, 2);
}
greatestMin = TSMath.Max(Y1, y);
leastMax = TSMath.Min(Y2, y);
if (greatestMin > leastMax)
{
distanceSquared += TSMath.Pow(greatestMin - leastMax, 2);
}
return((FP)TSMath.Sqrt(distanceSquared));
}
private Dictionary <Body, TSVector2> ApplyImpulse(TSVector2 pos, FP radius, FP force, FP maxForce, HashSet <Body> overlappingBodies)
{
Dictionary <Body, TSVector2> forces = new Dictionary <Body, TSVector2>(overlappingBodies.Count);
foreach (Body overlappingBody in overlappingBodies)
{
if (IsActiveOn(overlappingBody))
{
FP distance = TSVector2.Distance(pos, overlappingBody.Position);
FP forcePercent = GetPercent(distance, radius);
TSVector2 forceVector = pos - overlappingBody.Position;
forceVector *= 1f / FP.Sqrt(forceVector.x * forceVector.x + forceVector.y * forceVector.y);
forceVector *= TSMath.Min(force * forcePercent, maxForce);
forceVector *= -1;
overlappingBody.ApplyLinearImpulse(forceVector);
forces.Add(overlappingBody, forceVector);
}
}
return(forces);
}
/// <summary>
/// Gets the avoid direction vector.
/// </summary>
/// <param name="selfPos">This unit's position.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <param name="otherPos">The other unit's position.</param>
/// <param name="otherVelocity">The other unit's velocity.</param>
/// <param name="combinedRadius">The combined radius.</param>
/// <returns>An avoidance direction vector, if a collision is detected.</returns>
private static TSVector GetAvoidDirectionVector(TSVector selfPos, TSVector currentVelocity, TSVector otherPos, TSVector otherVelocity, FP combinedRadius, out TSVector selfCollisionPos)
{
selfCollisionPos = TSVector.zero;
// use a 2nd degree polynomial function to determine intersection points between moving units with a velocity and radius
FP a = ((currentVelocity.x - otherVelocity.x) * (currentVelocity.x - otherVelocity.x)) +
((currentVelocity.z - otherVelocity.z) * (currentVelocity.z - otherVelocity.z));
FP b = (2 * (selfPos.x - otherPos.x) * (currentVelocity.x - otherVelocity.x)) +
(2 * (selfPos.z - otherPos.z) * (currentVelocity.z - otherVelocity.z));
FP c = ((selfPos.x - otherPos.x) * (selfPos.x - otherPos.x)) +
((selfPos.z - otherPos.z) * (selfPos.z - otherPos.z)) -
(combinedRadius * combinedRadius);
FP d = (b * b) - (4 * a * c);
if (d <= 0)
{
// if there are not 2 intersection points, then skip
return(TSVector.zero);
}
// compute "heavy" calculations only once
FP dSqrt = TSMath.Sqrt(d);
FP doubleA = 2 * a;
// compute roots, which in this case are actually time values informing of when the collision starts and ends
FP t1 = (-b + dSqrt) / doubleA;
FP t2 = (-b - dSqrt) / doubleA;
if (t1 < 0 && t2 < 0)
{
// if both times are negative, the collision is behind us (compared to velocity direction)
return(TSVector.zero);
}
// find the lowest non-negative time, since this will be where the collision time interval starts
FP time = 0;
if (t1 < 0)
{
time = t2;
}
else if (t2 < 0)
{
time = t1;
}
else
{
time = TSMath.Min(t1, t2);
}
// the collision time we want is actually 25 % within the collision
time += TSMath.Abs(t2 - t1) * _collisionTimeFactor;
// compute actual collision positions
selfCollisionPos = selfPos + (currentVelocity * time);
// _selfCollisionPos = selfCollisionPos;
TSVector otherCollisionPos = otherPos + (otherVelocity * time);
// _lastAvoidPos = otherPos;
// return an avoid vector from the other's collision position to this unit's collision position
return(otherCollisionPos - selfCollisionPos);
}
public void Solve(ref TimeStep step, ref TSVector2 gravity)
{
FP dt = step.dt;
for (int i = 0; i < this.BodyCount; i++)
{
Body body = this.Bodies[i];
TSVector2 c = body._sweep.C;
FP a = body._sweep.A;
TSVector2 tSVector = body._linearVelocity;
FP fP = body._angularVelocity;
body._sweep.C0 = body._sweep.C;
body._sweep.A0 = body._sweep.A;
bool flag = body.BodyType == BodyType.Dynamic;
if (flag)
{
bool ignoreGravity = body.IgnoreGravity;
if (ignoreGravity)
{
tSVector += dt * (body._invMass * body._force);
}
else
{
tSVector += dt * (body.GravityScale * gravity + body._invMass * body._force);
}
fP += dt * body._invI * body._torque;
tSVector *= MathUtils.Clamp(1f - dt * body.LinearDamping, 0f, 1f);
fP *= MathUtils.Clamp(1f - dt * body.AngularDamping, 0f, 1f);
}
this._positions[i].c = c;
this._positions[i].a = a;
this._velocities[i].v = tSVector;
this._velocities[i].w = fP;
}
SolverData solverData = default(SolverData);
solverData.step = step;
solverData.positions = this._positions;
solverData.velocities = this._velocities;
this._contactSolver.Reset(step, this.ContactCount, this._contacts, this._positions, this._velocities, true);
this._contactSolver.InitializeVelocityConstraints();
this._contactSolver.WarmStart();
for (int j = 0; j < this.JointCount; j++)
{
bool enabled = this._joints[j].Enabled;
if (enabled)
{
this._joints[j].InitVelocityConstraints(ref solverData);
}
}
for (int k = 0; k < Settings.VelocityIterations; k++)
{
for (int l = 0; l < this.JointCount; l++)
{
Joint joint = this._joints[l];
bool flag2 = !joint.Enabled;
if (!flag2)
{
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
}
}
this._contactSolver.SolveVelocityConstraints();
}
this._contactSolver.StoreImpulses();
for (int m = 0; m < this.BodyCount; m++)
{
TSVector2 tSVector2 = this._positions[m].c;
FP fP2 = this._positions[m].a;
TSVector2 tSVector3 = this._velocities[m].v;
FP fP3 = this._velocities[m].w;
TSVector2 tSVector4 = dt * tSVector3;
bool flag3 = TSVector2.Dot(tSVector4, tSVector4) > Settings.MaxTranslationSquared;
if (flag3)
{
FP scaleFactor = Settings.MaxTranslation / tSVector4.magnitude;
tSVector3 *= scaleFactor;
}
FP fP4 = dt * fP3;
bool flag4 = fP4 * fP4 > Settings.MaxRotationSquared;
if (flag4)
{
FP y = Settings.MaxRotation / FP.Abs(fP4);
fP3 *= y;
}
tSVector2 += dt * tSVector3;
fP2 += dt * fP3;
this._positions[m].c = tSVector2;
this._positions[m].a = fP2;
this._velocities[m].v = tSVector3;
this._velocities[m].w = fP3;
}
bool flag5 = false;
for (int n = 0; n < Settings.PositionIterations; n++)
{
bool flag6 = this._contactSolver.SolvePositionConstraints();
bool flag7 = true;
for (int num = 0; num < this.JointCount; num++)
{
Joint joint2 = this._joints[num];
bool flag8 = !joint2.Enabled;
if (!flag8)
{
bool flag9 = joint2.SolvePositionConstraints(ref solverData);
flag7 &= flag9;
}
}
bool flag10 = flag6 & flag7;
if (flag10)
{
flag5 = true;
break;
}
}
for (int num2 = 0; num2 < this.BodyCount; num2++)
{
Body body2 = this.Bodies[num2];
body2._sweep.C = this._positions[num2].c;
body2._sweep.A = this._positions[num2].a;
body2._linearVelocity = this._velocities[num2].v;
body2._angularVelocity = this._velocities[num2].w;
body2.SynchronizeTransform();
}
this.Report(this._contactSolver._velocityConstraints);
bool allowSleep = Settings.AllowSleep;
if (allowSleep)
{
FP fP5 = Settings.MaxFP;
for (int num3 = 0; num3 < this.BodyCount; num3++)
{
Body body3 = this.Bodies[num3];
bool flag11 = body3.BodyType == BodyType.Static;
if (!flag11)
{
bool flag12 = !body3.SleepingAllowed || body3._angularVelocity * body3._angularVelocity > Island.AngTolSqr || TSVector2.Dot(body3._linearVelocity, body3._linearVelocity) > Island.LinTolSqr;
if (flag12)
{
body3._sleepTime = 0f;
fP5 = 0f;
}
else
{
body3._sleepTime += dt;
fP5 = TSMath.Min(fP5, body3._sleepTime);
}
}
}
bool flag13 = fP5 >= Settings.TimeToSleep & flag5;
if (flag13)
{
for (int num4 = 0; num4 < this.BodyCount; num4++)
{
Body body4 = this.Bodies[num4];
body4.Awake = false;
}
}
}
}
public static FP Min(FP value1, FP value2)
{
return(TSMath.Min(value1, value2));
}
public static bool LineIntersect2(ref TSVector2 a0, ref TSVector2 a1, ref TSVector2 b0, ref TSVector2 b1, out TSVector2 intersectionPoint)
{
intersectionPoint = TSVector2.zero;
bool flag = a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1;
bool result;
if (flag)
{
result = false;
}
else
{
FP x = a0.x;
FP y = a0.y;
FP x2 = a1.x;
FP y2 = a1.y;
FP x3 = b0.x;
FP y3 = b0.y;
FP x4 = b1.x;
FP y4 = b1.y;
bool flag2 = TSMath.Max(x, x2) < TSMath.Min(x3, x4) || TSMath.Max(x3, x4) < TSMath.Min(x, x2);
if (flag2)
{
result = false;
}
else
{
bool flag3 = TSMath.Max(y, y2) < TSMath.Min(y3, y4) || TSMath.Max(y3, y4) < TSMath.Min(y, y2);
if (flag3)
{
result = false;
}
else
{
FP fP = (x4 - x3) * (y - y3) - (y4 - y3) * (x - x3);
FP fP2 = (x2 - x) * (y - y3) - (y2 - y) * (x - x3);
FP fP3 = (y4 - y3) * (x2 - x) - (x4 - x3) * (y2 - y);
bool flag4 = FP.Abs(fP3) < Settings.Epsilon;
if (flag4)
{
result = false;
}
else
{
fP /= fP3;
fP2 /= fP3;
bool flag5 = 0 < fP && fP < 1 && 0 < fP2 && fP2 < 1;
if (flag5)
{
intersectionPoint.x = x + fP * (x2 - x);
intersectionPoint.y = y + fP * (y2 - y);
result = true;
}
else
{
result = false;
}
}
}
}
}
return(result);
}
public static TSVector2 Min(TSVector2 value1, TSVector2 value2)
{
return(new TSVector2(
TSMath.Min(value1.x, value2.x),
TSMath.Min(value1.y, value2.y)));
}
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
FP fP = 0f;
for (int i = 0; i < this._count; i++)
{
ContactPositionConstraint contactPositionConstraint = this._positionConstraints[i];
int indexA = contactPositionConstraint.indexA;
int indexB = contactPositionConstraint.indexB;
TSVector2 localCenterA = contactPositionConstraint.localCenterA;
TSVector2 localCenterB = contactPositionConstraint.localCenterB;
int pointCount = contactPositionConstraint.pointCount;
FP fP2 = 0f;
FP x = 0f;
bool flag = indexA == toiIndexA || indexA == toiIndexB;
if (flag)
{
fP2 = contactPositionConstraint.invMassA;
x = contactPositionConstraint.invIA;
}
FP fP3 = 0f;
FP x2 = 0f;
bool flag2 = indexB == toiIndexA || indexB == toiIndexB;
if (flag2)
{
fP3 = contactPositionConstraint.invMassB;
x2 = contactPositionConstraint.invIB;
}
TSVector2 tSVector = this._positions[indexA].c;
FP fP4 = this._positions[indexA].a;
TSVector2 tSVector2 = this._positions[indexB].c;
FP fP5 = this._positions[indexB].a;
for (int j = 0; j < pointCount; j++)
{
Transform transform = default(Transform);
Transform transform2 = default(Transform);
transform.q.Set(fP4);
transform2.q.Set(fP5);
transform.p = tSVector - MathUtils.Mul(transform.q, localCenterA);
transform2.p = tSVector2 - MathUtils.Mul(transform2.q, localCenterB);
TSVector2 tSVector3;
TSVector2 value;
FP fP6;
ContactSolver.PositionSolverManifold.Initialize(contactPositionConstraint, transform, transform2, j, out tSVector3, out value, out fP6);
TSVector2 a = value - tSVector;
TSVector2 a2 = value - tSVector2;
fP = TSMath.Min(fP, fP6);
FP x3 = MathUtils.Clamp(Settings.Baumgarte * (fP6 + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0f);
FP y = MathUtils.Cross(a, tSVector3);
FP y2 = MathUtils.Cross(a2, tSVector3);
FP fP7 = fP2 + fP3 + x * y * y + x2 * y2 * y2;
FP scaleFactor = (fP7 > 0f) ? (-x3 / fP7) : 0f;
TSVector2 tSVector4 = scaleFactor * tSVector3;
tSVector -= fP2 * tSVector4;
fP4 -= x * MathUtils.Cross(a, tSVector4);
tSVector2 += fP3 * tSVector4;
fP5 += x2 * MathUtils.Cross(a2, tSVector4);
}
this._positions[indexA].c = tSVector;
this._positions[indexA].a = fP4;
this._positions[indexB].c = tSVector2;
this._positions[indexB].a = fP5;
}
return(fP >= -1.5f * Settings.LinearSlop);
}
//void GenerateObstacleVOs(VOBuffer vos)
//{
// var range = maxSpeed * obstacleTimeHorizon;
// // Iterate through all obstacles that we might need to avoid
// for (int i = 0; i < simulator.obstacles.Count; i++)
// {
// var obstacle = simulator.obstacles[i];
// var vertex = obstacle;
// // Iterate through all edges (defined by vertex and vertex.dir) in the obstacle
// do
// {
// // Ignore the edge if the agent should not collide with it
// if (vertex.ignore || (vertex.layer & collidesWith) == 0)
// {
// vertex = vertex.next;
// continue;
// }
// // Start and end points of the current segment
// FP elevation1, elevation2;
// var p1 = To2D(vertex.position, out elevation1);
// var p2 = To2D(vertex.next.position, out elevation2);
// TSVector2 dir = (p2 - p1).normalized;
// // Signed distance from the line (not segment, lines are infinite)
// // TODO: Can be optimized
// FP dist = VO.SignedDistanceFromLine(p1, dir, position);
// if (dist >= -FP.One/100 && dist < range)
// {
// FP factorAlongSegment = TSVector2.Dot(position - p1, p2 - p1) / (p2 - p1).sqrMagnitude;
// // Calculate the elevation (y) coordinate of the point on the segment closest to the agent
// var segmentY = TSMath.Lerp(elevation1, elevation2, factorAlongSegment);
// // Calculate distance from the segment (not line)
// var sqrDistToSegment = (TSVector2.Lerp(p1, p2, factorAlongSegment) - position).sqrMagnitude;
// // Ignore the segment if it is too far away
// // or the agent is too high up (or too far down) on the elevation axis (usually y axis) to avoid it.
// // If the XY plane is used then all elevation checks are disabled
// if (sqrDistToSegment < range * range && (simulator.movementPlane == MovementPlane.XY || (elevationCoordinate <= segmentY + vertex.height && elevationCoordinate + height >= segmentY)))
// {
// vos.Add(VO.SegmentObstacle(p2 - position, p1 - position, TSVector2.zero, radius * FP.One/100, 1 / ObstacleTimeHorizon, 1 / simulator.DeltaTime));
// }
// }
// vertex = vertex.next;
// } while (vertex != obstacle && vertex != null && vertex.next != null);
// }
//}
#if USING_RVO
void GenerateNeighbourAgentVOs(VOBuffer vos, FP deltaTime)
{
FP inverseAgentTimeHorizon = 1 / agentTimeHorizon;
// The RVO algorithm assumes we will continue to
// move in roughly the same direction
TSVector2 optimalVelocity = currentVelocity;
int count = _behaviour.neighbours.Count;
for (int o = 0; o < count; o++)
{
IAgentBehaviour other = _behaviour.neighbours[o];
// Don't avoid ourselves
if (other == this)
{
continue;
}
// Interval along the y axis in which the agents overlap
FP maxY = TSMath.Min(elevationCoordinate + height, other.OrcaAgent.elevationCoordinate + other.OrcaAgent.height);
FP minY = TSMath.Max(elevationCoordinate, other.OrcaAgent.elevationCoordinate);
// The agents cannot collide since they are on different y-levels
if (maxY - minY < 0)
{
continue;
}
FP totalRadius = radius + other.colliderRadius;
// Describes a circle on the border of the VO
TSVector2 voBoundingOrigin = CustomMath.TSVecToVec2(other.position - _behaviour.position);
FP avoidanceStrength;
if (other.OrcaAgent.locked || other.OrcaAgent.manuallyControlled)
{
avoidanceStrength = 1;
}
else if (other.OrcaAgent.Priority > CustomMath.EPSILON || Priority > CustomMath.EPSILON)
{
avoidanceStrength = other.OrcaAgent.Priority / (Priority + other.OrcaAgent.Priority);
}
else
{
// Both this agent's priority and the other agent's priority is zero or negative
// Assume they have the same priority
avoidanceStrength = CustomMath.FPHalf;
}
// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
TSVector2 otherOptimalVelocity = TSVector2.Lerp(other.OrcaAgent.currentVelocity, other.OrcaAgent.desiredVelocity, 2 * avoidanceStrength - 1);
var voCenter = TSVector2.Lerp(optimalVelocity, otherOptimalVelocity, avoidanceStrength);
vos.Add(new VO(voBoundingOrigin, voCenter, totalRadius, inverseAgentTimeHorizon, 1 / deltaTime));
if (DebugDraw)
{
DrawVO(position + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position + voCenter);
}
}
}
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
output = default(RayCastOutput);
FP fP = -Settings.MaxFP;
FP fP2 = Settings.MaxFP;
TSVector2 point = input.Point1;
TSVector2 tSVector = input.Point2 - input.Point1;
TSVector2 tSVector2 = MathUtils.Abs(tSVector);
TSVector2 zero = TSVector2.zero;
bool result;
for (int i = 0; i < 2; i++)
{
FP x = (i == 0) ? tSVector2.x : tSVector2.y;
FP fP3 = (i == 0) ? this.LowerBound.x : this.LowerBound.y;
FP x2 = (i == 0) ? this.UpperBound.x : this.UpperBound.y;
FP fP4 = (i == 0) ? point.x : point.y;
bool flag = x < Settings.Epsilon;
if (flag)
{
bool flag2 = fP4 < fP3 || x2 < fP4;
if (flag2)
{
result = false;
return(result);
}
}
else
{
FP y = (i == 0) ? tSVector.x : tSVector.y;
FP y2 = 1f / y;
FP fP5 = (fP3 - fP4) * y2;
FP fP6 = (x2 - fP4) * y2;
FP fP7 = -1f;
bool flag3 = fP5 > fP6;
if (flag3)
{
MathUtils.Swap <FP>(ref fP5, ref fP6);
fP7 = 1f;
}
bool flag4 = fP5 > fP;
if (flag4)
{
bool flag5 = i == 0;
if (flag5)
{
zero.x = fP7;
}
else
{
zero.y = fP7;
}
fP = fP5;
}
fP2 = TSMath.Min(fP2, fP6);
bool flag6 = fP > fP2;
if (flag6)
{
result = false;
return(result);
}
}
}
bool flag7 = doInteriorCheck && (fP < 0f || input.MaxFraction < fP);
if (flag7)
{
result = false;
return(result);
}
output.Fraction = fP;
output.Normal = zero;
result = true;
return(result);
}
public static void Min(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
{
result.x = TSMath.Min(value1.x, value2.x);
result.y = TSMath.Min(value1.y, value2.y);
}
/// <summary>
/// Activate the explosion at the specified position.
/// </summary>
/// <param name="pos">The position where the explosion happens </param>
/// <param name="radius">The explosion radius </param>
/// <param name="maxForce">The explosion force at the explosion point (then is inversely proportional to the square of the distance)</param>
/// <returns>A list of bodies and the amount of force that was applied to them.</returns>
public Dictionary <Fixture, TSVector2> Activate(TSVector2 pos, FP radius, FP maxForce)
{
AABB aabb;
aabb.LowerBound = pos + new TSVector2(-radius, -radius);
aabb.UpperBound = pos + new TSVector2(radius, radius);
Fixture[] shapes = new Fixture[MaxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
World.QueryAABB(
fixture =>
{
if (fixture.TestPoint(ref pos))
{
if (IgnoreWhenInsideShape)
{
exit = true;
return(false);
}
containedShapes[containedShapeCount++] = fixture;
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return(true);
}, ref aabb);
if (exit)
{
return(new Dictionary <Fixture, TSVector2>());
}
Dictionary <Fixture, TSVector2> exploded = new Dictionary <Fixture, TSVector2>(shapeCount + containedShapeCount);
// Per shape max/min angles for now.
FP[] vals = new FP[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].Shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
TSVector2 vec = TSVector2.zero + new TSVector2(cs.Radius, 0);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(0, cs.Radius);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(-cs.Radius, cs.Radius);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(0, -cs.Radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
{
ps = shapes[i].Shape as PolygonShape;
}
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
{
TSVector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
FP angleToCentroid = FP.Atan2(toCentroid.y, toCentroid.x);
FP min = FP.MaxValue;
FP max = FP.MinValue;
FP minAbsolute = 0.0f;
FP maxAbsolute = 0.0f;
for (int j = 0; j < ps.Vertices.Count; ++j)
{
TSVector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
FP newAngle = FP.Atan2(toVertex.y, toVertex.x);
FP diff = (newAngle - angleToCentroid);
diff = (diff - FP.Pi) % (2 * FP.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
{
diff += 2 * FP.Pi; // correction for not handling negs
}
diff -= FP.Pi;
if (FP.Abs(diff) > FP.Pi)
{
continue; // Something's wrong, point not in shape but exists angle diff > 180
}
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture fixture = null;
FP midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
{
continue;
}
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + FP.PiTimes2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
TSVector2 p1 = pos;
TSVector2 p2 = radius * new TSVector2(FP.Cos(midpt), FP.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
World.RayCast((f, p, n, fr) =>
{
Body body = f.Body;
if (!IsActiveOn(body))
{
return(0);
}
hitClosest = true;
fixture = f;
return(fr);
}, p1, p2);
//draws radius points
if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
{
if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = fixture.Body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count > 1) &&
(i == valIndex - 1) &&
(_data.Last().Body == _data.First().Body) &&
(_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= FP.PiTimes2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count > 0) &&
(_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - FP.PiTimes2;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count; ++i)
{
if (!IsActiveOn(_data[i].Body))
{
continue;
}
FP arclen = _data[i].Max - _data[i].Min;
FP first = TSMath.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = FP.Ceiling((((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle)).AsInt();
if (insertedRays < 0)
{
insertedRays = 0;
}
FP offset = (arclen - first * 2.0f) / ((FP)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on FPs.
//Added FPEquals with a large epsilon.
for (FP j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FPEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
TSVector2 p1 = pos;
TSVector2 p2 = pos + radius * new TSVector2(FP.Cos(j), FP.Sin(j));
TSVector2 hitpoint = TSVector2.zero;
FP minlambda = FP.MaxValue;
List <Fixture> fl = _data[i].Body.FixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.RayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
FP impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / FP.Pi * (1.0f - SyncFrame.TSMath.Min(FP.One, minlambda));
// We Apply the impulse!!!
TSVector2 vectImp = TSVector2.Dot(impulse * new TSVector2(FP.Cos(j), FP.Sin(j)), -ro.Normal) * new TSVector2(FP.Cos(j), FP.Sin(j));
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
if (exploded.ContainsKey(f))
{
exploded[f] += vectImp;
}
else
{
exploded.Add(f, vectImp);
}
if (minlambda > 1.0f)
{
hitpoint = p2;
}
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!IsActiveOn(fix.Body))
{
continue;
}
FP impulse = MinRays * maxForce * 180.0f / FP.Pi;
TSVector2 hitPoint;
CircleShape circShape = fix.Shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
}
else
{
PolygonShape shape = fix.Shape as PolygonShape;
hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
}
TSVector2 vectImp = impulse * (hitPoint - pos);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!exploded.ContainsKey(fix))
{
exploded.Add(fix, vectImp);
}
}
return(exploded);
}
