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