/* protected void addSegment(float dist, float[] s) {
* // Insert neighbour based on the distance.
* Segment seg = new Segment();
* System.arraycopy(s, 0, seg.s, 0, 6);
* seg.d = dist;
* if (m_segs.isEmpty()) {
* m_segs.add(seg);
* } else if (dist >= m_segs.get(m_segs.size() - 1).d) {
* if (m_segs.size() >= MAX_LOCAL_SEGS) {
* return;
* }
* m_segs.add(seg);
* } else {
* // Insert inbetween.
* int i;
* for (i = 0; i < m_segs.size(); ++i)
* if (dist <= m_segs.get(i).d)
* break;
* m_segs.add(i, seg);
* }
* while (m_segs.size() > MAX_LOCAL_SEGS) {
* m_segs.remove(m_segs.size() - 1);
* }
* }*/
public virtual void update(long @ref, float[] pos, float collisionQueryRange, NavMeshQuery navquery, QueryFilter filter)
{
if (@ref == 0)
{
reset();
return;
}
DetourCommon.vCopy(m_center, pos);
// First query non-overlapping polygons.
navquery.findLocalNeighbourhood(@ref, pos, collisionQueryRange, filter, m_polys, null, ref m_npolys, MAX_LOCAL_POLYS);
// Secondly, store all polygon edges.
m_nsegs = 0;
int nsegs = 0;
float t = 0;
for (int j = 0; j < m_npolys; ++j)
{
navquery.getPolyWallSegments(m_polys[j], filter, segs, null, ref nsegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < nsegs; ++k)
{
Array.Copy(segs, k * 6, s, 0, 6);
// Skip too distant segments.
float dist = DetourCommon.distancePtSegSqr2D(pos, s, 0, 3, ref t);
if (dist > DetourCommon.sqr(collisionQueryRange))
{
continue;
}
addSegment(dist, s);
}
}
}
private void prepare(float[] pos, float[] dvel, float radius)
{
// Prepare obstacles
ObstacleCircle cir;
float[] pa;
float[] pb;
float a;
for (int i = 0; i < m_ncircles; ++i)
{
cir = m_circles[i];
// Side
pa = pos;
pb = cir.p;
DetourCommon.vSub(cir.dp, pb, pa);
DetourCommon.vNormalize(cir.dp);
DetourCommon.vSub(ppdv, cir.dvel, dvel);
a = DetourCommon.triArea2D(pporig, cir.dp, ppdv);
if (a < 0.01f)
{
cir.np[0] = -cir.dp[2];
cir.np[2] = cir.dp[0];
}
else
{
cir.np[0] = cir.dp[2];
cir.np[2] = -cir.dp[0];
}
}
ObstacleSegment seg;
float t;
for (int i = 0; i < m_nsegments; ++i)
{
seg = m_segments[i];
t = 0;
seg.touch = DetourCommon.distancePtSegSqr2D(pos, seg.p, seg.q, ref t) < DetourCommon.sqr(R);
}
}