test_PP_room.m

clear; clc; close all;
cc = distinguishable_colors(20);

addpath("stlTools/");
addpath('path-planning/');

%% load data
path = fullfile("stl/","PP_room.stl");
[V, F, ~, ~] = stlRead(path);
room.vertices = V * 0.0254;
room.faces = F;

% stlPlot(room.vertices, room.faces, 'test')

V = V *0.0254;
% 2) pick bottom faces
zmin = min(V(:,3));
tol  = 1e-6;
Z1 = V(F(:,1),3); Z2 = V(F(:,2),3); Z3 = V(F(:,3),3);
isBottom = abs(Z1-zmin)<tol & abs(Z2-zmin)<tol & abs(Z3-zmin)<tol;
bottomF = F(isBottom,:);

% isAllZero = ~any(bottomF(:));
isAllZero = 1;

if isAllZero == 0
    % 3) triangulation + free boundary
    TRb = triangulation(bottomF, V);
    E   = freeBoundary(TRb);        % L×2 list of edges :contentReference[oaicite:1]{index=1}

    % 4) group into loops
    loops = obj.edgeLoops(E);

    % 5) build P = [x y; NaN NaN; x y; NaN NaN; …]
    P = NaN(0,2);
    for k = 1:numel(loops)
        idxs = loops{k};
        coords = V(idxs,1:2);        % K×2
        P = [P; coords; NaN NaN];   %#ok<AGROW>
    end

    % 6) create a single polyshape with holes from P
    obj.pg = polyshape(P);         % NaN separators => holes :contentReference[oaicite:2]{index=2}

    % 1) Get the NaN-delimited boundary of pg
    [xb, yb] = boundary(obj.pg);           % xb,yb include NaNs between loops :contentReference[oaicite:0]{index=0}

    % 2) Isolate only the *first* loop (outer boundary)
    nanIdx = find(isnan(xb), 1);
    if isempty(nanIdx)
        outerX = xb;
        outerY = yb;
    else
        outerX = xb(1:nanIdx-1);
        outerY = yb(1:nanIdx-1);
    end

    % 3) Delaunay-triangulate those outer points
    DT = delaunayTriangulation(outerX, outerY);   % :contentReference[oaicite:1]{index=1}
    T  = DT.ConnectivityList;                     % M×3 array of triangles
    P  = DT.Points;                               % Mpoints×2 coordinates

    % 4) Keep only triangles whose centroids lie *inside* the outer polygon
    cents = ( P(T(:,1),:) + P(T(:,2),:) + P(T(:,3),:) )/3;  % M×2
    in   = inpolygon(cents(:,1), cents(:,2), outerX, outerY);
    TF  = T(in,:);

    % 5) Lift to 3D at z=0
    Vg = [P, zeros(size(P,1),1)];  % N×3 vertices at z=0
    Fg = TF;                        % faces
else
    % 1) Compute bounding box of the STL
    xmin = min(V(:,1));
    xmax = max(V(:,1));
    ymin = min(V(:,2));
    ymax = max(V(:,2));
    
    % Optional: expand slightly beyond the object
    pad = 0.1;  % 10 cm margin
    xmin = xmin - pad;
    xmax = xmax + pad;
    ymin = ymin - pad;
    ymax = ymax + pad;
    
    % 2) Define rectangle vertices (at z=0)
    Vg = [
        xmin ymin 0;
        xmax ymin 0;
        xmax ymax 0;
        xmin ymax 0
    ];
    
    % 3) Define faces (two triangles)
    Fg = [
        1 2 3;
        1 3 4
    ];
end
[vv,ff] = stlAddVerts(V, F, Vg, Fg);

stlPlot(vv, ff, 'results')