main.jl 11.7 KB
Newer Older
1
2
using DataFrames
using CSV
3
using JSON
4
using LightGraphs
5
using NearestNeighbors
6
using Statistics
7
using BenchmarkTools
8

9
10
11
12
13
14
15

struct contour
    pos
    time
end


16
struct problem
17
18
19
    contours::Vector{contour}
    G::SimpleDiGraph
    layers::Vector
20
    travel_dists::Dict
21
    layer_height::Number
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
end


function vecvec_to_matrix(vecvec)
    # convert vector of vectors int a matrix
    dim1 = length(vecvec)
    dim2 = length(vecvec[1])
    my_array = zeros(Float32, dim1, dim2)
    for i in 1:dim1
        for j in 1:dim2
            my_array[i,j] = vecvec[i][j]
        end
    end
    return my_array
end


39
function contour(d::Dict)
40
    return contour(vecvec_to_matrix(d["pos"]), d["time"])
41
end
42

43

44
function problem(cons::Vector{contour}, max_layers::Int, min_dist::Number)
45
    G = LightGraphs.SimpleDiGraph(0)
46

47
    # separate contours into layers
48
49
50
51
52
53
54
55
56
57
58
59
60
61
    layer_heights = sort(collect(Set([c.pos[1,3] for c in cons])))
    layers = [[] for i in 1:length(layer_heights)]
    clayeri = []
    contour_trees = []

    # place contours in layers and construct KDTree for each contour
    for i in 1:length(cons)
        l = searchsorted(layer_heights, cons[i].pos[1,3])[1]
        push!(layers[l], i)
        push!(clayeri, l)
        add_vertex!(G)
        push!(contour_trees, KDTree(transpose(cons[i].pos)))
    end

62
    # loop through contours from previous layer and compare waypoints
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
    for i in 1:length(cons)
        l = clayeri[i]

        # add contours from max_layers below
        if l > max_layers
            for c in layers[l-max_layers]
                add_edge!(G, c, i)
            end
        end

        if l == 1 || max_layers == 1
            continue
        end

        for c in layers[l-1]
            # if any points in contour i within min_dist of any points in contour c
            if any([length(b) > 0 for b in inrange(contour_trees[c], transpose(cons[i].pos), min_dist)])
                add_edge!(G, c, i) # mark i dependent on c
            end
        end
    end
84

85
    return problem(cons, G, layers, Dict(), layer_heights[1])
86
87
88
89
end


struct voxmap
90
91
92
    seglen::Vector{Float64}
    segoffset::Vector{Float64}
    segcontours::Vector{Int}
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
end


function seg_helper_orientation(p,q,r)
    val = (q[2]-p[2]) * (r[1]-q[1]) - (q[1]-p[1]) * (r[2]-q[2])
    if val > 0
        # clockwise
        return 1
    elseif val < 0
        # anticlockwise
        return 2
    else
        # colinear
        return 0
    end
end


function seg_intersect(p1,q1,p2,q2)

    o1 = seg_helper_orientation(p1, q1, p2)
    o2 = seg_helper_orientation(p1, q1, q2)
    o3 = seg_helper_orientation(p2, q2, p1)
    o4 = seg_helper_orientation(p2, q2, q1)

    if (o1  o2) && (o3  o4)
        return true
    elseif o1==0 || o2==0 || o3==0 || o4==0
        print("Colinearity! - TODO")
    end

    return false
end

127

128
129
130
dist(p1, p2) = √sum((p1 -p2).^2)
interpolate(p1, p2, xi, axis) = p1 + (p2-p1)*(xi-p1[axis])/(p2[axis]-p1[axis])
interpolate(p1, p2, x1, xi, x2) = p1 + (p2-p1)*(xi-x1)/(x2-x1)
131

132

133
function voxmap(vox, vox_d, prob::problem)
134
135
136

    # for one vox, get all contours which pass through it
    # only need to search contours in its layer
137
    l = Int((vox[3] + prob.layer_height/2)/prob.layer_height)
138
139
140
141
    voxx1 = vox[1] + vox_d/2
    voxx2 = vox[1] - vox_d/2
    voxy1 = vox[2] + vox_d/2
    voxy2 = vox[2] - vox_d/2
142

143
    seg_now = false
144
145
146
147
148
149
150
151
152
    seglen = Vector{Number}()
    segoffset = Vector{Number}()
    segcontours = Vector{Int}()
    seglen_sofar = 0
    t_start = 0

    for cid in prob.layers[l]

        c = prob.contours[cid]
153
154
155
156
157
158
159
160

        # check if contour passes thorough this vox
        for i in 2:size(c.pos)[1]

            # is this line segment completely outside vox?
            if c.pos[i, 1] > voxx1 && c.pos[i-1, 1] > voxx1 ||
                c.pos[i,1] < voxx2 && c.pos[i-1, 1] < voxx2 ||
                c.pos[i,2] < voxy2 && c.pos[i-1, 2] < voxy2 ||
161
                c.pos[i,2] > voxy1 && c.pos[i-1, 2] > voxy1
162

163
                # segment outside vox entirely
164
165
166
167
                if seg_now
                    print("Something's gone wrong: segment entirely outside voxel, but last segment inside")
                end
                continue
168
            end
169

170
171
172
173
            p1inside = c.pos[i-1, 1] < voxx1 && c.pos[i-1, 1] > voxx2 && c.pos[i-1, 2] > voxy2 && c.pos[i-1, 2] < voxy1
            p2inside = c.pos[i, 1] < voxx1 && c.pos[i,1] > voxx2 && c.pos[i,2] > voxy2 && c.pos[i,2] < voxy1
            # is this line segment completely inside vox?
            if p1inside && p2inside
174

175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
                seglen_sofar += dist(c.pos[i], c.pos[i-1]) # append to existing contour
                if !seg_now # start new seg
                    t_start = 0 #  0 bc contour must be starting for this case
                    seg_now = true

                    if i!=2
                        print("Whole segment inside but something wrong")
                    end
                    continue
                end
            end

            cross_side1 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx1, voxy1], [voxx1, voxy2])
            cross_side2 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx1, voxy1], [voxx2, voxy1])
            cross_side3 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx2, voxy1], [voxx2, voxy2])
            cross_side4 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx2, voxy2], [voxx1, voxy2])

            println(cid)
            println([cross_side1, cross_side2, cross_side3, cross_side4])

            # does this line segment intersect with vox only once
            if p1inside  p2inside

                # find intersection point
                if cross_side1 || cross_side3
                    # intersection with x
                    xi = [voxx1, voxx2][[cross_side1, cross_side3]][1]
                    p_i = interpolate(c.pos[i-1,:], c.pos[i,:], xi, 1)
                    t_i = interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], xi, c.pos[i,1])
                elseif cross_side3 || cross_side4
                    # intersection with y
                    yi = [voxy1, voxy2][[cross_side2, cross_side4]][1]
                    p_i = interpolate(c.pos[i-1,:], c.pos[i,:], yi, 2)
                    t_i = interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], yi, c.pos[i,2])
                end

                if p1inside
                    # end existing segment
                    if !seg_now
                        print("Something's gone wrong")
                    end
                    seglen_sofar += dist(c.pos[i-1, :], p_i)
                    push!(segcontours, cid)
                    push!(seglen, seglen_sofar)
                    push!(segoffset, (t_i + t_start)/2)
                    seglen_sofar = 0
                    seg_now = false
222
                else
223
224
225
226
                    # start new contour
                    t_start = t_i
                    seglen_sofar = dist(p_i, c.pos[i, :])
                    seg_now = true
227
                end
228
                continue
229

230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
            elseif sum([cross_side1, cross_side2, cross_side3, cross_side4]) >= 2
                # intersects twice
                p_is = []
                t_is = []
                if cross_side1
                    push!(p_is, interpolate(c.pos[i-1,:], c.pos[i,:], voxx1, 1))
                    push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], voxx1, c.pos[i,1]))
                end
                if cross_side2
                    push!(p_is, interpolate(c.pos[i-1,:], c.pos[i,:], voxy1, 2))
                    push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], voxy1, c.pos[i,2]))
                end
                if cross_side3
                    push!(p_is, interpolate(c.pos[i-1,:], c.pos[i,:], voxx2, 1))
                    push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], voxx2, c.pos[i,1]))
                end
                if cross_side4
                    push!(p_is, interpolate(c.pos[i-1,:], c.pos[i,:], voxy2, 2))
                    push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], voxy2, c.pos[i,2]))
                end
250

251
252
253
254
255
256
257
258
259
260
261
262
                if seg_now
                    print("Something's wrong")
                end

                push!(segoffset, mean(t_is))
                push!(segcontours, cid)
                n_intersections = sum([cross_side1, cross_side2, cross_side3, cross_side4])
                if n_intersections ==2
                    push!(seglen, dist(p_is[1], p_is[2]))
                else
                    push!(seglen, dist(p_is[1], p_is[3]))
                end
263
264
265
266
            end

        end

267
268
269
270
271
272
273
274
275
276
277
        # if contour ends inside the voxel
        if seg_now
            # end segment
            push!(segcontours, cid)
            push!(seglen, seglen_sofar)
            push!(segoffset, (t_start + last(c.time))/2)
            seglen_sofar = 0
            t_start = 0
            seg_now = false
        end

278
279
        # for those contours find exact segments
    end
280
    return voxmap(seglen, segoffset, segcontours)
281
282
end

283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325

function random_rollout(prob::problem)
    done_contours = Set{Int}()
    avail_contours = Set(prob.layers[1])
    todo_contours = Set(1:length(prob.contours))
    rollout = Vector{Int}()

    while length(avail_contours) > 0
        c = rand(avail_contours)
        push!(rollout, c)

        # remove selected contour from todo and avail, add to done
        delete!(avail_contours, c)
        delete!(todo_contours, c)
        push!(done_contours, c)

        # update available contours
        for i in todo_contours
            if i in avail_contours
                continue
            elseif length(inneighbors(prob.G, i)) == 0
                push!(avail_contours, i)
                continue
            end

            add = true
            for j in inneighbors(prob.G, i)
                if !(j in done_contours)
                    add = false
                    break
                end
            end

            if add
                push!(avail_contours, i)
            end
        end
    end

    return rollout
end


326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
function valid_swap(rollout::Vector{Int}, i::Int, j::Int, prob::problem)
    # would swapping indices i and j in rollout result in another valid rollout?
    # NOTE THIS FUNCTION DOESNT WORK
    # IT ONLY CHECKS DEPENDENCIES TO A DEPTH OF 1

    if i>j
        i,j = j,i
    elseif i==j
        return true
    end

    c1 = rollout[i]
    c2 = rollout[j]
    c2_dependson = inneighbors(prob.G, c2)

    if c1 in c2_dependson
        return false
    end

    c1_dependents = outneighbors(prob.G, c1)
    c_between = rollout[i+1:j-1]

    for c in c_between
        if c in c1_dependents || c in c2_dependson
            return false
        end
    end

    return true
end


358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
function check_validity(rollout::Vector{Int}, prob::problem)
    # make sure a given rollout is valid
    done_contours = Set{Int}()

    for c in rollout
        c_dependson = inneighbors(prob.G, c)

        if !issubset(c_dependson, done_contours)
            return false
        end

        push!(done_contours, c)
    end
    return true
end


375
#voxels = DataFrames.DataFrame(CSV.File("tensile-1-1.csv"))
376
377
378
379
contours = contour.(JSON.parse(open("tensilecontours.json")))
dt = problem(contours, 5, 5)
@benchmark random_rollout(dt.G)

380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404

function test_voxmap()
    # create vox
    vox = [0,0,0.5]
    vox_d = 2
    pos1 = [[0.5, -0.5] ones(2)*0.5 ones(2)]
    time1 = [0,1]
    pos2 = [[1.5, 0.5, -0.5, -1.5] ones(4)*0.5 ones(4)]
    time2 = Vector(0:3)
    pos3 = [[-0.5, -0.5] [2, -2] ones(2)]
    time3 = [0, 2.5]
    pos4 = [[0.5, 2.5] [-1.5, -0.5] ones(2)]
    time4 = [0,1]
    pos5 = [[-2,2] [-2,2] ones(2)]
    time5 = [0,1]
    contour1 = contour(pos1, time1)
    contour2 = contour(pos2,time2)
    contour3 = contour(pos3,time3)
    contour4 = contour(pos4,time4)
    contour5 = contour(pos5,time5)

    contours = [contour1, contour2, contour3, contour4, contour5]
    prob = problem(contours, 1, 1)
    vm = voxmap(vox, vox_d, prob)
    return vm
405
end