evaluate_cone Function

    pure elemental function evaluate_cone(this, pos) result(res)
        !! Evaluation function for Cone SDF.

        use utils, only : clamp

        class(cone),  intent(in) :: this
        type(vector), intent(IN) :: pos
        real(kind=wp) :: res

        real(kind=wp) :: rba, baba, papa, paba, x, cax, cay, k, f, cbx, cby, s
        type(vector) :: p

        p = pos .dot. this%transform

        rba = this%rb - this%ra
        baba = (this%b-this%a) .dot. (this%b-this%a)
        papa = (p-this%a) .dot. (p-this%a)
        paba =  ((p-this%a) .dot. (this%b-this%a))/ baba
        x = sqrt(papa - baba*paba**2)
        if(paba < 0.5_wp)then
            cax = max(0._wp, x - this%ra)
        else
            cax = max(0._wp, x - this%rb)
        end if
        cay = abs(paba - 0.5_wp) - .5_wp
        k = rba**2 + baba
        f = clamp((rba * (x - this%ra) + paba*baba) / k, 0._wp, 1._wp)
        cbx = x - this%ra - f*rba
        cby = paba - f
        if(cbx < 0._wp .and. cay < 0._wp)then
            s = -1._wp
        else
            s = 1._wp
        end if 
        res = s * sqrt(min(cax**2 + baba*cay**2, cbx**2 + baba*cby**2)) 

    end function evaluate_cone