Fixed polyhedron determination from planes

Signed-off-by: Nicola Vigano <nicola.vigano@esrf.fr>

zUtil_Maths/gtMathsGetPolyhedronVerticesFromPlaneNormals.m

@@ -19,8 +19,17 @@ function verts = gtMathsGetPolyhedronVerticesFromPlaneNormals(pl_normals)
     for ii = 1:num_pls
         [ps, ds_vers] = get_intersecting_lines(ii, pl_normals, pl_normals_vers, pl_normals_norm);
 
-        verts = [verts; get_intersection_points(ps, ds_vers, pl_normals, pl_normals_vers, pl_normals_norm)];
+        verts = [verts; ...
+            get_intersection_points(ps, ds_vers, pl_normals_vers, pl_normals_norm)]; %#ok<AGROW>
     end
+
+    validation_norms = pl_normals_norm + eps('single');
+    validation_pls = pl_normals_vers .* validation_norms(:, [1 1 1]);
+
+    % Let's remove the ones that already fall outside of the polyhedron
+    valid = gtMathsIsPointInPolyhedron(verts, [validation_pls, pl_normals_vers]);
+
+    verts = verts(valid, :);
 end
 
 function [ps, ds_vers] = get_intersecting_lines(pl_ii, pl_normals, pl_normals_vers, pl_normals_norm)
@@ -32,10 +41,6 @@ function [ps, ds_vers] = get_intersecting_lines(pl_ii, pl_normals, pl_normals_ve
     other_pls_vers = pl_normals_vers(pl_ii+1:end, :);
     other_pls_norm = pl_normals_norm(pl_ii+1:end);
 
-%     other_pls = pl_normals([1:pl_ii-1, pl_ii+1:end], :);
-%     other_pls_vers = pl_normals_vers([1:pl_ii-1, pl_ii+1:end], :);
-%     other_pls_norm = pl_normals_norm([1:pl_ii-1, pl_ii+1:end]);
-
     % No intersection is defined for parallel planes
     dot_prods_pl_pls = (pl_vers * other_pls_vers')';
     not_parallel = abs(dot_prods_pl_pls) < (1 - eps('single'));
@@ -62,52 +67,88 @@ function [ps, ds_vers] = get_intersecting_lines(pl_ii, pl_normals, pl_normals_ve
 
         % Position of the intersection line (hopefully closest to 0)
         ps = pl(ones_pls, :) + cs_norm(:, [1 1 1]) .* cs_vers;
-
-        % Let's remove the ones that already fall outside of the polyhedron
-        valid = gtMathsIsPointInPolyhedron(ps, [pl_normals, pl_normals_vers]);
-
-        ds_vers = ds_vers(valid, :);
-        ps = ps(valid, :);
     else
         ds_vers = zeros(0, 3);
         ps = zeros(0, 3);
     end
 end
 
-function points = get_intersection_points(ps, ds_vers, pl_normals, pl_normals_vers, pl_normals_norm)
-    points = zeros(0, 3);
+function points = get_intersection_points(ps, ds_vers, pl_normals_vers, pl_normals_norm)
+    num_ps = size(ps, 1);
+
+    % this is based on the fact that the intersection with the different planes
+    % will be of two types: the ones bounded from the top and the ones bounded
+    % from the bottom. We want to find the two points which are the most
+    % restrictive in both cases.
+    % If they are not valid, we will take care of it later. 
+
+    points = zeros(2 * num_ps, 3);
+
+    dot_prods_ds_pls = (ds_vers * pl_normals_vers')';
+    not_perpendiculars = abs(dot_prods_ds_pls) > eps('single');
 
-    for line_ii = 1:size(ps, 1)
+    for line_ii = 1:num_ps
         p = ps(line_ii, :);
         d_vers = ds_vers(line_ii, :);
 
-        dot_prods_d_pls = (d_vers * pl_normals_vers')';
-        not_perpendicular = abs(dot_prods_d_pls) > eps('single');
+        dot_prods_d_pls = dot_prods_ds_pls(:, line_ii);
+        not_perpendicular = not_perpendiculars(:, line_ii);
 
+        dot_prods_d_pls = dot_prods_d_pls(not_perpendicular);
         pls_vers = pl_normals_vers(not_perpendicular, :);
         pls_norm = pl_normals_norm(not_perpendicular);
 
-        ones_p = ones(numel(find(not_perpendicular)), 1);
+        ds_norm = (pls_norm - (p * pls_vers')') ./ dot_prods_d_pls;
 
-        ds_norm = (pls_norm - (p * pls_vers')') ./ dot_prods_d_pls(not_perpendicular);
+        min_norm = min(ds_norm(dot_prods_d_pls > 0));
+        max_norm = max(ds_norm(dot_prods_d_pls < 0));
 
-        tmp_points = p(ones_p, :) + d_vers(ones_p, :) .* ds_norm(:, [1 1 1]);
-
-        % Let's remove the ones that already fall outside of the polyhedron
-        valid = gtMathsIsPointInPolyhedron(tmp_points, [pl_normals, pl_normals_vers]);
-
-        points = [points; tmp_points(valid, :)];
+        points(2*(line_ii-1)+1, :) = p + d_vers * min_norm;
+        points(2*line_ii, :) = p + d_vers * max_norm;
     end
 end
 
-function plot_points(points, pl_normals, pl_normals_norm)
+function plot_points(points, pl_normals) %#ok<DEFNU>
+    f = figure();
+    ax = axes('parent', f);
+    hold(ax, 'on')
+    scatter3(ax, points(:, 1), points(:, 2), points(:, 3), 30, 'r', 'filled')
+    % I should do lines from each point, and then draw the pl_normals
+    zeros_pls = zeros(size(pl_normals, 1), 1);
+    quiver3(ax, zeros_pls, zeros_pls, zeros_pls, pl_normals(:, 1), pl_normals(:, 2), pl_normals(:, 3), 10/9)
+    hold(ax, 'off')
+end
+
+function plot_planes_and_points(points, pl_normals, pl_normals_norm) %#ok<DEFNU>
     f = figure();
     ax = axes('parent', f);
+    grid(ax, 'on')
     hold(ax, 'on')
     scatter3(ax, points(:, 1), points(:, 2), points(:, 3), 30, 'r', 'filled')
     % I should do lines from each point, and then draw the pl_normals
     zeros_pls = zeros(size(pl_normals, 1), 1);
     quiver3(ax, zeros_pls, zeros_pls, zeros_pls, pl_normals(:, 1), pl_normals(:, 2), pl_normals(:, 3), 10/9)
+
+    for ii = 1:size(pl_normals, 1)
+        plane = createPlane(pl_normals(ii, :), pl_normals(ii, :) / pl_normals_norm(ii));
+        drawPlane3d(plane);
+    end
+
+    hold(ax, 'off')
+end
+
+function plot_plane_and_lines(ii, pl_normals, pl_normals_vers, ps, ds_vers) %#ok<DEFNU>
+    f = figure();
+    ax = axes('parent', f);
+    % I should do lines from each point, and then draw the pl_normals
+    quiver3(ax, ps(:, 1), ps(:, 2), ps(:, 3), ds_vers(:, 1), ds_vers(:, 2), ds_vers(:, 3), 10/9)
+
+    grid(ax, 'on')
+    hold(ax, 'on')
+
+    plane = createPlane(pl_normals(ii, :), pl_normals_vers(ii, :));
+    drawPlane3d(plane);
+
     hold(ax, 'off')
 end