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));
}
}
}
}
public static TSVector Vector2ToTsvec(Vector2 vector)
{
TSVector vec = TSVector.zero;
vec.Set(vector.x, 0, vector.y);
return(vec);
}
public static TSVector Clamp(TSVector val, TSVector min, TSVector max)
{
TSVector vec = TSVector.zero;
vec.Set(TSMath.Clamp(val.x, min.x, max.x), TSMath.Clamp(val.y, min.y, max.y), TSMath.Clamp(val.z, min.z, max.z));
return(vec);
}
public static TSVector Vector3ToTsvec(Vector3 vector)
{
TSVector vec = TSVector.zero;
vec.Set(vector.x, vector.y, vector.z);
return(vec);
}
public QTBound ExpandBy(FP W)
{
TSVector vec = TSVector.zero;
vec.Set(W, 0, W);
min = min - vec;
max = max + vec;
return(this);
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
// I don't think that this can be done faster.
// 6 is the minimum number of SupportMap calls.
TSVector vec = TSVector.zero;
vec.Set(orientation.M11, orientation.M21, orientation.M31);
SupportMapping(ref vec, out vec);
box.max.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(orientation.M12, orientation.M22, orientation.M32);
SupportMapping(ref vec, out vec);
box.max.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(orientation.M13, orientation.M23, orientation.M33);
SupportMapping(ref vec, out vec);
box.max.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
vec.Set(-orientation.M11, -orientation.M21, -orientation.M31);
SupportMapping(ref vec, out vec);
box.min.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(-orientation.M12, -orientation.M22, -orientation.M32);
SupportMapping(ref vec, out vec);
box.min.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(-orientation.M13, -orientation.M23, -orientation.M33);
SupportMapping(ref vec, out vec);
box.min.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
}
internal static TSVector BoidsBehaviourAvoidObstacle(TSVector agentPos, GridMap map, TSVector towardDir)
{
IInt2 floor = IInt2.zero;// GetNearestGridCoordWithoutClamp(agent.position);
map.GetGridCoord(agentPos, ref floor);
TSVector aPos = agentPos;// map.GetWorldPosition(map.GetIdx(floor.x,floor.y));
TSVector desiredDirection = TSVector.zero;
int iCount = 0;
towardDir = CustomMath.Normalize(towardDir);
for (int i = floor.x - 1; i <= floor.x + 1; i++)
{
for (int j = floor.y - 1; j <= floor.y + 1; j++)
{
if (!map.IsWalkable(i, j, true))
{
// int idx = map.GetIdx(i,j);
TSVector pos = map.GetWorldPositionWithOutClamp(i, j);
TSVector dir = aPos;// agentPos;
dir.Set(aPos.x - pos.x, 0, aPos.z - pos.z);
//FP dstSqr = dir.sqrMagnitude;
FP dot = 1;// TSVector.Dot(dir,towardDir);
//if(dot<=FP.Zero)
//{
// dot = FP.One/(9*dstSqr);
//}
//else
//{
// dot = dot / dstSqr;
//}
desiredDirection = desiredDirection + dir * dot;
iCount++;
}
}
}
//if (iCount > 0)
//{
// desiredDirection = desiredDirection / iCount;
//}
return(desiredDirection);//.normalized*C_OBSTACLE_REPELL_FORCE;
}
void OnTap(TapGesture gesture)
{
return;
Ray ray = Camera.main.ScreenPointToRay(gesture.Position);
int mask = 1 << LayerMask.NameToLayer("terrain");
RaycastHit hit;
if (UnityEngine.Physics.Raycast(ray, out hit, 1000, mask))
{
TSVector pos = TSVector.zero;
pos.Set(CustomMath.FloatToFP(hit.point.x), CustomMath.FloatToFP(hit.point.y), CustomMath.FloatToFP(hit.point.z));
TSVector targetPos = pos;
foreach (TestAgent ab in _listAgents)
{
if (!ab.gameObject.activeSelf)
{
continue;
}
// if (ab.AgentType==EAgentType.astar)
{
ab.unit.ChangeAgentType(EAgentType.astar);
ab.unit._baseData.defaultTargetPos = targetPos;
ab.unit.agent.StartPos = ab.unit.position;
ab.unit.agent.TargetPos = targetPos;
//if(ab.unit.group==null ||ab.unit.group.leader==ab)
//{
// .FindQuickPath(ab.unit.agent,10, ab.unit.agent.StartPos, ab.unit.agent.TargetPos,ab.unit._map,false);
//}
}
//else
//{
// (ab.agent).TargetPos=targetPos;
//}
}
}
}
public void Insert(T T)
{
int i = 0;
QTBound r = bounds;
TSVector p = T.position;
T.next = null;
maxRadius = TSMath.Max(T.neighbourRadius, maxRadius);
int depth = 0;
TSVector min = TSVector.zero;
TSVector max = TSVector.zero;
while (true)
{
depth++;
if (nodes[i].childNode1 == i)
{
// Leaf node.
if (nodes[i].count < LeafSize || depth > 10)
{
nodes[i].Add(T);
// nodes[i].count++;
break;
}
else
{
// Split
QTNode node = nodes[i];
node.childNode1 = GetNodeIndex();
node.childNode2 = GetNodeIndex();
node.childNode3 = GetNodeIndex();
node.childNode4 = GetNodeIndex();
nodes[i] = node;
nodes[i].Distribute(nodes, r);
}
}
// Note, no else
if (nodes[i].childNode1 != i)
{
// Not a leaf node
TSVector c = r.center;
if (p.x > c.x)
{
if (p.z > c.z)
{
i = nodes[i].childNode4;
r = QTBound.MinMaxQTBound(c, r.max);
}
else
{
i = nodes[i].childNode3;
min.Set(c.x, 0, r.min.z);
max.Set(r.max.x, 0, c.z);
r = QTBound.MinMaxQTBound(min, max);
}
}
else
{
if (p.z > c.z)
{
i = nodes[i].childNode2;
min.Set(r.min.x, 0, c.z);
max.Set(c.x, 0, r.max.z);
r = QTBound.MinMaxQTBound(min, max);
}
else
{
i = nodes[i].childNode1;
r = QTBound.MinMaxQTBound(r.min, c);
}
}
}
}
}
public static void Set(this TSVector jVector, TSVector otherVector)
{
jVector.Set(otherVector.x, otherVector.y, otherVector.z);
}
void generateFlowField(TSVector destination)
{
//scan from end node
int pathEnd = _gridMap.GetGridNodeId(destination);
int endX = pathEnd % _gridWidth;
int endZ = pathEnd / _gridWidth;
//
for (int x = 0; x < _gridWidth; x++)
{
for (int z = 0; z < _gridHeight; z++)
{
bool bNodeCostMax = _gridDijkstraCost[x, z] == FP.MaxValue;
if (bNodeCostMax)
{
//
//#if USING_DYNAMIC_TREE
// _gridMap.AddObstacle(x,z);
//#endif
continue;
}
if (_gridLOS[x, z])
{
TSVector vec3 = TSVector.zero;
vec3.Set(endX - x, 0, endZ - z);
_flowField[x, z] = CustomMath.Normalize(vec3);
continue;
}
TSVector2 pos;
pos.x = x;
pos.y = z;
//find the one with the lowest distance of neighbours
TSVector2 min = pos;
min.Set(int.MaxValue, int.MaxValue);
FP minDist = FP.MaxValue;//
int count = GridMap.straightNeighbourOffset.Length;
for (var i = 0; i < count; i++)
{
int n_x = GridMap.straightNeighbourOffset[i].x + x;
int n_z = GridMap.straightNeighbourOffset[i].y + z;
_tmpValid[i] = IsValid(n_x, n_z);
if (_tmpValid[i])
{
FP dist = FP.Zero;
dist = _gridDijkstraCost[n_x, n_z] - _gridDijkstraCost[x, z];
if (dist < minDist)
{
min.Set(n_x, n_z);
minDist = dist;
}
}
}
for (var i = 0; i < count; i++)
{
if (_tmpValid[i] && _tmpValid[(i + 1) % 4])
{
int n_x = diagonalNeighbourOffset[i].x + x;
int n_z = diagonalNeighbourOffset[i].y + z;
if (IsValid(n_x, n_z))
{
FP dist = FP.Zero;
dist = _gridDijkstraCost[n_x, n_z] - _gridDijkstraCost[x, z];
if (dist < minDist)
{
min.Set(n_x, n_z);
minDist = dist;
}
}
}
}
//
if (minDist != FP.MaxValue)
{
TSVector2 v = min - pos;
TSVector vec3 = TSVector.zero;
vec3.Set(v.x, 0, v.y);
_flowField[x, z] = CustomMath.Normalize(vec3);
}
else
{
#if UNITY_EDITOR
UnityEngine.Debug.LogError("grid map has node surrounded with obstacles");
#endif
_flowField[x, z] = TSVector.zero;
}
}
}
//_gridMap._obstacleHasSetUp = true;
}
IEnumerator SpawnUnit()
{
if (randomSeed == 0)
{
randomSeed = (int)System.DateTime.Now.Ticks;
}
UnityEngine.Random.InitState(randomSeed);
TSRandom random = TSRandom.instance;
while (true && countLimit > 0)
{
for (int i = 0; i < 10 && countLimit > 0; i++)
{
TestAgent tagent = GameObject.Instantiate <TestAgent>(agent);
PathFindingAgentBehaviour unit = s_AstarAgent.New();
if (unit != null)
{
tagent._testPathFinding = this;
tagent.unit = unit;
int camp = countLimit % campCount;
//start pos
Vector3 vPos = startObj[camp].transform.position
+ new Vector3(UnityEngine.Random.Range(0, 2.0f), 0, UnityEngine.Random.Range(0, 2.0f));
TSVector pos = TSVector.zero;
pos.Set(CustomMath.FloatToFP(vPos.x), CustomMath.FloatToFP(vPos.y), CustomMath.FloatToFP(vPos.z));
TSVector sPos = pos;
tagent.gameObject.transform.position = CustomMath.TsVecToVector3(sPos);
// Debug.Log(pos);
//target pos
Vector3 tpos = destObj[camp].transform.position; // new Vector3(48.0f, 0.0f, 46.8f);
pos.Set(CustomMath.FloatToFP(tpos.x), CustomMath.FloatToFP(tpos.y), CustomMath.FloatToFP(tpos.z));
//get center
int idx = _map.GetGridNodeId(pos);
pos = _map.GetWorldPosition(idx);
TSVector targetPos = pos;
FP attackRange = _atkRanges[countLimit % _atkRanges.Length];
FP range = TSMath.Max(attackRange + GridMap.GetNodeSize() * CustomMath.FPHalf, FP.One * 3 * GridMap.GetNodeSize());
AgentBaseData data = new AgentBaseData();
data.id = countLimit;
data.mapId = 0;
data.playerId = camp;
data.eAgentType = EAgentType.none; //data.id%5== 0? EAgentType.ingoreObstacle:
#if !USING_FLOW_FIELD
data.defaultTargetPos = TSVector.zero; //astar
#else
data.defaultTargetPos = targetPos;
#endif
data.loopCallback = tagent.Loop;
data.boidsType = (byte)EBoidsActiveType.all;
data.maxSpeed = FP.One * maxSpeed;
data.position = sPos;
data.collidesWithLayer = 1;
data.viewRadius = FP.One * 6 * GridMap.GetNodeSize();
data.neighbourRadius = range;
data.random = random;
data.colliderRadius = 0.5f; //test
data.pfm = _pfm;
data.groupId = (byte)(data.eAgentType == EAgentType.ingoreObstacle ? 1 : 0);
data.targetFailCallback = tagent.FailFindPathCallback;
unit.enabled = false;
unit.Init(data);
EAgentType agentType = EAgentType.astar;
#if USING_FLOW_FIELD
agentType = EAgentType.flowFiled;
#endif
unit.ChangeAgentType(data.eAgentType == EAgentType.ingoreObstacle? data.eAgentType
: agentType);
unit.agent.TargetPos = targetPos;
// unit.OnEnable();//?????????
tagent.attackRange = attackRange; // FP.One * UnityEngine.Random.Range(0.8f, 5);
tagent._attackRange = attackRange.AsFloat();
// unit.AgentType = EAgentType.flowFiled;
#if !USING_FLOW_FIELD
unit.ChangeAgentType(EAgentType.astar); //astar
#endif
if (groupCount > 0)
{
AgentGroupManager.instance.UpdateGroup(unit, countLimit % groupCount);
}
tagent.transform.GetChild(0).GetComponent <SpriteRenderer>().color = _campColor[camp];
// unit.agent.StartPos = unit.position;
// unit._agent.TargetPos = (TSVector)destination;
tagent.gameObject.name = "unit" + countLimit;
tagent.transform.SetParent(Units);
// unit.agent.TargetPos = targetPos;
// unit.agent._activeBoids = (byte)EBoidsActiveType.all;
_listAgents.Add(tagent);
// PathFindingManager.Instance.AddAgent(this);
//_pm.FindFastPath(unit._agent, unit._agent.StartPos, unit._agent.TargetPos);//, unit._agent._vecPath
// break;
if (unit.group != null && (unit.group.leader as PathFindingAgentBehaviour) == unit && unit.AgentType == EAgentType.astar)
{
_pm.FindQuickPath(unit.agent, 10, unit.agent.StartPos, unit.agent.TargetPos, unit.map, false);
}
//if(unit.group!=null)
//{
// unit.agent._activeBoids = (byte)EBoidsActiveType.alignment& (byte)EBoidsActiveType.cohesion & (byte)EBoidsActiveType.terrainSeperation
//}
countLimit--;
if (countLimit % 5 == 0)
{
yield return(_wait);
}
}
}
}
yield return(null);
}