private List<Obstacle> ProcessTrackedClusters(SceneEstimatorTrackedClusterCollection clusters, Rect vehicleBox)
        {
            List<Obstacle> obstacles = new List<Obstacle>(clusters.clusters.Length);

            // get the list of previous id's
            SortedList<int, Obstacle> previousID;
            if (processedObstacles != null) {
                previousID = new SortedList<int, Obstacle>(processedObstacles.obstacles.Count);
                foreach (Obstacle obs in processedObstacles.obstacles) {
                    if (obs != null && obs.trackID != -1 && !previousID.ContainsKey(obs.trackID)) {
                        previousID.Add(obs.trackID, obs);
                    }
                }
            }
            else {
                previousID = new SortedList<int, Obstacle>();
            }

            List<Coordinates> goodPoints = new List<Coordinates>(1500);

            Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
            Polygon mergePolygon = mergeCircle.ToPolygon(24);

            foreach (SceneEstimatorTrackedCluster cluster in clusters.clusters) {
                // ignore deleted targets
                if (cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_DELETED || cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL || cluster.relativePoints == null || cluster.relativePoints.Length < 3)
                    continue;

                Obstacle obs = new Obstacle();

                obs.trackID = cluster.id;
                obs.speed = cluster.speed;
                obs.speedValid = cluster.speedValid;
                obs.occuluded = cluster.statusFlag != SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;

                // update the age
                Obstacle prevTrack = null;
                previousID.TryGetValue(cluster.id, out prevTrack);

                goodPoints.Clear();

                int numOccupancyDeleted = 0;
                foreach (Coordinates pt in cluster.relativePoints) {
                    if (!vehicleBox.IsInside(pt)) {
                        if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
                            occupancyDeletedCount++;
                            numOccupancyDeleted++;
                        }
                        else {
                            goodPoints.Add(pt);
                        }
                    }
                }

                if (goodPoints.Count < 3) {
                    continue;
                }

                IList<Polygon> polys;
                if (obs.occuluded && numOccupancyDeleted > 0) {
                    polys = WrapAndSplit(goodPoints, 1, 2.5);
                }
                else {
                    polys = new Polygon[] { Polygon.GrahamScan(goodPoints) };
                }

                obs.absoluteHeadingValid = cluster.headingValid;
                obs.absoluteHeading = cluster.absoluteHeading;

                // set the obstacle polygon for calculate obstacle distance
                Polygon obsPoly = Polygon.GrahamScan(goodPoints);
                double targetDistance = GetObstacleDistance(obsPoly);

                ObstacleClass impliedClass = ObstacleClass.DynamicUnknown;
                switch (cluster.targetClass) {
                    case SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE:
                        if (cluster.isStopped) {
                            impliedClass = ObstacleClass.DynamicStopped;
                        }
                        else {
                            impliedClass = ObstacleClass.DynamicCarlike;
                        }
                        break;

                    case SceneEstimatorTargetClass.TARGET_CLASS_NOTCARLIKE:
                        impliedClass = ObstacleClass.DynamicNotCarlike;
                        break;

                    case SceneEstimatorTargetClass.TARGET_CLASS_UNKNOWN:
                        impliedClass = ObstacleClass.DynamicUnknown;
                        break;
                }

                if (prevTrack == null) {
                    obs.age = 1;
                    // we haven't seen this track before, determine what the implied class is
                    if (targetDistance < target_class_ignore_dist) {
                        impliedClass = ObstacleClass.DynamicUnknown;
                    }
                }
                else {
                    obs.age = prevTrack.age+1;
                    // if we've seen this target before and we've labelled it as unknown and it is labelled as car-like now, check the distance
                    if (prevTrack.obstacleClass == ObstacleClass.DynamicUnknown && targetDistance < target_class_ignore_dist && obs.age < target_class_ignore_age) {
                        impliedClass = ObstacleClass.DynamicUnknown;
                    }
                }

                // get the off-road percentage
                double offRoadPercent = GetPercentOffRoad(obs.obstaclePolygon);

                if (offRoadPercent > 0.65) {
                    obs.offroadAge = obs.age;
                }

                // now check if we're labelling the obstacle as car-like if it has been off-road in the last second
                if ((impliedClass == ObstacleClass.DynamicCarlike || impliedClass == ObstacleClass.DynamicStopped) && (obs.age - obs.offroadAge) > 10 && obs.offroadAge > 0) {
                    // label as not car like
                    impliedClass = ObstacleClass.DynamicNotCarlike;
                }

                obs.obstacleClass = impliedClass;

                foreach (Polygon poly in polys) {
                    Obstacle newObs = obs.ShallowClone();

                    newObs.obstaclePolygon = poly;

                    // determine what to do with the cluster
                    if (cluster.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE && !cluster.isStopped) {
                        // if the heading is valid, extrude the car polygon and predict forward
                        if (cluster.headingValid) {
                            newObs.extrudedPolygon = ExtrudeCarPolygon(newObs.obstaclePolygon, cluster.relativeheading);
                        }
                    }

                    try {
                        newObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, newObs.AvoidancePolygon);
                    }
                    catch (Exception) {
                    }

                    obstacles.Add(newObs);
                }
            }

            return obstacles;
        }
        private List<Obstacle> ProcessUntrackedClusters(SceneEstimatorUntrackedClusterCollection clusters, List<Obstacle> trackedObstacles, Rect vehicleBox)
        {
            List<Obstacle> obstacles = new List<Obstacle>();

            SortedList<Obstacle, List<Coordinates>> point_splits = new SortedList<Obstacle, List<Coordinates>>();
            List<Coordinates> unclaimed_points = new List<Coordinates>(1500);

            foreach (SceneEstimatorUntrackedCluster cluster in clusters.clusters) {
                // clear out stored variables
                point_splits.Clear();
                unclaimed_points.Clear();

                // now determine if the point belongs to an old obstacle
                ObstacleClass targetClass;
                if (cluster.clusterClass == SceneEstimatorClusterClass.SCENE_EST_HighObstacle)
                    targetClass = ObstacleClass.StaticLarge;
                else
                    targetClass = ObstacleClass.StaticSmall;

                // only add points that are not within a tracked obstacle's extruded polygon
                for (int i = 0; i < cluster.points.Length; i++) {
                    Coordinates pt = cluster.points[i];
                    // iterate over all tracked cluster
                    bool did_hit = false;
                    if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
                        occupancyDeletedCount++;
                        did_hit = true;
                    }
                    else if (vehicleBox.IsInside(pt)) {
                        did_hit = true;
                    }
                    else if (trackedObstacles != null) {
                        foreach (Obstacle trackedObs in trackedObstacles) {
                            if (trackedObs.extrudedPolygon != null && trackedObs.extrudedPolygon.BoundingCircle.IsInside(pt) && trackedObs.extrudedPolygon.IsInside(pt)) {
                                did_hit = true;
                                break;
                            }
                        }
                    }

                    // if there was a hit, skip this point
                    if (!did_hit) {
                        unclaimed_points.Add(pt);
                    }
                    //if (did_hit)
                    //  continue;

                    //Obstacle oldObstacle = FindIntersectingCluster(pt, targetClass, previousObstacles);

                    //if (oldObstacle != null) {
                    //  List<Coordinates> obstacle_points;
                    //  if (!point_splits.TryGetValue(oldObstacle, out obstacle_points)) {
                    //    obstacle_points = new List<Coordinates>(100);
                    //    point_splits.Add(oldObstacle, obstacle_points);
                    //  }

                    //  obstacle_points.Add(pt);
                    //}
                    //else {
                    //  unclaimed_points.Add(pt);
                    //}
                }

                // we've split up all the points appropriately
                // now construct the obstacles

                // we'll start with the obstacle belonging to an existing polygon
                //foreach (KeyValuePair<Obstacle, List<Coordinates>> split in point_splits) {
                //  if (split.Value != null && split.Value.Count >= 3) {
                //    // the obstacle will inherit most of the properties of the old obstacle
                //    Obstacle obs = new Obstacle();
                //    obs.age = split.Key.age+1;
                //    obs.obstacleClass = split.Key.obstacleClass;

                //    // don't bother doing a split operation on these clusters -- they have already been split
                //    obs.obstaclePolygon = Polygon.GrahamScan(split.Value);

                //    obstacles.Add(obs);
                //  }
                //}

                // handle the unclaimed points
                IList<Polygon> polygons = WrapAndSplit(unclaimed_points, split_area_threshold, split_length_threshold);

                foreach (Polygon poly in polygons) {
                    // create a new obstacle
                    Obstacle obs = new Obstacle();
                    obs.age = 1;
                    obs.obstacleClass = targetClass;

                    obs.obstaclePolygon = poly;

                    obstacles.Add(obs);
                }

            }

            // test all old obstacles and see if they intersect any new obstacles
            // project the previous static obstacles to the current time frame

            if (processedObstacles != null) {
                try {
                    // get the relative transform
                    List<Obstacle> carryOvers = new List<Obstacle>();
                    Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
                    Polygon mergePolygon = mergeCircle.ToPolygon(24);

                    RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, clusters.timestamp);
                    foreach (Obstacle prevObs in processedObstacles.obstacles) {
                        if (prevObs.obstacleClass == ObstacleClass.StaticLarge || prevObs.obstacleClass == ObstacleClass.StaticSmall) {
                            prevObs.obstaclePolygon = prevObs.obstaclePolygon.Transform(transform);
                            prevObs.age++;

                            if (prevObs.age < 20) {
                                Coordinates centroid = prevObs.obstaclePolygon.GetCentroid();
                                double dist = GetObstacleDistance(prevObs.obstaclePolygon);
                                double angle = centroid.ArcTan;
                                if (dist < 30 && dist > 6 && Math.Abs(centroid.Y) < 15 && Math.Abs(angle) < Math.PI/2.0) {
                                    try {
                                        prevObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, prevObs.obstaclePolygon);
                                        if (!TestIntersection(prevObs.mergePolygon, obstacles)) {
                                            bool dropObstacle = false;
                                            for (int i = 0; i < prevObs.obstaclePolygon.Count; i++) {
                                                Coordinates pt = prevObs.obstaclePolygon[i];
                                                // iterate over all tracked cluster

                                                if (vehicleBox.IsInside(pt)) {
                                                    dropObstacle = true;
                                                }
                                                else if (useOccupancyGrid && externalUseOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
                                                  dropObstacle = true;
                                                }
                                                else if (trackedObstacles != null) {
                                                    foreach (Obstacle trackedObs in trackedObstacles) {
                                                        if (trackedObs.obstacleClass == ObstacleClass.DynamicCarlike) {
                                                            Polygon testPoly = trackedObs.extrudedPolygon ?? trackedObs.mergePolygon;

                                                            if (testPoly != null && testPoly.BoundingCircle.IsInside(pt) && testPoly.IsInside(pt)) {
                                                                dropObstacle = true;
                                                                break;
                                                            }
                                                        }
                                                    }
                                                }

                                                if (dropObstacle) {
                                                    break;
                                                }
                                            }

                                            if (!dropObstacle) {
                                                carryOvers.Add(prevObs);
                                            }
                                        }
                                    }
                                    catch (Exception) {
                                    }
                                }
                            }
                        }
                    }

                    obstacles.AddRange(carryOvers);
                }
                catch (Exception) {
                }
            }

            // create the merge polygon for all these duder
            /*Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
            Polygon mergePolygon = mergeCircle.ToPolygon(24);

            foreach (Obstacle obs in obstacles) {
                obs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, obs.obstaclePolygon);
            }*/

            return obstacles;
        }
        private bool DetermineReverseRecommended(IList<Obstacle> obstacles)
        {
            if (obstacles == null)
                return false;

            // determine if there are obstacles in front of us
            Rect frontRect = new Rect(0, -(TahoeParams.T + 1)/2, TahoeParams.VL + 4, TahoeParams.T + 1);

            foreach (Obstacle obs in obstacles) {
                // iterate through each point
                foreach (Coordinates pt in obs.AvoidancePolygon) {
                    if (frontRect.IsInside(pt)) {
                        return true;
                    }
                }
            }

            if (IsOffRoad())
                return true;

            return false;
        }