/// <summary>
/// Helper method to get points count base on matrix index inside container and XyRule, XRule
/// </summary>
/// <param name="dimension"></param>
/// <param name="indexInContainer"></param>
/// <param name="indexInMatrix"></param>
/// <returns></returns>
public int GetCountPointsXyAndXRule(PointDimension dimension, int indexInContainer, int indexInMatrix)
{
switch (dimension)
{
case PointDimension.Xy:
{
var path = new PointPath
{
IndexInContainer = indexInContainer,
IndexInMatrix = indexInMatrix
};
var customPointsNumber = XyConfig.Rules?.FirstOrDefault(r => r.Path.Equals(path))?.CountPoints;
return(customPointsNumber ?? XyConfig.DefaultCountPoints ?? CountPoints);
}
case PointDimension.X:
{
var path = new PointPath
{
IndexInContainer = indexInContainer,
IndexInMatrix = indexInMatrix
};
var customPointsNumber = XConfig.Rules?.FirstOrDefault(r => r.Path.Equals(path))?.CountPoints;
return(customPointsNumber ?? XyConfig.DefaultCountPoints ?? CountPoints);
}
default:
return(CountPoints);
}
}
static void PointTest()
{
var firstPoint = new Point3D(1, 1, 1);
var secondPoint = new Point3D(1, 2, 2);
Console.WriteLine(firstPoint);
Console.WriteLine("{0:F2}", PointDistance.Distance(firstPoint, secondPoint));
Console.WriteLine("Zero point: {0}", Point3D.PointO);
var pathOfPoints = new PointPath();
pathOfPoints.AddPoint(new Point3D(1, 2, 3));
pathOfPoints.AddPoint(new Point3D(2, 51, 23));
pathOfPoints.AddPoint(new Point3D(13, 12, 23));
pathOfPoints.AddPoint(new Point3D(42, 2, 11));
pathOfPoints.AddPoint(new Point3D(32, 2, 63));
Console.WriteLine("Path\n" + pathOfPoints);
PathStorage.SavePathToFile("MyPointPath.txt", pathOfPoints);
var pathFromFile = PathStorage.ReadPathFromFile("MyPointPath.txt");
Console.WriteLine("Path read from file\n" + pathFromFile);
}
public RRTNode GetNodeToGoal()
{
if (waypoints.Length == 0.0)
{
return(null);
}
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
bool foundPath = false;
Console.WriteLine("Finding a path");
do
{
startingNode = new RRTNode(currentState);
foundPath = rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode);
}while (!foundPath && receivedNewPath);
receivedNewPath = false;
return(goalNode);
}
}
public IPath GetPathToGoal()
{
if (waypoints.Length == 0.0)
{
return(null);
}
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
while (rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode) == false)
{
continue;
}
//startingNode = goalNode;
IPath segmentToGoal = GetFinalPath(goalNode);
// add each small segment of each segment to the path to return
//foreach (IPathSegment seg in segmentToGoal)
//path.Add(seg);
//}
return(segmentToGoal);
}
}
// Use this for initialization
void Start()
{
path = pathList[0];
agent = GetComponent <NavMeshAgent>();
pathIndex = 0;
agent.speed = speed;
}
/// <summary>
/// walks along the bezier path and creates a straight line segment approximation
/// </summary>
/// <param name="path"></param>
/// <param name="sampleDistance"></param>
/// <returns></returns>
public PointPath ConvertBezierPathToPointPath(IPath path, double sampleDistance)
{
PointPath pathOut = new PointPath();
foreach (IPathSegment seg in path)
{
if (seg is BezierPathSegment == false)
{
throw new InvalidOperationException();
}
BezierPathSegment bseg = seg as BezierPathSegment;
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = sampleDistance;
p2 = seg.AdvancePoint(p, ref refDist);
pathOut.Add(new LinePathSegment(p.pt, p2.pt));
p = p2;
}
}
return(pathOut);
}
//*********************** MANAGER PATH ************************//
/// <summary>
/// Insert new Path
/// </summary>
/// <param name="IdUser"></param>
/// <param name="Namepath"></param>
/// <param name="path"></param>
/// <returns></returns>
public string CreatePath(string IdUser, string Namepath, string[] path)
{
if (path.Length != 0)
{
int Lengpoint = path.Length / 3;
var arr = new List <PointPath>();
for (var i = 0; i < path.Length; i = i + 3)
{
int idArray = i / 3;
PointPath pts = new PointPath
{
NamePoint = path[i],
Lat = double.Parse(path[i + 1]),
Lg = double.Parse(path[i + 2]),
};
arr.Add(pts);
}
return(MapProvider.CreatPath(IdUser, Namepath, arr.ToArray()));
}
else
{
return("false");
}
}
private void DrawGizmos(bool selected)
{
Update_Path();
if (transform.childCount <= 1)
{
return;
}
PointPath prev = GetPathPoint(0.0f);
float dist = -1.0f;
while (dist < totalDistance)
{
dist = Mathf.Clamp(dist + 1.0f, 0, totalDistance);
PointPath next = GetPathPoint(dist);
Gizmos.color = selected ? new Color(0, 1, 1, 1) : new Color(0, 1, 1, 0.5f);
Gizmos.DrawLine(transform.TransformPoint(prev.point), transform.TransformPoint(next.point));
//draw up vector
Gizmos.color = selected ? Color.green : new Color(0, 1, 0, 0.5f);
Gizmos.DrawLine(transform.TransformPoint(next.point), transform.TransformPoint(next.point) + transform.TransformDirection(next.up));
//draw right vector
Gizmos.color = selected ? Color.red : new Color(1, 0, 0, 0.5f);
Gizmos.DrawLine(transform.TransformPoint(next.point), transform.TransformPoint(next.point) + transform.TransformDirection(next.right * 0.5f));
prev = next;
}
}
/// <summary>
/// Find the path from the goal to the starting point
/// </summary>
/// <param name="goal"></param>
/// <returns>path from the start point to the goal point</returns>
private IPath GetFinalPath(RRTNode goal)
{
IPath pathToGoal = new PointPath();
GetToRoot(goal, ref pathToGoal);
return(pathToGoal);
}
// Update is called once per frame
void Update()
{
if (path != null)
{
currentTarget = path.pathPoints[pathIndex].transform;
if (Vector3.Distance(currentTarget.transform.position, transform.position) < 0.5f)
{
pathIndex++;
if (pathIndex >= path.pathPoints.Count)
{
if (path == pathList[0])
{
if (leftPath)
{
path = pathList[1];
}
else
{
path = pathList[2];
}
pathIndex = 0;
}
else
{
ScenarioManager.Instance().GoToNextScenario();
path = null;
}
}
}
}
agent.SetDestination(currentTarget.position);
}
public IPath Wp2CubicPath(IPath path, List <Polygon> obstacles, double nothing)
{
PointPath ret = new PointPath();
CubicBezier[] bez = SmoothingSpline.BuildCardinalSpline(PathUtils.IPathToPoints(path).ToArray(), null, null, 1.0);
foreach (CubicBezier b in bez)
{
ret.Add(new BezierPathSegment(b, null, false));
}
return(ret);
}
public void UpdatePath(RRTNode goalNode)
{
//create IPath from rootNode
IPath newNodePath = new PointPath();
inputList = new List <RobotTwoWheelCommand>();
ConvertNodeToPath(newNodePath, goalNode, inputList);
lock (followerLock)
{
UpdatePath(newNodePath);
}
}
private Thread updatethread; //thread that continuously calls the paint function (to update all node movements)
#endregion Fields
#region Constructors
//Constructor
public Form_Bridge()
{
InitializeComponent();
label_algorithm.Text = ""; //text displaying algorithm thats being used
allNodes = new List<Node>();
selector = new SimulationSelector();
board = new PointPath(); //creates set of points
this.DoubleBuffered = true; //avoids flickering of nodes when updating their positions
this.Paint += new PaintEventHandler(Form_Bridge_Paint); //calls this function when form needs to be repainted
updatethread = new Thread(new ThreadStart(UpdateThread)); //creates the thread that will update the paint function
updatethread.Start(); //starts the update thread
}
/// <summary>
/// Compare two PointPath values
/// </summary>
/// <param name="other">point with which is compared</param>
/// <returns>Comparison result</returns>
public bool Equals(PointPath other)
{
if (other == null)
{
return(false);
}
if (other.IndexInContainer == null || IndexInContainer == null)
{
return(other.IndexInMatrix == IndexInMatrix);
}
return(other.IndexInContainer == IndexInContainer && other.IndexInMatrix == IndexInMatrix);
}
private void createPath()
{
PointPath path = new PointPath(Vector2.right, Vector2.up, .5f, 2f);
List <GameObject> objs = new List <GameObject>();
path.addPoint(new Vector2(-10f, -5f));
path.addRandomPoints(10);
foreach (Vector2 point in path.getPoints())
{
GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
obj.transform.position = point;
obj.transform.localScale = new Vector3(.1f, .1f, .1f);
objs.Add(obj);
}
}
public PointPath GetPathPoint(float dist)
{
dist = Mathf.Clamp(dist, 0.0f, totalDistance);
int[] _four_indices = new int[] { 0, 1, 2, 3 };
Vector3[] _four_points = new Vector3[] { Vector3.zero, Vector3.zero, Vector3.zero, Vector3.zero };
// find segment index
int index = 1;
if (_distances.Length > 0)
{
while (_distances[index] < dist)
{
index++;
}
// the segment in the middle
float _interpolation = Mathf.InverseLerp(_distances[index - 1], _distances[index], dist);
index = index % _numPoints;
_four_indices[0] = Mathf.Clamp(index - 2, 0, _numPoints - 1);
_four_indices[1] = ((index - 1) + _numPoints) % _numPoints;
_four_indices[2] = index % _numPoints;
_four_indices[3] = Mathf.Clamp(index + 1, 0, _numPoints - 1);
// assign the four points with the segment in the middle
_four_points[0] = _points[_four_indices[0]];
_four_points[1] = _points[_four_indices[1]];
_four_points[2] = _points[_four_indices[2]];
_four_points[3] = _points[_four_indices[3]];
PointPath _pathPoint = new PointPath();
_pathPoint.point = CatmullRom(_four_points[0], _four_points[1], _four_points[2], _four_points[3], _interpolation);
_pathPoint.forward = CatmullRom(_four_points[0], _four_points[1], _four_points[2], _four_points[3], _interpolation + 0.01f) - _pathPoint.point;
_pathPoint.forward.Normalize();
// 90 degree turn to right
Vector3 lerpBetweentwo = Vector3.Lerp(_upDirections[_four_indices[1]], _upDirections[_four_indices[2]], _interpolation);
_pathPoint.right = Vector3.Cross(lerpBetweentwo, _pathPoint.forward).normalized;
// 90 degree turn to up
_pathPoint.up = Vector3.Cross(_pathPoint.forward, _pathPoint.right).normalized;
return(_pathPoint);
}
return(new PointPath());
}
private void OnSceneGUI()
{
meshPath = target as MeshPath;
Transform transform = meshPath.transform;
//Draw all points on curve
Handles.color = Color.gray;
float dist = -1.0f;
path = meshPath.path;
if (path)
{
float totalDist = path.totalDistance;
//Draw all points
while (dist < totalDist)
{
dist = Mathf.Clamp(dist + 1.0f, 0, totalDist);
PointPath nextPoint = path.GetPathPoint(dist);
if (Handles.Button(transform.TransformPoint(nextPoint.point), Quaternion.identity, 0.5f, 0.5f, Handles.SphereCap))
{
selectedIndexStopPoint = dist;
positionToDelete = -1;
Repaint();
}
}
//Draw all stop points in blue
Handles.color = Color.blue;
for (int i = 0; i < path.stopPoints.Count; i++)
{
if (Handles.Button(transform.TransformPoint(path.GetPathPoint(path.stopPoints[i]).point), Quaternion.identity, 0.5f, 0.5f, Handles.DotCap))
{
positionToDelete = path.stopPoints[i];
selectedIndexStopPoint = -1;
Repaint();
}
}
}
}
public void Craft()
{
path = GetComponent <Path>();
PointPath pointA = path.GetPathPoint(0.0f);
PointPath pointB = pointA;
Debug.Log(path.totalDistance);
for (float dist = 0.0f; dist < path.totalDistance; dist += _segment_length)
{
pointB = path.GetPathPoint(Mathf.Clamp(dist + _segment_length, 0, path.totalDistance));
_helpTransform1.rotation = Quaternion.LookRotation(pointA.forward, pointA.up);
_helpTransform1.position = transform.TransformPoint(pointA.point);
_helpTransform2.rotation = Quaternion.LookRotation(pointB.forward, pointB.up);
_helpTransform2.position = transform.TransformPoint(pointB.point);
Add_Segment();
pointA = pointB;
}
}
/// <summary>
/// Output the last planned path
/// </summary>
public IPath GetPathToGoal()
{
lock (pathLock)
{
PointPath newPath = new PointPath();
if (outputNodeList.Count == 1)
{
//If there is only one ode in the planned path, return an IPath with a degenerate segment
newPath.Add(new LinePathSegment(outputNodeList[0].Value, outputNodeList[0].Value));
}
else if (outputNodeList.Count > 1)
{
//Build an IPath based on the values of each node in the last planned path
int segmentCount = outputNodeList.Count - 1;
for (int i = 0; i < segmentCount; i++)
{
newPath.Add(new LinePathSegment(outputNodeList[i].Value, outputNodeList[i + 1].Value));
}
}
return(newPath);
}
}
public IPath GetPathToGoal(IPath[] otherPaths, out IPath sparsePath, out bool inObstacle, out bool wpInObstacle)
{
inObstacle = false;
wpInObstacle = false;
sparsePath = null;
if (waypoints == null)
{
return(null);
}
if (waypointsAchieved >= waypoints.Count)
{
return(null);
}
if (og == null)
{
return(null);
}
if (bloatedObstacles == null)
{
Console.WriteLine("Convolved obstacles fail!");
return(null);
}
// Add current pose position to filteredWaypoints. (FilteredWaypoints is what we will be
// planning over. Waypoints is list of user given waypoints.)
List <Waypoint> filteredWaypoints = new List <Waypoint>();
filteredWaypoints.Add(new Waypoint(pose.ToVector2(), true, 0));
lock (ogLock)
{
if (og.GetCell(pose.x, pose.y) == 255)
{
inObstacle = true;
og.SetCell(pose.x, pose.y, 0);
}
}
// Only add user waypoints to PathPoints if they have not been achieved yet.
// If they are inside an obstacle, return a null path.
foreach (Waypoint wp in waypoints)
{
if (!wp.Achieved)
{
lock (bloatedObstacleLock)
{
foreach (Polygon p in bloatedObstacles)
{
if (p.IsInside(wp.Coordinate))
{
wpInObstacle = true;
return(null);
}
}
}
filteredWaypoints.Add(wp);
}
}
// Plan each segment of the filteredWaypoints individually and stitch them together.
List <Waypoint> completeRawPath = new List <Waypoint>();
for (int i = 0; i < filteredWaypoints.Count - 1; i++)
{
bool pathOk;
List <Waypoint> rawPath;
lock (ogLock)
{
rawPath = dstar.FindPath(filteredWaypoints[i], filteredWaypoints[i + 1], og, out pathOk);
}
if (!pathOk)
{
return(null);
}
else
{
int j;
// We want to only add the first path waypoint if it is the first
// segment of the whole path.
if (i == 0)
{
j = 0;
}
else
{
j = 1;
}
for (; j < rawPath.Count; j++)
{
completeRawPath.Add(rawPath.ElementAt(j));
}
}
}
if (completeRawPath.Count > 0)
{
IPath bezierPath;
PointPath segmentedBezierPath;
lock (bloatedObstacleLock)
{
bezierPath = smoother.Wp2BezierPath(completeRawPath, bloatedObstacles, collapseThreshold);
}
sparsePath = new PointPath(completeRawPath);
segmentedBezierPath = smoother.ConvertBezierPathToPointPath(bezierPath, bezierSegmentation);
// We want to check our sparse path against other sparse paths to see
// if there are any intersections. If there are, we want to know if the
// intersections will be a problem. If it is a problem, bloat that sparse path,
// add it to obstacles, and replan.
for (int i = 0; i < otherPaths.Length; i++)
{
if (otherPaths[i] != null && otherPaths[i].Count > 0 && i + 1 != robotID)
{
double intersectDist = DoPathsCollide(sparsePath, otherPaths[i]);
if (intersectDist != -1)
{
PointOnPath collisionPt;
int collisionSegmentIndex;
double tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = segmentedBezierPath.AdvancePoint(segmentedBezierPath.StartPoint, ref tempDist);
collisionSegmentIndex = segmentedBezierPath.IndexOf(collisionPt.segment);
segmentedBezierPath.RemoveRange(collisionSegmentIndex, segmentedBezierPath.Count - collisionSegmentIndex);
tempDist = intersectDist - collideBackoff;
tempDist = Math.Max(0, tempDist);
collisionPt = sparsePath.AdvancePoint(sparsePath.StartPoint, ref tempDist);
collisionSegmentIndex = sparsePath.IndexOf(collisionPt.segment);
sparsePath.RemoveRange(collisionSegmentIndex, sparsePath.Count - collisionSegmentIndex - 1);
sparsePath[collisionSegmentIndex].End = collisionPt.pt;
}
}
}
return(segmentedBezierPath);
}
return(null);
}
public IPath Wp2BezierPath(List <Waypoint> path, List <Polygon> obstacles, double collapseThreshold)
{
List <IPathSegment> segments = new List <IPathSegment>();
List <Vector2> newPath = new List <Vector2>();
PointPath bezPath;
Vector2 P0, P1, P2, P3, dXY, newPoint;
dXY = new Vector2(0, 0);
double previousCellValue = path.ElementAt(0).TerrainCost;
bool inPlateau = false, inLocalMin = false;
double localMin = Double.MaxValue, localMax = Double.MinValue;
// Go through the path and mark local minimas and maximas
for (int i = 1; i < path.Count; i++)
{
if (inPlateau)
{
if (previousCellValue != path.ElementAt(i).TerrainCost)
{
inPlateau = false;
//path.ElementAt(i - 1).Critical = true;
}
}
else
{
if (previousCellValue == path.ElementAt(i).TerrainCost)
{
inPlateau = true;
//path.ElementAt(i - 1).Critical = true;
}
if (inLocalMin)
{
if (path.ElementAt(i).TerrainCost < localMin)
{
localMin = path.ElementAt(i).TerrainCost;
}
else
{
localMin = Double.MaxValue;
inLocalMin = false;
//path.ElementAt(i - 1).Critical = true;
}
}
else
{
if (path.ElementAt(i).TerrainCost > localMax)
{
localMax = path.ElementAt(i).TerrainCost;
}
else
{
localMax = Double.MinValue;
inLocalMin = true;
path.ElementAt(i - 1).Critical = true;
}
}
}
previousCellValue = path.ElementAt(i).TerrainCost;
}
// Collapse nodes down based on proximity to one another
for (int i = 0; i < path.Count - 1; i++)
{
if (path.ElementAt(i).Coordinate.DistanceTo(path.ElementAt(i + 1).Coordinate) < collapseThreshold)
{
if (!path.ElementAt(i).UserWaypoint&& !path.ElementAt(i).Critical)
{
if (!path.ElementAt(i + 1).UserWaypoint&& !path.ElementAt(i + 1).Critical)
{
newPoint = (path.ElementAt(i).Coordinate + path.ElementAt(i + 1).Coordinate) / 2;
if (IsClear(newPoint, path.ElementAt(i - 1).Coordinate, obstacles) &&
IsClear(newPoint, path.ElementAt(i + 2).Coordinate, obstacles))
{
path.RemoveRange(i, 2);
path.Insert(i, new Waypoint(newPoint, false, 0));
i--;
}
}
else
{
newPoint = path.ElementAt(i + 1).Coordinate;
if (IsClear(newPoint, path.ElementAt(i - 1).Coordinate, obstacles))
{
path.RemoveAt(i);
i--;
}
}
}
else
{
if (!path.ElementAt(i + 1).UserWaypoint&& !path.ElementAt(i + 1).Critical)
{
newPoint = path.ElementAt(i).Coordinate;
if (IsClear(newPoint, path.ElementAt(i + 2).Coordinate, obstacles))
{
path.RemoveAt(i + 1);
i--;
}
}
}
}
}
// Change waypoints to Bezier curves
for (int i = path.Count - 1; i > 0; i--)
{
P3 = path.ElementAt(i).Coordinate;
P0 = path.ElementAt(i - 1).Coordinate;
if (i == path.Count - 1)
{
P2 = P3 - ((P3 - P0) / 3);
}
else
{
if (dXY.Length > ((P3 - P0) / 4).Length)
{
dXY *= ((P3 - P0) / 4).Length / dXY.Length;
}
P2 = P3 - dXY;
}
if (i == 1)
{
P1 = P0 + ((P3 - P0) / 3);
}
else
{
dXY = (P3 - path.ElementAt(i - 2).Coordinate) / 6;
if (dXY.Length > ((P3 - P0) / 4).Length)
{
dXY *= ((P3 - P0) / 4).Length / dXY.Length;
}
P1 = P0 + dXY;
}
segments.Add(new BezierPathSegment(P0, P1, P2, P3, 0.05, false));
}
segments.Reverse();
bezPath = new PointPath(segments);
return(bezPath);
}
public void SetPath(PointPath p)
{
path = p;
}
