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