/** 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);
}
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)));
}
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));
}
/** \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;
}
/// <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));
}
/// <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 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>
///
/// </summary>
/// <param name="forceToApply">basic force</param>
/// <param name="basicVelocity">basic Velocity</param>
/// <returns></returns>
protected TSVector ApplySeperateForce(TSVector toAcc, List <IAgentBehaviour> agents, bool bSkipStatic)//,out bool isTminStaticAgent
{
int count = agents.Count;
TSVector boidsVelocity = TSVector.zero;
TSVector forceToApply = TSVector.zero;
{
TSVector totalForce = TSVector.zero;
int neighboursCountSep = 0;
FP radius = _behaviour.colliderRadius * 4;
FP sepSqr = radius * radius;//
FP nrSqr = _behaviour.baseData.neighbourRadiusSqr * FP.EN2 * 25;
for (int j = 0; j < count; j++)
{
IAgentBehaviour a = agents[j];// _behaviour.neighbours
Boids.BoidsBehaviourSeparation(_behaviour, a, sepSqr, ref totalForce, ref neighboursCountSep, bSkipStatic);
}
if (count > 0)
{
TSVector sep = totalForce * (_behaviour.baseData.maxForce) / count;
FP lenSqr = sep.sqrMagnitude;
if (lenSqr > _behaviour.baseData.maxForceSqr)
{
FP fval = _behaviour.baseData.maxForce / TSMath.Sqrt(lenSqr);
sep = sep * fval;
}
forceToApply = sep;
if (PathFindingManager.DEBUG)
{
#if UNITY_5_5_OR_NEWER && !MULTI_THREAD
if (FP.Abs(forceToApply.x) > GridMap.SCALE * 1000 || FP.Abs(forceToApply.z) > GridMap.SCALE * 1000)
{
UnityEngine.Debug.LogError("forceToApply error!");
}
#endif
}
}
}
if (forceToApply != TSVector.zero)
{
FP max = TSMath.Max(TSMath.Abs(forceToApply.x), TSMath.Abs(forceToApply.z));
if (max > _behaviour.baseData.maxForce * FP.EN1 * 7)
{
forceToApply = forceToApply / max;
forceToApply = _behaviour.baseData.maxForce * forceToApply.normalized * FP.EN1 * 6;
}
return((forceToApply + toAcc) * _behaviour.baseData.invMass);//
}
return(forceToApply + toAcc);
}
public FP?IntersectsWithRay(TSVector2 origin, TSVector2 direction)
{
FP largestDistance = TSMath.Max(A.Position.x - origin.x, B.Position.x - origin.x) * 2f;
LineSegment raySegment = new LineSegment(new Vertex(origin, 0), new Vertex(origin + (direction * largestDistance), 0));
TSVector2?intersection = FindIntersection(this, raySegment);
FP? value = null;
if (intersection != null)
{
value = TSVector2.Distance(origin, intersection.Value);
}
return(value);
}
/// <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>
/// Gets an avoidance vector.
/// </summary>
/// <param name="selfPos">This unit's position.</param>
/// <param name="currentVelocity">This unit's current velocity.</param>
/// <param name="normalVelocity">This unit's normalized current velocity.</param>
/// <param name="unitData">This unit's UnitFacade.</param>
/// <param name="otherPos">The other unit's position.</param>
/// <param name="otherVelocity">The other unit's velocity.</param>
/// <param name="otherData">The other unit's UnitFacade.</param>
/// <param name="combinedRadius">The combined radius.</param>
/// <returns>An avoidance vector from the other unit's collision position to this unit's collision position - if a collision actually is detected.</returns>
private static TSVector GetAvoidVector(TSVector selfPos, TSVector currentVelocity, TSVector normalVelocity, IAgentBehaviour unitData, TSVector otherPos, TSVector otherVelocity, IAgentBehaviour otherData, FP combinedRadius)
{
TSVector selfCollisionPos = TSVector.zero;
TSVector avoidDirection = GetAvoidDirectionVector(selfPos, currentVelocity, otherPos, otherVelocity, combinedRadius, out selfCollisionPos);
FP avoidMagnitude = avoidDirection.magnitude;
if (avoidMagnitude == 0)
{
// if there is absolutely no magnitude to the found avoid direction, then ignore it
return(TSVector.zero);
}
FP vectorLength = combinedRadius * CustomMath.FPHalf;
if (vectorLength <= 0)
{
// if the units' combined radius is 0, then we cannot avoid
return(TSVector.zero);
}
// normalize the avoid vector and then set it's magnitude to the desired vector length (half of the combined radius)
TSVector avoidNormalized = (avoidDirection / avoidMagnitude);
TSVector avoidVector = avoidNormalized * vectorLength;
FP dotAngle = TSVector.Dot(avoidNormalized, normalVelocity);
if (dotAngle <= _cosAvoidAngle)
{
// the collision is considered "head-on", thus we compute a perpendicular avoid vector instead
avoidVector = new TSVector(avoidVector.z, avoidVector.y, -avoidVector.x);
}
else if (preventPassingInFront
//&& (otherData.determination > unitData.determination)
&& (TSVector.Dot(otherVelocity, avoidVector) > 0 && TSVector.Dot(currentVelocity, otherVelocity) >= 0))
{
// if supposed to be preventing front-passing, then check whether we should prevent it in this case and if so compute a different avoid vector
avoidVector = selfCollisionPos - selfPos;
}
// scale the avoid vector depending on the distance to collision, shorter distances need larger magnitudes and vice versa
FP collisionDistance = TSMath.Max(1, (selfPos - selfCollisionPos).magnitude);
avoidVector *= currentVelocity.magnitude / collisionDistance;
return(avoidVector);
}
public FP CalculateMaxSpeed(QTNode[] nodes, int index)
{
if (childNode1 == index)
{
// Leaf node
for (var data = nextData; data != null; data = (T)data.next)
{
maxSpeed = TSMath.Max(maxSpeed, data.speed);
}
}
else
{
maxSpeed = TSMath.Max(nodes[childNode1].CalculateMaxSpeed(nodes, childNode1), nodes[childNode2].CalculateMaxSpeed(nodes, childNode2));
maxSpeed = TSMath.Max(maxSpeed, nodes[childNode3].CalculateMaxSpeed(nodes, childNode3));
maxSpeed = TSMath.Max(maxSpeed, nodes[childNode4].CalculateMaxSpeed(nodes, childNode4));
}
return(maxSpeed);
}
/// <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));
}
/** Traces the vector field constructed out of the velocity obstacles.
* Returns the position which gives the minimum score (approximately).
*
* \see https://en.wikipedia.org/wiki/Gradient_descent
*/
TSVector2 Trace(VOBuffer vos, TSVector2 p, out FP score)
{
// Pick a reasonable initial step size
FP stepSize = TSMath.Max(radius, FP.One / 5 * desiredSpeed);
FP bestScore = FP.MaxValue;
TSVector2 bestP = p;
// TODO: Add momentum to speed up convergence?
const int MaxIterations = 50;
for (int s = 0; s < MaxIterations; s++)
{
FP step = 1 - (s * FP.One / MaxIterations);
step = Sqr(step) * stepSize;
FP value;
var gradient = EvaluateGradient(vos, p, out value);
if (value < bestScore)
{
bestScore = value;
bestP = p;
}
// TODO: Add cutoff for performance
gradient.Normalize();
gradient *= step;
TSVector2 prev = p;
p += gradient;
// if (DebugDraw) Debug.DrawLine(FromXZ(prev + position), FromXZ(p + position), Rainbow(s * 0.1f) * new Color(1, 1, 1, 1f));
}
score = bestScore;
return(bestP);
}
private void PostSolve(Contact contact, ContactVelocityConstraint impulse)
{
bool flag = !this.Broken;
if (flag)
{
bool flag2 = this.Parts.Contains(contact.FixtureA) || this.Parts.Contains(contact.FixtureB);
if (flag2)
{
FP fP = 0f;
int pointCount = contact.Manifold.PointCount;
for (int i = 0; i < pointCount; i++)
{
fP = TSMath.Max(fP, impulse.points[i].normalImpulse);
}
bool flag3 = fP > this.Strength;
if (flag3)
{
this._break = true;
}
}
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FP dvx, dvy, dvz;
dvx = (body2.angularVelocity.y * relativePos2.z) - (body2.angularVelocity.z * relativePos2.y) + body2.linearVelocity.x;
dvy = (body2.angularVelocity.z * relativePos2.x) - (body2.angularVelocity.x * relativePos2.z) + body2.linearVelocity.y;
dvz = (body2.angularVelocity.x * relativePos2.y) - (body2.angularVelocity.y * relativePos2.x) + body2.linearVelocity.z;
dvx = dvx - (body1.angularVelocity.y * relativePos1.z) + (body1.angularVelocity.z * relativePos1.y) - body1.linearVelocity.x;
dvy = dvy - (body1.angularVelocity.z * relativePos1.x) + (body1.angularVelocity.x * relativePos1.z) - body1.linearVelocity.y;
dvz = dvz - (body1.angularVelocity.x * relativePos1.y) + (body1.angularVelocity.y * relativePos1.x) - body1.linearVelocity.z;
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * normal.z) - (relativePos1.z * normal.y);
rantra.y = (relativePos1.z * normal.x) - (relativePos1.x * normal.z);
rantra.z = (relativePos1.x * normal.y) - (relativePos1.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * normal.z) - (relativePos2.z * normal.y);
rbntrb.y = (relativePos2.z * normal.x) - (relativePos2.x * normal.z);
rbntrb.z = (relativePos2.x * normal.y) - (relativePos2.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kNormal += rantra.x * normal.x + rantra.y * normal.y + rantra.z * normal.z;
}
if (!treatBody2AsStatic)
{
kNormal += rbntrb.x * normal.x + rbntrb.y * normal.y + rbntrb.z * normal.z;
}
massNormal = FP.One / kNormal;
FP num = dvx * normal.x + dvy * normal.y + dvz * normal.z;
tangent.x = dvx - normal.x * num;
tangent.y = dvy - normal.y * num;
tangent.z = dvz - normal.z * num;
num = tangent.x * tangent.x + tangent.y * tangent.y + tangent.z * tangent.z;
if (num != FP.Zero)
{
num = FP.Sqrt(num);
tangent.x /= num;
tangent.y /= num;
tangent.z /= num;
}
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * tangent.z) - (relativePos1.z * tangent.y);
rantra.y = (relativePos1.z * tangent.x) - (relativePos1.x * tangent.z);
rantra.z = (relativePos1.x * tangent.y) - (relativePos1.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * tangent.z) - (relativePos2.z * tangent.y);
rbntrb.y = (relativePos2.z * tangent.x) - (relativePos2.x * tangent.z);
rbntrb.z = (relativePos2.x * tangent.y) - (relativePos2.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kTangent += TSVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += TSVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = normal.x * dvx + normal.y * dvy + normal.z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -(tangent.x * dvx + tangent.y * dvy + tangent.z * dvz);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
TSVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
// modified by tiger, uncommmented.
//if (relNormalVel < -FP.One && newContact)
//{
// restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
//}
// added by seok
//if (!newContact)
if (!newContact || TSMath.Abs(relNormalVel *= restitution) < restitution)
{
relNormalVel = 0;
}
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse.x = normal.x * accumulatedNormalImpulse + tangent.x * accumulatedTangentImpulse;
impulse.y = normal.y * accumulatedNormalImpulse + tangent.y * accumulatedTangentImpulse;
impulse.z = normal.z * accumulatedNormalImpulse + tangent.z * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.x -= (impulse.x * body1.inverseMass);
body1.linearVelocity.y -= (impulse.y * body1.inverseMass);
body1.linearVelocity.z -= (impulse.z * body1.inverseMass);
if (!body1IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y;
num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z;
num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x;
FP num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
FP num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
FP num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.x -= num3;
body1.angularVelocity.y -= num4;
body1.angularVelocity.z -= num5;
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.x += (impulse.x * body2.inverseMass);
body2.linearVelocity.y += (impulse.y * body2.inverseMass);
body2.linearVelocity.z += (impulse.z * body2.inverseMass);
if (!body2IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y;
num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z;
num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x;
FP num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
FP num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
FP num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.x += num3;
body2.angularVelocity.y += num4;
body2.angularVelocity.z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
TSVector dv = CalculateRelativeVelocity();
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref normal, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kNormal += TSVector.Dot(ref rantra, ref normal);
}
if (!treatBody2AsStatic)
{
kNormal += TSVector.Dot(ref rbntrb, ref normal);
}
massNormal = FP.One / kNormal;
tangent = dv - TSVector.Dot(dv, normal) * normal;
tangent.Normalize();
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
TSVector.Cross(ref relativePos1, ref normal, out rantra);
TSVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
TSVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
TSVector.Cross(ref relativePos2, ref tangent, out rbntrb);
TSVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
TSVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kTangent += TSVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += TSVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = TSVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -TSVector.Dot(ref tangent, ref dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
TSVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
//同时求解和恢复是不可能的，所以当有新的接触时，仅仅应用恢复脉冲，有点虚假。
if (relNormalVel < -FP.One && newContact)
{
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
// 如果穿透是负的（这意味着物体还没有接触，但是它们将来会接触），
// 我们将当前弹跳偏置存储在变量“lostSpeculativeBounce”中，并在下一个框架中应用它，此时投机接触已经解决。
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(ref impulse);
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// Hull making.
/// </summary>
/// <remarks>Based/Completely from http://www.xbdev.net/physics/MinkowskiDifference/index.php
/// I don't (100%) see why this should always work.
/// </remarks>
/// <param name="triangleList"></param>
/// <param name="generationThreshold"></param>
public virtual void MakeHull(ref List <TSVector> triangleList, int generationThreshold)
{
FP distanceThreshold = FP.Zero;
if (generationThreshold < 0)
{
generationThreshold = 4;
}
Stack <ClipTriangle> activeTriList = new Stack <ClipTriangle>();
TSVector[] v = new TSVector[] // 6 Array
{
new TSVector(-1, 0, 0),
new TSVector(1, 0, 0),
new TSVector(0, -1, 0),
new TSVector(0, 1, 0),
new TSVector(0, 0, -1),
new TSVector(0, 0, 1),
};
int[,] kTriangleVerts = new int[8, 3] // 8 x 3 Array
{
{ 5, 1, 3 },
{ 4, 3, 1 },
{ 3, 4, 0 },
{ 0, 5, 3 },
{ 5, 2, 1 },
{ 4, 1, 2 },
{ 2, 0, 4 },
{ 0, 2, 5 }
};
for (int i = 0; i < 8; i++)
{
ClipTriangle tri = new ClipTriangle();
tri.n1 = v[kTriangleVerts[i, 0]];
tri.n2 = v[kTriangleVerts[i, 1]];
tri.n3 = v[kTriangleVerts[i, 2]];
tri.generation = 0;
activeTriList.Push(tri);
}
// surfaceTriList
while (activeTriList.Count > 0)
{
ClipTriangle tri = activeTriList.Pop();
TSVector p1; SupportMapping(ref tri.n1, out p1);
TSVector p2; SupportMapping(ref tri.n2, out p2);
TSVector p3; SupportMapping(ref tri.n3, out p3);
FP d1 = (p2 - p1).sqrMagnitude;
FP d2 = (p3 - p2).sqrMagnitude;
FP d3 = (p1 - p3).sqrMagnitude;
if (TSMath.Max(TSMath.Max(d1, d2), d3) > distanceThreshold && tri.generation < generationThreshold)
{
ClipTriangle tri1 = new ClipTriangle();
ClipTriangle tri2 = new ClipTriangle();
ClipTriangle tri3 = new ClipTriangle();
ClipTriangle tri4 = new ClipTriangle();
tri1.generation = tri.generation + 1;
tri2.generation = tri.generation + 1;
tri3.generation = tri.generation + 1;
tri4.generation = tri.generation + 1;
tri1.n1 = tri.n1;
tri2.n2 = tri.n2;
tri3.n3 = tri.n3;
TSVector n = FP.Half * (tri.n1 + tri.n2);
n.Normalize();
tri1.n2 = n;
tri2.n1 = n;
tri4.n3 = n;
n = FP.Half * (tri.n2 + tri.n3);
n.Normalize();
tri2.n3 = n;
tri3.n2 = n;
tri4.n1 = n;
n = FP.Half * (tri.n3 + tri.n1);
n.Normalize();
tri1.n3 = n;
tri3.n1 = n;
tri4.n2 = n;
activeTriList.Push(tri1);
activeTriList.Push(tri2);
activeTriList.Push(tri3);
activeTriList.Push(tri4);
}
else
{
if (((p3 - p1) % (p2 - p1)).sqrMagnitude > TSMath.Epsilon)
{
triangleList.Add(p1);
triangleList.Add(p2);
triangleList.Add(p3);
}
}
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < this._count; i++)
{
ContactVelocityConstraint contactVelocityConstraint = this._velocityConstraints[i];
int indexA = contactVelocityConstraint.indexA;
int indexB = contactVelocityConstraint.indexB;
FP invMassA = contactVelocityConstraint.invMassA;
FP invIA = contactVelocityConstraint.invIA;
FP invMassB = contactVelocityConstraint.invMassB;
FP invIB = contactVelocityConstraint.invIB;
int pointCount = contactVelocityConstraint.pointCount;
TSVector2 tSVector = this._velocities[indexA].v;
FP fP = this._velocities[indexA].w;
TSVector2 tSVector2 = this._velocities[indexB].v;
FP fP2 = this._velocities[indexB].w;
TSVector2 normal = contactVelocityConstraint.normal;
TSVector2 tSVector3 = MathUtils.Cross(normal, 1f);
FP friction = contactVelocityConstraint.friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
for (int j = 0; j < pointCount; j++)
{
VelocityConstraintPoint velocityConstraintPoint = contactVelocityConstraint.points[j];
TSVector2 value = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint.rA);
FP x = TSVector2.Dot(value, tSVector3) - contactVelocityConstraint.tangentSpeed;
FP fP3 = velocityConstraintPoint.tangentMass * -x;
FP fP4 = friction * velocityConstraintPoint.normalImpulse;
FP fP5 = MathUtils.Clamp(velocityConstraintPoint.tangentImpulse + fP3, -fP4, fP4);
fP3 = fP5 - velocityConstraintPoint.tangentImpulse;
velocityConstraintPoint.tangentImpulse = fP5;
TSVector2 tSVector4 = fP3 * tSVector3;
tSVector -= invMassA * tSVector4;
fP -= invIA * MathUtils.Cross(velocityConstraintPoint.rA, tSVector4);
tSVector2 += invMassB * tSVector4;
fP2 += invIB * MathUtils.Cross(velocityConstraintPoint.rB, tSVector4);
}
bool flag = contactVelocityConstraint.pointCount == 1;
if (flag)
{
VelocityConstraintPoint velocityConstraintPoint2 = contactVelocityConstraint.points[0];
TSVector2 value2 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint2.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint2.rA);
FP x2 = TSVector2.Dot(value2, normal);
FP fP6 = -velocityConstraintPoint2.normalMass * (x2 - velocityConstraintPoint2.velocityBias);
FP fP7 = TSMath.Max(velocityConstraintPoint2.normalImpulse + fP6, 0f);
fP6 = fP7 - velocityConstraintPoint2.normalImpulse;
velocityConstraintPoint2.normalImpulse = fP7;
TSVector2 tSVector5 = fP6 * normal;
tSVector -= invMassA * tSVector5;
fP -= invIA * MathUtils.Cross(velocityConstraintPoint2.rA, tSVector5);
tSVector2 += invMassB * tSVector5;
fP2 += invIB * MathUtils.Cross(velocityConstraintPoint2.rB, tSVector5);
}
else
{
VelocityConstraintPoint velocityConstraintPoint3 = contactVelocityConstraint.points[0];
VelocityConstraintPoint velocityConstraintPoint4 = contactVelocityConstraint.points[1];
TSVector2 tSVector6 = new TSVector2(velocityConstraintPoint3.normalImpulse, velocityConstraintPoint4.normalImpulse);
Debug.Assert(tSVector6.x >= 0f && tSVector6.y >= 0f);
TSVector2 value3 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint3.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint3.rA);
TSVector2 value4 = tSVector2 + MathUtils.Cross(fP2, velocityConstraintPoint4.rB) - tSVector - MathUtils.Cross(fP, velocityConstraintPoint4.rA);
FP x3 = TSVector2.Dot(value3, normal);
FP x4 = TSVector2.Dot(value4, normal);
TSVector2 tSVector7 = new TSVector2
{
x = x3 - velocityConstraintPoint3.velocityBias,
y = x4 - velocityConstraintPoint4.velocityBias
} -MathUtils.Mul(ref contactVelocityConstraint.K, tSVector6);
TSVector2 tSVector8 = -MathUtils.Mul(ref contactVelocityConstraint.normalMass, tSVector7);
bool flag2 = tSVector8.x >= 0f && tSVector8.y >= 0f;
if (flag2)
{
TSVector2 tSVector9 = tSVector8 - tSVector6;
TSVector2 tSVector10 = tSVector9.x * normal;
TSVector2 tSVector11 = tSVector9.y * normal;
tSVector -= invMassA * (tSVector10 + tSVector11);
fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector10) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector11));
tSVector2 += invMassB * (tSVector10 + tSVector11);
fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector10) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector11));
velocityConstraintPoint3.normalImpulse = tSVector8.x;
velocityConstraintPoint4.normalImpulse = tSVector8.y;
}
else
{
tSVector8.x = -velocityConstraintPoint3.normalMass * tSVector7.x;
tSVector8.y = 0f;
x3 = 0f;
x4 = contactVelocityConstraint.K.ex.y * tSVector8.x + tSVector7.y;
bool flag3 = tSVector8.x >= 0f && x4 >= 0f;
if (flag3)
{
TSVector2 tSVector12 = tSVector8 - tSVector6;
TSVector2 tSVector13 = tSVector12.x * normal;
TSVector2 tSVector14 = tSVector12.y * normal;
tSVector -= invMassA * (tSVector13 + tSVector14);
fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector13) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector14));
tSVector2 += invMassB * (tSVector13 + tSVector14);
fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector13) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector14));
velocityConstraintPoint3.normalImpulse = tSVector8.x;
velocityConstraintPoint4.normalImpulse = tSVector8.y;
}
else
{
tSVector8.x = 0f;
tSVector8.y = -velocityConstraintPoint4.normalMass * tSVector7.y;
x3 = contactVelocityConstraint.K.ey.x * tSVector8.y + tSVector7.x;
x4 = 0f;
bool flag4 = tSVector8.y >= 0f && x3 >= 0f;
if (flag4)
{
TSVector2 tSVector15 = tSVector8 - tSVector6;
TSVector2 tSVector16 = tSVector15.x * normal;
TSVector2 tSVector17 = tSVector15.y * normal;
tSVector -= invMassA * (tSVector16 + tSVector17);
fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector16) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector17));
tSVector2 += invMassB * (tSVector16 + tSVector17);
fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector16) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector17));
velocityConstraintPoint3.normalImpulse = tSVector8.x;
velocityConstraintPoint4.normalImpulse = tSVector8.y;
}
else
{
tSVector8.x = 0f;
tSVector8.y = 0f;
x3 = tSVector7.x;
x4 = tSVector7.y;
bool flag5 = x3 >= 0f && x4 >= 0f;
if (flag5)
{
TSVector2 tSVector18 = tSVector8 - tSVector6;
TSVector2 tSVector19 = tSVector18.x * normal;
TSVector2 tSVector20 = tSVector18.y * normal;
tSVector -= invMassA * (tSVector19 + tSVector20);
fP -= invIA * (MathUtils.Cross(velocityConstraintPoint3.rA, tSVector19) + MathUtils.Cross(velocityConstraintPoint4.rA, tSVector20));
tSVector2 += invMassB * (tSVector19 + tSVector20);
fP2 += invIB * (MathUtils.Cross(velocityConstraintPoint3.rB, tSVector19) + MathUtils.Cross(velocityConstraintPoint4.rB, tSVector20));
velocityConstraintPoint3.normalImpulse = tSVector8.x;
velocityConstraintPoint4.normalImpulse = tSVector8.y;
}
}
}
}
}
this._velocities[indexA].v = tSVector;
this._velocities[indexA].w = fP;
this._velocities[indexB].v = tSVector2;
this._velocities[indexB].w = fP2;
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
var dv = CalculateRelativeVelocity();
var kn = CalcK(body1, relativePos1, normal) +
CalcK(body2, relativePos2, normal);
massNormal = FP.One / kn;
tangent = dv - TSVector.Dot(dv, normal) * normal;
tangent.Normalize();
var kt = CalcK(body1, relativePos1, tangent) +
CalcK(body2, relativePos2, tangent);
massTangent = FP.One / kt;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = TSVector.Dot(normal, dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -TSVector.Dot(tangent, dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -FP.One && newContact)
{
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
var impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(impulse);
lastTimeStep = timestep;
newContact = false;
}
public void Insert(T T)
{
int i = 0;
QTBound r = bounds;
TSVector p = T.position;
T.next = null;
maxRadius = TSMath.Max(T.neighbourRadius, maxRadius);
int depth = 0;
TSVector min = TSVector.zero;
TSVector max = TSVector.zero;
while (true)
{
depth++;
if (nodes[i].childNode1 == i)
{
// Leaf node.
if (nodes[i].count < LeafSize || depth > 10)
{
nodes[i].Add(T);
// nodes[i].count++;
break;
}
else
{
// Split
QTNode node = nodes[i];
node.childNode1 = GetNodeIndex();
node.childNode2 = GetNodeIndex();
node.childNode3 = GetNodeIndex();
node.childNode4 = GetNodeIndex();
nodes[i] = node;
nodes[i].Distribute(nodes, r);
}
}
// Note, no else
if (nodes[i].childNode1 != i)
{
// Not a leaf node
TSVector c = r.center;
if (p.x > c.x)
{
if (p.z > c.z)
{
i = nodes[i].childNode4;
r = QTBound.MinMaxQTBound(c, r.max);
}
else
{
i = nodes[i].childNode3;
min.Set(c.x, 0, r.min.z);
max.Set(r.max.x, 0, c.z);
r = QTBound.MinMaxQTBound(min, max);
}
}
else
{
if (p.z > c.z)
{
i = nodes[i].childNode2;
min.Set(r.min.x, 0, c.z);
max.Set(c.x, 0, r.max.z);
r = QTBound.MinMaxQTBound(min, max);
}
else
{
i = nodes[i].childNode1;
r = QTBound.MinMaxQTBound(r.min, c);
}
}
}
}
}
/** Creates a VO for avoiding another agent.
* Note that the segment is directed, the agent will want to be on the left side of the segment.
*/
public static VO SegmentObstacle(TSVector2 segmentStart, TSVector2 segmentEnd, TSVector2 offset, FP radius, FP inverseDt, FP inverseDeltaTime)
{
var vo = new VO();
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
vo.weightFactor = 1;
// Just higher than anything else
vo.weightBonus = TSMath.Max(radius, 1) * 40;
var closestOnSegment = CustomMath.ClosestPointOnSegment(segmentStart, segmentEnd, TSVector2.zero);
// Collision?
if (closestOnSegment.magnitude <= radius)
{
vo.colliding = true;
vo.line1 = closestOnSegment.normalized * (closestOnSegment.magnitude - radius) * 3 / 10 * inverseDeltaTime;
vo.dir1 = new TSVector2(vo.line1.y, -vo.line1.x).normalized;
vo.line1 += offset;
vo.cutoffDir = TSVector2.zero;
vo.cutoffLine = TSVector2.zero;
vo.dir2 = TSVector2.zero;
vo.line2 = TSVector2.zero;
vo.radius = 0;
vo.segmentStart = TSVector2.zero;
vo.segmentEnd = TSVector2.zero;
vo.segment = false;
}
else
{
vo.colliding = false;
segmentStart *= inverseDt;
segmentEnd *= inverseDt;
radius *= inverseDt;
var cutoffTangent = (segmentEnd - segmentStart).normalized;
vo.cutoffDir = cutoffTangent;
vo.cutoffLine = segmentStart + new TSVector2(-cutoffTangent.y, cutoffTangent.x) * radius;
vo.cutoffLine += offset;
// See documentation for details
// The call to Max is just to prevent floating point errors causing NaNs to appear
var startSqrMagnitude = segmentStart.LengthSquared();
var normal1 = -ComplexMultiply(segmentStart, new TSVector2(radius, TSMath.Sqrt(TSMath.Max(0, startSqrMagnitude - radius * radius)))) / startSqrMagnitude;
var endSqrMagnitude = segmentEnd.LengthSquared();
var normal2 = -ComplexMultiply(segmentEnd, new TSVector2(radius, -TSMath.Sqrt(TSMath.Max(0, endSqrMagnitude - radius * radius)))) / endSqrMagnitude;
vo.line1 = segmentStart + normal1 * radius + offset;
vo.line2 = segmentEnd + normal2 * radius + offset;
// Note that the normals are already normalized
vo.dir1 = new TSVector2(normal1.y, -normal1.x);
vo.dir2 = new TSVector2(normal2.y, -normal2.x);
vo.segmentStart = segmentStart;
vo.segmentEnd = segmentEnd;
vo.radius = radius;
vo.segment = true;
}
return(vo);
}
//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);
}
}
}
