CanonBallCalculator2D.cs

using REST_JSON_API;
using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class CanonBallProblemCalculator2D
{
    private int Dim_const, Dim_A, Dim_B;
    private int MaxIter;

    private float Bounce;

    private FactRecorder FactOrganizer;

    private Vector3 StartPos;
    private Vector3 StartVel;
    private Vector3 Gravity;

    private List<LineFact> Walls;
    public string Result_MovementFunc_Id;
    public List<string> Result_ArgsFunc_Id;
    public List<string> Result_FuncCall_Id;

    public CanonBallProblemCalculator2D(List<LineFact> walls, Vector3 starPos, Vector3 starVec, Vector3 gravity, float bounce, int dim_const, int dim_A, int dim_B, FactRecorder factOrganizer, int maxIter = 32)
    {
        Walls = walls;
        Dim_const = dim_const;
        Dim_A = dim_A;
        Dim_B = dim_B;
        MaxIter = maxIter;
        StartPos = starPos;
        StartVel = starVec;
        Gravity = gravity;
        FactOrganizer = factOrganizer;
        this.Bounce = bounce;

        Compute();
    }

    private void Compute()
    {
        SOMDoc BuildOMA_XVA()
        {   //Pos = Pos + Vel * t + 0.5 * g * t**2
            return new FUN(new FUN.Param[]
                {
                    new("Pos", new OMS(MMTConstants.TYPE_TO_OMS[typeof(Vector3)])),
                    new("Vel", new OMS(MMTConstants.TYPE_TO_OMS[typeof(Vector3)])),
                    new("Acc", new OMS(MMTConstants.TYPE_TO_OMS[typeof(Vector3)])),
                    new("t", new OMS(MMTConstants.TYPE_TO_OMS[typeof(float)])),
                },
                new OMA(
                    new OMS(MMTConstants.AddRealLit),
                    new SOMDoc[] {
                        new OMV("Pos"),
                        new OMA(
                            new OMS(MMTConstants.AddRealLit),
                            new[] {
                                new OMA(
                                    new OMS(MMTConstants.TimesRealLit),
                                    new[] {
                                        new OMV("Vel"),
                                        new OMV("t"),
                                    }
                                ),
                                new OMA(
                                    new OMS(MMTConstants.TimesRealLit),
                                    new SOMDoc[] {
                                        new OMLIT<float>(0.5f),
                                        new OMA(
                                            new OMS(MMTConstants.TimesRealLit),
                                            new SOMDoc[] {
                                                new OMV("Acc"),
                                                new OMA(
                                                    new OMS(MMTConstants.TimesRealLit),
                                                    new[] {
                                                        new OMV("t"),
                                                        new OMV("t"),
            }),}),}),}),}));
        }

        SOMDoc BuildOMAPath(Vector3 Pos, Vector3 Vel, float last_t)
        {   //t -> [Pos, Vel, g, t]
            return new FUN(new FUN.Param[]
                {
                    new("t", new OMS(MMTConstants.TYPE_TO_OMS[typeof(float)])),
                },
                new OMA(
                    new OMS(MMTConstants.MakeObjectArray),
                    new SOMDoc[] {
                        SOMDoc.MakeVector3(Pos),
                        SOMDoc.MakeVector3(Vel),
                        SOMDoc.MakeVector3(Gravity),
                        new OMA(
                            new OMS(MMTConstants.AddRealLit),
                            new SOMDoc[]{
                                new OMV("t"),
                                new OMA(
                                    new OMS(MMTConstants.MinusRealLit),
                                    new[]{ new OMLIT<float>(last_t) }
            )})}));
        }

        //Pos = Pos + Vel * t + 0.5 * g * t**2
        Vector3 UpdatePos(Vector3 Pos, Vector3 Vel, float t)
            => Pos + Vel * t + 0.5f * t * t * Gravity;

        //Vel = Vel + g * t
        Vector3 UpdateVel(Vector3 Vel, float t)
            => Vel + Gravity * t;

        // p1 + v1 * t + 0.5 * g1 * t * t = o1 + d1 * x,
        // p2 + v2 * t + 0.5 * g2 * t * t = o2 + d2 * x,

        // T = (-b  sqrt(b ^ 2 - 4ac)) / (2a)
        //    Where:
        //        a = 0.5 * (G1 * D2 - G2 * D1)
        //        b = V1 * D2 - V2 * D1
        //        c = - P2 * D1 + P1 * D2 + O2 * D1 - O1 * D2
        (float, float) SolveForTime(Vector3 Pos, Vector3 Vel, LineFact Top)
        {
            double
                a = 0.5 * (Gravity[Dim_A] * Top.Dir[Dim_B] - Gravity[Dim_B] * Top.Dir[Dim_A]),
                b = Vel[Dim_A] * Top.Dir[Dim_B] - Vel[Dim_B] * Top.Dir[Dim_A],
                c = -Pos[Dim_B] * Top.Dir[Dim_A] + Pos[Dim_A] * Top.Dir[Dim_B]
                    + Top.Point1.Point[Dim_B] * Top.Dir[Dim_A] - Top.Point1.Point[Dim_A] * Top.Dir[Dim_B];

            if (Math.Abs(a) < Math3d.vectorPrecission)
                return ((float)(-c / b), -1);

            float
                t1 = (float)((-b + Math.Sqrt(b * b - 4 * a * c)) / (2 * a)),
                t2 = (float)((-b - Math.Sqrt(b * b - 4 * a * c)) / (2 * a));

            return (t1, t2);
        };

        // X = (P1 + V1 * T + 0.5 * G1 * T ^ 2 - O1) / D1
        float SolveForDistanceOnPlane(Vector3 Pos, Vector3 Vel, LineFact Top, float t)
        {
            Vector3 dir = Top.Point1.Point - UpdatePos(Pos, Vel, t);

            float dot = Vector3.Dot(dir, Top.Dir);
            if (float.IsNaN(dot))
                return -1;

            return dir.magnitude * Math.Sign(dot);
        };

        var Result_Movement = new FunctionFact(BuildOMA_XVA());
        Result_MovementFunc_Id = FactOrganizer.Add(Result_Movement, out _, false, null, null);

        Vector3 PlaneNorm = Vector3.zero;
        PlaneNorm[Dim_const] = 1;

        Vector3 pos = StartPos;
        Vector3 vel = StartVel;

        Result_ArgsFunc_Id = new();
        Result_FuncCall_Id = new();

        float last_t = 0;
        for (int i = 0; i < MaxIter; i++)
        {
            pos = UpdatePos(pos, vel, (float)Math3d.vectorPrecission); //minimum step

            var hits = Walls
                .Select(w => (w, tarr: SolveForTime(pos, vel, w)))
                .SelectMany(wt => new[] { (wt.w, t: wt.tarr.Item1), (wt.w, t: wt.tarr.Item2) })
                .Where(wt => wt.t >= 0)
                .Select(wt => (wt.w, wt.t, x: SolveForDistanceOnPlane(pos, vel, wt.w, wt.t)))
                .Where(wt => wt.x >= 0 && wt.x <= wt.w.Distance)
                .OrderBy(wt => wt.t)
                .ToArray();

            (LineFact next_Wall, float next_t, float x_fac_debug) = hits.Length > 0
                ? hits[0]
                : (default(LineFact), float.PositiveInfinity, default);

            Result_ArgsFunc_Id.Add(
                FactOrganizer.Add(
                    new FunctionFact(BuildOMAPath(pos, vel, last_t - (float)Math3d.vectorPrecission)),
                    out _, true, null, null));

            Result_FuncCall_Id.Add(
                FactOrganizer.Add(
                    new FunctionCallFact(Result_MovementFunc_Id, Result_ArgsFunc_Id.Last(), (-(float)Math3d.vectorPrecission + last_t, next_t + last_t)),
                    out _, true, null, null));

            if (hits.Length == 0)
                break;

            pos = UpdatePos(pos, vel, next_t);
            Vector3 vel_at_bounce = UpdateVel(vel, next_t);
            Vector3 LineFactNorm = Vector3.Cross(PlaneNorm, next_Wall.Dir);

            vel = Bounce * Vector3.Reflect(vel_at_bounce, LineFactNorm);
            last_t += next_t;
        }

        return;
    }
}