Пример #1
0
    // 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 XZ 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 max_height_offset,
                                                   out Vector3 fire_velocity,
                                                   out float gravity,
                                                   out Vector3 impact_point)
    {
        // Handling these cases is up to your project's coding standards
        Debug.Assert(proj_pos != target && lateral_speed > 0, "fts.solve_ballistic_arc_lateral called with invalid data");

        // Initialize output variables
        fire_velocity = Vector3.zero;
        gravity       = 0f;
        impact_point  = Vector3.zero;

        // Ground plane terms
        Vector3 targetVelXZ = new Vector3(target_velocity.x, 0f, target_velocity.z);
        Vector3 diffXZ      = target - proj_pos;

        diffXZ.y = 0;

        // Derivation
        //   (1) Base formula: |P + V*t| = S*t
        //   (2) Substitute variables: |diffXZ + targetVelXZ*t| = S*t
        //   (3) Square both sides: Dot(diffXZ,diffXZ) + 2*Dot(diffXZ, targetVelXZ)*t + Dot(targetVelXZ, targetVelXZ)*t^2 = S^2 * t^2
        //   (4) Quadratic: (Dot(targetVelXZ,targetVelXZ) - S^2)t^2 + (2*Dot(diffXZ, targetVelXZ))*t + Dot(diffXZ, diffXZ) = 0
        float  c0 = Vector3.Dot(targetVelXZ, targetVelXZ) - lateral_speed * lateral_speed;
        float  c1 = 2f * Vector3.Dot(diffXZ, targetVelXZ);
        float  c2 = Vector3.Dot(diffXZ, diffXZ);
        double t0, t1;
        int    n = Fts.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 = Mathf.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, 0f, dir.z).normalized *lateral_speed;

        // Solve system of equations. Hit max_height at t=.5*time. Hit target at t=time.
        //
        // peak = y0 + vertical_speed*halfTime + .5*gravity*halfTime^2
        // end = y0 + 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.y;                                                // initial
        float b = Mathf.Max(proj_pos.y, impact_point.y) + max_height_offset; // peak
        float c = impact_point.y;                                            // final

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

        return(true);
    }
Пример #2
0
    // 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;

        /*  substitute 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 = Fts.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 = System.Math.Sqrt(u);
            }
            else
            {
                return(0);
            }

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

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

            num = Fts.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 += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);
            }
            if (num == 1)
            {
                num += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s1, out s2);
            }
            if (num == 2)
            {
                num += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s2, out s3);
            }
        }

        /* resubstitute */
        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);
    }