void k_closest_points(KInt2 Xq, int cnt, List <int> idxs)
{
// initialize search data
Bmin = KInt2.min;
Bmax = KInt2.max;
this.k = cnt;
// call search on the root [0] fill the queue
// with elements from the search
knn_search(Xq);
// scan the created pq and extract the first "k" elements
// pop the remaining
int N = pq.size();
for (int i = 0; i < N; i++)
{
ClsTuple <float, int> topel = pq.top();
pq.pop();
if (i >= N - k)
{
idxs.Add(topel.Value);
}
}
// invert the vector, passing first closest results
idxs.Reverse();
}
/** @see ball_query, range_query
*
* Returns all the points withing the ball bounding box and their distances
*
* @note this is similar to "range_query" i just replaced "lies_in_range" with "euclidean_distance"
*/
void ball_bbox_query(int nodeIdx, KInt2 pmin, KInt2 pmax, List <int> inrange_idxs, List <float> distances, KInt2 point, float radiusSquared, int dim = 0)
{
GameKdTreeNode node = nodesPtrs[nodeIdx];
// if it's a leaf and it lies in R
if (node.isLeaf())
{
float distance = (points[node.pIdx] - point).sqrMagnitude;
if (distance <= radiusSquared)
{
inrange_idxs.Add(node.pIdx);
if (distances != null)
{
distances.Add(distance);
}
return;
}
}
else
{
if (node.key >= pmin[dim] && node.LIdx != -1)
{
ball_bbox_query(node.LIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
if (node.key <= pmax[dim] && node.RIdx != -1)
{
ball_bbox_query(node.RIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
}
}
public bool Equals(KInt2 first, KInt2 second)
{
if ((start == first && end == second) || (end == first && start == second))
{
return(true);
}
return(false);
}
private static bool InRange(KInt2 p1, KInt2 k1, KInt2 k2)
{
if (((p1.IntX >= k1.IntX && p1.IntX <= k2.IntX) || (p1.IntX >= k2.IntX && p1.IntX <= k1.IntX)) &&
((p1.IntY >= k1.IntY && p1.IntY <= k2.IntY) || (p1.IntY >= k2.IntY && p1.IntY <= k1.IntY)))
{
return(true);
}
return(false);
}
public bool ContainsSegment(Segment segment)
{
KInt2 selfdir = this.direction;
KInt2 otherdir = segment.direction;
if (selfdir == otherdir || selfdir == -otherdir)
{
return(InRange(segment.start, this.start, this.end) && InRange(segment.end, this.start, this.end));
}
return(false);
}
public void ball_query(KInt2 point, float radius, List <int> idxsInRange, List <float> distances)
{
if (nodesPtrs.Count > 0)
{
// create pmin pmax that bound the sphere
KInt2 pmin = new KInt2(point.x - radius, point.y - radius);
KInt2 pmax = new KInt2(point.x + radius, point.y + radius);
// start from root at zero-th dimension
ball_bbox_query(0, pmin, pmax, idxsInRange, distances, point, radius * radius, 0);
}
}
public Segment(KInt2 first, KInt2 second, int id)
{
this.start = first;
this.end = second;
cachedir = null;
uid = id;
this.MinX = KMath.Min(this.start.IntX, this.end.IntX);
this.MaxX = KMath.Max(this.start.IntX, this.end.IntX);
this.MinY = KMath.Min(this.start.IntY, this.end.IntY);
this.MaxY = KMath.Max(this.start.IntY, this.end.IntY);
}
void AddtoList(HashSet <Segment> container, List <KInt2> linequeue)
{
if (linequeue.Count > 2)
{
container.Add(new Segment(linequeue [0], linequeue [1]));
for (int i = 1; i < linequeue.Count - 1; ++i)
{
KInt2 current = linequeue [i];
KInt2 next = linequeue [i + 1];
container.Add(new Segment(current, next));
}
}
}
public static bool isConnectRetPoints(KInt2 selfstart, KInt2 selfend, KInt2 otherstart, KInt2 otherend, out KInt2 p1, out KInt2 p2, out KInt2 p3)
{
p1 = selfstart;
p2 = selfend;
p3 = selfstart;
bool b1 = selfstart == otherstart;
bool b2 = selfend == otherend;
bool b3 = selfstart == otherend;
bool b4 = selfend == otherstart;
KInt2 dir = (otherend - otherstart).normalized;
KInt2 selfdir = (selfend - selfstart).normalized;
if ((b1 && b2) || (b3 && b4))
{
return(false);
}
else if (b1 || b2)
{
p1 = b1 ? selfstart:selfend;
p2 = b1 ? selfend :selfstart;
p3 = b1 ? otherend:otherstart;
if (selfdir == dir)
{
return(false);
}
return(true);
}
else if (b3 || b4)
{
p1 = b3?selfstart:selfend;
p2 = b3? selfend :selfstart;
p3 = b3 ? otherstart:otherend;
if (selfdir == -dir)
{
return(false);
}
return(true);
}
return(false);
}
int isInside(KInt2 p1, KInt2 p2, KInt2 p3, KInt2 target, int radius)
{
long A = doublearea(p1, p2, p3);
long A1 = doublearea(target, p2, p3);
long A2 = doublearea(p1, target, p3);
long A3 = doublearea(p1, p2, target);
long delta = A - (A1 + A2 + A3);
if (delta == 0)
{
return(1);
}
if (delta < radius && delta > -radius)
{
return(2);
}
return(-1);
}
void ball_bbox_query(int nodeIdx, KInt2 pmin, KInt2 pmax, List <KInt2> inrange_idxs, List <float> distances, KInt2 point, float radiusSquared, int dim = 0)
{
GameKdTreeNode node = nodesPtrs[nodeIdx];
// if it's a leaf and it lies in R
if (node.isLeaf())
{
float distance = (points[node.pIdx] - point).sqrMagnitude;
if (distance <= radiusSquared)
{
bool insert = false;
for (int i = 0; i < distances.Count; ++i)
{
if (distance < distances[i])
{
insert = true;
distances.Insert(i, distance);
inrange_idxs.Insert(i, this.points[node.pIdx]);
break;
}
}
if (!insert)
{
distances.Add(distance);
inrange_idxs.Add(this.points[node.pIdx]);
}
return;
}
}
else
{
if (node.key >= pmin[dim] && node.LIdx != -1)
{
ball_bbox_query(node.LIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
if (node.key <= pmax[dim] && node.RIdx != -1)
{
ball_bbox_query(node.RIdx, pmin, pmax, inrange_idxs, distances, point, radiusSquared, (dim + 1) % ndim);
}
}
}
public bool isSharedEdge(NavmeshTriangle other, out int idx)
{
for (int i = 0; i < 3; ++i)
{
KInt2 p1 = GetXZVertex(i);
KInt2 p2 = GetXZVertex((i + 1) % 3);
for (int k = 0; k < 3; ++k)
{
KInt2 p3 = other.GetXZVertex(k);
KInt2 p4 = other.GetXZVertex((k + 1) % 3);
if ((p1 == p3 && p2 == p4) || (p1 == p4 && p2 == p3))
{
idx = i;
return(true);
}
}
}
idx = -1;
return(false);
}
public void GetSegmentWithPoint(int idx, out KInt2 p1, out KInt2 p2)
{
if (idx > 2 || idx < 0)
{
throw new FrameWorkArgumentException("Get Segment Error");
}
if (idx == 0)
{
p1 = v03xz;
p2 = v13xz;
}
else if (idx == 1)
{
p1 = v13xz;
p2 = v23xz;
}
p1 = v23xz;
p2 = v03xz;
}
bool isAllLine(List <KInt2> linequeue)
{
if (linequeue.Count <= 2)
{
return(true);
}
KInt2 firstdir = linequeue [1] - linequeue [0];
for (int i = 1; i < linequeue.Count - 1; ++i)
{
KInt2 current = linequeue [i];
KInt2 next = linequeue [i + 1];
KInt det = RVOMath.det(next - current, firstdir);
if (det != 0)
{
return(false);
}
}
return(true);
}
public static bool Intersect(KInt2 p1, KInt2 p2, KInt2 p3, KInt2 p4, out KInt2 result)
{
result = KInt2.zero;
float s1_x = p2.x - p1.x;
float s1_y = p2.y - p1.y;
float s2_x = p4.x - p3.x;
float s2_y = p4.y - p3.y;
float v1 = -s2_x * s1_y + s1_x * s2_y;
float v2 = -s2_x * s1_y + s1_x * s2_y;
float s = (-s1_y * (p1.x - p3.x) + s1_x * (p1.y - p3.y)) / v1;
float t = (s2_x * (p1.y - p3.y) - s2_y * (p1.x - p3.x)) / v2;
if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
{
// Collision detected
result = new KInt2((p1.x + t * s1_x), +(p1.y + t * s1_y));
return(true);
}
return(false); // No collision
}
bool TryConnectUntilFindTarget(KInt2 current, KInt2 target, List <KInt2> linequeue, HashSet <KInt2> openlist, Dictionary <KInt2, HashSet <KInt2> > connectlines)
{
openlist.Add(current);
HashSet <KInt2> conncects = connectlines [current];
if (conncects.Count > 0)
{
var en = conncects.GetEnumerator();
while (en.MoveNext())
{
KInt2 pt = en.Current;
if (linequeue.Count > 1)
{
if (pt.Equals(target))
{
linequeue.Add(target);
return(true);
}
if (openlist.Contains(pt))
{
continue;
}
}
linequeue.Add(pt);
if (TryConnectUntilFindTarget(pt, target, linequeue, openlist, connectlines))
{
return(true);
}
else
{
linequeue.Remove(pt);
}
}
}
return(false);
}
bool bounds_overlap_ball(KInt2 Xq)
{
// k-closest still not found. termination test unavailable
if (pq.size() < k)
{
return(true);
}
float sum = 0;
//extract best distance from queue top
float best_dist_sq = pq.top().Key;
// cout << "current best dist: " << best_dist_sq << endl;
for (int d = 0; d < ndim; d++)
{
// lower than low boundary
if (Xq[d] < Bmin[d])
{
sum += (Xq[d] - Bmin[d]) * (Xq[d] - Bmin[d]);
if (sum > best_dist_sq)
{
return(false);
}
}
else if (Xq[d] > Bmax[d])
{
sum += (Xq[d] - Bmax[d]) * (Xq[d] - Bmax[d]);
if (sum > best_dist_sq)
{
return(false);
}
}
// else it's in range, thus distance 0
}
return(true);
}
public bool ContainsPoint(KInt2 point)
{
return(this.start == point || this.end == point);
}
void RemoveUnused()
{
//sort
BeginSample("SortByXcoordinate");
SortByXcoordinate(segments);
EndSample();
BeginSample("connectlines check");
HashSet <KInt2> allpoints = new HashSet <KInt2> ();
Dictionary <KInt2, HashSet <KInt2> > connectlines = new Dictionary <KInt2, HashSet <KInt2> >(segments.Count);
for (int i = 0; i < segments.Count; ++i)
{
Segment seg = segments [i];
if (allpoints.Add(seg.start))
{
connectlines [seg.start] = new HashSet <KInt2> ();
}
if (allpoints.Add(seg.end))
{
connectlines [seg.end] = new HashSet <KInt2> ();
}
}
KInt2 p1, p2, p3;
for (int i = 0; i < segments.Count; ++i)
{
Segment firstline = segments [i];
for (int j = i + 1; j < segments.Count; ++j)
{
Segment secondline = segments [j];
if (firstline.MinX > secondline.MaxX)
{
break;
}
long maxminy = KMath.Max(firstline.MinY, secondline.MinY);
long minmaxy = KMath.Min(firstline.MaxY, secondline.MaxY);
if (maxminy > minmaxy)
{
continue;
}
if (Segment.isConnectRetPoints(firstline, secondline, out p1, out p2, out p3))
{
//only three point
connectlines [p1].Add(p2);
connectlines [p1].Add(p3);
}
}
ShowProgress("创建链接点", (float)i / segments.Count);
}
EndSample();
this.segments.Clear();
HashSet <Segment> realList = new HashSet <Segment> ();
List <KInt2> linequeue = new List <KInt2> (segments.Count);
HashSet <KInt2> openlist = new HashSet <KInt2> ();
HashSet <KInt2> enabledlist = new HashSet <KInt2> ();
BeginSample("combine polygon check");
int progress = 0;
var pointen = allpoints.GetEnumerator();
while (pointen.MoveNext())
{
KInt2 pt = pointen.Current;
if (enabledlist.Contains(pt) || connectlines [pt].Count < 2)
{
continue;
}
openlist.Clear();
linequeue.Clear();
linequeue.Add(pt);
if (TryConnectUntilFindTarget(pt, pt, linequeue, openlist, connectlines))
{
if (!isAllLine(linequeue))
{
AddtoList(realList, linequeue);
for (int k = 0; k < linequeue.Count; ++k)
{
enabledlist.Add(linequeue [k]);
}
}
else
{
#if UNITY_EDITOR
if (ObstacleDebug >= DebugMode.ShowAndLog)
{
LogMgr.LogFormat("准备剔除 原始目标点 为 :{0}", pt);
for (int k = 0; k < linequeue.Count; ++k)
{
LogMgr.LogFormat("准备剔除 :{0}", linequeue [k]);
}
LogMgr.Log("剔除结束");
}
#endif
}
}
progress++;
ShowProgress("检测非多边形点", (float)progress / allpoints.Count);
}
EndSample();
segments.Capacity = realList.Count;
var realen = realList.GetEnumerator();
while (realen.MoveNext())
{
segments.Add(realen.Current);
}
if (ExactMode >= ExactType.TWO)
{
BeginSample("PolygonIntersect");
PolygonIntersect();
EndSample();
}
}
static long cross(KInt2 p1, KInt2 p2, KInt2 p3)
{
return(p1.IntX * (p2.IntY - p3.IntY) + p2.IntX * (p3.IntY - p1.IntY) + p3.IntX * (p1.IntY - p2.IntY));
}
long doublearea(KInt2 p1, KInt2 p2, KInt2 p3)
{
return(KMath.Abs((p1.IntX * (p2.IntY - p3.IntY) + p2.IntX * (p3.IntY - p1.IntY) + p3.IntX * (p1.IntY - p2.IntY))));
}
void knn_search(KInt2 Xq, int nodeIdx = 0, int dim = 0)
{
// cout << "at node: " << nodeIdx << endl;
GameKdTreeNode node = nodesPtrs[nodeIdx];
float temp;
// We are in LEAF
if (node.isLeaf())
{
float distance = (Xq - points[node.pIdx]).sqrMagnitude;
// pqsize is at maximum size k, if overflow and current record is closer
// pop further and insert the new one
if (pq.size() == k && pq.top().Key > distance)
{
pq.pop(); // remove farther record
pq.push(distance, node.pIdx); //push new one
}
else if (pq.size() < k)
{
pq.push(distance, node.pIdx);
}
return;
}
////// Explore the sons //////
// recurse on closer son
if (Xq[dim] <= node.key)
{
temp = Bmax[dim]; Bmax[dim] = node.key;
knn_search(Xq, node.LIdx, (dim + 1) % ndim);
Bmax[dim] = temp;
}
else
{
temp = Bmin[dim]; Bmin[dim] = node.key;
knn_search(Xq, node.RIdx, (dim + 1) % ndim);
Bmin[dim] = temp;
}
// recurse on farther son
if (Xq[dim] <= node.key)
{
temp = Bmin[dim]; Bmin[dim] = node.key;
if (bounds_overlap_ball(Xq))
{
knn_search(Xq, node.RIdx, (dim + 1) % ndim);
}
Bmin[dim] = temp;
}
else
{
temp = Bmax[dim]; Bmax[dim] = node.key;
if (bounds_overlap_ball(Xq))
{
knn_search(Xq, node.LIdx, (dim + 1) % ndim);
}
Bmax[dim] = temp;
}
}
bool Mesh2Obstacle()
{
if (mesh.vertexCount == 0 || mesh.triangles.Length == 0)
{
LogMgr.LogError("Convert Error");
return(false);
}
BeginSample("Mesh2Obstacle");
//old
Vector3[] vectorVertices = mesh.vertices;
int[] triangles = mesh.triangles;
//create new
KInt3[] vertices = new KInt3[mesh.vertexCount];
Dictionary <KInt3, int> hashedVerts = new Dictionary <KInt3, int> (mesh.vertexCount);
int[] newVertices = new int[vertices.Length];
//从模型坐标系转换到世界坐标系
for (int i = 0; i < vertices.Length; i++)
{
vertices [i] = (KInt3)MeshMatrix.MultiplyPoint3x4(vectorVertices [i]);
ShowProgress("从模型坐标系转换到世界坐标系", (float)i / vertices.Length);
}
//旧顶点在新顶点中的索引
int newVerIdx = 0;
for (int i = 0; i < vertices.Length; i++)
{
//当hash 顶点集合不包含这个顶点的时候，把这个点加入进去,去除共点
if (!hashedVerts.ContainsKey(vertices [i]))
{
newVertices [newVerIdx] = i;
hashedVerts.Add(vertices [i], newVerIdx);
newVerIdx++;
}
ShowProgress("构建映射", (float)i / vertices.Length);
}
for (int x = 0; x < triangles.Length; x++)
{
//把老的顶点索引映射到新的顶点索引上
KInt3 vertex = vertices [triangles [x]];
triangles [x] = hashedVerts [vertex];
ShowProgress("三角映射", (float)x / triangles.Length);
}
KInt3[] totalIntVertices = vertices;
vertices = new KInt3[newVerIdx];
for (int i = 0; i < newVerIdx; i++)
{
vertices [i] = totalIntVertices [newVertices [i]];
ShowProgress("顶点索引转换", (float)i / newVerIdx);
}
//顶点创建结束
#if UNITY_EDITOR
this.ObsVertices = vertices;
#endif
if (triangles.Length % 3 != 0)
{
LogMgr.LogErrorFormat("triangle lenth error :{0}", triangles.Length);
}
//创建三角
ObsTriangles = new NavmeshTriangle[triangles.Length / 3];
for (int i = 0; i < ObsTriangles.Length; i++)
{
NavmeshTriangle tri = new NavmeshTriangle();
ObsTriangles [i] = tri;
#if UNITY_EDITOR
//init
tri.uid = i;
#endif
//
tri.v0 = triangles [i * 3];
tri.v1 = triangles [i * 3 + 1];
tri.v2 = triangles [i * 3 + 2];
if (RVOMath.IsClockwiseXZ(vertices [tri.v0], vertices [tri.v1], vertices [tri.v2]) == false)
{
int tmp = tri.v0;
tri.v0 = tri.v2;
tri.v2 = tmp;
}
//finish position
tri.v03 = vertices [tri.v0];
tri.v13 = vertices [tri.v1];
tri.v23 = vertices [tri.v2];
tri.v03xz = new KInt2(tri.v03.x, tri.v03.z);
tri.v13xz = new KInt2(tri.v13.x, tri.v13.z);
tri.v23xz = new KInt2(tri.v23.x, tri.v23.z);
tri.averagePos = (tri.v03 + tri.v13 + tri.v23) / 3;
if (this.ExactMode >= ExactType.TWO)
{
tri.xzPos = new KInt2(tri.averagePos.x, tri.averagePos.z);
if (point2triangle.ContainsKey(tri.xzPos))
{
point2triangle [tri.xzPos].Add(tri);
}
else
{
point2triangle [tri.xzPos] = new List <NavmeshTriangle> ();
point2triangle [tri.xzPos].Add(tri);
}
}
ShowProgress("构建三角形", (float)i / ObsTriangles.Length);
}
Dictionary <KInt2, NavmeshTriangle> sides = new Dictionary <KInt2, NavmeshTriangle> (triangles.Length * 3);
//创建三角形的边
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides [KInt2.ToInt2(triangles [i + 0], triangles [i + 1])] = ObsTriangles [j];
sides [KInt2.ToInt2(triangles [i + 1], triangles [i + 2])] = ObsTriangles [j];
sides [KInt2.ToInt2(triangles [i + 2], triangles [i + 0])] = ObsTriangles [j];
}
HashSet <NavmeshTriangle> connections = new HashSet <NavmeshTriangle> ();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
NavmeshTriangle node = ObsTriangles [j];
for (int q = 0; q < 3; q++)
{
NavmeshTriangle other;
//如果是mesh中的边，则加入
if (sides.TryGetValue(KInt2.ToInt2(triangles [i + ((q + 1) % 3)], triangles [i + q]), out other))
{
connections.Add(other);
}
}
//拷贝当前的一份连接点，而不是赋值引用过去
node.connections = new List <NavmeshTriangle> ();
node.connections.AddRange(connections);
node.CreateSharedInfo();
ShowProgress("构建连接点", (float)i / (triangles.Length / 3));
}
ClearProgressBar();
EndSample();
return(true);
}
public static bool isConnectRetPoints(Segment self, Segment other, out KInt2 p1, out KInt2 p2, out KInt2 p3)
{
return(isConnectRetPoints(self.start, self.end, other.start, other.end, out p1, out p2, out p3));
}
