List <float[]> getVelocityObstacle(MotionModelSwedish buddy)
    {
        List <float[]> vertices = new List <float[]>();

        //Skip if buddy to far away??1

        float   theta     = Mathf.Acos((buddy.getRadius() + getRadius()) / ((buddy.getPosition() - getPosition()).magnitude));
        Vector3 D         = getPosition() - buddy.getPosition();
        float   baseAngle = Vector3.Angle(Vector3.right, D) * Mathf.Deg2Rad;

        if (D.z < 0)
        {
            baseAngle = -baseAngle;
        }
        float x1 = buddy.getPosition().x + (buddy.getRadius() + getRadius()) * Mathf.Cos(baseAngle + theta);
        float z1 = buddy.getPosition().z + (buddy.getRadius() + getRadius()) * Mathf.Sin(baseAngle + theta);

        float x2 = buddy.getPosition().x + (buddy.getRadius() + getRadius()) * Mathf.Cos(baseAngle - theta);
        float z2 = buddy.getPosition().z + (buddy.getRadius() + getRadius()) * Mathf.Sin(baseAngle - theta);


        Vector3 dir1   = new Vector3(x1 - getPosition().x, 0, z1 - getPosition().z).normalized;
        Vector3 dir2   = new Vector3(x2 - getPosition().x, 0, z2 - getPosition().z).normalized;
        Vector3 point1 = getPosition() + dir1 * vMax * dt;
        Vector3 point2 = getPosition() + dir2 * vMax * dt;

        point1 = point1 + buddy.getVelocity();
        point2 = point2 + buddy.getVelocity();
        Vector3 point3 = getPosition() + buddy.getVelocity();

        vertices.Add(new float[] { point1.x, point1.z });
        vertices.Add(new float[] { point2.x, point2.z });
        vertices.Add(new float[] { point3.x, point3.z });
        return(vertices);
    }
    OrcaLine getOrcaLine(MotionModelSwedish buddy, float time, float dt)
    {
        Vector3 relPos     = buddy.getPosition() - getPosition();
        Vector3 relVel     = getVelocity() - buddy.getVelocity();
        float   combRadius = buddy.getRadius() + getRadius();

        OrcaLine orcaline = new OrcaLine();
        Vector3  u;

        Vector3 w           = relVel - (relPos / time);
        float   decBoundary = Vector3.Dot(w, relPos);

        if (relPos.sqrMagnitude > Mathf.Pow(combRadius, 2))
        {
            if (!(decBoundary < 0.0f && Mathf.Pow(decBoundary, 2) > Mathf.Pow(combRadius, 2) * w.sqrMagnitude))
            {
                // Legs
                float distance = Mathf.Sqrt(relPos.sqrMagnitude - Mathf.Pow(combRadius, 2));

                if (Vector3.Cross(relPos, w).y < 0.0f)
                {
                    orcaline.direction = new Vector3(relPos.x * distance - relPos.z * combRadius, 0.0f, relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
                }
                else
                {
                    orcaline.direction = -new Vector3(relPos.x * distance + relPos.z * combRadius, 0.0f, -relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
                }
                float uLen = Vector3.Dot(relVel, orcaline.direction);
                u = uLen * orcaline.direction - relVel;
            }
            else
            {
                Vector3 wNorm = w.normalized;
                orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
                u = ((combRadius / time) - w.magnitude) * wNorm;
            }
        }

        else
        {
            w = relVel - (relPos / dt);
            Vector3 wNorm = w.normalized;
            orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
            u = ((combRadius / dt) - w.magnitude) * wNorm;
        }
        orcaline.point = getVelocity() + 0.5f * u;

        //Debug.DrawLine(getPosition() + orcaline.point - (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, getPosition() + orcaline.point + (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, Color.blue, Time.deltaTime);
        //Debug.DrawLine(getPosition() + orcaline.point, getPosition() + orcaline.point + orcaline.direction, Color.blue, Time.deltaTime);

        return(orcaline);
    }