VectorCSPF.cs

Go to the documentation of this file.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Wolfram.NETLink;

namespace PolyLink
{
    struct LeaderBoardEntry
    {
        public VectorCone Cone { get; set; }
        public Vector Vector { get; set; }
    }

    public class VectorCSPF : MathLinked
    {
        public Polyhedron Poly { get; private set; }
        public HalfEdge StartHE { get; private set; }
        public PointRef StartPoint { get; private set; }
        public Facet EndFacet { get; private set; }
        public PointRef EndPoint { get; private set; }

        public Expr StartGraphics { get; set; }

        private readonly Dictionary<PointRef, LeaderBoardEntry> _leaderBoard
            = new Dictionary<PointRef, LeaderBoardEntry>();

        public VectorCSPF(Polyhedron poly, HalfEdge startHe, Facet endFacet, PointRef start, PointRef end)
        {
            Poly = poly;
            StartHE = startHe;
            EndFacet = endFacet;
            EndPoint = end;
            StartPoint = start;
        }

        public bool SeeIfConeHasShortestPath(VectorCone subject, Vector vector, PointRef vert)
        {
            var entry = _leaderBoard[vert];
            if (vector.Length.CompareTo(entry.Vector.Length) < 0)
            {
                if (entry.Cone != null)
                {
                    entry.Cone.Invalidate();
                }
                _leaderBoard[vert] = new LeaderBoardEntry{Cone = subject, Vector = vector};
                return true;
            }
            return false;
        }

        public Expr ComputeShortestPath()
        {
            var infinity = new Vector("{Infinity,Infinity,Infinity}".MsEvalWith());
            foreach (var point in Poly.HalfEdges.Select(he => he.Start))
            {
                _leaderBoard[point] = new LeaderBoardEntry
                    {
                        Cone = null,
                        Vector = infinity,
                    };
            }
            _leaderBoard[EndPoint] = new LeaderBoardEntry
                {
                    Cone = null,
                    Vector = infinity,
                };
            var startHEClone = StartHE.FacetCloneWithOriginalOpposites;
            var startPointClone = StartPoint.DeepClone;
            startHEClone.Anchor(new[] {startPointClone});
            var transVector = new Vector(ML["-{0}"].Format(startPointClone.Expr).Eval());
            startHEClone.NegativeTranslate(transVector);

            StartGraphics = ML["List"].Bracket(
                    startHEClone.ForceNewFacet.Graphics,
                    ML["Point"].Bracket(PointRef.Origin).Eval()
                ).Eval();

            var queue = new Queue<VectorCone>();

            var hePairs = StartHE.HEsFromHere
                .NetZip(startHEClone.HEsFromHere, (o, c) => new WorkingCopy<HalfEdge>(o, c));

            foreach (var pair in hePairs)
            {
                var vectToStart = Vector.FromPoints(PointRef.Origin, pair.Copy.Start.Expr);
                _leaderBoard[pair.Original.Start] = new LeaderBoardEntry
                    {
                        Cone = null,
                        Vector = vectToStart,
                    };
                var vectToEnd = Vector.FromPoints(PointRef.Origin, pair.Copy.End.Expr);

                //TODO Find some way to start cones at neighbor facets for these cases
                if ((bool) ML["{0} == {1}"].Format(vectToStart.Expr, "{0,0,0}").EvalObject())
                {
                    continue;
                }
                if ((bool) ML["{0} == {1}"].Format(vectToEnd.Expr, "{0,0,0}").EvalObject())
                {
                    continue;
                }


                IEnumerable<PointRef> newEndPoint = null;
                var newHE = new WorkingCopy<HalfEdge>(pair.Original.Opposite,
                                                             pair.Original.Opposite.FacetCloneWithOriginalOpposites);
                if (newHE.Original.Facet == EndFacet)
                {
                    newEndPoint = new[] {EndPoint.DeepClone};
                }
                newHE.Copy.Opposite = pair.Copy;
                newHE.Copy.Unfold(newEndPoint);
                newHE.Copy.Opposite = pair.Original;
                PointRef endPoint = (newEndPoint != null) ? newEndPoint.First() : null;

                queue.Enqueue(
                    new VectorCone(
                        parent: null,
                        currentEdge: newHE,
                        cspf: this,
                        lBind: vectToStart,
                        rBind: vectToEnd,
                        transformedEndPoint: endPoint
                        )
                    );
            }
            //Put the loop here!!
            long loopNumber = 0;
            while (queue.Count > 0)
            {
                loopNumber++;
                if (loopNumber == 10)
                {
                    int x = 0;
                }

                var cone = queue.Dequeue();

                if ((bool) ML["{0}[[3]] < -.0001 || {0}[[3]] > .0001"].Format(cone.CurrentEdge.Copy.Start.Expr).EvalObject())
                {
                    throw new Exception("Relative source is not on plane");
                }

                if (!cone.Valid)
                    continue;
                var splitCones = cone.TryExtend();
                if (!cone.Valid)
                    continue;
                if (splitCones.Left != null)
                {
                    if ((bool)ML["{0} < {1}"]
                       .Format(cone.Aperture, splitCones.Left.Aperture)
                       .EvalObject())
                            {
                                Console.WriteLine("Child larger than parent");
                            }
                    queue.Enqueue(splitCones.Left);
                }
                if (splitCones.Right != null)
                {
                    if ((bool)ML["{0} < {1}"]
                       .Format(cone.Aperture, splitCones.Right.Aperture)
                       .EvalObject())
                    {
                        Console.WriteLine("Child larger than parent");
                    }
                    queue.Enqueue(splitCones.Right);
                }
            }
            return _leaderBoard[EndPoint].Vector.Expr;
        }

 
    }
}