csharp/ACEmulator/ACViewer/ACViewer/Physics/Trajectory.cs

Trajectory.cs

// LICENSE
//
//   This software is dual-licensed to the public domain and under the following
//   license: you are granted a perpetual, irrevocable license to copy, modify,
//   publish, and distribute this file as you see fit.
//
// VERSION 
//   0.1.0  (2016-06-01)  Initial release
//
// AUTHOR
//   Forrest Smith
//
// ADDITIONAL READING
//   https://blog.forrestthewoods.com/solving-ballistic-trajectories-b0165523348c
//
// API
//    int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, float gravity, out Vector3 low, out Vector3 high);
//    int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, Vector3 target_velocity, float gravity, out Vector3 s0, out Vector3 s1, out Vector3 s2, out Vector3 s3);
//    bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target, float max_height, out float vertical_speed, out float gravity);
//    bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target, Vector3 target_velocity, float max_height_offset, out Vector3 fire_velocity, out float gravity, out Vector3 impact_point);
//
//    float ballistic_range(float speed, float gravity, float initial_height);
//
//    bool IsZero(double d);
//    int SolveQuadric(double c0, double c1, double c2, out double s0, out double s1);
//    int SolveCubic(double c0, double c1, double c2, double c3, out double s0, out double s1, out double s2);
//    int SolveQuartic(double c0, double c1, double c2, double c3, double c4, out double s0, out double s1, out double s2, out double s3);


using ACE.Server.Physics.Extensions;

using System;
using System.Diagnostics;
using System.Numerics;

namespace ACE.Server.Physics
{
    public clast Trajectory
    {

        // SolveQuadric, SolveCubic, and SolveQuartic were ported from C as written for Graphics Gems I
        // Original Author: Jochen Schwarze ([email protected])
        // https://github.com/erich666/GraphicsGems/blob/240a34f2ad3fa577ef57be74920db6c4b00605e4/gems/Roots3And4.c

        // Utility function used by SolveQuadratic, SolveCubic, and SolveQuartic
        public static bool IsZero(double d)
        {
            const double eps = 1e-9;
            return d > -eps && d < eps;
        }

        // Solve quadratic equation: c0*x^2 + c1*x + c2. 
        // Returns number of solutions.
        public static int SolveQuadric(double c0, double c1, double c2, out double s0, out double s1)
        {
            s0 = double.NaN;
            s1 = double.NaN;

            double p, q, D;

            /* normal form: x^2 + px + q = 0 */
            p = c1 / (2 * c0);
            q = c2 / c0;

            D = p * p - q;

            if (IsZero(D))
            {
                s0 = -p;
                return 1;
            }
            else if (D < 0)
            {
                return 0;
            }
            else /* if (D > 0) */
            {
                double sqrt_D = Math.Sqrt(D);

                s0 = sqrt_D - p;
                s1 = -sqrt_D - p;
                return 2;
            }
        }

        // Solve cubic equation: c0*x^3 + c1*x^2 + c2*x + c3. 
        // Returns number of solutions.
        public static int SolveCubic(double c0, double c1, double c2, double c3, out double s0, out double s1, out double s2)
        {
            s0 = double.NaN;
            s1 = double.NaN;
            s2 = double.NaN;

            int num;
            double sub;
            double A, B, C;
            double sq_A, p, q;
            double cb_p, D;

            /* normal form: x^3 + Ax^2 + Bx + C = 0 */
            A = c1 / c0;
            B = c2 / c0;
            C = c3 / c0;

            /*  subssatute x = y - A/3 to eliminate quadric term:  x^3 +px + q = 0 */
            sq_A = A * A;
            p = 1.0 / 3 * (-1.0 / 3 * sq_A + B);
            q = 1.0 / 2 * (2.0 / 27 * A * sq_A - 1.0 / 3 * A * B + C);

            /* use Cardano's formula */
            cb_p = p * p * p;
            D = q * q + cb_p;

            if (IsZero(D))
            {
                if (IsZero(q)) /* one triple solution */
                {
                    s0 = 0;
                    num = 1;
                }
                else /* one single and one double solution */
                {
                    double u = Math.Pow(-q, 1.0 / 3.0);
                    s0 = 2 * u;
                    s1 = -u;
                    num = 2;
                }
            }
            else if (D < 0) /* Casus irreducibilis: three real solutions */
            {
                double phi = 1.0 / 3 * Math.Acos(-q / Math.Sqrt(-cb_p));
                double t = 2 * Math.Sqrt(-p);

                s0 = t * Math.Cos(phi);
                s1 = -t * Math.Cos(phi + Math.PI / 3);
                s2 = -t * Math.Cos(phi - Math.PI / 3);
                num = 3;
            }
            else /* one real solution */
            {
                double sqrt_D = Math.Sqrt(D);
                double u = Math.Pow(sqrt_D - q, 1.0 / 3.0);
                double v = -Math.Pow(sqrt_D + q, 1.0 / 3.0);

                s0 = u + v;
                num = 1;
            }

            /* resubssatute */
            sub = 1.0 / 3 * A;

            if (num > 0) s0 -= sub;
            if (num > 1) s1 -= sub;
            if (num > 2) s2 -= sub;

            return num;
        }

        // Solve quartic function: c0*x^4 + c1*x^3 + c2*x^2 + c3*x + c4. 
        // Returns number of solutions.
        public static int SolveQuartic(double c0, double c1, double c2, double c3, double c4, out double s0, out double s1, out double s2, out double s3)
        {
            s0 = double.NaN;
            s1 = double.NaN;
            s2 = double.NaN;
            s3 = double.NaN;

            double[] coeffs = new double[4];
            double z, u, v, sub;
            double A, B, C, D;
            double sq_A, p, q, r;
            int num;

            /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
            A = c1 / c0;
            B = c2 / c0;
            C = c3 / c0;
            D = c4 / c0;

            /*  subssatute x = y - A/4 to eliminate cubic term: x^4 + px^2 + qx + r = 0 */
            sq_A = A * A;
            p = -3.0 / 8 * sq_A + B;
            q = 1.0 / 8 * sq_A * A - 1.0 / 2 * A * B + C;
            r = -3.0 / 256 * sq_A * sq_A + 1.0 / 16 * sq_A * B - 1.0 / 4 * A * C + D;

            if (IsZero(r))
            {
                /* no absolute term: y(y^3 + py + q) = 0 */

                coeffs[3] = q;
                coeffs[2] = p;
                coeffs[1] = 0;
                coeffs[0] = 1;

                num = SolveCubic(coeffs[0], coeffs[1], coeffs[2], coeffs[3], out s0, out s1, out s2);
            }
            else
            {
                /* solve the resolvent cubic ... */
                coeffs[3] = 1.0 / 2 * r * p - 1.0 / 8 * q * q;
                coeffs[2] = -r;
                coeffs[1] = -1.0 / 2 * p;
                coeffs[0] = 1;

                SolveCubic(coeffs[0], coeffs[1], coeffs[2], coeffs[3], out s0, out s1, out s2);

                /* ... and take the one real solution ... */
                z = s0;

                /* ... to build two quadric equations */
                u = z * z - r;
                v = 2 * z - p;

                if (IsZero(u))
                    u = 0;
                else if (u > 0)
                    u = Math.Sqrt(u);
                else
                    return 0;

                if (IsZero(v))
                    v = 0;
                else if (v > 0)
                    v = Math.Sqrt(v);
                else
                    return 0;

                coeffs[2] = z - u;
                coeffs[1] = q < 0 ? -v : v;
                coeffs[0] = 1;

                num = SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);

                coeffs[2] = z + u;
                coeffs[1] = q < 0 ? v : -v;
                coeffs[0] = 1;

                if (num == 0) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);
                if (num == 1) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s1, out s2);
                if (num == 2) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s2, out s3);
            }

            /* resubssatute */
            sub = 1.0 / 4 * A;

            if (num > 0) s0 -= sub;
            if (num > 1) s1 -= sub;
            if (num > 2) s2 -= sub;
            if (num > 3) s3 -= sub;

            return num;
        }

        // Calculate the maximum range that a ballistic projectile can be fired on given speed and gravity.
        //
        // speed (float): projectile velocity
        // gravity (float): force of gravity, positive is down
        // initial_height (float): distance above flat terrain
        //
        // return (float): maximum range
        public static float ballistic_range(float speed, float gravity, float initial_height)
        {
            // Handling these cases is up to your project's coding standards
            Debug.astert(speed > 0 && gravity > 0 && initial_height >= 0, "fts.ballistic_range called with invalid data");

            // Derivation
            //   (1) x = speed * time * cos O
            //   (2) z = initial_height + (speed * time * sin O) - (.5 * gravity*time*time)
            //   (3) via quadratic: t = (speed*sin O)/gravity + sqrt(speed*speed*sin O + 2*gravity*initial_height)/gravity    [ignore smaller root]
            //   (4) solution: range = x = (speed*cos O)/gravity * sqrt(speed*speed*sin O + 2*gravity*initial_height)    [plug t back into x=speed*time*cos O]
            var angle = 45 * 0.0174533; // no air resistence, so 45 degrees provides maximum range
            var cos = Math.Cos(angle);
            var sin = Math.Sin(angle);

            var range = (speed * cos / gravity) * (speed * sin + Math.Sqrt(speed * speed * sin * sin + 2 * gravity * initial_height));
            return (float)range;
        }

        // Solve firing angles for a ballistic projectile with speed and gravity to hit a fixed position.
        //
        // proj_pos (Vector3): point projectile will fire from
        // proj_speed (float): scalar speed of projectile
        // target (Vector3): point projectile is trying to hit
        // gravity (float): force of gravity, positive down
        //
        // s0 (out Vector3): firing solution (low angle) 
        // s1 (out Vector3): firing solution (high angle)
        //
        // return (int): number of unique solutions found: 0, 1, or 2.
        public static int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, float gravity, out Vector3 s0, out Vector3 s1, out float t0, out float t1)
        {

            // Handling these cases is up to your project's coding standards
            Debug.astert(proj_pos != target && proj_speed > 0 && gravity > 0, "fts.solve_ballistic_arc called with invalid data");

            // C# requires out variables be set
            s0 = Vector3.Zero;
            s1 = Vector3.Zero;
            t0 = float.PositiveInfinity;
            t1 = float.PositiveInfinity;

            // Derivation
            //   (1) x = v*t*cos O
            //   (2) z = v*t*sin O - .5*g*t^2
            // 
            //   (3) t = x/(cos O*v)                                        [solve t from (1)]
            //   (4) z = v*x*sin O/(cos O * v) - .5*g*x^2/(cos^2 O*v^2)     [plug t into z=...]
            //   (5) z = x*tan O - g*x^2/(2*v^2*cos^2 O)                    [reduce; cos/sin = tan]
            //   (6) z = x*tan O - (g*x^2/(2*v^2))*(1+tan^2 O)              [reduce; 1+tan O = 1/cos^2 O]
            //   (7) 0 = ((-g*x^2)/(2*v^2))*tan^2 O + x*tan O - (g*x^2)/(2*v^2) - z    [re-arrange]
            //   Quadratic! a*p^2 + b*p + c where p = tan O
            //
            //   (8) let gxv = -g*x*x/(2*v*v)
            //   (9) p = (-x +- sqrt(x*x - 4gxv*(gxv - z)))/2*gxv           [quadratic formula]
            //   (10) p = (v^2 +- sqrt(v^4 - g(g*x^2 + 2*z*v^2)))/gx        [multiply top/bottom by -2*v*v/x; move 4*v^4/x^2 into root]
            //   (11) O = atan(p)

            Vector3 diff = target - proj_pos;
            Vector3 diffXY = new Vector3(diff.X, diff.Y, 0);
            float groundDist = diffXY.Length();

            float speed2 = proj_speed * proj_speed;
            float speed4 = proj_speed * proj_speed * proj_speed * proj_speed;
            float z = diff.Z;
            float x = groundDist;
            float gx = gravity * x;

            float root = speed4 - gravity * (gravity * x * x + 2 * z * speed2);

            // No solution
            if (root < 0)
                return 0;

            root = (float)Math.Sqrt(root);

            var lowAng = Math.Atan2(speed2 - root, gx);
            var highAng = Math.Atan2(speed2 + root, gx);
            int numSolutions = lowAng != highAng ? 2 : 1;

            Vector3 groundDir = diffXY.Normalize();
            s0 = groundDir * (float)Math.Cos(lowAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(lowAng) * proj_speed;
            if (numSolutions > 1)
                s1 = groundDir * (float)Math.Cos(highAng) * proj_speed + Vector3.UnitZ * (float)Math.Sin(highAng) * proj_speed;

            t0 = x / ((float)Math.Cos(lowAng) * proj_speed);
            t1 = x / ((float)Math.Cos(highAng) * proj_speed);

            return numSolutions;
        }

        // Solve firing angles for a ballistic projectile with speed and gravity to hit a target moving with constant, linear velocity.
        //
        // proj_pos (Vector3): point projectile will fire from
        // proj_speed (float): scalar speed of projectile
        // target (Vector3): point projectile is trying to hit
        // target_velocity (Vector3): velocity of target
        // gravity (float): force of gravity, positive down
        //
        // s0 (out Vector3): firing solution (fastest time impact) 
        // s1 (out Vector3): firing solution (next impact)
        // s2 (out Vector3): firing solution (next impact)
        // s3 (out Vector3): firing solution (next impact)
        //
        // return (int): number of unique solutions found: 0, 1, 2, 3, or 4.
        public static int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target_pos, Vector3 target_velocity, float gravity, out Vector3 s0, out Vector3 s1, out float time)
        {
            // Initialize output parameters
            s0 = Vector3.Zero;
            s1 = Vector3.Zero;

            // Derivation 
            //
            //  For full derivation see: blog.forrestthewoods.com
            //  Here is an abbreviated version.
            //
            //  Four equations, four unknowns (solution.x, solution.y, solution.z, time):
            //
            //  (1) proj_pos.x + solution.x*time = target_pos.x + target_vel.x*time
            //  (2) proj_pos.y + solution.y*time = target_pos.y + target_vel.y*time
            //  (3) proj_pos.z + solution.z*time + .5*G*t = target_pos.z + target_vel.z*time
            //  (4) proj_speed^2 = solution.x^2 + solution.y^2 + solution.z^2
            //
            //  (5) Solve for solution.x and solution.y in equations (1) and (2)
            //  (6) Square solution.x and solution.y from (5)
            //  (7) Solve solution.z^2 by plugging (6) into (4)
            //  (8) Solve solution.z by rearranging (2)
            //  (9) Square (8)
            //  (10) Set (8) = (7). All solution.xyz terms should be gone. Only time remains.
            //  (11) Rearrange 10. It will be of the form a*^4 + b*t^3 + c*t^2 + d*t * e. This is a quartic.
            //  (12) Solve the quartic using SolveQuartic.
            //  (13) If there are no positive, real roots there is no solution.
            //  (14) Each positive, real root is one valid solution
            //  (15) Plug each time value into (1) (2) and (3) to calculate solution.xyz
            //  (16) The end.

            double G = gravity;

            double A = proj_pos.X;
            double B = proj_pos.Z;
            double C = proj_pos.Y;
            double M = target_pos.X;
            double N = target_pos.Z;
            double O = target_pos.Y;
            double P = target_velocity.X;
            double Q = target_velocity.Z;
            double R = target_velocity.Y;
            double S = proj_speed;

            double H = M - A;
            double J = O - C;
            double K = N - B;
            double L = -.5f * G;

            // Quartic Coeffecients
            double c0 = L * L;
            double c1 = 2 * Q * L;
            double c2 = Q * Q + 2 * K * L - S * S + P * P + R * R;
            double c3 = 2 * K * Q + 2 * H * P + 2 * J * R;
            double c4 = K * K + H * H + J * J;

            // Solve quartic
            double[] times = new double[4];
            int numTimes = SolveQuartic(c0, c1, c2, c3, c4, out times[0], out times[1], out times[2], out times[3]);

            // Sort so faster collision is found first
            Array.Sort(times);

            // Plug quartic solutions into base equations
            // There should never be more than 2 positive, real roots.
            Vector3[] solutions = new Vector3[2];
            int numSolutions = 0;

            time = 0.0f;
            for (int i = 0; i < numTimes && numSolutions < 2; ++i)
            {
                double t = times[i];
                if (t  0) s0 = solutions[0];
            if (numSolutions > 1) s1 = solutions[1];

            return numSolutions;
        }

        /// 
        /// Solve for a firing arc with a fixed gravity. Method was adapted for use by ACE by gmriggs from the original
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        public static bool SolveBallisticArc(Vector3 projectilePosition, float lateralSpeed, Vector3 targetPosition, out Vector3 velocityVector, out float time)
        {

            // Handling these cases is up to your project's coding standards
            Debug.astert(projectilePosition != targetPosition && lateralSpeed > 0, "fts.solve_ballistic_arc called with invalid data");

            velocityVector = Vector3.Zero;
            time = float.NaN;

            Vector3 diff = targetPosition - projectilePosition;
            Vector3 diffXY = new Vector3(diff.X, diff.Y, 0f);
            float lateralDist = diffXY.Length();

            if (lateralDist == 0)
                return false;

            time = lateralDist / lateralSpeed;

            velocityVector = diffXY.Normalize() * lateralSpeed;

            // System of equations. Hit max_height at t=.5*time. Hit target at t=time.
            //
            // peak = z0 + vertical_speed*halfTime + .5*gravity*halfTime^2
            // end = z0 + vertical_speed*time + .5*gravity*time^s
            // Wolfram Alpha: solve b = a + .5*v*t + .5*g*(.5*t)^2, c = a + vt + .5*g*t^2 for g, v
            float a = projectilePosition.Z; // initial
            float c = targetPosition.Z;     // final

            // Gravity value pulled from ACE property
            var g = PhysicsGlobals.Gravity;
            var b = (4 * a + 4 * c - g * time * time) / 8;

            velocityVector.Z = (2 * a - 2 * c + g * time * time) / (time * 2) * -1;

            return true;
        }

        // Solve the firing arc with a fixed lateral speed. Vertical speed and gravity varies. 
        // This enables a visually pleasing arc.
        //
        // proj_pos (Vector3): point projectile will fire from
        // lateral_speed (float): scalar speed of projectile along XY plane
        // target_pos (Vector3): point projectile is trying to hit
        // max_height (float): height above Max(proj_pos, impact_pos) for projectile to peak at
        //
        // fire_velocity (out Vector3): firing velocity
        // gravity (out float): gravity necessary to projectile to hit precisely max_height
        //
        // return (bool): true if a valid solution was found
        public static bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target_pos, float max_height, out Vector3 fire_velocity, out float gravity)
        {

            // Handling these cases is up to your project's coding standards
            Debug.astert(proj_pos != target_pos && lateral_speed > 0 && max_height > proj_pos.Z, "fts.solve_ballistic_arc called with invalid data");

            fire_velocity = Vector3.Zero;
            gravity = float.NaN;

            Vector3 diff = target_pos - proj_pos;
            Vector3 diffXY = new Vector3(diff.X, diff.Y, 0f);
            float lateralDist = diffXY.Length();

            if (lateralDist == 0)
                return false;

            float time = lateralDist / lateral_speed;

            fire_velocity = diffXY.Normalize() * lateral_speed;

            // System of equations. Hit max_height at t=.5*time. Hit target at t=time.
            //
            // peak = z0 + vertical_speed*halfTime + .5*gravity*halfTime^2
            // end = z0 + vertical_speed*time + .5*gravity*time^s
            // Wolfram Alpha: solve b = a + .5*v*t + .5*g*(.5*t)^2, c = a + vt + .5*g*t^2 for g, v
            float a = proj_pos.Z;       // initial
            float b = max_height;       // peak
            float c = target_pos.Z;     // final

            gravity = -4 * (a - 2 * b + c) / (time * time);
            fire_velocity.Z = -(3 * a - 4 * b + c) / time;

            return true;
        }

        // Solve the firing arc with a fixed lateral speed. Vertical speed and gravity varies. 
        // This enables a visually pleasing arc.
        //
        // proj_pos (Vector3): point projectile will fire from
        // lateral_speed (float): scalar speed of projectile along XY plane
        // target_pos (Vector3): point projectile is trying to hit
        // max_height (float): height above Max(proj_pos, impact_pos) for projectile to peak at
        //
        // fire_velocity (out Vector3): firing velocity
        // gravity (out float): gravity necessary to projectile to hit precisely max_height
        // impact_point (out Vector3): point where moving target will be hit
        //
        // return (bool): true if a valid solution was found
        public static bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target, Vector3 target_velocity, float gravity, out Vector3 fire_velocity, out float time, out Vector3 impact_point)
        {

            // Handling these cases is up to your project's coding standards
            Debug.astert(proj_pos != target && lateral_speed > 0, "fts.solve_ballistic_arc_lateral called with invalid data");

            // Initialize output variables
            fire_velocity = Vector3.Zero;
            time = 0.0f;
            impact_point = Vector3.Zero;

            // Ground plane terms
            Vector3 targetVelXY = new Vector3(target_velocity.X, target_velocity.Y, 0f);
            Vector3 diffXY = target - proj_pos;
            diffXY.Z = 0;

            // Derivation
            //   (1) Base formula: |P + V*t| = S*t
            //   (2) Subssatute variables: |diffXY + targetVelXY*t| = S*t
            //   (3) Square both sides: Dot(diffXY,diffXY) + 2*Dot(diffXY, targetVelXY)*t + Dot(targetVelXY, targetVelXY)*t^2 = S^2 * t^2
            //   (4) Quadratic: (Dot(targetVelXY,targetVelXY) - S^2)t^2 + (2*Dot(diffXY, targetVelXY))*t + Dot(diffXY, diffXY) = 0
            float c0 = Vector3.Dot(targetVelXY, targetVelXY) - lateral_speed * lateral_speed;
            float c1 = 2f * Vector3.Dot(diffXY, targetVelXY);
            float c2 = Vector3.Dot(diffXY, diffXY);
            double t0, t1;
            int n = SolveQuadric(c0, c1, c2, out t0, out t1);

            // pick smallest, positive time
            bool valid0 = n > 0 && t0 > 0;
            bool valid1 = n > 1 && t1 > 0;

            float t;
            if (!valid0 && !valid1)
                return false;
            else if (valid0 && valid1)
                t = Math.Min((float)t0, (float)t1);
            else
                t = valid0 ? (float)t0 : (float)t1;

            // Calculate impact point
            impact_point = target + (target_velocity * t);

            // Calculate fire velocity along XZ plane
            Vector3 dir = impact_point - proj_pos;
            fire_velocity = new Vector3(dir.X, dir.Y, 0f).Normalize() * lateral_speed;

            // Solve system of equations. Hit max_height at t=.5*time. Hit target at t=time.
            //
            // peak = z0 + vertical_speed*halfTime + .5*gravity*halfTime^2
            // end = z0 + vertical_speed*time + .5*gravity*time^s
            // Wolfram Alpha: solve b = a + .5*v*t + .5*g*(.5*t)^2, c = a + vt + .5*g*t^2 for g, v
            float a = proj_pos.Z;       // initial
            //float b = Math.Max(proj_pos.Z, impact_point.Z) + max_height_offset;  // peak
            float c = impact_point.Z;   // final

            //gravity = -4 * (a - 2 * b + c) / (t * t);
            //fire_velocity.Z = -(3 * a - 4 * b + c) / t;

            var g = gravity;
            var b = (4 * a + 4 * c - g * t * t) / 8;

            fire_velocity.Z = (2 * a - 2 * c + g * t * t) / (t * 2) * -1;

            time = t;

            return true;
        }
    }
}