fnc_willCollide.sqf

/* checks if two objects will likely collide in 3d */

/*
Let P be the position, and v be the velocity of object1,
let Q be the position, and w be the velocity of object2.

    P = position1, Q = position2,
    v = velocity1, w = velocity2

        P1       Q1       v1       w1
    P = P2,  Q = Q2,  v = v2,  w = w2
        P3       Q3       v3       w3


Let A be the position of object1 at time t,
let B be the position of object2 at time t:

    A(t) = P + v*t
    B(t) = Q + w*t


Let d be the distance between point A and B:

    d(t) = |AB(t)|
         = |B(t) - A(t)|

    d(t) = |Q + w*t - P - v*t|


Then:
    d(t)² = (Q1 + w1*t - P1 - v1*t)² +
            (Q2 + w2*t - P2 - v2*t)² +
            (Q3 + w3*t - P3 - v3*t)²

Combine like terms:
    d(t)² = [(Q1-P1) + (w1-v1)*t]² +
            [(Q2-P2) + (w2-v2)*t]² +
            [(Q3-P3) + (w3-v3)*t]²

Multiply out:
    d(t)² = (Q1-P1)² + 2*(Q1-P1)*(w1-v1) * t + (w1-v1)² * t² +
            (Q2-P2)² + 2*(Q2-P2)*(w2-v2) * t + (w2-v2)² * t² +
            (Q3-P3)² + 2*(Q3-P3)*(w3-v3) * t + (w3-v3)² * t²

Sort terms and combine, subtract d² from both sides
    0 = [(w1-v1)² + (w2-v2)² + (w3-v3)²] * t² +
        2*[(Q1-P1)*(w1-v1) + (Q2-P2)*(w2-v2) + (Q3-P3)*(w3-v3)] * t +
        [(Q1-P1)² + (Q2-P2)² + (Q3-P3)² - d²]

Set:
    alpha = [(w1-v1)² + (w2-v2)² + (w3-v3)²]
     beta = 2*[(Q1-P1)*(w1-v1) + (Q2-P2)*(w2-v2) + (Q3-P3)*(w3-v3)]
    gamma = [(Q1-P1)² + (Q2-P2)² + (Q3-P3)² - d²]

Then:
    0 = alpha*t² + beta*t + gamma
    0 = t² + (beta/alpha)*t + (gamma/alpha)

Apply pq-formula:
    t1/2 = -(beta/(2*alpha)) ± sqrt[ (beta²/(4*alpha²)) - (gamma/alpha) ]


determinat = [ (beta²/(4*alpha²)) - (gamma/alpha) ]

If determinant < 0, then chosen distance d is never reached at any time.
If determinant = 0, then chosen distance d is reached at time t1 = t2 only.
If determinant > 0, then distance d is reached at time t1 and t2, and
    between t1 and t2, the distance of A and B is smaller than chosen d

A collision occurs between t1 and t2 if determinant > 0
*/

commy_fnc_willCollide = {
    params [
        ["_object1", objNull, [objNull]],
        ["_object2", objNull, [objNull]]
    ];

    private _allowedDistance = 10;
    private _timeToCheckAhead = 60;

    getPosWorld _object1 params ["_P1", "_P2", "_P3"];
    getPosWorld _object2 params ["_Q1", "_Q2", "_Q3"];

    velocity _object1 params ["_v1", "_v2", "_v3"];
    velocity _object2 params ["_w1", "_w2", "_w3"];

    private _alpha = (_w1-_v1)^2 + (_w2-_v2)^2 + (_w3-_v3)^2;
    private _beta = 2*((_Q1-_P1)*(_w1-_v1) + (_Q2-_P2)*(_w2-_v2) + (_Q3-_P3)*(_w3-_v3));
    private _gamma = (_Q1-_P1)^2 + (_Q2-_P2)^2 + (_Q3-_P3)^2 - _allowedDistance^2;

    // if this value is 0, both objects move with the same velocity and their distance will never change
    if (_alpha isEqualTo 0) exitWith {false};

    private _determinant = _beta^2/_alpha^2/4 - _gamma/_alpha;

    // no collision, objects will never come close enough
    if (_determinant < 0) exitWith {false};

    private _t1 = -_beta/_alpha/2 - sqrt _determinant;
    private _t2 = -_beta/_alpha/2 + sqrt _determinant;

    // collision already happened if we assume current flight path, objects missed each other
    if (_t2 < 0) exitWith {false};

    // collision would happen a long time from now, ignore
    if (_t1 > _timeToCheckAhead) exitWith {false};

    // otherwise collision will happen in _t1
    DEBUG_T1 = _t1; // debug, delete
    DEBUG_T2 = _t2; // debug, delete
    true
};

// debugging
["DEBUG_MATH", "onEachFrame", {
    params ["_plane1", "_plane2"];

    // debug line drawing
    private _pos1 = getPosWorld _plane1;
    private _pos2 = getPosWorld _plane2;

    private _dir1 = velocity _plane1;
    private _dir2 = velocity _plane2;

    private _speed1 = vectorMagnitude _dir1;
    private _speed2 = vectorMagnitude _dir2;

    private _dir1 = vectorNormalized _dir1;
    private _dir2 = vectorNormalized _dir2;

    // handle standing objects
    if (_speed1 < 1e-10) then {
        _speed1 = 1e-10;
        _dir1 = [0,0,1];
    };

    if (_speed2 < 1e-10) then {
        _speed2 = 1e-10;
        _dir2 = [0,0,1];
    };

    if ([_plane1, _plane2] call commy_fnc_willCollide) then {
        playSound "Beep_Target";

        if (DEBUG_T1 > 0) then {
            // blue line for object1, red line starting at danger zone
            drawLine3D [
                ASLToAGL _pos1,
                ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply (DEBUG_T1*_speed1))),
                [0,0,1,1]
            ];
            drawLine3D [
                ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply (DEBUG_T1*_speed1))),
                ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply (DEBUG_T2*_speed1))),
                [1,0,0,1]
            ];
        } else {
            // red line for object1, already in danger zone
            drawLine3D [
                ASLToAGL _pos1,
                ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply (DEBUG_T2*_speed1))),
                [1,0,0,1]
            ];
        };

        // blue line for object1, after danger zone
        drawLine3D [
            ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply (DEBUG_T2*_speed1))),
            ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply 1000)),
            [0,0,1,1]
        ];
    } else {
        // blue line for object1, no collision
        drawLine3D [
            ASLToAGL _pos1,
            ASLToAGL (_pos1 vectorAdd (_dir1 vectorMultiply 1000)),
            [0,0,1,1]
        ];
    };

    // green line for object2 heading
    drawLine3D [
        ASLToAGL _pos2,
        ASLToAGL (_pos2 vectorAdd (_dir2 vectorMultiply 1000)),
        [0,1,0,1]
    ];
}, [plane1, plane2]] call BIS_fnc_addStackedEventHandler;