| public static NearestPointStrict ( Vector2 lineStart, Vector2 lineEnd, Vector2 point ) : Vector2 | ||
| lineStart | Vector2 | |
| lineEnd | Vector2 | |
| point | Vector2 | |
| return | Vector2 | |
public void Update()
{
if (lastPosition != tr.position)
{
Teleport(tr.position);
}
UpdateAgentProperties();
RaycastHit hit;
//The non-interpolated position
Vector3 realPos = rvoAgent.InterpolatedPosition;
realPos.y = adjustedY;
if (Physics.Raycast(realPos + Vector3.up * height * 0.5f, Vector3.down, out hit, float.PositiveInfinity, mask))
{
adjustedY = hit.point.y;
}
else
{
adjustedY = 0;
}
realPos.y = adjustedY;
rvoAgent.Position = new Vector3(rvoAgent.Position.x, adjustedY, rvoAgent.Position.z);
List <ObstacleVertex> obst = rvoAgent.NeighbourObstacles;
Vector3 force = Vector3.zero;
for (int i = 0; i < obst.Count; i++)
{
Vector3 a = obst[i].position;
Vector3 b = obst[i].next.position;
Vector3 closest = position - Mathfx.NearestPointStrict(a, b, position);
if (closest == a || closest == b)
{
continue;
}
float dist = closest.sqrMagnitude;
closest /= dist * falloff;
force += closest;
}
#if ASTARDEBUG
Debug.DrawRay(position, desiredVelocity + force * wallAvoidForce);
#endif
rvoAgent.DesiredVelocity = desiredVelocity + force * wallAvoidForce;
tr.position = rvoAgent.InterpolatedPosition + Vector3.up * height * 0.5f + center;
lastPosition = tr.position;
}
public static Texture2D PaintLine(Vector2 from, Vector2 to, float rad, Color col, float hardness, Texture2D tex)
{
float width = rad * 2;
float extent = rad;
float stY = Mathf.Clamp(Mathf.Min(from.y, to.y) - extent, 0, tex.height);
float stX = Mathf.Clamp(Mathf.Min(from.x, to.x) - extent, 0, tex.width);
float endY = Mathf.Clamp(Mathf.Max(from.y, to.y) + extent, 0, tex.height);
float endX = Mathf.Clamp(Mathf.Max(from.x, to.x) + extent, 0, tex.width);
float lengthX = endX - stX;
float lengthY = endY - stY;
float sqrRad = rad * rad;
float sqrRad2 = (rad + 1) * (rad + 1);
Color[] pixels = tex.GetPixels((int)stX, (int)stY, (int)lengthX, (int)lengthY, 0);
Vector2 start = new Vector2(stX, stY);
//Debug.Log (widthX + " "+ widthY + " "+ widthX*widthY);
for (int y = 0; y < lengthY; y++)
{
for (int x = 0; x < lengthX; x++)
{
Vector2 p = new Vector2(x, y) + start;
Vector2 center = p + new Vector2(0.5f, 0.5f);
float dist = (center - Mathfx.NearestPointStrict(from, to, center)).sqrMagnitude;
if (dist > sqrRad2)
{
continue;
}
dist = Mathfx.GaussFalloff(Mathf.Sqrt(dist), rad) * hardness;
//dist = (samples[i]-pos).sqrMagnitude;
Color c = Color.white;
int index = (int)(y * lengthX + x);
//不能超过限度
index = Mathf.Min(pixels.Length - 1, index);
if (dist > 0)
{
c = Color.Lerp(pixels[index], col, dist);
}
else
{
c = pixels[index];
}
pixels[index] = c;
}
}
tex.SetPixels((int)start.x, (int)start.y, (int)lengthX, (int)lengthY, pixels, 0);
return(tex);
}
public static Texture2D PaintLineInMaskColor(Vector2 from, Vector2 to, float rad, Texture2D tex, Texture2D mask, Color color, out int pixcelCount)
{
pixcelCount = 0;
var width = rad * 2;
var extent = rad;
int stY = (int)(Mathf.Clamp(Mathf.Min(from.y, to.y) - extent, 0, tex.height));
int stX = (int)(Mathf.Clamp(Mathf.Min(from.x, to.x) - extent, 0, tex.width));
int endY = (int)(Mathf.Clamp(Mathf.Max(from.y, to.y) + extent, 0, tex.height));
int endX = (int)(Mathf.Clamp(Mathf.Max(from.x, to.x) + extent, 0, tex.width));
int lengthX = endX - stX;
int lengthY = endY - stY;
//Debug.Log("from:"+from+",to:"+to+"."+ stX + " " + endX + " " + stY + " " + endY);
var sqrRad = rad * rad;
Color[] pixels = tex.GetPixels(stX, stY, lengthX, lengthY, 0);
Color[] maskPixels = mask.GetPixels(stX, stY, lengthX, lengthY, 0);
var start = new Vector2(stX, stY);
if (from == to)
{
return(tex);
}
for (int y = 0; y < (int)lengthY; y++)
{
for (int x = 0; x < (int)lengthX; x++)
{
var p = new Vector2(x, y) + start;
var center = p + new Vector2(0.5f, 0.5f);
float dist = (center - Mathfx.NearestPointStrict(from, to, center)).sqrMagnitude;
if (dist > sqrRad)
{
continue;
}
if (maskPixels[y * lengthX + x].a != 0)
{
//pixels[y * (int)lengthX + x] = maskPixels[y * lengthX + x];
if (pixels[y * (int)lengthX + x].a == 0)
{
pixcelCount++;
}
pixels[y * (int)lengthX + x] = color;
}
}
}
tex.SetPixels(stX, stY, lengthX, lengthY, pixels, 0);
return(tex);
}
public static Texture2D PaintLine(Vector2 from, Vector2 to, float rad, Color col, float hardness, Texture2D tex)
{
float num2 = rad;
float y = Mathf.Clamp(Mathf.Min(from.y, to.y) - num2, 0, tex.height);
float x = Mathf.Clamp(Mathf.Min(from.x, to.x) - num2, 0, tex.width);
float num5 = Mathf.Clamp(Mathf.Max(from.y, to.y) + num2, 0, tex.height);
float num7 = Mathf.Clamp(Mathf.Max(from.x, to.x) + num2, 0, tex.width) - x;
float num8 = num5 - y;
float num10 = (rad + 1) * (rad + 1);
Color[] colors = tex.GetPixels((int)x, (int)y, (int)num7, (int)num8, 0);
Vector2 vector = new Vector2(x, y);
for (int i = 0; i < num8; i++)
{
for (int j = 0; j < num7; j++)
{
Vector2 vector2 = new Vector2(j, i) + vector;
Vector2 point = vector2 + new Vector2(0.5f, 0.5f);
Vector2 vector4 = point - Mathfx.NearestPointStrict(from, to, point);
float sqrMagnitude = vector4.sqrMagnitude;
if (sqrMagnitude <= num10)
{
Color color;
sqrMagnitude = Mathfx.GaussFalloff(Mathf.Sqrt(sqrMagnitude), rad) * hardness;
var index0 = ((int)(i * num7)) + j;
//print(index0);
//print(colors.Length);
if (sqrMagnitude > 0)
{
color = Color.Lerp(colors[index0], col, sqrMagnitude);
}
else
{
color = colors[index0];
}
colors[index0] = color;
}
}
}
tex.SetPixels((int)vector.x, (int)vector.y, (int)num7, (int)num8, colors, 0);
return(tex);
}
/*public override void ApplyOriginal (Path p) {
*
* if (exactStartPoint) {
* pStart = GetClampedPoint (p.path[0].position, p.originalStartPoint, p.path[0]);
*
* if (!addPoints) {
* p.startPoint = pStart;
* }
* }
*
* if (exactEndPoint) {
* pEnd = GetClampedPoint (p.path[p.path.Length-1].position, p.originalEndPoint, p.path[p.path.Length-1]);
*
* if (!addPoints) {
* p.endPoint = pEnd;
* }
* }
* }*/
public override void Apply(Path _p, ModifierData source)
{
ABPath p = _p as ABPath;
//Only for ABPaths
if (p == null)
{
return;
}
if (p.vectorPath.Count == 0)
{
return;
}
else if (p.vectorPath.Count < 2 && !addPoints)
{
//Vector3[] arr = new Vector3[2];
//arr[0] = p.vectorPath[0];
//arr[1] = p.vectorPath[0];
//p.vectorPath = arr;
p.vectorPath.Add(p.vectorPath[0]);
}
//Debug.DrawRay (p.originalEndPoint,Vector3.up,Color.red);
//Debug.DrawRay (p.startPoint,Vector3.up,Color.red);
//Debug.DrawRay (p.endPoint,Vector3.up,Color.green);
Vector3 pStart = Vector3.zero,
pEnd = Vector3.zero;
if (exactStartPoint == Exactness.Original)
{
pStart = GetClampedPoint((Vector3)p.path[0].position, p.originalStartPoint, p.path[0]);
}
else if (exactStartPoint == Exactness.ClosestOnNode)
{
pStart = GetClampedPoint((Vector3)p.path[0].position, p.startPoint, p.path[0]);
}
else if (exactStartPoint == Exactness.Interpolate)
{
pStart = GetClampedPoint((Vector3)p.path[0].position, p.originalStartPoint, p.path[0]);
pStart = Mathfx.NearestPointStrict((Vector3)p.path[0].position, (Vector3)p.path[1 >= p.path.Count?0:1].position, pStart);
}
else
{
pStart = (Vector3)p.path[0].position;
}
if (exactEndPoint == Exactness.Original)
{
pEnd = GetClampedPoint((Vector3)p.path[p.path.Count - 1].position, p.originalEndPoint, p.path[p.path.Count - 1]);
}
else if (exactEndPoint == Exactness.ClosestOnNode)
{
pEnd = GetClampedPoint((Vector3)p.path[p.path.Count - 1].position, p.endPoint, p.path[p.path.Count - 1]);
}
else if (exactEndPoint == Exactness.Interpolate)
{
pEnd = GetClampedPoint((Vector3)p.path[p.path.Count - 1].position, p.originalEndPoint, p.path[p.path.Count - 1]);
pEnd = Mathfx.NearestPointStrict((Vector3)p.path[p.path.Count - 1].position, (Vector3)p.path[p.path.Count - 2 < 0?0:p.path.Count - 2].position, pEnd);
}
else
{
pEnd = (Vector3)p.path[p.path.Count - 1].position;
}
if (!addPoints)
{
//p.vectorPath[0] = p.startPoint;
//p.vectorPath[p.vectorPath.Length-1] = p.endPoint;
//Debug.DrawLine (p.vectorPath[0],pStart,Color.green);
//Debug.DrawLine (p.vectorPath[p.vectorPath.Length-1],pEnd,Color.green);
p.vectorPath[0] = pStart;
p.vectorPath[p.vectorPath.Count - 1] = pEnd;
}
else
{
//Vector3[] newPath = new Vector3[p.vectorPath.Length+(exactStartPoint != Exactness.SnapToNode ? 1 : 0) + (exactEndPoint != Exactness.SnapToNode ? 1 : 0)];
if (exactEndPoint != Exactness.SnapToNode)
{
//newPath[0] = pStart;
p.vectorPath.Insert(0, pStart);
}
if (exactEndPoint != Exactness.SnapToNode)
{
//newPath[newPath.Length-1] = pEnd;
p.vectorPath.Add(pEnd);
}
/*int offset = exactStartPoint != Exactness.SnapToNode ? 1 : 0;
* for (int i=0;i<p.vectorPath.Length;i++) {
* newPath[i+offset] = p.vectorPath[i];
* }
* p.vectorPath = newPath;*/
}
}
public static Texture2D DrawLine(Vector2 from, Vector2 to, float w, Color col, Texture2D tex, bool stroke, Color strokeCol, float strokeWidth)
{
w = Mathf.Round(w);//It is important to round the numbers otherwise it will mess up with the texture width
strokeWidth = Mathf.Round(strokeWidth);
var extent = w + strokeWidth;
var stY = Mathf.Clamp(Mathf.Min(from.y, to.y) - extent, 0, tex.height);//This is the topmost Y value
var stX = Mathf.Clamp(Mathf.Min(from.x, to.x) - extent, 0, tex.width);
var endY = Mathf.Clamp(Mathf.Max(from.y, to.y) + extent, 0, tex.height);
var endX = Mathf.Clamp(Mathf.Max(from.x, to.x) + extent, 0, tex.width);//This is the rightmost Y value
strokeWidth = strokeWidth / 2;
var strokeInner = (w - strokeWidth) * (w - strokeWidth);
var strokeOuter = (w + strokeWidth) * (w + strokeWidth);
var strokeOuter2 = (w + strokeWidth + 1) * (w + strokeWidth + 1);
var sqrW = w * w;//It is much faster to calculate with squared values
var lengthX = endX - stX;
var lengthY = endY - stY;
var start = new Vector2(stX, stY);
Color[] pixels = tex.GetPixels((int)stX, (int)stY, (int)lengthX, (int)lengthY, 0);//Get all pixels
for (int y = 0; y < lengthY; y++)
{
for (int x = 0; x < lengthX; x++)
{//Loop through the pixels
var p = new Vector2(x, y) + start;
var center = p + new Vector2(0.5f, 0.5f);
float dist = (center - Mathfx.NearestPointStrict(from, to, center)).sqrMagnitude;//The squared distance from the center of the pixels to the nearest point on the line
if (dist <= strokeOuter2)
{
var samples = Sample(p);
var c = Color.black;
var pc = pixels[y * (int)lengthX + x];
for (int i = 0; i < samples.Length; i++)
{ //Loop through the samples
dist = (samples[i] - Mathfx.NearestPointStrict(from, to, samples[i])).sqrMagnitude; //The squared distance from the sample to the line
if (stroke)
{
if (dist <= strokeOuter && dist >= strokeInner)
{
c += strokeCol;
}
else if (dist < sqrW)
{
c += col;
}
else
{
c += pc;
}
}
else
{
if (dist < sqrW)
{//Is the distance smaller than the width of the line
c += col;
}
else
{
c += pc;//No it wasn't, set it to be the original colour
}
}
}
c /= samples.Length;//Get the avarage colour
pixels[y * (int)lengthX + x] = c;
}
}
}
tex.SetPixels((int)stX, (int)stY, (int)lengthX, (int)lengthY, pixels, 0);
tex.Apply();
return(tex);
}
public bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
{
if (left.Count <= 3)
{
return(false);
}
System.Console.WriteLine("Start");
//Remove identical vertices
while (left[1] == left[2] && right[1] == right[2])
{
System.Console.WriteLine("Removing identical left and right");
left.RemoveAt(1);
right.RemoveAt(1);
}
/*while (right[1] == right[2]) {
* System.Console.WriteLine ("Removing identical right");
* right.RemoveAt (1);
* left.RemoveAt (1);
* }*/
Vector3 swPoint = left[2];
if (swPoint == left[1])
{
swPoint = right[2];
}
/*if (Polygon.IsColinear (left[0],left[1],right[1])) {
* System.Console.WriteLine (" Colinear");
* left[0] += (left[2]-left[0]).normalized*0.001F;
* if (Polygon.IsColinear (left[0],left[1],right[1])) {
* Debug.LogError ("WUT!!!");//NOTE - CAN ACTUALLY HAPPEN!
* }
* }*/
//Solves cases where the start point lies on the wrong side of the first funnel portal
if (Polygon.IsColinear(left[0], left[1], right[1]) || Polygon.Left(left[1], right[1], swPoint) == Polygon.Left(left[1], right[1], left[0]))
{
Debug.DrawLine(left[1], right[1], new Color(0, 0, 0, 0.5F));
Debug.DrawLine(left[0], swPoint, new Color(0, 0, 0, 0.5F));
System.Console.WriteLine("Wrong Side");
left[0] = Mathfx.NearestPointStrict(left[1], right[1], left[0]);
left[0] += (left[0] - swPoint).normalized * 0.001F; //Tiny move to the right side to prevent floating point errors, too bad with that .normalized call though, could perhaps be optimized
right[0] = left[0];
}
//Switch left and right to really be on the "left" and "right" sides
if (!Polygon.IsClockwise(left[0], left[1], right[1]) && !Polygon.IsColinear(left[0], left[1], right[1]))
{
System.Console.WriteLine("Wrong Side 2");
List <Vector3> tmp = left;
left = right;
right = tmp;
}
/*for (int i=1;i<leftFunnel.Length-1;i++) {
*
* float unitWidth = 5;
* Int3 normal = (rightFunnel[i]-leftFunnel[i]);
* float magn = normal.worldMagnitude;
* normal /= magn;
* normal *= Mathf.Clamp (unitWidth,0,(magn/2F));
* leftFunnel[i] += normal;
* rightFunnel[i] -= normal;
* }*/
funnelPath.Add(left[0]);
Vector3 portalApex = left[0];
Vector3 portalLeft = left[1];
Vector3 portalRight = right[1];
int apexIndex = 0;
int rightIndex = 1;
int leftIndex = 1;
//yield return 0;
for (int i = 2; i < left.Count; i++)
{
if (funnelPath.Count > 200)
{
Debug.LogWarning("Avoiding infinite loop");
break;
}
Vector3 pLeft = left[i];
Vector3 pRight = right[i];
/*Debug.DrawLine (portalApex,portalLeft,Color.red);
* Debug.DrawLine (portalApex,portalRight,Color.yellow);
* Debug.DrawLine (portalApex,left,Color.cyan);
* Debug.DrawLine (portalApex,right,Color.cyan);*/
if (Polygon.TriangleArea2(portalApex, portalRight, pRight) >= 0)
{
if (portalApex == portalRight || Polygon.TriangleArea2(portalApex, portalLeft, pRight) <= 0)
{
portalRight = pRight;
rightIndex = i;
}
else
{
funnelPath.Add(portalLeft);
portalApex = portalLeft;
apexIndex = leftIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
//yield return 0;
continue;
}
}
if (Polygon.TriangleArea2(portalApex, portalLeft, pLeft) <= 0)
{
if (portalApex == portalLeft || Polygon.TriangleArea2(portalApex, portalRight, pLeft) >= 0)
{
portalLeft = pLeft;
leftIndex = i;
}
else
{
funnelPath.Add(portalRight);
portalApex = portalRight;
apexIndex = rightIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
//yield return 0;
continue;
}
}
//yield return 0;
}
//yield return 0;
funnelPath.Add(left[left.Count - 1]);
return(true);
}
public static Vector3 GetNextTarget(Path path, Vector3 currPosition, float offset, float pickNextWaypointMargin)
{
if (path.error)
{
return(currPosition);
}
Int3 currentPosition = (Int3)currPosition;
int startIndex = 0;
int endIndex = path.path.Length;
//Int3 pos = (Int3)currentPosition;
//int minDist = -1;
//int minNode = -1;
INavmesh navmeshGraph = AstarData.GetGraph(path.path[0]) as INavmesh;
if (navmeshGraph == null)
{
Debug.LogError("Couldn't cast graph to the appropriate type (graph isn't a Navmesh type graph, it doesn't implement the INavmesh interface)");
return(currPosition); ///Apply (path,start,end, startIndex, endIndex, graph);
}
Int3[] vertices = navmeshGraph.vertices;
//float minDist2 = -1;
//int minNode2 = -1;
//Debug.Log (meshPath.Length + " "+path.Length +" "+startIndex+" "+endIndex);
/*for (int i=startIndex;i< endIndex;i++) {
* MeshNode node = path.path[i] as MeshNode;
*
* if (node == null) {
* Debug.LogWarning ("Path could not be casted to Mesh Nodes");
* return currentPosition;//return base.Apply (path,start,end, startIndex, endIndex, graph);
* }
*
* //if (Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],currentPosition) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],currentPosition) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],currentPosition) >= 0) {
*
* if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos)) {
*
* if (minDist == -1) {
* float dist2 = (node.position-pos).sqrMagnitude;
* if (minDist2 == -1 || dist2 < minDist2) {
* minDist2 = dist2;
* minNode2 = i;
* }
* }
* continue;
* }
*
* Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.blue);
* Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.blue);
* Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.blue);
*
*
* int dist = node.position.y-pos.y;
*
* if (minDist == -1 || dist < minDist) {
* minDist = dist;
* minNode = i;
* }
*
* }*/
MeshNode lastNode = path.path[endIndex - 1] as MeshNode;
if (lastNode == null)
{
Debug.LogWarning("Path could not be casted to Mesh Nodes");
return(currentPosition); //return base.Apply (path,start,end, startIndex, endIndex, graph);
}
PathNNConstraint constraint = PathNNConstraint.Default;
constraint.SetStart(lastNode);
NNInfo nninfo = NavMeshGraph.GetNearestForce(path.path, vertices, currPosition, constraint);
currentPosition = nninfo.clampedPosition;
/*for (int i=startIndex;i< endIndex;i++) {
* if (nninfo.node == path.path[i]) {
* minNode = i;
* }
* }*/
//Node minNode = nninfo.node;
/*startIndex = minNode;
* if (startIndex == -1) {
* startIndex = minNode2;
* currentPosition = path.path[minNode2].position;
* }
* if (startIndex == -1) {
* Debug.Log ("Couldn't find current node");
* return currentPosition;
* }*/
MeshNode[] meshPath = new MeshNode[endIndex - startIndex];
for (int i = startIndex; i < endIndex; i++)
{
meshPath[i - startIndex] = path.path[i] as MeshNode;
}
//return Vector3.zero;
//Vector3[] vertices = null;
if (leftFunnel == null || leftFunnel.Length < meshPath.Length + 1)
{
leftFunnel = new Int3[meshPath.Length + 1];
}
if (rightFunnel == null || rightFunnel.Length < meshPath.Length + 1)
{
rightFunnel = new Int3[meshPath.Length + 1];
}
int leftFunnelLength = meshPath.Length + 1;
//int rightFunnelLength = meshPath.Length+1;
leftFunnel[0] = currentPosition;
rightFunnel[0] = currentPosition;
leftFunnel[meshPath.Length] = path.endPoint;
rightFunnel[meshPath.Length] = path.endPoint;
int lastLeftIndex = -1;
int lastRightIndex = -1;
for (int i = 0; i < meshPath.Length - 1; i++)
{
//Find the connection between the nodes
MeshNode n1 = meshPath[i];
MeshNode n2 = meshPath[i + 1];
bool foundFirst = false;
int first = -1;
int second = -1;
for (int x = 0; x < 3; x++)
{
//Vector3 vertice1 = vertices[n1.vertices[x]];
//n1[x] gets the vertice index for vertex number 'x' in the node. Equal to n1.GetVertexIndex (x)
int vertice1 = n1[x];
for (int y = 0; y < 3; y++)
{
//Vector3 vertice2 = vertices[n2.vertices[y]];
int vertice2 = n2[y];
if (vertice1 == vertice2)
{
if (foundFirst)
{
second = vertice2;
break;
}
else
{
first = vertice2;
foundFirst = true;
}
}
}
}
//Debug.DrawLine ((Vector3)vertices[first]+Vector3.up*0.1F,(Vector3)vertices[second]+Vector3.up*0.1F,Color.cyan);
//Debug.Log (first+" "+second);
if (first == lastLeftIndex)
{
leftFunnel[i + 1] = vertices[first];
rightFunnel[i + 1] = vertices[second];
lastLeftIndex = first;
lastRightIndex = second;
}
else if (first == lastRightIndex)
{
leftFunnel[i + 1] = vertices[second];
rightFunnel[i + 1] = vertices[first];
lastLeftIndex = second;
lastRightIndex = first;
}
else if (second == lastLeftIndex)
{
leftFunnel[i + 1] = vertices[second];
rightFunnel[i + 1] = vertices[first];
lastLeftIndex = second;
lastRightIndex = first;
}
else
{
leftFunnel[i + 1] = vertices[first];
rightFunnel[i + 1] = vertices[second];
lastLeftIndex = first;
lastRightIndex = second;
}
}
//Switch the arrays so the right funnel really is on the right side (and vice versa)
if (!Polygon.IsClockwise(currentPosition, leftFunnel[1], rightFunnel[1]))
{
Int3[] tmp = leftFunnel;
leftFunnel = rightFunnel;
rightFunnel = tmp;
}
for (int i = 1; i < leftFunnelLength - 1; i++)
{
//float unitWidth = 2;
Int3 normal = (rightFunnel[i] - leftFunnel[i]);
float magn = normal.worldMagnitude;
normal /= magn;
normal *= Mathf.Clamp(offset, 0, (magn / 2F));
leftFunnel[i] += normal;
rightFunnel[i] -= normal;
//Debug.DrawLine (rightFunnel[i],leftFunnel[i],Color.blue);
}
/*for (int i=0;i<path.Length-1;i++) {
* Debug.DrawLine (path[i].position,path[i+1].position,Color.blue);
* }*/
/*for (int i=0;i<leftFunnel.Length-1;i++) {
* Debug.DrawLine (leftFunnel[i],leftFunnel[i+1],Color.red);
* Debug.DrawLine (rightFunnel[i],rightFunnel[i+1],Color.magenta);
* }*/
if (tmpList == null)
{
tmpList = new List <Vector3>(3);
}
List <Vector3> funnelPath = tmpList;
funnelPath.Clear();
funnelPath.Add(currentPosition);
Int3 portalApex = currentPosition;
Int3 portalLeft = leftFunnel[0];
Int3 portalRight = rightFunnel[0];
int apexIndex = 0;
int rightIndex = 0;
int leftIndex = 0;
//yield return 0;
for (int i = 1; i < leftFunnelLength; i++)
{
Int3 left = leftFunnel[i];
Int3 right = rightFunnel[i];
/*Debug.DrawLine (portalApex,portalLeft,Color.red);
* Debug.DrawLine (portalApex,portalRight,Color.yellow);
* Debug.DrawLine (portalApex,left,Color.cyan);
* Debug.DrawLine (portalApex,right,Color.cyan);*/
if (Polygon.TriangleArea2(portalApex, portalRight, right) >= 0)
{
if (portalApex == portalRight || Polygon.TriangleArea2(portalApex, portalLeft, right) <= 0)
{
portalRight = right;
rightIndex = i;
}
else
{
funnelPath.Add((Vector3)portalLeft);
//if (funnelPath.Count > 3)
//break;
portalApex = portalLeft;
apexIndex = leftIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
//yield return 0;
continue;
}
}
if (Polygon.TriangleArea2(portalApex, portalLeft, left) <= 0)
{
if (portalApex == portalLeft || Polygon.TriangleArea2(portalApex, portalRight, left) >= 0)
{
portalLeft = left;
leftIndex = i;
}
else
{
funnelPath.Add((Vector3)portalRight);
//if (funnelPath.Count > 3)
//break;
portalApex = portalRight;
apexIndex = rightIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
//yield return 0;
continue;
}
}
//yield return 0;
}
//yield return 0;
funnelPath.Add(path.endPoint);
Vector3[] p = funnelPath.ToArray();
if (p.Length <= 1)
{
return(currentPosition);
}
for (int i = 1; i < p.Length - 1; i++)
{
Vector3 closest = Mathfx.NearestPointStrict(p[i], p[i + 1], currentPosition);
if ((closest - (Vector3)currentPosition).sqrMagnitude < pickNextWaypointMargin * pickNextWaypointMargin)
{
continue;
}
else
{
return(p[i]);
}
}
return(p[p.Length - 1]);
}
// @return relative (0-1) position of given point
// - for position near the left edge result will be near to 0.0f
// - for position near the right edge result will be near to 1.0f
public float GetPositionEdgeRelative(Vector3 Position)
{
Vector3 EdgePos = Mathfx.NearestPointStrict(_LeftEdge, _RightEdge, Position);
return(Mathf.Clamp01((_LeftEdge - EdgePos).magnitude / _EdgeLength));
}
public Vector3 GetNearestPointOnCover(Vector3 pos)
{
return(Mathfx.NearestPointStrict(_LeftEdge, _RightEdge, pos));
}
public float GetDistanceTo(Vector3 pos)
{
Vector3 p = Mathfx.NearestPointStrict(_LeftEdge, _RightEdge, pos);
return((pos - p).magnitude);
}