//不旋转的box
public static FP SDBox(TSVector2 x, TSVector2 c, TSVector2 b)
{
TSVector2 p = x - c;
p.x = TSMath.Abs(p.x);
p.y = TSMath.Abs(p.y);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
//旋转的box
public static FP SDOrientedBox(TSVector2 x, TSVector2 c, TSVector2 rot, TSVector2 b)
{
TSVector2 v = x - c;
FP px = TSMath.Abs(TSVector2.Dot(v, rot)); //在box的x轴的投影长度
FP py = TSMath.Abs(TSVector2.Dot(v, new TSVector2(-rot.y, rot.x))); //在box的y轴的投影长度
TSVector2 p = new TSVector2(px, py);
TSVector2 d = p - b;
return(TSVector2.Max(d, TSVector2.zero).sqrMagnitude + TSMath.Min(TSMath.Max(d.x, d.y), FP.Zero));
}
/// <summary>
/// 处理所有的碰撞记录
/// </summary>
//void ProcessHitData()
//{
// foreach(var ballpair in ballPairHit)//每个球对应的碰撞记录集合
// {
// int ID = ballpair.Key;
// var baseHits = ballpair.Value;
// for(int i =0; i < baseHits.Count;i++)//其中一个球的碰撞记录集合
// {
// var baseHit = baseHits[i];
// if(baseHit.hitType==HitType.Ball)
// {
// var _baseHit = baseHit as fastHitBall;
// int otherID = _baseHit.staticballObj.ID;
// if (ballPairHit.ContainsKey(otherID))
// {
// var other_baseHits = ballPairHit[otherID];
// for(int j = 0; j<other_baseHits.Count;j++)
// {
// var otherHit = other_baseHits[j];
// if(otherHit.hitType==HitType.Ball)
// {
// var _otherHit = otherHit as fastHitBall;
// if (_baseHit.runballObj.ID == _otherHit.staticballObj.ID)
// {
// continue;
// }
// else
// {
// if (/*_baseHit.CalHitTime()*/_baseHit.t_percent * _baseHit.staticballObj.deltaTime > _otherHit.CalHitTime())
// {
// _baseHit.valid = false;
// }
// else
// _otherHit.valid = false;
// }
// }
// else
// {
// var _otherHit = otherHit as fastEdge;
// if (/*_baseHit.CalHitTime()*/ _baseHit.t_percent * _baseHit.staticballObj.deltaTime > _otherHit.CalHitTime())
// {
// _baseHit.valid = false;
// }
// else
// _otherHit.valid = false;
// }
// }
// }
// else
// Debug.Log("一般碰撞对都是成对记录出现,不可能走到这!");
// }
// }
// }
// bool isFlag = false;
// foreach (var ballpair in ballPairHit)
// {
// int ID = ballpair.Key;
// var baseHits = ballpair.Value.OrderBy((m)=>m.CalHitTime()).ToList();//按时间来排序
// BaseHit baseHit=null;
// for(int i = 0; i < baseHits.Count;i++)
// {
// if (baseHits[i].valid == false) continue;
// baseHit = baseHits[i];
// break;
// }
// if (baseHit!=null&&baseHit.valid == true && !baseHit.Isprocess())
// {
// isFlag = true;
// baseHit.TagProcess();
// if (baseHit.hitType == HitType.Ball)
// {
// var _baseHit = baseHit as fastHitBall;
// _baseHit.runballObj.CalBallPos(_baseHit.t_percent * _baseHit.runballObj.deltaTime);
// _baseHit.staticballObj.CalBallPos(_baseHit.t_percent * _baseHit.staticballObj.deltaTime);
// //updateDirAndTimeByBall(_baseHit.t_percent, _baseHit.runballObj, _baseHit.staticballObj);
// }
// else if (baseHit.hitType == HitType.Edge)
// {
// var _baseHit = baseHit as fastEdge;
// _baseHit.ball.CalBallPos(_baseHit.t_percent * _baseHit.ball.deltaTime);
// //updateDirAndTimeByEdge(_baseHit.t_percent, _baseHit.tbe, _baseHit.ball);
// }
// }
// }
// if(isFlag==false)
// {
// Debug.Log("虽然有碰撞记录,但是没有执行碰撞,这里可能会导致死循环的情况,因为一直检测到有碰撞,却不执行");
// }
// UnLockBalls();
// ballPairHit.Clear();
//}
public bool CheckBound(TSVector2 pos)
{
var x = TSMath.Abs(pos.x);
var y = TSMath.Abs(pos.y);
if (x + TSMath.Epsilon > tableWidth / 2.0 || y + TSMath.Epsilon > TableHeight / 2.0)
{
return(true);
}
return(false);
}
/// <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);
}
public void ReqSetForward(TSVector forward, bool immediately = true)
{
if (TSMath.Abs(TSVector.Angle(m_sCurForward, forward)) < FP.EN1)
{
return;
}
Frame_ReqSetForward_Data data = new Frame_ReqSetForward_Data();
data.unitId = id;
data.forward = GameInTool.ToProtoVector2(forward);
data.immediately = immediately;
NetSys.Instance.SendMsg(NetChannelType.Game, (short)PacketOpcode.Frame_ReqSetForward, data);
}
public override void OnSyncedUpdate()
{
if (useSpring)
{
//Adding a spring and damper Term to the Equation of Motion
thisBody.AddTorque((-1) * TSWorldAxis * ((thisJoint.getHingeAngle() - Spring.tagetPosition) * Spring.spring + thisJoint.getAngularVel() * Spring.damper));
}
if (TSMath.Abs(thisJoint.AppliedImpulse) >= breakForce)//@TODO: Add break torque
{
thisJoint.Deactivate();
Destroy(this);
}
}
static void DrawVO(TSVector2 circleCenter, FP radius, TSVector2 origin)
{
FP alpha = TSMath.Atan2((origin - circleCenter).y, (origin - circleCenter).x);
FP gamma = radius / (origin - circleCenter).magnitude;
FP delta = gamma <= FP.One ? TSMath.Abs(TSMath.Acos(gamma)) : 0;
// Draw.Debug.CircleXZ(FromXZ(circleCenter), radius, Color.black, alpha - delta, alpha + delta);
TSVector2 p1 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta)) * radius;
TSVector2 p2 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta)) * radius;
TSVector2 p1t = -new TSVector2(-p1.y, p1.x);
TSVector2 p2t = new TSVector2(-p2.y, p2.x);
p1 += circleCenter;
p2 += circleCenter;
// Debug.DrawRay(FromXZ(p1), FromXZ(p1t).normalized * 100, Color.black);
// Debug.DrawRay(FromXZ(p2), FromXZ(p2t).normalized * 100, Color.black);
}
/// <summary>
/// 圆碰撞的反弹方向
/// </summary>
/// <param name="tedge">线段</param>
/// <param name="circlePos">圆心</param>
/// <param name="radius">半径</param>
/// <param name="moveDir">圆的速度方向</param>
/// <returns></returns>
public static TSVector2 CheckCircle_LineCollision(tableEdge tedge, TSVector2 circlePos, FP radius, TSVector2 moveDir)
{
TSVector2 C = circlePos;
TSVector2 A = tedge.farstart;
TSVector2 B = tedge.farend;
TSVector2 V = moveDir;
TSVector2 AC = C - A;
TSVector2 AB = B - A;
TSVector2 ABnormal = TSVector2.Normalize(AB);
TSVector2 HC = AC - ABnormal * TSMath.Abs(TSVector2.Dot(AC, ABnormal));
TSVector2 HCnormal = TSVector2.Normalize(HC);
FP VP = TSMath.Abs(TSVector2.Dot(V, HCnormal));
if (TSMath.Abs(VP) > 10000)
{
Debug.Log("圆碰边速度过大");
}
TSVector2 VF = V + HCnormal * VP * 2;//反射方向
return(VF);
}
void CreateTable2()
{
int big = 99999;
tableWidth = 0;
tableHeight = 0;
int factor = 10;
FP[] edges = new FP[] { -1.39, 0.46, -1.44, 0.53, -1.36, 0.63, -1.31, 0.59, -0.08, 0.59, -0.07, 0.64, 0.07, 0.64, 0.08, 0.59, 1.31, 0.59, 1.36, 0.63, 1.44, 0.53, 1.39, 0.46,
1.39, -0.46, 1.44, -0.53, 1.36, -0.63, 1.31, -0.59, 0.08, -0.59, 0.07, -0.64, -0.07, -0.64, -0.08, -0.59, -1.31, -0.59, -1.36, -0.63, -1.44, -0.53, -1.39, -0.46, -1.39, 0.46 };
for (int j = 0, i = 0; j < edges.Length - 2; i++)
{
tableEdges.Add(new tableEdge(new TSVector2(edges[j] * factor, edges[j + 1] * factor), new TSVector2(edges[j + 2] * factor, edges[j + 3] * factor), i));
var w = TSMath.Abs(edges[j] * factor);
var h = TSMath.Abs(edges[j + 1] * factor);
if (tableWidth < w)
{
tableWidth = w;
}
if (tableHeight < h)
{
tableHeight = h;
}
w = TSMath.Abs(edges[j + 2] * factor);
h = TSMath.Abs(edges[j + 3] * factor);
if (tableWidth < w)
{
tableWidth = w;
}
if (tableHeight < h)
{
tableHeight = h;
}
j += 2;
}
tableWidth *= 2;
tableHeight *= 2;
}
void MoveCallBack(Vector2 tVec2)
{
TSVector2 mTSVector2 = new TSVector2((FP)(tVec2.x - 0.001f), (FP)(tVec2.y - 0.001f)).normalized;//这里减0.001f是因为这里精度有问题,会传来一个极其微小大于1的浮点数,后面使用时候导致FP Acos()中算出来的1值(4294967297)大于FP.ONE值(4294967296)
FP anglenew = TSVector2.Angle(mTSVector2, TSVector2.up);
FP angle = TSQuaternion.Angle(AllTSTransform.rotation, RotateTSTransform.rotation);
//FP iTanDeg = TSMath.Atan2((FP)tVec2.y, (FP)tVec2.x) * FP.Rad2Deg;
//Debug.LogErrorFormat("MoveCallBack===12=======>{0},{1},{2},{3},iTanDeg={4}", mTSVector2.x, mTSVector2.y, mTSVector2.ToString(), anglenew, iTanDeg);
if (mTSVector2.x < 0 && (360 - anglenew) < 183)
{
anglenew = 360 - anglenew; //这里让他们夹角在0-183范围内,刚好与下面的0-180错开3(灵敏度5的一半),这样就少算了一次RotateTransform.rotation.eulerAngles的值
}
//if (mTSVector2.y < 0) anglenew = 360 - anglenew;
//if (RotateTransform.rotation.eulerAngles.x < 0) angle = 360 - angle;
//Debug.LogErrorFormat("MoveCallBack==2====>{0},{1},ToString={2}", anglenew, angle, RotateTSTransform.rotation.ToString());
if (TSMath.Abs(angle - anglenew) > 5)
{
int x = FP.ToInt(mTSVector2.x * 1000);
int y = FP.ToInt(mTSVector2.y * 1000);
//Debug.LogErrorFormat("MoveCallBack=3==>{0},{1}", x, y);
_UdpSendManager.SendChangeAngle(x, y);
}
}
/// <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);
}
internal TSVector steeringBehaviourFlowField(IAgentBehaviour agent)
{
//bilinear interpolation
TSVector vec = _gridMap.GetGridFloatCoord(agent.position);
IInt2 floor = IInt2.zero;// GetNearestGridCoordWithoutClamp(agent.position);
floor.x = TSMath.Floor(vec.x).AsInt();
floor.y = TSMath.Floor(vec.z).AsInt();
bool bIn1 = IsValid(floor.x, floor.y);
TSVector f00 = (bIn1) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y)) : TSVector.zero;//(posOrigin - pos1) ;// TSVector.zero
int offsetX = f00.x > 0 ? 1 : -1;
int offsetY = f00.z > 0 ? 1 : -1;
bool bIn2 = IsValid(floor.x, floor.y + offsetY);
bool bIn3 = IsValid(floor.x + offsetX, floor.y);
bool bIn4 = IsValid(floor.x + offsetX, floor.y + offsetY);
// bool bAllIn = bIn1 && bIn2 && bIn3 && bIn4;//bAllIn ||!
//
//TSVector posOrigin = (TSVector)vec;
//TSVector pos1 = TSVector.zero;
//pos1.Set((float)floor.x, 0, floor.y);
//TSVector pos2 = TSVector.zero;
//pos2.Set((float)floor.x, 0, floor.y + 1);
//TSVector pos3 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y);
//TSVector pos4 = TSVector.zero;
//pos3.Set((float)floor.x + 1, 0, floor.y + 1);
//TSVector f00 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y)] : (bIn1?TSVector.zero: ((bIn2&& bIn3&& bIn4) ? TSVector._northEst:(bIn2?TSVector._forward:(bIn3?TSVector._right:TSVector.zero))));
//TSVector f01 = (bAllIn) ? _flowField[ClampX(floor.x), ClampZ(floor.y + 1)] : (bIn2 ? TSVector.zero : ((bIn1 && bIn3 && bIn4) ? TSVector._southEst : (bIn1 ? -TSVector._forward : (bIn4 ? TSVector._right : TSVector.zero))));
//TSVector f10 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y)] : (bIn3 ? TSVector.zero : ((bIn2 && bIn1 && bIn4) ? TSVector._northWest : (bIn4 ? TSVector._forward : (bIn1 ? -TSVector._right : TSVector.zero))));
//TSVector f11 = (bAllIn) ? _flowField[ClampX(floor.x + 1), ClampZ(floor.y + 1)] : (bIn4 ? TSVector.zero : ((bIn2 && bIn3 && bIn1) ? TSVector._southWest : (bIn3 ? -TSVector._forward : (bIn2 ? -TSVector._right : TSVector.zero))));
TSVector f01 = (bIn2) ? GetFlowField(_gridMap.ClampX(floor.x), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; //(posOrigin - pos2);
TSVector f10 = (bIn3) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y)) : TSVector.zero; //(posOrigin - pos3);
TSVector f11 = (bIn4) ? GetFlowField(_gridMap.ClampX(floor.x + offsetX), _gridMap.ClampZ(floor.y + offsetY)) : TSVector.zero; // (posOrigin - pos4);
//Do the x interpolations
FP fx = agent.position.x;
FP fy = agent.position.z;
FP w = FP.One * _gridWidth * GridMap.GetNodeSize();
FP h = FP.One * _gridHeight * GridMap.GetNodeSize();
FP dstX0 = TSMath.Abs(vec.x - (floor.x + CustomMath.FPHalf));
FP dstX1 = TSMath.Abs(vec.x - (floor.x + offsetX + CustomMath.FPHalf));
FP dstY0 = TSMath.Abs(vec.z - (floor.y + CustomMath.FPHalf));
FP dstY1 = TSMath.Abs(vec.z - (floor.y + offsetY + CustomMath.FPHalf));
FP xW = (dstX0) / (dstX0 + dstX1);
FP xWeight = (fx < w && fx >= FP.Zero) ? (xW) : FP.Zero;//vec.x - floor.x
TSVector top = f00 * (1 - xWeight) + f10 * (xWeight);
TSVector bottom = f01 * (1 - xWeight) + f11 * xWeight;
FP yW = (dstY0) / (dstY0 + dstY1);
//Do the y interpolation
FP yWeight = (fy < h && fy >= FP.Zero) ? (yW) : FP.Zero;//vec.z - floor.y
//
TSVector desiredDirection = (top * (1 - yWeight) + bottom * yWeight);
//}
// desiredDirection = desiredDirection + Boids.BoidsBehaviourTerrainSeparation(vec,IsValid);
//desiredDirection = desiredDirection.normalized;
// return Boids.SteerTowards(agent, desiredDirection);
return(desiredDirection);
}
/// <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);
}
/// <summary>
/// 更通用的圆和边碰撞,对端点的碰撞也能适用
/// </summary>
/// <param name="tedge"></param>
/// <param name="circlePos"></param>
/// <param name="radius"></param>
/// <param name="moveDir"></param>
/// <returns></returns>
public static TSVector2 CheckCircle_EdgeCollision(TSVector2 hitNormal, TSVector2 moveDir)
{
FP VP = TSVector2.Dot(moveDir, hitNormal);
return(moveDir - hitNormal * VP + hitNormal * TSMath.Abs(VP));
}
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;
//}
}
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);
}
/** Creates a VO for avoiding another agent.
* \param center The position of the other agent relative to this agent.
* \param offset Offset of the velocity obstacle. For example to account for the agents' relative velocities.
* \param radius Combined radius of the two agents (radius1 + radius2).
* \param inverseDt 1 divided by the local avoidance time horizon (e.g avoid agents that we will hit within the next 2 seconds).
* \param inverseDeltaTime 1 divided by the time step length.
*/
public VO(TSVector2 center, TSVector2 offset, FP radius, FP inverseDt, FP inverseDeltaTime)
{
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
this.weightFactor = 1;
weightBonus = 0;
//this.radius = radius;
TSVector2 globalCenter;
circleCenter = center * inverseDt + offset;
FP tmp = Sqr(center.LengthSquared() / (radius * radius));//exp(-tmp),
// tmp = 1 +tmp+tmp*tmp/2+tmp*tmp*tmp/6; //simple use Taylor's Formula
this.weightFactor = 4 * System.Math.Exp(-tmp.AsFloat()) + 1;// 4 /tmp + 1;//exp()
// Collision?
if (center.magnitude < radius)
{
colliding = true;
// 0.001 is there to make sure lin1.magnitude is not so small that the normalization
// below will return TSVector2.zero as that will make the VO invalid and it will be ignored.
line1 = center.normalized * (center.magnitude - radius - FP.One / 1000) * 3 / 10 * inverseDeltaTime;
dir1 = new TSVector2(line1.y, -line1.x).normalized;
line1 += offset;
cutoffDir = TSVector2.zero;
cutoffLine = TSVector2.zero;
dir2 = TSVector2.zero;
line2 = TSVector2.zero;
this.radius = 0;
}
else
{
colliding = false;
center *= inverseDt;
radius *= inverseDt;
globalCenter = center + offset;
// 0.001 is there to make sure cutoffDistance is not so small that the normalization
// below will return TSVector2.zero as that will make the VO invalid and it will be ignored.
var cutoffDistance = center.magnitude - radius + FP.One / 1000;
cutoffLine = center.normalized * cutoffDistance;
cutoffDir = new TSVector2(-cutoffLine.y, cutoffLine.x).normalized;
cutoffLine += offset;
FP alpha = TSMath.Atan2(-center.y, -center.x);
FP delta = TSMath.Abs(TSMath.Acos(radius / center.magnitude));
this.radius = radius;
// Bounding Lines
// Point on circle
line1 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir1 = new TSVector2(line1.y, -line1.x);
// Point on circle
line2 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir2 = new TSVector2(line2.y, -line2.x);
line1 = line1 * radius + globalCenter;
line2 = line2 * radius + globalCenter;
}
segmentStart = TSVector2.zero;
segmentEnd = TSVector2.zero;
segment = false;
}
public static TSVector2 Abs(TSVector2 v)
{
v.x = TSMath.Abs(v.x);
v.y = TSMath.Abs(v.y);
return(v);
}
public int RayCast(DynamicTreeRayCastCallback callback, ref RayCastInput input, Stack <int> _tmpStack, bool bTestHasNode, int excludeProxyId = -1)
{
TSVector2 p1 = input.p1;
TSVector2 p2 = input.p2;
TSVector2 r = p2 - p1;
Assert(r.LengthSquared() >= FP.Zero);
r.Normalize();
// v is perpendicular to the segment.
TSVector2 v = CustomMath.Cross(FP.One, r);
TSVector2 abs_v = CustomMath.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FP maxFraction = input.maxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
TSVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = TSVector2.Min(p1, t);
segmentAABB.upperBound = TSVector2.Max(p1, t);
}
Stack <int> stack = _tmpStack;
stack.Clear();
stack.Push(_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == TreeNode <T> .nullNode)
{
continue;
}
TreeNode <T> node = _nodes[nodeId];
if (TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
TSVector2 c = node.aabb.GetCenter();
TSVector2 h = node.aabb.GetExtents();
FP separation = TSMath.Abs(TSVector2.Dot(v, p1 - c)) - TSVector2.Dot(abs_v, h);
if (separation > FP.Zero)
{
continue;
}
if (node.IsLeaf())
{
if (bTestHasNode && nodeId != excludeProxyId)
{
return(nodeId);
}
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
FP value = FP.One;
if (callback != null)
{
value = callback(ref subInput, nodeId);
}
if (value == FP.Zero)
{
// The client has terminated the ray cast.
return(-1);
}
if (value > FP.Zero)
{
// Update segment bounding box.
maxFraction = value;
TSVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = TSVector2.Min(p1, t);
segmentAABB.upperBound = TSVector2.Max(p1, t);
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
return(-1);
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FP dvx, dvy, dvz;
dvx = (body2.angularVelocity.y * relativePos2.z) - (body2.angularVelocity.z * relativePos2.y) + body2.linearVelocity.x;
dvy = (body2.angularVelocity.z * relativePos2.x) - (body2.angularVelocity.x * relativePos2.z) + body2.linearVelocity.y;
dvz = (body2.angularVelocity.x * relativePos2.y) - (body2.angularVelocity.y * relativePos2.x) + body2.linearVelocity.z;
dvx = dvx - (body1.angularVelocity.y * relativePos1.z) + (body1.angularVelocity.z * relativePos1.y) - body1.linearVelocity.x;
dvy = dvy - (body1.angularVelocity.z * relativePos1.x) + (body1.angularVelocity.x * relativePos1.z) - body1.linearVelocity.y;
dvz = dvz - (body1.angularVelocity.x * relativePos1.y) + (body1.angularVelocity.y * relativePos1.x) - body1.linearVelocity.z;
FP kNormal = FP.Zero;
TSVector rantra = TSVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * normal.z) - (relativePos1.z * normal.y);
rantra.y = (relativePos1.z * normal.x) - (relativePos1.x * normal.z);
rantra.z = (relativePos1.x * normal.y) - (relativePos1.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
TSVector rbntrb = TSVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * normal.z) - (relativePos2.z * normal.y);
rbntrb.y = (relativePos2.z * normal.x) - (relativePos2.x * normal.z);
rbntrb.z = (relativePos2.x * normal.y) - (relativePos2.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kNormal += rantra.x * normal.x + rantra.y * normal.y + rantra.z * normal.z;
}
if (!treatBody2AsStatic)
{
kNormal += rbntrb.x * normal.x + rbntrb.y * normal.y + rbntrb.z * normal.z;
}
massNormal = FP.One / kNormal;
FP num = dvx * normal.x + dvy * normal.y + dvz * normal.z;
tangent.x = dvx - normal.x * num;
tangent.y = dvy - normal.y * num;
tangent.z = dvz - normal.z * num;
num = tangent.x * tangent.x + tangent.y * tangent.y + tangent.z * tangent.z;
if (num != FP.Zero)
{
num = FP.Sqrt(num);
tangent.x /= num;
tangent.y /= num;
tangent.z /= num;
}
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * tangent.z) - (relativePos1.z * tangent.y);
rantra.y = (relativePos1.z * tangent.x) - (relativePos1.x * tangent.z);
rantra.z = (relativePos1.x * tangent.y) - (relativePos1.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * tangent.z) - (relativePos2.z * tangent.y);
rbntrb.y = (relativePos2.z * tangent.x) - (relativePos2.x * tangent.z);
rbntrb.z = (relativePos2.x * tangent.y) - (relativePos2.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kTangent += TSVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += TSVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = normal.x * dvx + normal.y * dvy + normal.z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -(tangent.x * dvx + tangent.y * dvy + tangent.z * dvz);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
TSVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
// modified by tiger, uncommmented.
//if (relNormalVel < -FP.One && newContact)
//{
// restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
//}
// added by seok
//if (!newContact)
if (!newContact || TSMath.Abs(relNormalVel *= restitution) < restitution)
{
relNormalVel = 0;
}
restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse.x = normal.x * accumulatedNormalImpulse + tangent.x * accumulatedTangentImpulse;
impulse.y = normal.y * accumulatedNormalImpulse + tangent.y * accumulatedTangentImpulse;
impulse.z = normal.z * accumulatedNormalImpulse + tangent.z * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.x -= (impulse.x * body1.inverseMass);
body1.linearVelocity.y -= (impulse.y * body1.inverseMass);
body1.linearVelocity.z -= (impulse.z * body1.inverseMass);
if (!body1IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y;
num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z;
num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x;
FP num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
FP num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
FP num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.x -= num3;
body1.angularVelocity.y -= num4;
body1.angularVelocity.z -= num5;
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.x += (impulse.x * body2.inverseMass);
body2.linearVelocity.y += (impulse.y * body2.inverseMass);
body2.linearVelocity.z += (impulse.z * body2.inverseMass);
if (!body2IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y;
num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z;
num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x;
FP num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
FP num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
FP num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.x += num3;
body2.angularVelocity.y += num4;
body2.angularVelocity.z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
public bool RayCast(ref RayCastOutput output, ref RayCastInput input)
{
FP tmin = FP.MinValue;
FP tmax = FP.MaxValue;
TSVector2 p = input.p1;
TSVector2 d = input.p2 - input.p1;
TSVector2 absD = d;
absD.x = TSMath.Abs(absD.x);
absD.y = TSMath.Abs(absD.y);
TSVector2 normal = TSVector2.zero;
for (int i = 0; i < 2; ++i)
{
FP pi = CustomMath.Get(p, i);
FP li = CustomMath.Get(lowerBound, i);
FP ui = CustomMath.Get(upperBound, i);
FP di = CustomMath.Get(d, i);
if (CustomMath.Get(absD, i) < CustomMath.EPSILON)
{
// Parallel.
if (pi < li || ui < pi)
{
return(false);
}
}
else
{
FP inv_d = 1 / di;
FP t1 = (li - pi) * inv_d;
FP t2 = (ui - pi) * inv_d;
// Sign of the normal vector.
FP s = -1;
if (t1 > t2)
{
CustomMath.Swap <FP>(ref t1, ref t2);
s = 1;
}
// Push the min up
if (t1 > tmin)
{
normal = TSVector2.zero;
CustomMath.Set(ref normal, i, s);
tmin = t1;
}
// Pull the max down
tmax = TSMath.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0 || input.maxFraction < tmin)
{
return(false);
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
return(true);
}