public static SimWaypoint CreateLocal(Vector3 from, CollisionPlane CP)
        {
            SimPathStore   PathStore        = CP.PathStore;
            float          POINTS_PER_METER = PathStore.POINTS_PER_METER;
            int            PX = PathStore.ARRAY_X(from.X);
            int            PY = PathStore.ARRAY_Y(from.Y);
            SimWaypoint    WP;
            CollisionIndex CI = CollisionIndex.CreateCollisionIndex(from, PathStore);

            lock (CI)
            {
                WP = CI.FindWayPoint(from.Z);
                if (WP != null)
                {
                    return(WP);
                }
                from.X = PX / POINTS_PER_METER;
                from.Y = PY / POINTS_PER_METER;
                Vector3d GlobalPos = PathStore.LocalToGlobal(from);
                if (GlobalPos.X < 256 || GlobalPos.Y < 256)
                {
                    CollisionPlane.Debug("bad global " + GlobalPos);
                }
                WP            = new SimWaypointImpl(from, GlobalPos, CI, CP, PathStore);
                WP.IsPassable = true;
            }
            // wp._IncomingArcs = new ArrayList();
            // wp._OutgoingArcs = new ArrayList();
            //  PathStore.EnsureKnown(wp);
            return(WP);
        }
        public static CollisionIndex CreateCollisionIndex(Vector3 from, SimPathStore PathStore)
        {
            float          POINTS_PER_METER = PathStore.POINTS_PER_METER;
            int            PX = PathStore.ARRAY_X(from.X);
            int            PY = PathStore.ARRAY_Y(from.Y);
            CollisionIndex WP;

            lock (PathStore.MeshIndex)
            {
                WP = PathStore.MeshIndex[PX, PY];
                if (WP != null)
                {
                    return(WP);
                }
                from.X = PX / POINTS_PER_METER;
                from.Y = PY / POINTS_PER_METER;
                Vector3d GlobalPos = PathStore.GetPathStore().LocalToGlobal(from);
                WP = new CollisionIndex(from, GlobalPos, PX, PY, PathStore);
            }
            return(WP);
        }
        internal void Refresh(Box3Fill changed, float BumpConstraint)
        {
            changed.Constrain(OuterBox);
            byte[,] ToMatrix            = ByteMatrix;
            float[,] Heights            = HeightMap;
            float[,] GroundPlane        = PathStore.GroundPlane;
            CollisionIndex[,] MeshIndex = PathStore.MeshIndex;

            int xs = PathStore.ARRAY_X(changed.MinX);

            if (xs < 0)
            {
                xs = 0;
            }
            int xe   = PathStore.ARRAY_X(changed.MaxX);
            int xend = MaxXPt - 1;

            if (xe > xend)
            {
                xe = xend;
            }
            int ys = PathStore.ARRAY_Y(changed.MinY);

            if (ys < 0)
            {
                ys = 0;
            }
            int ye   = PathStore.ARRAY_Y(changed.MaxY);
            int yend = MaxYPt - 1;

            if (ye > yend)
            {
                ye = yend;
            }

            for (int x = xs; x <= xe; x++)
            {
                for (int y = ys; y <= ye; y++)
                {
                    Heights[x, y] = DefaultHeight(x, y, GroundPlane, Heights, MeshIndex);
                }
            }

            if (xs < 1)
            {
                xs = 1;
            }
            xend = MaxXPt - 2;
            if (xe > xend)
            {
                xe = xend;
            }
            if (ys < 1)
            {
                ys = 1;
            }
            yend = MaxYPt - 2;
            if (ye > yend)
            {
                ye = yend;
            }

            for (int x = xs; x <= xe; x++)
            {
                for (int y = ys; y <= ye; y++)
                {
                    ToMatrix[x, y] = DefaultCollisionValue(x, y, BumpConstraint, GroundPlane, ToMatrix[x, y],
                                                           Heights, MeshIndex);
                }
            }
        }