public static common.Vector LV2PV(Luna3D.Vector3 vec)
{
common.Vector ProtoVec = new common.Vector();
ProtoVec.x = vec.x;
ProtoVec.y = vec.y;
ProtoVec.z = vec.z;
return(ProtoVec);
}
/// <summary>
/// Builds a blocky path following the middle of the connecting edges across the nav mesh.
/// </summary>
public void GeneratePath(Luna3D.Vector3 start, Luna3D.Vector3 end)
{
startPos = new float[] { start.x, start.y, start.z };
endPos = new float[] { end.x, end.y, end.z };
Status status = _navMeshQuery.FindNearestPoly(startPos, new[] { 2f, 4f, 2f }, filter, ref startRef, ref nearestPt);
Debug.Log(string.Format("Found start position status: {0}, Ref {1}, pos {2}, {3}, {4} ", status, startRef, startPos[0], startPos[1], startPos[2]));
status = _navMeshQuery.FindNearestPoly(endPos, new[] { 2f, 4f, 2f }, filter, ref endRef, ref nearestPt);
Debug.Log(string.Format("Found end position status: {0}, Ref {1}, pos {2}, {3}, {4} ", status, endRef, endPos[0], endPos[1], endPos[2]));
status = _navMeshQuery.FindPath(startRef, endRef, startPos, endPos, filter, ref polys, ref polyCount, MaxPolys);
}
public void Init(RoleTransform transform, LunaNavmeshQuery navmeshQuery)
{
_navQuery = navmeshQuery;
_transform = transform;
if (_navQuery != null)
{
_navMeshQuery = _navQuery._navMeshQuery;
filter = _navQuery.filter;
crowd = _navQuery.Crowd;
param = new CrowdAgentParams
{
Radius = Radius,
Height = Height,
MaxAcceleration = MaxAcceleration,
MaxSpeed = MaxSpeed,
CollisionQueryRange = CollisionQueryRange,
PathOptimizationRange = PathOptimizationRange,
UpdateFlags = UpdateFlags,
ObstacleAvoidanceType = ObstacleAvoidanceType,
SeparationWeight = SeparationWeight
};
Luna3D.Vector3 pos = _transform.GetPosition();
AgentId = _navQuery.Crowd.AddAgent(new[] { pos.x, pos.y, pos.z }, param);
//ResetTarget();
_crowdAgent = crowd.GetAgent(AgentId);
float[] resetpos = _crowdAgent.npos;
_transform.SetPosition(new Vector3(resetpos[0], resetpos[1], resetpos[2]));
}
else
{
System.Console.WriteLine("Scene does not have a Nav Mesh Query, one must be added.");
}
}
/// <summary>
/// Calls GeneratePath. Builds a smooth path by going to corners and trying to find the straightest paths possible across
/// the NavMesh from the StartPosition to the EndPosition
/// </summary>
public void SmoothGeneratedPath(Luna3D.Vector3 start, Luna3D.Vector3 end)
{
GeneratePath(start, end);
long[] smoothPolys = new long[MaxPolys];
Array.Copy(polys, smoothPolys, polyCount);
int smoothPolyCount = polyCount;
float[] iterPos = new float[3], targetPos = new float[3];
_navMeshQuery.ClosestPointOnPoly(startRef, startPos, ref iterPos);
_navMeshQuery.ClosestPointOnPoly(smoothPolys[smoothPolyCount - 1], endPos, ref targetPos);
float StepSize = 0.5f;
float Slop = 0.01f;
SmoothPathNum = 0;
Array.Copy(iterPos, 0, SmoothPath, SmoothPathNum * 3, 3);
SmoothPathNum++;
while (smoothPolyCount > 0 && SmoothPathNum < 2048)
{
float[] steerPos = new float[3];
short steerPosFlag = 0;
long steerPosRef = 0;
if (!GetSteerTarget(_navMeshQuery, iterPos, targetPos, Slop, smoothPolys, smoothPolyCount, ref steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & StraightPathEnd) != 0;
bool offMeshConnection = (steerPosFlag & StraightPathOffMeshConnection) != 0;
float[] delta = Helper.VSub(steerPos[0], steerPos[1], steerPos[2], iterPos[0], iterPos[1], iterPos[2]);
float len = (float)Math.Sqrt(Helper.VDot(delta, delta));
if ((endOfPath || offMeshConnection) && len < StepSize)
{
len = 1;
}
else
{
len = StepSize / len;
}
float[] moveTarget = new float[3];
Helper.VMad(ref moveTarget, iterPos, delta, len);
float[] result = new float[3];
long[] visited = new long[16];
int nVisited = 0;
_navMeshQuery.MoveAlongSurface(smoothPolys[0], iterPos, moveTarget, filter, ref result, ref visited, ref nVisited, 16);
smoothPolyCount = FixupCorridor(ref smoothPolys, smoothPolyCount, MaxPolys, visited, nVisited);
float h = 0;
_navMeshQuery.GetPolyHeight(smoothPolys[0], result, ref h);
result[1] = h;
Array.Copy(result, iterPos, 3);
if (endOfPath && InRange(iterPos, steerPos, Slop, 1.0f))
{
Array.Copy(targetPos, iterPos, 3);
if (SmoothPathNum < 2048)
{
Array.Copy(iterPos, 0, SmoothPath, SmoothPathNum * 3, 3);
SmoothPathNum++;
}
break;
}
else if (offMeshConnection && InRange(iterPos, steerPos, Slop, 1.0f))
{
float[] startPosOffMesh = new float[3], endPosOffMesh = new float[3];
long prevRef = 0, polyRef = smoothPolys[0];
int npos = 0;
while (npos < smoothPolyCount && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = smoothPolys[npos];
npos++;
}
for (int i = npos; i < smoothPolyCount; i++)
{
smoothPolys[i - npos] = smoothPolys[i];
}
smoothPolyCount -= npos;
Status status = _navMeshQuery.NavMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPosOffMesh, ref endPosOffMesh);
if ((status & Status.Success) != 0)
{
if (SmoothPathNum < MaxSmooth)
{
Array.Copy(startPosOffMesh, 0, SmoothPath, SmoothPathNum * 3, 3);
SmoothPathNum++;
if ((SmoothPathNum & 1) == 1)
{
Array.Copy(startPosOffMesh, 0, SmoothPath, SmoothPathNum * 3, 3);
SmoothPathNum++;
}
}
Array.Copy(endPosOffMesh, iterPos, 3);
float eh = 0.0f;
_navMeshQuery.GetPolyHeight(smoothPolys[0], iterPos, ref eh);
iterPos[1] = eh;
}
}
if (SmoothPathNum < 2048)
{
Array.Copy(iterPos, 0, SmoothPath, SmoothPathNum * 3, 3);
SmoothPathNum++;
}
}
doneSmoothing = true;
}
