public void predictTrajectory(PointQ gpsCoordinates, List <PointQ> allEndPoints, PointQ gpsCoordinate_previous) { gpsCoordinate_current = gpsCoordinates; // { // gpsCoordinate_previous = gpsCoordinate_current; // flag = 1; // } //Initial condition to check if there exists a previous coordinate if (gpsCoordinate_previous == null && gpsCoordinate_current != null) { //No prediction } else { //Initialize a list of type PointQ with all possible end points/coordinates List <PointQ> destinationPoints = allEndPoints; // gpsCoordinate_previous = gpsCoordinate_current; makePrediction(gpsCoordinate_current, gpsCoordinate_previous, destinationPoints); // gpsCoordinate_previous = gpsCoordinate_current; } // return gpsCoordinate_previous; }
private static double GetDistance(PointQ point1, PointQ point2) { //Pythagorean theorem for distance calculation double a = (double)(point2.X - point1.X); double b = (double)(point2.Y - point1.Y); return(Math.Sqrt(a * a + b * b)); }
private void makePrediction(PointQ currentCoordinate, PointQ previousCoordinate, List <PointQ> allEndPoints) { foreach (PointQ endPoint in allEndPoints) { Double previous_distance = GetDistance(previousCoordinate, endPoint); Double current_distance = GetDistance(currentCoordinate, endPoint); //distance to endpoint is increasing which means that the pedestrian is moving away from this point and therefore moving towards another direction if (current_distance > previous_distance) { //rule out this destination/Path ProbabilityMatrix pM = new ProbabilityMatrix(endPoint, false); listProbabilityMatrix.Add(pM); } //if distance is decreasing towards a certain endpoint this means that pedestrian is moving near to that endpoint else { ProbabilityMatrix pM = new ProbabilityMatrix(endPoint, true); listProbabilityMatrix.Add(pM); } } foreach (ProbabilityMatrix pM in listProbabilityMatrix) { //Add valid paths for calculating probabilty if (pM.isValid == true) { probabilityCount += 1; validDestination.Add(pM.allEndPoints); } else { //Dont include in calculating probabilty } } //Total number of Paths/Routes => 6 in our case double totalnumberofendpoints = allEndPoints.Count(); // total number of valid endpoints => 3 in our case double totalnumberofValidDestinations = validDestination.Count(); foreach (PointQ p in validDestination) { double probability = (1 / totalnumberofValidDestinations) * 100.00; Console.WriteLine("The probability of pedestrian reaching the point (" + p.X + ", " + p.Y + ") is :" + probability.ToString()); //Write Probability of Pedestrian of Valid paths in CSV file using (FileStream fs = new FileStream("Ped.csv", FileMode.Append, FileAccess.Write)) { StreamWriter sw = new StreamWriter(fs); sw.Write("The probability of pedestrian reaching the point (" + p.X + ", " + p.Y + ") is :" + probability.ToString()); sw.Close(); } } }