public bool GetCrossTrackErr(Vector3 pos, ref float err) { if (iActiveSpan >= nodes.Count - 2) { return(false); } PathNode a = nodes[iActiveSpan]; PathNode b = nodes[iActiveSpan + 1]; //2d path. pos.y = a.pos.y; LineSeg3d pathSeg = new LineSeg3d(ref a.pos, ref b.pos); pathSeg.Draw(Color.green); LineSeg3d.SegResult segRes = new LineSeg3d.SegResult(); Vector3 closePt = pathSeg.ClosestPointOnSegmentTo(ref pos, ref segRes); Debug.DrawLine(a.pos, closePt, Color.blue); if (segRes == LineSeg3d.SegResult.GreaterThanEnd) { iActiveSpan++; } else if (segRes == LineSeg3d.SegResult.LessThanOrigin) { if (iActiveSpan > 0) { iActiveSpan--; } } Vector3 errVec = pathSeg.ClosestVectorTo(ref pos); Debug.DrawRay(closePt, errVec, Color.white); float sign = 1.0f; Vector3 cp = Vector3.Cross(pathSeg.m_dir.normalized, errVec.normalized); if (cp.y > 0.0f) { sign = -1f; } err = errVec.magnitude * sign; return(true); }
public void SmoothPath(float factor = 0.5f) { LineSeg3d.SegResult segRes = new LineSeg3d.SegResult(); for (int iN = 1; iN < nodes.Count - 2; iN++) { PathNode p = nodes[iN - 1]; PathNode c = nodes[iN]; PathNode n = nodes[iN + 1]; LineSeg3d seg = new LineSeg3d(ref p.pos, ref n.pos); Vector3 closestP = seg.ClosestPointOnSegmentTo(ref c.pos, ref segRes); Vector3 dIntersect = closestP - c.pos; c.pos += dIntersect.normalized * factor; } }
public bool GetCrossTrackErr(Vector3 pos, ref int iActiveSpan, ref float err, int lookAhead = 1) { int nextIActiveSpan = (iActiveSpan + 1) % (nodes.Count); int aheadIActiveSpan = (iActiveSpan + lookAhead) % (nodes.Count); PathNode a = nodes[iActiveSpan]; PathNode b = nodes[nextIActiveSpan]; PathNode c = nodes[aheadIActiveSpan]; //2d path. pos.y = a.pos.y; LineSeg3d pathSeg = new LineSeg3d(ref a.pos, ref c.pos); LineSeg3d.SegResult segRes = new LineSeg3d.SegResult(); Vector3 closePt = pathSeg.ClosestPointOnSegmentTo(ref pos, ref segRes); Vector3 errVec = pathSeg.ClosestVectorTo(ref pos); pathSeg.Draw(Color.green); Debug.DrawLine(a.pos, closePt, Color.blue); Debug.DrawRay(closePt, errVec, Color.white); float sign = 1.0f; Vector3 cp = Vector3.Cross(pathSeg.m_dir.normalized, errVec.normalized); if (cp.y > 0.0f) { sign = -1f; } err = errVec.magnitude * sign; int oldActiveSpan = iActiveSpan; float dista = Vector3.Distance(a.pos, pos); float distb = Vector3.Distance(b.pos, pos); if (dista > distb) { iActiveSpan = (iActiveSpan + 1) % (nodes.Count); } // if (iActiveSpan - oldActiveSpan <= 0) { return true; } // we lapped return(false); // we are on the same lap }