Whip's Quadratic Missile Intercept [OLD]

//Whip's Quadratic Missile Intercept Code
Vector3D CalculateMissileIntercept(double missileSpeed, Vector3D missilePosition, Vector3D targetVelocityVec, Vector3D targetPos)
{
    Vector3D directHeadingVec = targetPos - missilePosition;

    var targetSpeed = targetVelocityVec.Length();

    if (targetSpeed == 0 || missileSpeed == 0)
    {
        return directHeadingVec;
    }

    //Using law of cosines we assume that all the missile's velocity is in the direction of intercept
    double A = targetSpeed * targetSpeed - missileSpeed * missileSpeed;
    double B = 2 * targetVelocityVec.Dot(directHeadingVec);
    double C = directHeadingVec.LengthSquared();
    double D = B * B - 4 * A * C;

    double tf = 0;
    if (A == 0) //check for linear case: t = C / -B
    {
        if (B == 0)
        {
            return directHeadingVec;
        }
        else
        {
            tf = C / -B;
            //debugMe += "Linear: " + Math.Round(tf).ToString();
            if (tf <= 0)
            {
                //return targetVelocityVec;
                return directHeadingVec;
            }
        }
    }
    else if (D >= 0) //ensures real answer for quadratic formula
    {
        double t1 = (-B + Math.Sqrt(D)) / (2 * A);
        double t2 = (-B - Math.Sqrt(D)) / (2 * A);

        if (t1 < 0 && t2 >= 0)
        {
            tf = t2;
        }
        else if (t1 >= 0 && t2 < 0)
        {
            tf = t1;
        }
        else if (t1 < 0 && t2 < 0)
        {
            return directHeadingVec; //travel direct
        }
        else if (t1 >= 0 && t2 >= 0)
        {
            if (t1 > t2)
            {
                tf = t2; //we want shortest one
            }
            else
            {
                tf = t1;
            }
        }
        else
        {
            //debugMe = "None of the above";
            return directHeadingVec; //travel direct
        }
    }
    else
    { //non real answer
        return directHeadingVec;
    }

    Vector3D adjustedVecTargetVel = targetVelocityVec * tf + directHeadingVec;
    return adjustedVecTargetVel;
}