using System;

using System.Numerics;          

using System.Collections.Generic;

​

/*

​

TO DO:

1. Find general minimized path

2. Implement implement M. P. calculation from values

3. Create interface for path and obstacle setup

4. Automatic field converter to potential formula

5. Dynamic obstacles (NOTE: requires moving TO path in case of large adjustment, unless starting point is taken to be current location [unlikely] )

6. Port to Java or java communacable language

7. Conditional objects

8. 

9. Optimize (In java)

10. GUI

​

*/

​

public class Program

{

    public static void Main()

    {

        List<Vector2> sampleSpikeObs = new List<Vector2> {

            new Vector2(-3, 3),

            new Vector2(3, -3)

        };

        List<RTap.WallObstacle> sampleWallObs = new List<RTap.WallObstacle>() {

            new RTap.WallObstacle(new Vector2(0, 5), new Vector2(0, 1)),

        };

        RTap r = new RTap(sampleSpikeObs, sampleWallObs);

        BezierCurve b = new BezierCurve(new List<Vector2> {

            new Vector2(-10, -3), 

            new Vector2(-3, 1), 

            new Vector2(-6, -8)

        });

        r.Path = b.BezFunc;

        r.DerivativePath = b.DervBezFunc;

        Console.WriteLine(b.DervBezFunc(0));

        Console.WriteLine(b.DervBezFunc(0.5f));

        Console.WriteLine(b.DervBezFunc(1));

        Console.WriteLine("PENALTY: " + r.Penalty());

    }

}

​

public class RTap {

    public List<SpikeObstacle> spikeObstacles = new List<SpikeObstacle>();

    private const float spikeConst = 50;

    public bool includingWalls;

    private const int wallCount = 4;

    public List<WallObstacle> wallObstacles = new List<WallObstacle>();

    public Func<float, Vector2> Path;

    public Func<float, Vector2> DerivativePath;

    private const float potentialPenaltyWeight = 1; //default 1 (for some reason)

    private const float motionPenaltyWeight = 2; //default 2 (for some reason)

    public RTap(List<Vector2> sO, List<WallObstacle> wO, bool iW = true) {

        foreach (Vector2 v in sO) {

            spikeObstacles.Add(new SpikeObstacle(v));

        }

        wallObstacles = wO;

        includingWalls = iW;

    }

    public RTap(List<SpikeObstacle> sO, List<WallObstacle> wO, bool iW = true) {

        spikeObstacles = sO;

        wallObstacles = wO;

        includingWalls = iW;

    }

    public float Potential(Vector2 pos) {

        float sum = 0;

        foreach (SpikeObstacle s in spikeObstacles) {

            sum += spikeConst * MathF.Exp( -MathF.Pow((pos.X - s.Position.X), 2) - MathF.Pow((pos.Y - s.Position.Y), 2));

        }

        if (includingWalls) {

            foreach (WallObstacle w in wallObstacles) {

                //Console.WriteLine(pos);

                //Console.WriteLine(MathF.Exp(5 * Vector2.Dot(w.Direction, pos - w.Offset)));

                //sum += MathF.Exp( 5 * Mathf.Abs(w.Direction.X)*((pos.X - w.Offset.X) +  w.Direction.Y*(pos.Y - w.Offset.Y)) ); //either the x's or the y's will be zero. Never both, never neither

                sum += MathF.Exp(5 * Vector2.Dot(w.Direction, pos - w.Offset));

            }

        }

        return sum;

    }

    public float Penalty() {

        Func<float, float> integrandFromPotential = (float t) => { return potentialPenaltyWeight * Potential( Path(t) ); };

        Func<float, float> integrandFromMotion = (float t) => { Vector2 tempPos = DerivativePath(t); return motionPenaltyWeight*MathF.Sqrt( MathF.Pow(tempPos.X, 2) + MathF.Pow(tempPos.Y, 2) ); }; //TODO: figure out why I gotta multiply by 2

        float penaltyFromPotential = AuxiliaryMath.BruteDefIntegral(integrandFromPotential, 0, 1);

        float penaltyFromMotion = AuxiliaryMath.BruteDefIntegral(integrandFromMotion, 0, 1);

        //Console.WriteLine("MOTION PENALTY " + penaltyFromMotion + ", POTENTIAL PENALTY " + penaltyFromPotential);

        return penaltyFromPotential + penaltyFromMotion;

    }

    //class for the spike obstacle

    public class SpikeObstacle {

        public Vector2 Position;

        public float size;

        public SpikeObstacle() {

​

        }

        public SpikeObstacle (Vector2 pos, float s = 50) {

            Position = pos;

            size = s;

        }

        public float ContributionPotential(Vector2 loc) {

            return size * MathF.Exp( -MathF.Pow((loc.X - Position.X), 2) - MathF.Pow((loc.Y - Position.Y), 2));

        }

    }

    //class for the wall obstacle

    public class WallObstacle {

        //both of these are used to make the math more convenient. thus, either both x's will be zero or both y's (never both and never neither)

        public Vector2 Offset;

        public Vector2 Direction; //Facing TOWARDS the f(x)-->inf part

        public WallObstacle() {

        }

        public WallObstacle(Vector2 o, Vector2 d) {

            Offset = o;

            Direction = Vector2.Normalize(d);

        }

        public WallObstacle(float o, string d) {

            string dir = d.Substring(0, 1).ToLower();

            float off = Math.Abs(o);

            switch (dir) {

                case "n":

                    Direction = new Vector2(0, 1);

                    break;

                case "e":

                    Direction = new Vector2(1, 0);

                    break;

                case "s":

                    Direction = new Vector2(0, -1);

                    break;

                case "w":

                    Direction = new Vector2(-1, 0);

                    break;

                default:

                    break;

            }

            Offset = off * Direction;

        }

    }

}

​

public static class AuxiliaryMath {

    private const float d = 0.0001f;

    public static float BruteDefIntegral(Func<float, float> expression, float min, float max) {

        float sum = 0;

        //float r;

        for (float i = min; i <= max; i += d) {

            //r = expression(i);

            //if (Math.Abs(i - 0.45) < 0.01) {Console.WriteLine("----------------");}

            sum += expression(i) * d;

            //Console.WriteLine(r);

        }

        return sum;

    }

    public static int Fact(int n) {

        int ret = n;

        if (n == 0) {

            return 1;

        }

        for (int i = n - 1; i > 1; i--) {

            ret *= i;

        }

        return ret;

    }

}

​

public class BezierCurve {

    public int degree;

    public List<Vector2> points;

    public Func<float, Vector2> parametric;

    public BezierCurve(List<Vector2> p) {

        degree = p.Count;

        points = p;

        parametric = (float t) => { return BezFunc(t); };

    }

    private float BezFactor(int n, int k) {

        //Console.WriteLine("#############"+ AuxiliaryMath.Fact(n) + " | " + AuxiliaryMath.Fact(k) + " | " + AuxiliaryMath.Fact(n - k));

        return (float)AuxiliaryMath.Fact(n) / (float)( AuxiliaryMath.Fact(k) * AuxiliaryMath.Fact(n - k) );

    }

    private float BezComp(float t, int K, float v, int Nrep = -1) {

        int N;

        if (Nrep != -1) {

            N = Nrep;

        } else {

            N = degree - 1;

        }

        //Console.WriteLine("--------------" + BezFactor(N, K) + " | " +  MathF.Pow((1 - t), (N - K)) + " | " +  MathF.Pow(t, K) + " | " + v);

        return BezFactor(N, K) * MathF.Pow((1 - t), (N - K)) * MathF.Pow(t, K) * v;

    }

    public Vector2 BezFunc(float t) {

        //Console.WriteLine("\nin BezFunc------------------------------");

        Vector2 sum = new Vector2(0, 0);

        for (int i = 0; i < degree; i++) {

            sum += new Vector2( BezComp(t, i, points[i].X), BezComp(t, i, points[i].Y) );

            //Console.WriteLine(new Vector2( BezComp(t, i, points[i].X), BezComp(t, i, points[i].Y) ));

        }

        return sum;

    }

    public Vector2 DervBezFunc(float t) {

        Vector2 sum = new Vector2(0, 0);

        for (int i = 0; i < degree - 1; i++) {

            sum += new Vector2( 

                BezComp(t, i, degree * (points[i + 1].X - points[i].X), degree - 2),  

                BezComp(t, i, degree * (points[i + 1].Y - points[i].Y), degree - 2)

            );

        }

        sum /= degree;

        return sum;

    }

}

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​

​