working on using variational and masks

collision_detection.m

@@ -0,0 +1,48 @@
+% detection collision of multiple trajectory
+% by if trajectories are overlapped in the spatial domain
+%
+% input: cpslist, nb_time_points * spatial_dim * nb_trajectory
+%
+% output: clist, a 1*K vector, of index in cpslist first dimension(time
+% points), clist(ii) to clist(ii+1) is continuous 
+
+function clist = collision_detection(g, cpslist)
+
+% nb_T = size(cpslist, 1);
+% nb_cps = size(cpslist, 3);
+
+nb_T = g.nb_T;
+nb_cps = g.nb_cps;
+r = g.r;
+
+clist(1) = 1;
+for ii = 2:nb_T;
+    is_collision = 0;
+    for jj = 1:nb_cps
+        for kk = jj+1:nb_cps
+
+            d2 = dist2(cpslist(clist(end):ii, :, jj), cpslist(clist(end):ii, :, kk));
+            if (min(d2(:)) < r)
+                clist(end+1) = ii;
+                is_collision = 1;
+                break;
+            end;
+
+        end;
+
+        if (is_collision)
+            break;
+        end;
+    end;
+
+    if (clist(end) == nb_T)
+        break;
+    end;
+
+end;
+
+if (clist(end) ~= nb_T)
+    clist(end+1) = nb_T;
+end;
+
+% clist = clist(1:20:end);

collision_detection_conflicted_velocity.m

@@ -0,0 +1,60 @@
+function clist = collision_detection_conflicted_velocity(g, cpslist, vcpslist)
+
+% if two trajectories are very close AND the velocities on close points are
+% not equal (within a threshold), then it is conflict
+
+nb_T = g.nb_T;
+nb_cps = g.nb_cps;
+r = g.r;
+dv = g.dv; % difference of velocity
+
+clist(1) = 1;
+for ii = 2:nb_T;
+    is_collision = 0;
+    for jj = 1:nb_cps
+        for kk = jj+1:nb_cps
+
+            offset = clist(end);
+            d2 = dist2(cpslist(clist(end):ii, :, jj), cpslist(clist(end):ii, :, kk));
+
+            [mind, mind_idx] = min(d2(:));
+
+            if (mind < r)
+
+                % check if velocities are different
+
+                [J, K] = ind2sub(size(d2), mind_idx);
+                J = J + offset - 1;
+                K = K + offset - 1;
+
+                cpslist(J, :, jj)
+                cpslist(K, :, kk)
+                vcpslist(K, :, kk)
+                vcpslist(J, :, jj)
+
+
+                vd2 = dist2(vcpslist(J, :, jj), vcpslist(K, :, kk))
+                if vd2 > dv
+                    clist(end+1) = ii;
+                    is_collision = 1;
+                    break;
+                end;
+            end;
+
+        end;
+
+        if (is_collision)
+            break;
+        end;
+    end;
+
+    if (clist(end) == nb_T)
+        break;
+    end;
+
+end;
+
+if (clist(end) ~= nb_T)
+    clist(end+1) = nb_T;
+end;
+

collision_detection_conflicted_velocity_multibeam.m

@@ -0,0 +1,80 @@
+function clist = collision_detection_conflicted_velocity_multibeam(g, cpslist, vcpslist)
+
+% if two trajectories are very close AND the velocities on close points are
+% not equal (within a threshold), then it is conflict
+
+nb_T = g.nb_T;
+nb_cps = g.nb_cps;
+r1 = g.r1; % intra-beam distance
+r2 = g.r2; % inter-beam distance
+dv = g.dv; % difference of velocity
+
+clist(1) = 1;
+for ii = 2:nb_T;
+    is_collision = 0;
+    for jj = 1:nb_cps
+        for kk = jj+1:nb_cps
+
+            offset = clist(end);
+            d2 = dist2(cpslist(clist(end):ii, :, jj), cpslist(clist(end):ii, :, kk));
+
+            [mind, mind_idx] = min(d2(:));
+
+
+            if g.ind(jj) == g.ind(kk)
+                % intra beam, use small distance r1 + velocity overlapping
+                if (mind < r1)
+                    % check if velocities are different
+                    [J, K] = ind2sub(size(d2), mind_idx);
+                    J = J + offset - 1;
+                    K = K + offset - 1;
+                    vd2 = dist2(vcpslist(J, :, jj), vcpslist(K, :, kk));
+                    if vd2 > dv
+                fprintf(2, 'intra collsion:\n');        
+                cpslist(J, :, jj)
+                cpslist(K, :, kk)
+                vcpslist(K, :, kk)
+                vcpslist(J, :, jj)
+
+                        clist(end+1) = ii;
+                        is_collision = 1;
+                        break;
+                    end;
+                end;
+
+            else
+
+                if (mind < r2)
+                                        [J, K] = ind2sub(size(d2), mind_idx);
+                    J = J + offset - 1;
+                    K = K + offset - 1;
+                    fprintf(2, 'inter collsion:\n');
+                cpslist(J, :, jj)
+                cpslist(K, :, kk)
+
+                    clist(end+1) = ii;
+                    is_collision = 1;
+                    break;
+                end;
+                % inter beam, use large distance r2 + position overlapping
+            end;
+
+
+
+        end;
+
+        if (is_collision)
+            break;
+        end;
+    end;
+
+    if (clist(end) == nb_T)
+        break;
+    end;
+
+end;
+
+if (clist(end) ~= nb_T)
+    clist(end+1) = nb_T;
+end;
+

dist2.m

@@ -0,0 +1,14 @@
+
+% A = [p1; p2; p3], each row is a vector
+function d2 = dist2(A, B)
+
+nb_A = size(A, 1);
+nb_B = size(B, 1);
+d2 = zeros(nb_A, nb_B);
+
+dim = size(A, 2);
+
+for d = 1:dim
+    tmp = A(:, d) * ones(1, nb_B) - ones(nb_A, 1) * B(:, d)';
+    d2 = d2 + tmp.*tmp;
+end;

dist2_rowvec_to_single.m

@@ -0,0 +1,5 @@
+function d2 = dist2_rowvec_to_single(plist, q)
+
+nb_p = size(plist, 1);
+d = plist - ones(nb_p, 1) * q;
+d2 = sum(d.*d, 2);

est_collision_detection_conflict_by_velocity.m

@@ -0,0 +1,19 @@
+% test_collision_detection_conflict_by_velocity
+
+cpslist = zeros(100, 2, 2);
+vcpslist = zeros(100, 2, 2);
+
+% construct cpslist with overlapped trajectories but same velocities
+
+cpslist(:, 1, 1) = linspace(15.1, 30.2, 100);
+cpslist(:, 2, 1) = linspace(10.9, 10.9, 100);
+vcpslist(:, 1, 1) = 5.7 * cpslist(:, 1, 1) + 2.2 * cpslist(:, 2, 1) + 3.1; 
+vcpslist(:, 1, 1) = -0.4 * cpslist(:, 1, 1) + 1.2 * cpslist(:, 2, 1) + 2.7; 
+
+cpslist(:, 1, 2) = linspace(22.3, 36.9, 100);
+cpslist(:, 2, 2) = linspace(10.9, 10.9, 100);
+vcpslist(:, 1, 2) = 5.7 * cpslist(:, 1, 2) + 2.2 * cpslist(:, 2, 2) + 3.1; 
+vcpslist(:, 1, 2) = -0.4 * cpslist(:, 1, 2) + 1.2 * cpslist(:, 2, 2) + 2.7; 
+
+
+% construct cpslist with overlapped trajectories but different velocities

get_A_and_t.m

@@ -0,0 +1,5 @@
+function [A, t] = get_A_and_t(theta, c, t0, s)
+
+A = [cos(theta/180*pi), -sin(theta/180*pi); sin(theta/180*pi), cos(theta/180*pi)];
+A = A * diag(s);
+t = (eye(2) - A) * c + t0;

get_Lv_from_At.m

@@ -0,0 +1,5 @@
+function [L, v] = get_Lv_from_At(A, t)
+Lv = logm([A, t; 0 0 1]);
+L = Lv(1:2, 1:2);
+v = Lv(1:2, 3);
+

get_affine_on_cps.m

@@ -0,0 +1,6 @@
+function cps_desired_target = get_affine_on_cps(g)
+
+cps_desired_target = zeros(g.dim, g.nb_cps);
+for ii = 1:g.nb_cps
+    cps_desired_target(:, ii) = g.aff{ii}.A * g.cps(:, ii) + g.aff{ii}.t;
+end;

get_control_point_and_transform.m

@@ -0,0 +1,75 @@
+function g = get_control_point_and_transform(g)
+
+
+% control point
+% column vector
+% cps = [-10 10 0   0;
+%         0  0  10 -10 ];
+
+%      cps = [-10 0;
+%               0 30];
+
+% s is the sigma to computer affine velocity decreasing
+s = 2;
+dim = 2;
+
+cps = [-50 0;
+    0 0];
+
+nb_cps = size(cps, 2);
+g.aff = cell(0);
+
+
+[A, t] = get_A_and_t(0, [0;0], [50; 0], [1, 1]);
+[L, v] = get_Lv_from_At(A, t);
+g.aff{end+1}.A = A;
+g.aff{end}.t = t;
+g.aff{end}.L = L;
+g.aff{end}.v = v;
+g.aff{end}.s = s;
+
+[A, t] = get_A_and_t(0, [0; 0], [0; 50], [1,1]);
+[L, v] = get_Lv_from_At(A, t);
+g.aff{end+1}.A = A;
+g.aff{end}.t = t;
+g.aff{end}.L = L;
+g.aff{end}.v = v;
+g.aff{end}.s = s;
+
+
+% [A, t] = get_A_and_t(0, [0; 0], [0;0], [1,1]);
+% [L, v] = get_Lv_from_At(A, t);
+% g.aff{end+1}.A = A;
+% g.aff{end}.t = t;
+% g.aff{end}.L = L;
+% g.aff{end}.v = v;
+% g.aff{end}.s = s;
+
+% [A, t] = get_A_and_t(0, [0; -10], [0;20], [1,1]);
+% [L, v] = get_Lv_from_At(A, t);
+% g.aff{4}.A = A;
+% g.aff{4}.t = t;
+% g.aff{4}.L = L;
+% g.aff{4}.v = v;
+% g.aff{4}.s = s;
+
+% [A, t] = get_A_and_t(0, [-10;0], [40;0], [2, 1]);
+% [L, v] = get_Lv_from_At(A, t);
+% g.aff{1}.A = A;
+% g.aff{1}.t = t;
+% g.aff{1}.L = L;
+% g.aff{1}.v = v;
+% g.aff{1}.s = s;
+%
+% [A, t] = get_A_and_t(0, [0; 10], [0;-40], [1,1]);
+% [L, v] = get_Lv_from_At(A, t);
+% g.aff{2}.A = A;
+% g.aff{2}.t = t;
+% g.aff{2}.L = L;
+% g.aff{2}.v = v;
+% g.aff{2}.s = s;
+%
+
+g.dim = dim;
+g.nb_cps = nb_cps;
+g.cps = cps;