public FP ComputeSquaredDistanceToPoint(TSVector Point)
{
//
FP DistSquared = FP.Zero;
if (Point.x < min.x)
{
DistSquared += TSMath.Sqrt(Point.x - min.x);
}
else if (Point.x > max.x)
{
DistSquared += TSMath.Sqrt(Point.x - max.x);
}
if (Point.z < min.z)
{
DistSquared += TSMath.Sqrt(Point.z - min.z);
}
else if (Point.z > max.z)
{
DistSquared += TSMath.Sqrt(Point.z - max.z);
}
return(DistSquared);
}
internal static FP CalculateHValue(IInt2 gridPos1, IInt2 gridPos2, Heuristic heuristic)
{
FP h = FP.Zero;
switch (heuristic)
{
case Heuristic.Euclidean:
h = TSMath.Sqrt((gridPos1 - gridPos2).sqrMagnitudeLong) * HeuristicFactor;
break;
case Heuristic.Manhattan:
h = (Math.Abs(gridPos2.x - gridPos1.x) + Math.Abs(gridPos2.y - gridPos1.y)) * HeuristicFactor;
// return h;
break;
case Heuristic.DiagonalManhattan:
IInt2 p = gridPos2 - gridPos1;
p.x = Math.Abs(p.x);
p.y = Math.Abs(p.y);
int diag = Math.Min(p.x, p.y);
int diag2 = Math.Max(p.x, p.y);
h = ((CustomMath.DiagonalCost * diag + (diag2 - diag))) * HeuristicFactor;
break;
}
return(h * GridMap.GetNodeSize());
}
private TSVector TargetPosition(int index, TSVector sphere, int agentsnum)
{
if (agentsnum != 0)
{
int separation = 150;
agentsPerSide[index] = agentsnum / 3 + (agentsnum % 3 > 0 ? 1 : 0);
int length = agentsnum * 200;
int side = index % 3;
FP lengthMultiplier = (index / 3) / (FP)agentsPerSide[side];
lengthMultiplier = 1 - (lengthMultiplier - (int)lengthMultiplier);
FP height = length / 2 * TSMath.Sqrt(3); // Equilaterial triangle height
if (index == 0)
{
return(sphere);
}
else
{
return(sphere + new TSVector(separation * (index % 2 == 0 ? -1 : 1) * (((index - 1) / 2) + 1), 0, separation * (((index - 1) / 2) + 1)));
}
}
else
{
return(sphere);
}
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
if (normal.sqrMagnitude < TSMath.Epsilon)
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = TSVector.forward;
}
else
{
normal = normal.normalized;
}
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
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));
}
}
}
}
/** Evaluate gradient and value of the cost function at velocity p */
TSVector2 EvaluateGradient(VOBuffer vos, TSVector2 p, out FP value)
{
TSVector2 gradient = TSVector2.zero;
value = 0;
// Avoid other agents
for (int i = 0; i < vos.length; i++)
{
FP w;
var grad = vos.buffer[i].ScaledGradient(p, out w);
if (w > value)
{
value = w;
gradient = grad;
}
}
// Move closer to the desired velocity
var dirToDesiredVelocity = desiredVelocity - p;
var distToDesiredVelocity = dirToDesiredVelocity.magnitude;
if (distToDesiredVelocity > CustomMath.EPSILON)
{
gradient += dirToDesiredVelocity * (DesiredVelocityWeight / distToDesiredVelocity);
value += distToDesiredVelocity * DesiredVelocityWeight;
}
// Prefer speeds lower or equal to the desired speed
// and avoid speeds greater than the max speed
var sqrSpeed = p.LengthSquared();
if (sqrSpeed > desiredSpeed * desiredSpeed)
{
var speed = TSMath.Sqrt(sqrSpeed);
if (speed > maxSpeed)
{
FP MaxSpeedWeight = 3;
value += MaxSpeedWeight * (speed - maxSpeed);
gradient -= MaxSpeedWeight * (p / speed);
}
// Scale needs to be strictly greater than DesiredVelocityWeight
// otherwise the agent will not prefer the desired speed over
// the maximum speed
FP scale = 2 * DesiredVelocityWeight;
value += scale * (speed - desiredSpeed);
gradient -= scale * (p / speed);
}
return(gradient);
}
/// <summary>
/// 圆和边的平面动态相交检测（注意是边的平面 并不是线段）
/// </summary>
/// <param name="tedge"></param>
/// <param name="crd"></param>
/// <returns>是否在该段时间内相交</returns>
public static bool CheckCircle_LineContact(tableEdge tedge, CircleRunData crd, ref FP t_percent)
{
//Sc
TSVector2 Sc = PointToLineDir(tedge.start, tedge.end, crd.cur_pos);
//Se
TSVector2 Se = PointToLineDir(tedge.start, tedge.end, crd.next_pos);
TSVector2 Scnormal = Sc.normalized;
TSVector2 Senormal = Se.normalized;
//TSVector2 Scnormal = Sc.normalized;
//TSVector2 Senormal = Se.normalized;
//只有两种结果 同向和 反向
FP result = TSVector2.Dot(Scnormal, Senormal); //1同向,0垂直,-1反向
FP Scnorm = TSMath.Sqrt(TSMath.Abs(TSVector2.Dot(Sc, Sc))); //Sc模
FP Senorm = TSMath.Sqrt(TSMath.Abs(TSVector2.Dot(Se, Se))); //Se模
//FP radius_square = crd.radius * crd.radius;
if (result > 0 && Scnorm > crd.radius && Senorm > crd.radius)//Sc,Se同向，俩圆圆半径大于到直线距离,不相交
{
return(false);
}
else//相交 求t
{
FP S = 0;
if (result > 0)
{
S = Scnorm - Senorm;
}
else
{
S = Scnorm + Senorm;
}
//TSVector2 sce = Sc - Se;
//FP S = TSMath.Sqrt( TSVector2.Dot(sce, sce));
t_percent = (Scnorm - crd.radius) / S;//圆心到达撞击点的距离/圆心经过的总距离 来求出时间占比
if (t_percent > 1)
{
return(false);
Debug.Log("路程百分比大于1,注意!");
}
//if (t_percent < 0)
//{
// if (Detection.CheckCircle_tableEdgeEndContact(crd, tedge, ref t_percent))
// {
// Debug.Log("修正");
// }
//}
return(t_percent >= 0?true:false);
}
}
/// <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);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
TSMatrix.Inverse(ref effectiveMass, out effectiveMass);
TSMatrix orientationDifference;
TSMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
TSMatrix.Transpose(ref orientationDifference, out orientationDifference);
TSMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
TSVector axis;
FP x = q.M32 - q.M23;
FP y = q.M13 - q.M31;
FP z = q.M21 - q.M12;
FP r = TSMath.Sqrt(x * x + y * y + z * z);
FP t = q.M11 + q.M22 + q.M33;
FP angle = FP.Atan2(r, t - 1);
axis = new TSVector(x, y, z) * angle;
if (r != FP.Zero)
{
axis = axis * (FP.One / r);
}
bias = axis * biasFactor * (-FP.One / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += TSVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += TSVector.Transform(-FP.One * accumulatedImpulse, body2.invInertiaWorld);
}
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
normal = normal.normalized;
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
public static FP LineCircleIntersectionFactor(TSVector circleCenter, TSVector linePoint1, TSVector linePoint2, FP radius)
{
FP segmentLength;
var normalizedDirection = Normalize(linePoint2 - linePoint1, out segmentLength);
var dirToStart = linePoint1 - circleCenter;
var dot = TSVector.Dot(dirToStart, normalizedDirection);
var discriminant = dot * dot - (dirToStart.sqrMagnitude - radius * radius);
if (discriminant < 0)
{
// No intersection, pick closest point on segment
discriminant = 0;
}
var t = -dot + TSMath.Sqrt(discriminant);
return(segmentLength > EPSILON ? t / segmentLength : 1);
}
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));
}
// static readonly FP r_dstMaxSqrInv =(FP.One / r_factor)* (FP.One / r_factor)*10;
//Separation navigation
internal static void BoidsBehaviourSeparation(IAgentBehaviour agent, IAgentBehaviour a, FP nrSqr, ref TSVector totalForce, ref int neighboursCount, bool bSkipStatic)
{
if (a != agent && a.enabled)
{
TSVector otherPos = a.position;
if (agent.agent == null)//static agent
{
if (bSkipStatic)
{
return;
}
otherPos = a.map.GetWorldPosition(a.map.GetGridNodeId(a.position));
}
TSVector pushForce = (agent.position - otherPos);
FP distanceSqr = pushForce.sqrMagnitude;
if (distanceSqr < nrSqr)//&& distanceSqr > 0
{
//Vector to other agent
//var length = pushForce.Normalize(); //
FP r = (agent.colliderRadius + a.colliderRadius);
// FP dst = distanceSqr;// TSMath.Sqrt(distanceSqr);
FP fWeight = 0;
FP dstMax = r_factor * r;
FP rSqr = r * r;
FP dstMaxSqr = dstMax * dstMax;
FP dst = TSMath.Sqrt(distanceSqr);
// fWeight = (1 - (dst - r) / (agent.neighbourRadius - r));//(r+CustomMath.FPHalf- dst)/ dst;//
if (distanceSqr > rSqr && distanceSqr < dstMaxSqr)
{
//if ( a.agent != null)
{
fWeight = (1 - (dst - r) / (agent.neighbourRadius - r)); // (dstMax);// (1 - (dst - r) / (agent.neighbourQTRadius - r));
if (a.agent == null) //static agent
{
fWeight *= 10;
}
}
}
else if (rSqr >= distanceSqr)
{
if (distanceSqr > r_staticFactorSqr * rSqr)
{
fWeight = 1 + CustomMath.FPHalf / 2;
}
else if (dst > FP.One / 10 * GridMap.SCALE)
{
{
fWeight = (r / dst * 2);
fWeight = fWeight * fWeight;
}
}
else
{
if (pushForce == TSVector.zero)
{
pushForce = agent.velocity - a.velocity;
}
fWeight = 10;
}
}
fWeight = TSMath.Min(30, fWeight);
if (fWeight > 0)
{
totalForce = totalForce + (CustomMath.Normalize(pushForce) * fWeight);
}
neighboursCount++;
}
}
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 p = MathUtils.Mul(ref xf, Position);
FP l = -(TSVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * _2radius);
}
//Magic
FP l2 = l * l;
FP area = _2radius * (FP)((TSMath.Asin((l / Radius)) + TSMath.PiOver2) + l * TSMath.Sqrt(_2radius - l2));
// TODO - PORT
//FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area;
FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
internal static TSVector2 computeForces(IAgentBehaviour behaviour, List <CircleObstacleAngleData> circleObstacles, TSVector basicVelocity, bool bUseForwardPos,
FP maxTime, FP obstacleMaxTime, out bool isTminStaticAgent, out FP time, ref bool isCollidering, bool bIgnoreObstacle = false)//,
{
time = -1;
TSVector2 F = TSVector2.zero;
List <IAgentBehaviour> neighbours = behaviour.neighbours;
int icount = neighbours.Count;
isTminStaticAgent = false;
FP tmin = FP.MaxValue;
TSVector forwardPos = TSVector.zero;
if (bUseForwardPos)
{
forwardPos = behaviour.position + basicVelocity.normalized * behaviour.colliderRadius;
}
//return TSVector2.zero;
TSVector pos = (bUseForwardPos ? forwardPos : behaviour.position);
// compute the anticipatory force from each neighbor
for (int i = 0; i < icount; ++i)
{
IAgentBehaviour other = neighbours[i];
FP radiusSum = other.colliderRadius;// + behaviour.colliderRadius;
if (!bUseForwardPos)
{
radiusSum = other.colliderRadius + behaviour.colliderRadius;
}
if (behaviour != other)
{
maxTime = other.agent == null? obstacleMaxTime:maxTime;
F += ComputeForce(behaviour, pos, other.position, radiusSum, other.agent == null, maxTime,
basicVelocity, other.velocity, ref isTminStaticAgent, ref tmin, ref time, ref isCollidering);
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFindingManager.DEBUG && behaviour.agent != null)
{
if (TSMath.Abs(F.x) > behaviour.baseData.maxForce * 1000 ||
TSMath.Abs(F.y) > behaviour.baseData.maxForce * 1000)
{
UnityEngine.Debug.Log("F over flow!");
}
}
#endif
}
}
//
if (!bIgnoreObstacle)
{
FP dirDst = GridMap.blockDirDst;
TSVector testDir = basicVelocity.normalized;
TSVector pos2 = behaviour.position + testDir * dirDst;
behaviour.pathManager._queryStack.Clear();
TSVector blockedPos = TSVector.zero;
bool hasObstacle = behaviour.map.IsBlockedByObstacleBetween2Point(behaviour.position, pos2
, behaviour.pathManager._queryStack, ref blockedPos);
if (hasObstacle)
{
F = F - CustomMath.TSVecToVec2(testDir) * behaviour.baseData.maxForce;
time = FP.EN1;//near static obstacle
isTminStaticAgent = true;
}
}
// behaviour.pathManager._queryStack.Clear();
//
//icount = circleObstacles.Count;
//bool temp=false;
//for (int i = 0; i < icount; ++i)
//{
// CircleObstacle other = circleObstacles[i];
// FP radiusSum = other._radius;// + behaviour.colliderRadius;
// if (!bUseForwardPos)
// {
// radiusSum = other._radius + behaviour.colliderRadius;
// }
// maxTime = obstacleMaxTime;
// F += ComputeForce(behaviour, pos, CustomMath.TSVec2ToVec(other._center), radiusSum, true, maxTime,
// basicVelocity, TSVector.zero, ref isTminStaticAgent, ref tmin, ref time,ref temp);
//}
#if USE_OBSTACLE
//forces from static obstacles
List <LineObstacle> obstacleNeighbours = behaviour.neighbourObstacles;
icount = obstacleNeighbours.Count;
TSVector2 position = CustomMath.TSVecToVec2(behaviour.position);
TSVector2 velocity = CustomMath.TSVecToVec2(basicVelocity);
FP radiusSqr = behaviour.colliderRadius * behaviour.colliderRadius;
FP neighbourDstSqr = behaviour.neighbourRadius * behaviour.neighbourRadius;
for (int i = 0; i < icount; ++i)
{
LineObstacle obstacle = obstacleNeighbours[i];
TSVector2 n_w = CustomMath.ClosestPointLineSegment(obstacle._p1, obstacle._p2, position) - position;
FP d_w = n_w.LengthSquared();
if (velocity * n_w < 0 || d_w == radiusSqr || d_w > neighbourDstSqr) // Agent is moving away from obstacle, already colliding or obstacle too far away
{
continue;
}
FP radius = d_w < radiusSqr?TSMath.Sqrt(d_w) : behaviour.colliderRadius; // correct the radius, if the Agent is already colliding
FP a = velocity * velocity;
bool discCollision = false, segmentCollision = false;
FP t_min = FP.MaxValue;
FP c = FP.Zero, b = FP.Zero, discr = FP.Zero;
FP b_temp = FP.Zero, discr_temp = FP.Zero, c_temp = FP.Zero, D_temp = FP.Zero;
TSVector2 w_temp = TSVector2.zero, w = TSVector2.zero, o1_temp = TSVector2.zero
, o2_temp = TSVector2.zero, o_temp = TSVector2.zero, o = TSVector2.zero, w_o = TSVector2.zero;
// time-to-collision with disc_1 of the capped rectangle (capsule)
w_temp = obstacle._p1 - position;
b_temp = w_temp * velocity;
c_temp = w_temp * w_temp - (radius * radius);
discr_temp = b_temp * b_temp - a * c_temp;
if (discr_temp > 0 && (a < -CustomMath.EPSILON || a > CustomMath.EPSILON))
{
discr_temp = TSMath.Sqrt(discr_temp);
FP t = (b_temp - discr_temp) / a;
if (t > 0 && t < C_MaxColliderTime)
{
t_min = t;
b = b_temp;
discr = discr_temp;
w = w_temp;
c = c_temp;
discCollision = true;
}
}
// time-to-collision with disc_2 of the capsule
w_temp = obstacle._p2 - position;
b_temp = w_temp * velocity;
c_temp = w_temp * w_temp - (radius * radius);
discr_temp = b_temp * b_temp - a * c_temp;
if (discr_temp > 0 && (a < -CustomMath.EPSILON || a > CustomMath.EPSILON))
{
discr_temp = TSMath.Sqrt(discr_temp);
FP t = (b_temp - discr_temp) / a;
if (t > 0 && t < t_min)
{
t_min = t;
b = b_temp;
discr = discr_temp;
w = w_temp;
c = c_temp;
discCollision = true;
}
}
// time-to-collision with segment_1 of the capsule
o1_temp = obstacle._p1 + radius * obstacle._normal;
o2_temp = obstacle._p2 + radius * obstacle._normal;
o_temp = o2_temp - o1_temp;
D_temp = CustomMath.det(velocity, o_temp);
if (D_temp != 0)
{
FP inverseDet = 1 / D_temp;
FP t = CustomMath.det(o_temp, position - o1_temp) * inverseDet;
FP s = CustomMath.det(velocity, position - o1_temp) * inverseDet;
if (t > 0 && s >= 0 && s <= 1 && t < t_min)
{
t_min = t;
o = o_temp;
w_o = position - o1_temp;
discCollision = false;
segmentCollision = true;
}
}
// time-to-collision with segment_2 of the capsule
o1_temp = obstacle._p1 - radius * obstacle._normal;
o2_temp = obstacle._p2 - radius * obstacle._normal;
o_temp = o2_temp - o1_temp;
D_temp = CustomMath.det(velocity, o_temp);
if (D_temp != 0)
{
FP inverseDet = 1 / D_temp;
FP t = CustomMath.det(o_temp, position - o1_temp) * inverseDet;
FP s = CustomMath.det(velocity, position - o1_temp) * inverseDet;
if (t > 0 && s >= 0 && s <= 1 && t < t_min)
{
t_min = t;
o = o_temp;
w_o = position - o1_temp;
discCollision = false;
segmentCollision = true;
}
}
bool bMax = false;
TSVector2 val = TSVector2.zero;
if (discCollision)
{
if (t_min < FP.EN1 * 2)
{
bMax = true;
// F += (-(velocity - (b * velocity - a * w) / discr)).normalized * behaviour.baseData.maxForce;
}
else
{
val = -_k *System.Math.Exp((-t_min / _t0).AsFloat()) * (velocity - (b * velocity - a * w) / discr)
/ (a * System.Math.Pow(t_min.AsFloat(), _m)) * (_m / t_min + FP.One / _t0);
}
if (bMax || TSMath.Abs(val.x) > behaviour.baseData.maxForce ||
TSMath.Abs(val.y) > behaviour.baseData.maxForce)
{
val = (-(velocity - (b * velocity - a * w) / discr)).normalized * behaviour.baseData.maxForce;
}
F += val;
}
else if (segmentCollision)
{
if (t_min < FP.EN1 * 2)
{
bMax = true;
// F += (CustomMath.det(velocity, o)* new TSVector2(-o.y, o.x)).normalized * behaviour.baseData.maxForce;
}
else
{
val = _k * System.Math.Exp((-t_min / _t0).AsFloat()) / (System.Math.Pow(t_min.AsFloat(), _m)
* CustomMath.det(velocity, o)) * (_m / t_min + FP.One / _t0) * new TSVector2(-o.y, o.x);
}
if (bMax || TSMath.Abs(val.x) > behaviour.baseData.maxForce ||
TSMath.Abs(val.y) > behaviour.baseData.maxForce)
{
val = (CustomMath.det(velocity, o) * new TSVector2(-o.y, o.x)).normalized
* behaviour.baseData.maxForce;
}
F += val;
}
#if UNITY_EDITOR
if (PathFindingManager.DEBUG && behaviour.agent != null)
{
if (TSMath.Abs(F.x) > behaviour.baseData.maxForce * 1000 ||
TSMath.Abs(F.y) > behaviour.baseData.maxForce * 1000)
{
UnityEngine.Debug.Log("F over flow!");
}
}
#endif
}
#endif
//if(desiredF.LengthSquared()<F.LengthSquared())
{
return(F);// + desiredF;
}
//else
//{
// return desiredF;
//}
}
/** 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);
}
static TSVector2 ComputeForce(IAgentBehaviour behaviour, TSVector pos, TSVector otherPosition,
FP radiusSum, bool isStatic, FP maxTime, TSVector basicVelocity, TSVector otherVelocity
, ref bool isTminStaticAgent, ref FP tmin, ref FP time, ref bool isCollidering)
{
TSVector2 F = TSVector2.zero;
TSVector dir = otherPosition - pos;
if (otherVelocity == TSVector.zero && TSVector.Dot(dir, basicVelocity) <= 0)
{
return(F);
}
FP distanceSq = dir.sqrMagnitude;
FP radiusSq = radiusSum * radiusSum;
// bool newMin = false;
// if (distanceSq != radiusSq)
{
// if ForceBasedAgents are actually colliding use their separation distance
TSVector2 v = CustomMath.TSVecToVec2(basicVelocity - otherVelocity);//warning
if (distanceSq < radiusSq)
{
isCollidering = true;
FP r = radiusSum - TSMath.Sqrt(distanceSq);
radiusSq = r * r;
//F = -CustomMath.TSVecToVec2(dir).normalized * behaviour.baseData.maxForce * 3;
//return F;
}
TSVector2 w = CustomMath.TSVecToVec2(dir);
FP a = v * v;
FP b = w * v;
FP c = w * w - radiusSq;
FP discr = b * b - a * c;//(a*t^2-2*b*t+w*w-rSqr=0)
if (discr > 0 && (a < -CustomMath.EPSILON || a > CustomMath.EPSILON))
{
discr = TSMath.Sqrt(discr);
FP t = (b - discr) / a;
bool bMax = false;
TSVector2 val = TSVector2.zero;
if (t > 0 && t < maxTime)
{
if (t < tmin)
{
tmin = t;
time = t;
isTminStaticAgent = isStatic;
// newMin = true;
}
TSVector2 vecDir = (v - (b * v - a * w));
//other.agent!=null? (v - (b * v - a * w))
//: CustomMath.perpendicular(v - (b * v - a * w));
//int rIdx=behaviour.pathManager.RadomIdx()%2;
// vecDir = rIdx == 0 ? vecDir : vecDir * -1;
if (t < FP.EN1 * 2)
{
bMax = true;
// F +=(-(v - (b * v - a * w) / discr)).normalized*behaviour.baseData.maxForce;
}
else
{
//-_k * System.Math.Exp((-t / _t0).AsFloat()) * (v - (b * v - a * w) / discr)
// (a * System.Math.Pow(t.AsFloat(), _m)) * (_m / t + FP.One / _t0);
//
FP fValX = -t / _t0;
val = -_k *CustomMath.ApproximateExp2(fValX) * vecDir
/ (discr * a * t * t) * (_m / t + FP.One / _t0);//System.Math.Exp(().AsFloat())//System.Math.Pow(t.AsFloat(), _m)
}
if (bMax || TSMath.Abs(val.x) > behaviour.baseData.maxForce * c_maxForceFactor ||
TSMath.Abs(val.y) > behaviour.baseData.maxForce * c_maxForceFactor)
{
FP maxXY = TSMath.Max(TSMath.Abs(vecDir.x), TSMath.Abs(vecDir.y));
if (maxXY > 1)
{
vecDir = vecDir / maxXY;
}
val = -(vecDir).normalized * behaviour.baseData.maxForce * c_maxForceFactor;
}
//if(newMin)
//{
// F = val;//only the most threatening agent
//}
F = val;
}
}
}
return(F);
}
/// <summary>
/// 计算球和球的互相作用方向
/// </summary>
/// <returns></returns>
//public static TSVector2[] CheckCircle_CircleCollision(CircleRunData runCircle, CircleRunData staticCircle)
//{
// TSVector2 V = runCircle.next_pos - runCircle.cur_pos;
// TSVector2 U = staticCircle.next_pos - staticCircle.cur_pos;
// TSVector2 AB = staticCircle.cur_pos - runCircle.cur_pos;
// TSVector2 BA = runCircle.cur_pos - staticCircle.cur_pos;
// TSVector2 Vx = TSVector2.Dot(V, AB.normalized)*AB.normalized;
// TSVector2 Vy = V - Vx;
// TSVector2 Ux = TSVector2.Dot(U, BA.normalized) * BA.normalized;
// TSVector2 Uy = U - Ux;
// V = Ux + Vy;//反弹后的方向
// U = Vx + Uy;//反弹后的方向
// return new TSVector2[2] { V,U };
//}
//public static TSVector2[] CheckCircle_CircleCollision(TSVector2 V, TSVector2 U)
//{
// TSVector2 Vx = new TSVector2(V.x, 0);
// TSVector2 Vy = new TSVector2(0, V.y);
// TSVector2 Ux = new TSVector2(U.x, 0);
// TSVector2 Uy = new TSVector2(0, U.y);
// V = Ux + Vy;//反弹后的方向
// U = Vx + Uy;//反弹后的方向
// return new TSVector2[2] { V, U };
//}
/// <summary>
/// 圆和圆的动态相交检测(根据相对运动,抽象为一方是运动,另一方是静止)
/// </summary>
/// <param name="cd"></param>
/// <param name="crd"></param>
/// <returns></returns>
public static bool CheckCircle_CircleContact(CircleRunData runCircle, CircleRunData staticCircle, FP deltaTime, ref FP _percent)
{
TSVector2 VA = runCircle.next_pos - runCircle.cur_pos;
TSVector2 VB = staticCircle.next_pos - staticCircle.cur_pos;
//两个运动方向描述为一方运动另一方静止 so
TSVector2 VAB = VA - VB; //runCircle相对于staticCircle的运动方向pc
TSVector2 Idir = staticCircle.cur_pos - runCircle.cur_pos; //射线起点到静态圆的方向
//FP Idir_length_square = TSVector2.Dot(Idir, Idir);
FP Idir_length_square = Idir.LengthSquared();
FP static_radius_square = (staticCircle.radius * 2) * (staticCircle.radius * 2);
//Func<TSVector2,FP,FP> calHitInfo = (e_dir,a_projvalue) =>
////可以在返回true的时候再计算，后优化
//{
// //TSVector2 e_dir = staticCircle.cur_pos - runCircle.cur_pos;
// //FP a_projvalue = TSMath.Abs(TSVector2.Dot(e_dir, VAB.normalized));
// a_projvalue = TSMath.Abs(a_projvalue);
// FP b_squar = TSVector2.Dot(e_dir, e_dir) - a_projvalue * a_projvalue;
// FP f = TSMath.Sqrt(static_radius_square - b_squar);
// FP t = a_projvalue - f;//碰撞到静态圆所走的路程，总路程是runCircle.cur_pos+VAB*delataTime;
// return t /*/ (VAB * deltaTime).magnitude*/;//求出占比
//};
if (Idir_length_square < static_radius_square) //射线起点在圆心内部,相交
{
//_percent = calHitInfo();
//_percent = 1;//一开始就相交的
//Debug.Log("射线起点在圆心内部");
return(false);
}
else//射线起点在圆心外部的情况
{
FP a_projvalue = TSVector2.Dot(Idir, VAB.normalized);
if (a_projvalue < 0)//球体位于射线原点的后面 不相交
{
return(false);
}
else
{
FP m_square = Idir_length_square - a_projvalue * a_projvalue; //球心到投影点距离的平方
if (m_square - static_radius_square > 0) //预测不相交
{
return(false);
}
else//有可能有交点，因为有可能距离不够
{
//var t = calHitInfo(Idir, a_projvalue);
FP b_squar = m_square;
FP f = TSMath.Sqrt(static_radius_square - b_squar); //理论上来说 f是开跟后的结果,应该有俩个值？
FP t1 = a_projvalue - f; //碰撞到静态圆所走的路程，总路程是runCircle.cur_pos+VAB*delataTime;
FP t2 = a_projvalue + f;
FP per = 0;
bool isFlag = false;
if (t1 > 0 && t1 - VAB.magnitude < 0)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
per = t1 / VAB.magnitude;
}
if (t2 > 0 && t2 - VAB.magnitude < 0)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
var per2 = t2 / VAB.magnitude;
if (per2 < per)
{
per = per2;
}
}
_percent = per;
if (isFlag && _percent < FP.EN4)
{
return(false);
}
return(isFlag);
}
}
}
}
/// <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);
}
private bool Collide(TSVector sphereCenter, FP r, ref TSVector[] vertices,
ref TSVector point, ref TSVector point1, ref TSVector point2, ref TSVector resultNormal, ref FP penetration)
{
TSVector c = sphereCenter;
TSVector delta = TSVector.one;
TSVector normal = TSVector.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
normal = TSVector.Normalize(normal);
TSVector p1ToCentre = c - vertices[0];
FP distanceFromPlane = TSVector.Dot(p1ToCentre, normal);
if (distanceFromPlane < FP.Zero)
{
//triangle facing the other way
distanceFromPlane *= -1;
normal *= -1;
}
FP contactMargin = FP.Zero; //TODO: PersistentManifold.ContactBreakingThreshold;
bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
bool isInsideShellPlane = distanceFromPlane < r;
FP deltaDotNormal = TSVector.Dot(delta, normal);
if (!isInsideShellPlane && deltaDotNormal >= FP.Zero)
{
return(false);
}
// Check for contact / intersection
bool hasContact = false;
TSVector contactPoint = TSVector.zero;
if (isInsideContactPlane)
{
if (FaceContains(ref c, ref vertices, ref normal))
{
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
contactPoint = c - normal * distanceFromPlane;
}
else
{
// Could be inside one of the contact capsules
FP contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
TSVector nearestOnEdge = TSVector.zero;
for (int i = 0; i < 3; i++)
{
TSVector pa, pb;
pa = vertices[i];
pb = vertices[(i + 1) % 3];
FP distanceSqr = SegmentSquareDistance(pa, pb, c, ref nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr)
{
// Yep, we're inside a capsule
contactCapsuleRadiusSqr = distanceSqr;
hasContact = true;
contactPoint = nearestOnEdge;
}
}
}
}
if (hasContact)
{
FP MaxOverlap = FP.Zero; // TODO:
TSVector contactToCentre = c - contactPoint;
FP distanceSqr = contactToCentre.sqrMagnitude;
if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))
{
FP distance = TSMath.Sqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal = TSVector.Normalize(resultNormal);
point = contactPoint;
point1 = point;
resultNormal = TSVector.Negate(resultNormal);
point2 = sphereCenter + resultNormal * r;
penetration = r - distance;
return(true);
}
if (TSVector.Dot(delta, contactToCentre) >= FP.Zero)
{
return(false);
}
// Moving towards the contact point -> collision
point = point1 = point2 = contactPoint;
return(true);
}
return(false);
}
/// <summary>
/// 检测圆是否和线段的端点相交,参考了俩个动态圆的动态相交测试,都是转化为射线和圆的相交测试
/// </summary>
/// <returns></returns>
public static bool CheckCircle_tableEdgeEndContact(CircleRunData runCircle, tableEdge segement, ref FP _percent, ref TSVector2 _nearestPos)
{
TSVector2 cirPos = runCircle.cur_pos;
TSVector2 nearestPos = TSVector2.zero;
//先确定圆的起始位置离哪个端点最近
if (TSVector2.DistanceSquared(runCircle.cur_pos, segement.start) < TSVector2.DistanceSquared(runCircle.cur_pos, segement.end))
{
_nearestPos = nearestPos = segement.start;
}
else
{
_nearestPos = nearestPos = segement.end;
}
//TSVector2 VA = runCircle.next_pos - runCircle.cur_pos;
//TSVector2 VB = staticCircle.next_pos - staticCircle.cur_pos;
//两个运动方向描述为一方运动另一方静止 so
//TSVector2 VAB = VA - VB;//runCircle相对于staticCircle的运动方向pc
TSVector2 VAB = runCircle.next_pos - runCircle.cur_pos; //动态圆射线运动方向
TSVector2 Idir = nearestPos - cirPos; //射线起点到静态圆的方向
//FP Idir_length_square = TSVector2.Dot(Idir, Idir);
FP Idir_length_square = Idir.LengthSquared();
FP static_radius_square = runCircle.radius * runCircle.radius;
if (Idir_length_square < static_radius_square)//射线起点在圆心内部,相交
{
//_percent = calHitInfo();
//_percent = 1;//一开始就相交的
//Debug.Log("射线起点在圆心内部");
return(false);
}
else//射线起点在圆心外部的情况
{
FP a_projvalue = TSVector2.Dot(Idir, VAB.normalized);
if (a_projvalue < 0)//球体位于射线原点的后面 不相交
{
return(false);
}
else
{
FP m_square = Idir_length_square - a_projvalue * a_projvalue; //球心到投影点距离的平方
if (m_square - static_radius_square > 0) //预测不相交
{
return(false);
}
else//有可能有交点，因为有可能距离不够
{
//var t = calHitInfo(Idir, a_projvalue);
FP b_squar = m_square;
FP f = TSMath.Sqrt(static_radius_square - b_squar); //理论上来说 f是开跟后的结果,应该有俩个值？
FP t1 = a_projvalue - f; //碰撞到静态圆所走的路程，总路程是runCircle.cur_pos+VAB*delataTime;
FP t2 = a_projvalue + f;
FP per = 0;
bool isFlag = false;
if (t1 > 0 && t1 - VAB.magnitude < FP.EN8)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
per = t1 / VAB.magnitude;
}
if (t2 > 0 && t2 - VAB.magnitude < 0)
{
isFlag = true;
if (VAB.magnitude < 0)
{
Debug.Log("除数不能为0");
}
var per2 = t2 / VAB.magnitude;
if (per2 < per)
{
per = per2;
}
}
_percent = per;
if (_percent > 1)
{
Debug.Log("路程百分比大于1,注意!");
}
if (isFlag && _percent < FP.EN4)
{
return(false);
}
return(isFlag);
}
}
}
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
TSVector2 tSVector = MathUtils.Mul(ref xf, this.Position);
FP fP = -(TSVector2.Dot(normal, tSVector) - offset);
bool flag = fP < -base.Radius + Settings.Epsilon;
FP result;
if (flag)
{
result = 0;
}
else
{
bool flag2 = fP > base.Radius;
if (flag2)
{
sc = tSVector;
result = Settings.Pi * this._2radius;
}
else
{
FP y = fP * fP;
FP fP2 = this._2radius * (TSMath.Asin(fP / base.Radius) + TSMath.PiOver2 + fP * TSMath.Sqrt(this._2radius - y));
FP y2 = new FP(-2) / new FP(3) * Math.Pow((double)(this._2radius - y).AsFloat(), 1.5) / fP2;
sc.x = tSVector.x + normal.x * y2;
sc.y = tSVector.y + normal.y * y2;
result = fP2;
}
}
return(result);
}
public FP GetSphereDistance(ref BoxShape box, ref TSVector boxPosition, ref TSMatrix boxOrientation, ref TSVector sphereCenter, FP radius,
ref TSVector pointOnBox, ref TSVector pointOnSphere, ref TSVector normal)
{
FP margins;
bounds[0] = TSVector.Negate(box.halfSize);
bounds[1] = box.halfSize;
margins = FP.Zero; //TODO: box.Margin; //also add sphereShape margin?
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
TSVector marginsVec = TSVector.one * margins;
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
TSVector prel, v3P;
FP sep = FP.MaxValue;
FP sepThis;
// convert point in local space
TSVector.Subtract(ref sphereCenter, ref boxPosition, out sphereCenter);
TSMatrix invBoxOrientation;
TSMatrix.Inverse(ref boxOrientation, out invBoxOrientation);
TSVector.Transform(ref sphereCenter, ref invBoxOrientation, out prel);
bool found = false;
v3P = prel;
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (TSVector.Dot(v3P - bounds[j], n[i]))) > FP.Zero)
{
v3P = v3P - n[i] * sepThis;
found = true;
}
}
if (found)
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = TSVector.Normalize(prel - v3P);
pointOnBox = v3P + normal * margins;
pointOnSphere = prel - normal * radius;
if ((TSVector.Dot(pointOnSphere - pointOnBox, normal)) > FP.Zero)
{
return(FP.One);
}
FP seps2 = (pointOnBox - pointOnSphere).sqrMagnitude;
//if this fails, fallback into deeper penetration case, below
if (seps2 > TSMath.Epsilon)
{
// transform back in world space
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
TSVector.Transform(ref pointOnSphere, ref boxOrientation, out pointOnSphere);
TSVector.Add(ref pointOnSphere, ref boxPosition, out pointOnSphere);
sep = -TSMath.Sqrt(seps2);
normal = (pointOnBox - pointOnSphere);
normal *= FP.One / sep;
}
return(sep);
}
return(FP.One);
}
/// <summary>
///
/// </summary>
/// <param name="forceToApply">basic force</param>
/// <param name="basicVelocity">basic Velocity</param>
/// <returns></returns>
protected TSVector ApplyForce(FP deltaTime, TSVector desiredDirection, TSVector basicVelocity, TSVector pos, List <IAgentBehaviour> agents, bool bUsePreForce, bool bSkipStatic)//
{
// isTminStaticAgent = false;
int count = agents.Count;
TSVector boidsVelocity = TSVector.zero;
//TSVector forceToApply = TSVector.zero;
TSVector forceToApply = TSVector.zero;
TSVector avoidObstacle = TSVector.zero;
FP mAvoidObstacle = FP.Zero;
if (IsBoisTypeActive(EBoidsActiveType.terrainSeperation) && false)
{
avoidObstacle = Boids.BoidsBehaviourAvoidObstacle(pos, _behaviour.map, basicVelocity);
//desiredDirection = (desiredDirection + dir * S_terrainSepFactor);
//if(desiredDirection!=TSVector.zero)
//{
// desiredDirection = desiredDirection.normalized;
//}
avoidObstacle = CustomMath.Normalize(avoidObstacle, out mAvoidObstacle);
}
if (desiredDirection != TSVector.zero)
{
desiredDirection = CustomMath.Normalize(desiredDirection);
forceToApply += Boids.SteerTowards(_behaviour, desiredDirection, basicVelocity);
}
if (mAvoidObstacle > FP.Zero)
{
forceToApply += Boids.SteerTowards(_behaviour, avoidObstacle, basicVelocity);
}
if (PathFindingManager.c_useAvoidUnit)
{
if (IsBoisTypeActive(EBoidsActiveType.avoidUnit))
{
TSVector avoidVector = SteerForUnitAvoidance.GetDesiredSteering(_behaviour);
TSVector force = TSVector.ClampMagnitude(avoidVector / deltaTime * _behaviour.baseData.invMass, _behaviour.baseData.maxForce);
forceToApply += force;
}
//else if (IsBoisTypeActive(EBoidsActiveType.collisionAvoidance))
//{
// // bool isTminStaticAgent = false;
// forceToApply += CustomMath.TSVec2ToVec(ForceBasedAgent.computeForces(_behaviour, basicVelocity,out isTminStaticAgent));
//}
}
if (_activeBoids > 0 && !bUsePreForce &&
(IsBoisTypeActive(EBoidsActiveType.seperation) || IsBoisTypeActive(EBoidsActiveType.cohesion) ||
IsBoisTypeActive(EBoidsActiveType.alignment)))
{
// int count =(i%3== _updateStart)? this.neighbours.Count:0;//_group._thiss
TSVector totalForce = TSVector.zero;
TSVector averageHeading = TSVector.zero;
TSVector centerOfMass = TSVector.zero;
int neighboursCountSep = 0;
int neighboursCountAlig = 0;
int neighboursCountCoh = 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
if (IsBoisTypeActive(EBoidsActiveType.seperation))
{
Boids.BoidsBehaviourSeparation(_behaviour, a, sepSqr, ref totalForce, ref neighboursCountSep, bSkipStatic);
}
if (IsBoisTypeActive(EBoidsActiveType.alignment))
{
Boids.BoidsBehaviourAlignment(_behaviour, a, nrSqr, ref averageHeading, ref neighboursCountAlig);
}
if (IsBoisTypeActive(EBoidsActiveType.cohesion))
{
Boids.BoidsBehaviourCohesion(_behaviour, a, nrSqr, ref centerOfMass, ref neighboursCountCoh);
}
if (a.enabled && a.agent == null) //the ally neighbour has a target,
//cause this agent change type to atar type targeted to the same target
{
PathFindingAgentBehaviour agent = _behaviour as PathFindingAgentBehaviour;
if (agent.AgentType == EAgentType.flowFiled)
{
TSVector target = (a as PathFindingAgentBehaviour).targetPos;
bool isDefaultTaget = target == a.baseData.defaultTargetPos;
if (target != TSVector.MaxValue && target != TSVector.MinValue && !isDefaultTaget)
{
agent.stopMoving = false;
agent._hasNeighbourTargetedDefaultPos = true;
target.y = 0;
agent.SetNewTargetPos(target, true);
}
if (!isDefaultTaget)
{
agent.preVelocity = TSVector.zero;
forceToApply = TSVector.zero;
// agent.velocity = TSVector.zero;
// agent.ChangeAgentType(EAgentType.astar);
return(forceToApply);
}
}
}
}
if (count > 0)
{
TSVector sep = TSVector.zero;
if (totalForce != TSVector.zero)
{
//totalForce.Multiply(agent.maxForce / neighboursCount)
sep = totalForce * (_behaviour.baseData.maxForce) / neighboursCountSep;
//sep=Boids.SteerTowards(_behaviour, sep, basicVelocity);
//
FP lenSqr = sep.sqrMagnitude;
if (lenSqr > _behaviour.baseData.maxForceSqr)
{
FP fval = _behaviour.baseData.maxForce / TSMath.Sqrt(lenSqr);
sep = sep * fval;
}
}
TSVector avh = TSVector.zero;
if (averageHeading != TSVector.zero) //average heading
{
averageHeading = averageHeading / (neighboursCountAlig);
avh = Boids.SteerTowards(_behaviour, CustomMath.Normalize(averageHeading), basicVelocity);
}
//average position
TSVector coh = TSVector.zero;
if (centerOfMass != TSVector.zero)// seek that position
{
centerOfMass = centerOfMass + _behaviour.position;
neighboursCountCoh++;
centerOfMass = centerOfMass / neighboursCountCoh;
coh = Boids.BoidsBehaviourSeek(_behaviour, basicVelocity, centerOfMass);
}
_preBoidsForce = sep * S_seperationFactor + avh * S_alignmentFactor + coh * S_cohesionFactor;
forceToApply += _preBoidsForce;
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
}
}
}
else if (bUsePreForce)
{
forceToApply += _preBoidsForce;
}
if (forceToApply != TSVector.zero)
{
if (TSMath.Abs(forceToApply.x) > _behaviour.baseData.maxForce ||
TSMath.Abs(forceToApply.z) > _behaviour.baseData.maxForce)
//FP lengthSquared = forceToApply.sqrMagnitude;
//if (lengthSquared > _behaviour.baseData.maxForceSqr)//&& _activeBoids!=(byte)EBoidsActiveType.seperation)
{
forceToApply = forceToApply * FP.EN3;
forceToApply = _behaviour.baseData.maxForce * forceToApply.normalized;
}
return(forceToApply * _behaviour.baseData.invMass);//
}
return(forceToApply);
}
