299r Janson Rotation - PRM code

0-README.md

Implementing Sampling Based Motion Planning Algorithm in Julia

Throwing this up quickly. Note that this code is not documented and not cleaned up*. It got the job done (albeit slowly), and in theory this would be a good project to refactor for speed.

• PRM.jl implements PRM (probabilistic road map)
• runPRM.jl creates a basic room with two obstacles, starting in lower left and ending in upper right, and runs PRM
• clutterExperimentSuccessRate.jl creates a similar room, then evaluates the impact of number of samples (PRM is a sampling-based motion planning algorithm) on success rate and cost of path found (basically, was the path longer than needed)

Note: Julia was awful to work with at the time (fall 2017). I had to file bugs in some of the libraries I used.

clutterExperimentSuccessRate.jl

include("PRM.jl")
#using Distributions

gr()


#### HELPER FXN 

    function areaRect(r::HyperRectangle)
        w, h = widths(r)
        area = w*h
    end

    function unionAreaRects(r1::HyperRectangle, r2::HyperRectangle)
        area1 = areaRect(r1)
        area2 = areaRect(r2)
        overlap = areaRect(intersect(r1, r2))
        area = area1 + area2 - overlap
        return area
    end
### END 


## PARAMETERS
numSamples = 500 
connectRadius = 2 
param = algT.AlgParameters(numSamples, connectRadius)

targetNumObs = 3
clutterPercentage = 0.15
roomWidth,roomHeight  = 20,20

startstate = Point(4.,4)
goalstate = Point(15.,15)



## INIT
obstacles = Vector{HyperRectangle}()
perimeter = HyperRectangle(Vec(0.,0), Vec(roomWidth,roomHeight))
wallsPerimeter = algfxn.decompRect(perimeter)

walls = Vector{LineSegment}()
for l in wallsPerimeter
    push!(walls, l)
end


# implement function to create clutter
# Let's say on average we want to create 4 obstacles... (otherwise, *most* of
# the time we will product one large rectangle that is falling out of the room

#targetNumObs = 2 

roomArea = roomWidth * roomHeight
targetSumObsArea= roomArea * clutterPercentage
sumObsArea = 0

while sumObsArea < targetSumObsArea
    #x,y = rand(Uniform(1, roomWidth),2)
    x,y = rand(1.0:roomWidth,2)

    randWidth, randHeight = rand(Uniform(1, roomWidth/targetNumObs),2)
    #randWidth, randHeight = rand(1:roomWidth/targetNumObs,2)
    protoObstacle = HyperRectangle(Vec(x, y), Vec(randWidth, randHeight)) #Todo
    if contains(perimeter, protoObstacle)
        push!(obstacles, protoObstacle)
        sumObsArea += randWidth*randHeight
    end
end
print("obstacles generated")


#plot!(walls, color =:black)
#plot!(obstacles, fillalpha=0.5)


@show targetSumObsArea
@show sumObsArea


### Run PRM on cluttered room

r = algT.Room(roomWidth,roomHeight,walls,obstacles)
plotfxn.plotRoom(r)

## Run preprocessing
nodeslist, edgeslist = preprocessPRM(r, param)
print("\n ---- pre-processed --- \n")

## Query created RM

pathcost, isPathFound, solPath = queryPRM(startstate, goalstate, nodeslist, edgeslist, obstacles)
print("\n ---- queried ---- \n")

## Plot path found
timestamp = Base.Dates.now()
title = "PRM with # samples=$numSamples, maxDist=$connectRadius, \npathcost = $pathcost, 
        timestamp=$timestamp)\n\n"
roadmap = algT.roadmap(startstate, goalstate, nodeslist, edgeslist)


plot = plotfxn.plotPRM(roadmap, solPath, title::String)
gui()

print("\n --- Time --- \n")
@show timestamp
print("\n --- Obstacles --- \n")
@show obstacles 
print("\n --- Nodes --- \n")
@show nodeslist 
print("\n --- Edges --- \n")
@show edgeslist 
print("\n -------- \n")


# todo: save one "representative" figure from each run


@show listCosts
@show listpSucc
sizeplot = (800, 800)

# sanity check
plot()


supTitle="\nPRM with maxDist=$connectRadius, nTrials=$nTrials.
        (time to run:$timeExperiment, timestamp=$timestamp)\n\n"
costTitle= "numsamples vs pathcost\n"

pPRMcost = scatter(nSamples_list, listCosts',
    color=:black,
    title = supTitle * costTitle, ylabel = "euclidean path cost", xlabel = "numsamples",
    yaxis=((0,100), 0:20:100))

successTitle = "\nnumsamples vs pSuccess\n"
pPRMsuccess = scatter(nSamples_list, listpSucc',
    color = :orange, markersize= 6, 
    title = successTitle, ylabel = ("P(success)=numSucc/$nTrials trials"), xlabel = "numsamples",
    yaxis=((0, 1.2), 0:0.1:1))

plot(pPRMcost, pPRMsuccess, layout=(2,1), legend=false,
    xaxis=((0, 320), 0:50:300),
    size = sizeplot, sizeplot = (800, 800))

PRM.jl

####################################################

#  Implementing PRM and functions for plotting PRM
#  Parameters incude num samples, connection distance, a list of obstacles, and start and goal states
#  Graph search implemented is Astar. Edge cost is assumed to be equal to euclidean distance.
#  To use, call ` include("PRM.jl") ` from any Julia file in the same folder.
#  nouyang 2017

####################################################

using Plots
using DataStructures
using GeometryTypes 
using Distributions
gr()

module algT
    using GeometryTypes 
    # export GraphNode, Edge, Obstacle, Room
    # struct PointAlg
        # x::Int64
        # y::Int64

    struct GraphNode
        id::Int64
        state::Point{2, Float64}
    end

    struct Edge
        startID::Int64
        endID::Int64
        #edge::LineSegment(Point{2, Float64}, Point{2, Float64})
        #edge::LineSegment{}
    end

    struct Obstacle
        id::Int64
        rect::HyperRectangle{2, Vec}
        #color
        #fillalpha
    end

    struct Room
        width::Int64
        height::Int64
        walls::Vector{LineSegment}
        #walls::HyperRectangle
        obstacles::Vector{HyperRectangle}
    end

    struct AlgParameters
        #startGoal::Point{2, Float64}
        #endGoal::Point{2, Float64}
        numSamples::Int64
        connectRadius::Int64
        # goal region?? should be rectangle? aka error from goal allowed
    end

    struct queueTmp
        #pt::Point{2, Float64}
        node::algT.GraphNode
        statesList::Vector{algT.GraphNode}
        cost::Int64
    end

    struct roadmap
        startstate::Point{2,Float64}
        goalstate::Point{2,Float64}
        nodeslist::Vector{GraphNode}
        edgeslist::Vector{Edge}
    end

    #Base.show(io::IO, v::Vertex) =# #print(io, "V($(v.id), ($(v.state.x),$(v.state.y)))")
    #Base.show(io::IO, p::Point) =# #print(io, "P($(p.x),$(p.y))")
    #Base.show(io::IO, q::tempQueueType) =# #print(io, "Q($(q.v),$(q.statesList) $(q.cost))")
    Base.isless(q1::queueTmp, q2::queueTmp) = q1.cost < q2.cost
    #Base.isless(p1::Point, p2::Point) = q1.x< q2.x
    #Base.isless(p1::Point, p2::Point) = q1[1] < q2[1]

end


module algfxn 
    using GeometryTypes
    using algT

	function ccw(A,B,C)
        # determines direction of lines formed by three points
        return (C[2]-A[2]) * (B[1]-A[1]) > (B[2]-A[2]) * (C[1]-A[1])
    end


    function intersects(line1, line2) #no ":" at the end!
        A, B = line1[1], line1[2]
        C, D = line2[1], line2[2]
        return ( (ccw(A, C, D) != ccw(B, C, D)) && ccw(A, B, C) != ccw(A, B, D))
    end

    function decompRect(r::HyperRectangle) #GeometryTypes.HyperRectangle{2,Float64}
        corners = decompose(Point{2, Float64}, r)
        corners = [Point(pt) for pt in corners]
        lineBottom  = LineSegment(corners[1], corners[2])
        lineTop     = LineSegment(corners[3], corners[4])
        lineLeft    = LineSegment(corners[1], corners[3])
        lineRight   = LineSegment(corners[2], corners[4])
        lines = Vector{LineSegment}()
        push!(lines, lineTop, lineRight, lineBottom, lineLeft)
        return lines
    end


    #function isCollidingNode(node::algT.GraphNode, obsList::Vector{algT.Obstacle}
    function isCollidingNode(node::Point{2, Float64}, obsList::Vector{HyperRectangle})
        for obs in obsList
            if contains(obs, node)
                return true
            end
        end
        return false
    end

    function isCollidingEdge(line::LineSegment, obsList::Vector{HyperRectangle})
        for obs in obsList
            rectLines = decompRect(obs)
            for rectline in rectLines
                if intersects(line, rectline)[1]
                    return true
                end
            end
        end
        return false
    end


    function findNearestNodes(nodestate, nodeslist, maxDist)
        # given maxDist, return all nodes within that distance of node
        nearestNodes = Vector{Tuple{algT.GraphNode, Float64}}()
        for n in nodeslist
            dist = min_euclidean(Vec(nodestate), Vec(n.state))
            if dist < maxDist 
                push!(nearestNodes, (n, dist))
            end
        end
        # return list sorted by distance?
        # or just return list with distances included for now...
        return nearestNodes 
    end
    
    function costPath(solPath)
        pathcost = 0
        for i in 2:length(solPath)
            curN  = solPath[i].state
            prevN = solPath[i-1].state
            pathcost += min_euclidean(Vec(curN), Vec(prevN))
        end
        return pathcost
    end

    function findNode(nodeID, nodeslist)
        #since we haven't removed any nodes, nodeslist should be sorted by index. but just in case, let's search through it
        index = findfirst([node.id for node in nodeslist], nodeID)
        return nodeslist[index]
    end

end

module plotfxn

    using GeometryTypes
    using Plots
    using algfxn
    using algT

    ###  Plots.jl recipes

    @recipe function f(r::HyperRectangle)
        points = decompose(Point{2,Float64}, r)
        rectpoints = points[[1,2,4,3],:]
        xs = [pt[1] for pt in rectpoints];
        ys = [pt[2] for pt in rectpoints];
        seriestype := :shape
        color = :orange
        #m = (:black, stroke(0))
        s = Shape(xs[:], ys[:])
    end

    @recipe function f(pt::Point)
        xs = [pt[1]]
        ys = [pt[2]]
        seriestype --> :scatter
        color = :orange
        markersize := 3
        xs, ys
    end
    
    @recipe function f(l::LineSegment)
        xs = [ l[1][1], l[2][1] ]
        ys = [ l[1][2], l[2][2] ]
        # seriestype = :line
        color = :red
        lw := 3
        xs, ys
    end


    @recipe function f(rectList::Vector{<:HyperRectangle})
        for r in rectList
            @series begin
                r 
            end
        end
    end

    @recipe function f(ptList::Vector{<:Point})
        for p in ptList
            @series begin
                p
            end
        end
    end

    @recipe function f(lineList::Vector{<:LineSegment})
        for l in lineList 
            @series begin
                l 
            end
        end

    end 


    ####

    function plotRoom(room)
        roomWidth, roomHeight, walls, obstacles = room.width, room.height, room.walls, room.obstacles
        plot()
       # #print("\nPlotting Room\n")
        plot!(walls, color =:black)
        plot!(obstacles, fillalpha=0.5)
    end

    function plotPRM(roadmap, solPath, title)
        startstate, goalstate, nodeslist, edgeslist = roadmap.startstate, roadmap.goalstate, roadmap.nodeslist, roadmap.edgeslist

        x = [n.state[1] for n in nodeslist]
        y = [n.state[2] for n in nodeslist]
        scatter!(x,y, color=:black) 

        edgeXs, edgeYs = [], []
        for e in edgeslist
            startN = algfxn.findNode(e.startID, nodeslist)
            endN = algfxn.findNode(e.endID, nodeslist)
            x1,y1 = startN.state[1], startN.state[2]
            x2,y2 = endN.state[1], endN.state[2]
            push!(edgeXs, x1, x2, NaN) #the NaNs, keep spaces between edges correctly unplotted
            push!(edgeYs, y1, y2, NaN)
        end

        plot!( edgeXs, edgeYs, color=:tan, linewidth=0.3)


        # plot solution
        plotSolPath(solPath)
        
        title!(title)
        plot!(legend=false, size=(600,600), xaxis=((-5,25), 0:1:20 ), 
              yaxis=((-5,25), 0:1:20), foreground_color_grid= :black)
    end

    function plotSolPath(solPath)
        print("\n ---Solution Path----- \n")
        @show solPath
        print("\n -------- \n")
        if solPath != Void
            xPath = [n.state[1] for n in solPath] 
            yPath = [n.state[2] for n in solPath]
            xstart, ystart = solPath[1].state
            xend, yend = solPath[end].state
            # plot start pt
            scatter!([xstart], [ystart], 
                     markercolor= :red, markershape = :circle,  markersize = 6, markerstrokealpha = 0.5, markerstrokewidth=1)
            # plot goal pt
            scatter!([xend], [yend], 
                     markerstrokecolor = :green, markershape = :star,  markersize = 5, markerstrokealpha = 1, markerstrokewidth=5)

            # plot path
            plot!( xPath, yPath, color = :orchid, linewidth=3, fillalpha = 0.3)
        else
           # #print("\n --- No solution path found ----- \n")
        end
    end


end

function preprocessPRM(room, parameters) 
    roomWidth, roomHeight, walls, obstacles = room.width, room.height, room.walls, room.obstacles
    numPts, connectRadius = parameters.numSamples, parameters.connectRadius

    nodeslist = Vector{algT.GraphNode}()
    edgeslist = Vector{algT.Edge}()

    currID = 1

    # Sample points, create list of nodes 
    for i in 1:numPts
        xrand, yrand = rand(Uniform(1, roomWidth-1), 2)
		#xrand,yrand = rand(1.0:roomWidth-1,2)
        n = Point(xrand, yrand) #new point in room
        if !algfxn.isCollidingNode(n, obstacles) #todo
            newNode = algT.GraphNode(currID, n)
            currID += 1
            push!(nodeslist, newNode)

        else
            print("\n NODE REMOVED: $n \n")
        end
    end

    # Connect each node to its neighboring nodes within a ball or radius r, creating edges
    for startnode in nodeslist 
        neighbors = [item[1] for item in algfxn.findNearestNodes(startnode.state, nodeslist, connectRadius)] # parent point
        n = [algfxn.findNearestNodes(startnode.state, nodeslist, connectRadius)] # parent point
        for endnode in neighbors
            candidateEdge = LineSegment(startnode.state, endnode.state)
            if !algfxn.isCollidingEdge(candidateEdge, obstacles) #todo
                #line = LineSegment(startnode.state, endnode.state)
                newEdge = algT.Edge(startnode.id, endnode.id) #by ID, or just store node? #wait no, i'd have multiple copies of same node for no real reason, mulitple edges per node
                push!(edgeslist, newEdge)
            else 
                print("\n EDGE REMOVED: $startnode WITH $endnode  \n")
            end
        end
    end

    #@show edgeslist 
    #@printf("\nPath found? %s Length of nodeslist: %d\n", isPathFound, length(nodeslist))
    return nodeslist, edgeslist
end

function getSuccessors(curNode, edgeslist, nodeslist) #assuming bidirectional for now
    #Find all edges that start or end at current node, and then make a list of the corresponding start or end nodes
    nodeID = curNode.id
    successorNodeIDs = Vector{Int}()
    successors = Vector{algT.GraphNode}()

    for e in edgeslist
        if e.startID == nodeID
            push!(successorNodeIDs, e.endID)
        end
        if e.endID == nodeID #should I include endID? it is bidirectional after all. 
            push!(successorNodeIDs,  e.startID) #yes, because local planner connects in bidirectional way
        end
    end

    for id in successorNodeIDs
        node = algfxn.findNode(id, nodeslist)
        push!(successors, node)
    end

    return successors
end

function queryPRM(startstate, goalstate, nodeslist, edgeslist, obstaclesList)
    # to find nearest node, set connect radius to be infinity for now #todo
    connectRadius = 99;
    n_nearStart= algfxn.findNearestNodes(startstate, nodeslist, connectRadius) #Get distance from start, for all points in graph.
    n_nearGoal= algfxn.findNearestNodes(goalstate, nodeslist, connectRadius) 

    ## SECTION remove nodes that we cannot reach in a straight line without going through an obstacle
    #todo: this is expensive, we should only check distances in order

    # n_nearStart is tuple of node and distance
    sort!(n_nearStart, by=n_nearStart->n_nearStart[2]) #Sort by distance and pick node with smallest distance from start
    sort!(n_nearGoal, by=n_nearGoal->n_nearGoal[2])
    #print("\n ----------------- \n")
    #@show n_nearStart
    #print("\n ----------------- \n")
    #@show n_nearGoal

    nodestart = algT.GraphNode(9999,Point(9999,9999))
    nodegoal = algT.GraphNode(9999,Point(9999,9999))

    for (n, dist) in n_nearStart 
        # bar = typeof(startstate)
        # bar2 = typeof(n)
        ## #print("\n$bar, $bar2\n")
        candidateline = LineSegment(Point(startstate), Point(n.state))
        if !algfxn.isCollidingEdge(candidateline, obstaclesList)
            nodestart = n
            break
        end
    end

    for (n, dist) in n_nearGoal
        candidateline = LineSegment(Point(goalstate), n.state)
        if !algfxn.isCollidingEdge(candidateline, obstaclesList)
            nodegoal = n
            break
        end
    end


    if nodestart == algT.GraphNode(9999,Point(9999,9999))
        nodestart = algT.GraphNode(0, Point(0,0))
        #print("ahhhhhh didn't find a start node")
    end
    if nodegoal == algT.GraphNode(9999,Point(9999,9999))
        nodegoal = algT.GraphNode(0, Point(0,0))
       # #print("ahhhhhh didn't find a goal node")
    end

    ## SECTION END

   # #print("This is the beginState $(startstate) and the endState $(goalstate)\n")
   # #print("This is the beginVertex--> $(nodestart) >>> and the endVertex--> $(nodegoal)\n")


    pathNodes = Vector{algT.GraphNode}()
    visited = Vector{algT.GraphNode}() #nodes we've searched through

    frontier = PriorityQueue()
    queue1 = algT.queueTmp(nodestart, pathNodes, 1) 
    enqueue!(frontier, queue1, 1) #root node has cost 0  
    pathcost = 99999

    #################### A star search
    while length(frontier) != 0
        front = DataStructures.dequeue!(frontier)
        pathVertices = []

        curNode, pathNodes, totalEdgeCost = front.node, front.statesList, front.cost

        if curNode == nodegoal
           # #print("Hurrah! endState reached! \n")
            unshift!(pathNodes, nodestart) #prepend our first path node back to pathVertices
            unshift!(pathNodes, algT.GraphNode(0, startstate)) #prepend the start 
            push!(pathNodes, algT.GraphNode(0, goalstate)) #append the goal 
            finalPathCost = algfxn.costPath(pathNodes) #Assuming edge cost is Euclidean cost
            isPathFound = true
            return (finalPathCost, isPathFound, pathNodes) #list of nodes in solution path

        else
            if !(curNode in visited) 
                push!(visited, curNode) # Add all successors to the stack

                for candidateNode in getSuccessors(curNode, edgeslist, nodeslist)
                    newEdgeCost = min_euclidean(Vec(curNode.state),
                                                Vec(candidateNode.state))
                    #edgecost is euclidean dist(state,state). better to pass
                   # Node than to perform node lookup everytime (vs passing id)
                    f_x = totalEdgeCost + newEdgeCost + min_euclidean(Vec(candidateNode.state), Vec(nodegoal.state)) #heuristic = euclidean distance to end goal
                    p = deepcopy(pathNodes)
                    push!(p, candidateNode)
                    if !(candidateNode in keys(frontier))
                        newQ = algT.queueTmp(candidateNode, p, ceil(totalEdgeCost+ newEdgeCost))
                        enqueue!(frontier, newQ, ceil(f_x)) 
                    end
                end

            end
        end
    end    

    # Return None if no solution found
    #@printf("No solution found! This is length of frontier, %d\n", length(frontier))
    finalPathCost = Void
    isPathFound = false
    pathNodes = Void
    return (finalPathCost, isPathFound, pathNodes)
end

runPRM.jl

####################################################

#  This file is used for development and testing of PRM.jl. 
#  It runs the PRM once and plots the results.
#  nouyang 2017

####################################################
# MAIN
####################################################
include("PRM.jl")

function main()
    numSamples = 25
    connectRadius =10 
    param = algT.AlgParameters(numSamples, connectRadius)

    print("\n ---- Running PRM ------ \n")
    ## Define obstacles
    obs1 = HyperRectangle(Vec(8,3.), Vec(2,2.)) #Todo
    obs2 = HyperRectangle(Vec(4,4.), Vec(2,10.)) #Todo

    obstacles = Vector{HyperRectangle}()
    push!(obstacles, obs1, obs2)

    # Define walls
    walls = Vector{LineSegment}()
    w,h  = 20,20
    perimeter = HyperRectangle(Vec(0.,0), Vec(w,h))
    roomPerimeter = algfxn.decompRect(perimeter)
    #internalwalls =  (linesegs
    for l in roomPerimeter
        push!(walls, l)
    end

    r = algT.Room(w,h,walls,obstacles)
    plotfxn.plotRoom(r)

    ## Run preprocessing
    nodeslist, edgeslist = preprocessPRM(r, param)

    ## Query created RM
    startstate = Point(1.,1)
    goalstate = Point(18.,18)

    pathcost, isPathFound, solPath = queryPRM(startstate, goalstate, nodeslist, edgeslist, obstacles)

    ## Plot path found
    title = "PRM with # samples =$numSamples, \nPathfound = $isPathFound, \npathcost = $pathcost"
    roadmap = algT.roadmap(startstate, goalstate, nodeslist, edgeslist)

    plot = plotfxn.plotPRM(roadmap, solPath, title::String)

####
#startGoal = algT.GraphNode(0, Point(0,0))
#endNode = algT.GraphNode(0, Point(0,0))

end


function collTest()

####
    function decompRect(r::HyperRectangle) #GeometryTypes.HyperRectangle{2,Float64}
        corners = decompose(Point{2, Float64}, r)
        corners = [Point(pt) for pt in corners]
        lineBottom  = LineSegment(corners[1], corners[2])
        lineTop     = LineSegment(corners[3], corners[4])
        lineLeft    = LineSegment(corners[1], corners[3])
        lineRight   = LineSegment(corners[2], corners[4])
        lines = Vector{LineSegment}()
        push!(lines, lineTop, lineRight, lineBottom, lineLeft)
        return lines
    end


    function isCollidingEdge(line::LineSegment, obsList::Vector{HyperRectangle})

        print("\n -------- \n")
		@show line 
        print("\n -------- \n")
        for obs in obsList
            rectLines = decompRect(obs)
            for rectline in rectLines
                @show intersects(line, rectline)[1]
				@show rectline
        print("\n -------- \n")
                if intersects(line, rectline)[1]
                    return true
                end
            end
        end
        return false
    end

function ccw(A,B,C)
    # determines direction of lines formed by three points
	return (C[2]-A[2]) * (B[1]-A[1]) > (B[2]-A[2]) * (C[1]-A[1])
	#return (C.y-A.y) * (B.x-A.x) > (B.y-A.y) * (C.x-A.x)
end


function intersectLineSeg(line1, line2) #no ":" at the end!
	A, B = line1[1], line1[2]
	C, D = line2[1], line2[2]
 	return ( (ccw(A, C, D) != ccw(B, C, D)) && ccw(A, B, C) != ccw(A, B, D))
end
####
	#obs1 = GeometryTypes.HyperRectangle{2,Float64}([7.67199, 10.8904], [1.67166, 2.51185])
	obs1 = GeometryTypes.HyperRectangle{2,Float64}([7,10], [2,3])

    obstacles = Vector{HyperRectangle}()
    push!(obstacles, obs1)

	#nodeslist = algT.GraphNode[algT.GraphNode(1, [18.7716, 16.8013]), algT.GraphNode(2, [12.8554, 11.3548]), algT.GraphNode(3, [13.4626, 13.6564]), algT.GraphNode(4, [14.5381, 7.52624]), algT.GraphNode(5, [15.5076, 17.8328]), algT.GraphNode(6, [4.65531, 14.1306]), algT.GraphNode(7, [15.3949, 17.6171]), algT.GraphNode(8, [1.34579, 4.52833]), algT.GraphNode(9, [15.2216, 11.153]), algT.GraphNode(10, [6.29213, 11.7306])]

    #anEdge = algT.Edge(2, 10)

	#startnode = nodeslist[2]
	startnode = algT.GraphNode(1, [6,11])
	#endnode = nodeslist[10]
	endnode = algT.GraphNode(2, [12,11])

	l = LineSegment(Point(6.0,11), Point(12.0,11))

    coll = isCollidingEdge(l, obstacles)
    @show coll
    #isCollidingNode

#(intersects(line, rectline))[1] = true
#r1 = GeometryTypes.Point{2,Float64}[[9.0, 10.0], [9.0, 13.0]]
r1 =LineSegment( Point(9.0,10.0), Point(9.0, 13.0))


 #-------- 
#(intersects(line, rectline))[1] = false
#r2 = GeometryTypes.Point{2,Float64}[[9.34365, 10.8904], [9.34365, 13.4022]]
r2 =LineSegment( Point(9.34365, 10.890), Point(9.34365, 13.402))

@show f= intersects(l, r1)
@show intersects(l, r2)
@show intersectLineSeg(l, r1)
@show intersectLineSeg(l, r2)

#https://github.com/JuliaGeometry/GeometryTypes.jl/blob/master/src/lines.jl

end


function rectUnionAreaTest()
    #obs1 = HyperRectangle(Vec(8,3.), Vec(4,4.)) #Todo
    #obs2 = HyperRectangle(Vec(10,5.), Vec(4,4.)) #Todo
    c = HyperRectangle(Vec(0,0),Vec(2,2))
    d = HyperRectangle(Vec(1,1),Vec(2,2))

    function areaRect(r::HyperRectangle)
        w, h = widths(r)
        area = w*h
    end

    function unionAreaRects(r1::HyperRectangle, r2::HyperRectangle)
        area1 = areaRect(r1)
        area2 = areaRect(r2)
        overlap = areaRect(intersect(r1, r2))
        area = area1 + area2 - overlap
        return area
    end

    bool = intersect(c,d) == HyperRectangle(Vec(1,1), Vec(1,1))
	@show bool

    areaRect(c)
    areaRect(d)
    unionAreaRects(c,d)

end


########################################
# Call main() function
########################################   

main()
#collTest()
#rectUnionAreaTest()