Math3D.ClosestPointsOnTwoLines C# (CSharp) Method

    public static bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2)
    {
        closestPointLine1 = Vector3.zero;
        closestPointLine2 = Vector3.zero;

        float a = Vector3.Dot(lineVec1, lineVec1);
        float b = Vector3.Dot(lineVec1, lineVec2);
        float e = Vector3.Dot(lineVec2, lineVec2);

        float d = a*e - b*b;

        //lines are not parallel
        if(d != 0.0f){

            Vector3 r = linePoint1 - linePoint2;
            float c = Vector3.Dot(lineVec1, r);
            float f = Vector3.Dot(lineVec2, r);

            float s = (b*f - c*e) / d;
            float t = (a*f - c*b) / d;

            closestPointLine1 = linePoint1 + lineVec1 * s;
            closestPointLine2 = linePoint2 + lineVec2 * t;

            return true;
        }

        else{
            return false;
        }
    }

    /// <summary>
    /// Rediculously slow function to attempt to fully connect a set of nodes
    /// TO DO: Think of a better way to do this
    /// </summary>
    /// <param name="nodes"></param>
    public void GenerateIntersectionNodes(List <Edge> edges)
    {
        var directions          = new List <Vector3>();
        var directionStartNodes = new List <Node>();

        foreach (Edge e in edges)
        {
            Vector3 dir1 = (e.Nodes[1].transform.position - e.Nodes[0].transform.position).normalized;
            Vector3 dir2 = (e.Nodes[0].transform.position - e.Nodes[1].transform.position).normalized;
            directions.Add(dir1);
            directions.Add(dir2);
            directionStartNodes.Add(e.Nodes[0]);
            directionStartNodes.Add(e.Nodes[1]);

            List <Node> neighborsToRemove = new List <Node>();
            neighborsToRemove.Add(e.Nodes[1]);
            RemoveNeighbors(e.Nodes[0], neighborsToRemove);
            neighborsToRemove.Clear();
            neighborsToRemove.Add(e.Nodes[0]);
            RemoveNeighbors(e.Nodes[1], neighborsToRemove);
            neighborsToRemove.Clear();
        }



        for (int i = 0; i < directionStartNodes.Count; i++)
        {
            for (int j = 0; j < directionStartNodes.Count; j++)
            {
                if (i > j)
                {
                    Vector3 closestPoint1    = new Vector3(0, 0, 0);
                    Vector3 closestPoint2    = new Vector3(0, 0, 0);
                    Vector3 nodeHitPosition1 = directionStartNodes[i].transform.position;
                    Vector3 nodeHitPosition2 = directionStartNodes[j].transform.position;

                    RaycastHit hit;
                    // TO DO: Add Layermask
                    Ray ray1 = new Ray(directionStartNodes[i].transform.position, directions[i]);
                    Ray ray2 = new Ray(directionStartNodes[j].transform.position, directions[j]);


                    if (Physics.Raycast(ray1, out hit))
                    {
                        Node n = hit.transform.gameObject.GetComponent <Node>();
                        if (n != null)
                        {
                            nodeHitPosition1 = n.transform.position;
                        }
                    }

                    if (Physics.Raycast(ray2, out hit))
                    {
                        Node n = hit.transform.gameObject.GetComponent <Node>();
                        if (n != null)
                        {
                            nodeHitPosition2 = n.transform.position;
                        }
                    }

                    //Debug.Log("Checking for intersections between " + directionStartNodes[i].name + " and " + directionStartNodes[j].name);
                    if (Math3D.ClosestPointsOnTwoLines(out closestPoint1, out closestPoint2, directionStartNodes[i].transform.position, directions[i], directionStartNodes[j].transform.position, directions[j]))
                    {
                        Debug.Log("Intersect found!");

                        float distance2Intersect1 = Vector3.Distance(directionStartNodes[i].transform.position, closestPoint1);
                        float distance2Node1      = Vector3.Distance(directionStartNodes[i].transform.position, nodeHitPosition1);

                        if (distance2Intersect1 < distance2Node1)
                        {
                            if (!Physics.CheckSphere(closestPoint1, 0.5f))
                            {
                                Debug.Log("No node found at intersect point, adding a node");
                                AddNode(closestPoint1);
                            }
                        }
                    }
                }
            }
        }

        for (int i = 0; i < directionStartNodes.Count; i++)
        {
            RaycastHit hit;
            // TO DO: Add Layermask
            Vector3 dir = directions[i];
            Ray     ray = new Ray(directionStartNodes[i].transform.position, dir);

            if (Physics.Raycast(ray, out hit))
            {
                Node n = hit.transform.gameObject.GetComponent <Node>();
                if (n != null)
                {
                    Debug.Log(n.name);
                    AddNeighbor(directionStartNodes[i], n);
                }
            }
        }
    }