mmath

Fork

% Copyright (C) 2019-2020 AlaskanEmily
%
% This Source Code Form is subject to the terms of the Mozilla Public
% License, v. 2.0. If a copy of the MPL was not distributed with this
% file, You can obtain one at http://mozilla.org/MPL/2.0/.

:- module polyhedron.

%==============================================================================%
% Efficient representation of polyhedron with triangular faces.
:- interface.
%==============================================================================%

:- import_module list.

%------------------------------------------------------------------------------%
% The parameter T is the type of vertex.
:- type polyhedron(T).

%------------------------------------------------------------------------------%

:- type key(T).

%------------------------------------------------------------------------------%

:- func init = polyhedron(T).

%------------------------------------------------------------------------------%
% Adds a vertex to the polyhedron.
% This will not insert duplicates, but instead output the key for an existing
% vertex if one already exists in the polyhedron.
:- pred add_vertex(T, polyhedron(T), polyhedron(T), key(T)).
:- mode add_vertex(in, in, out, out) is det.

%------------------------------------------------------------------------------%
% Adds a vertex to the polyhedron.
% This will not insert duplicates, but instead fails if the vertex already
% exists in the polyhedron.
:- pred insert_vertex(T, polyhedron(T), polyhedron(T), key(T)).
:- mode insert_vertex(in, in, out, out) is semidet.

%------------------------------------------------------------------------------%
% Adds a vertex to the polyhedron.
% This will not insert duplicates, but instead throws an exception if the
% vertex already exists in the polyhedron.
:- pred det_insert_vertex(T, polyhedron(T), polyhedron(T), key(T)).
:- mode det_insert_vertex(in, in, out, out) is det.

%------------------------------------------------------------------------------%
% Adds an edge to the polyhedron.
% This will not insert duplicates.
:- pred add_edge(key(T), key(T), polyhedron(T), polyhedron(T)).
:- mode add_edge(in, in, in, out) is det.

%------------------------------------------------------------------------------%
% Adds an edge to the polyhedron.
% This fails if the edge is a duplicate.
:- pred insert_edge(key(T), key(T), polyhedron(T), polyhedron(T)).
:- mode insert_edge(in, in, in, out) is semidet.

%------------------------------------------------------------------------------%
% Adds an edge to the polyhedron.
% This throws an exception if the edge is a duplicate.
:- pred det_insert_edge(key(T), key(T), polyhedron(T), polyhedron(T)).
:- mode det_insert_edge(in, in, in, out) is det.

%------------------------------------------------------------------------------%
% Looks up the vertex given a key.
:- pred lookup(key(T)::in, polyhedron(T)::in, T::out) is det.

%------------------------------------------------------------------------------%
% Finds the key for a vertex.
% This is not a fast action, and in general you should hold on to the keys
% when you add vertices.
% Fails if the vertex is not in the polyhedron.
:- pred value(T::in, polyhedron(T)::in, key(T)::out) is semidet.

%------------------------------------------------------------------------------%
% Finds the key for a vertex.
% This is not a fast action, and in general you should hold on to the keys
% when you add vertices.
% Throws an exception if the vertex is not in the polyhedron.
:- pred det_value(T::in, polyhedron(T)::in, key(T)::out) is det.

%------------------------------------------------------------------------------%
% Gathers a list of vertices connected to this vertex.
:- pred collect_edges(key(T)::in, polyhedron(T)::in, list(key(T))::out) is det.

%------------------------------------------------------------------------------%
% Calls the predicate once for each triangle that includes the vertex
:- pred triangles(pred(polyhedron(T), key(T), key(T), key(T), A, A),
    key(T),
    polyhedron(T),
    A, A).
:- mode triangles(pred(in, in, in, in, in, out) is det, in, in, in, out) is det.
:- mode triangles(pred(in, in, in, in, in, out) is semidet, in, in, in, out) is semidet.
:- mode triangles(pred(in, in, in, in, di, uo) is det, in, in, di, uo) is det.
:- mode triangles(pred(in, in, in, in, mdi, muo) is det, in, in, mdi, muo) is det.
:- mode triangles(pred(in, in, in, in, mdi, muo) is semidet, in, in, mdi, muo) is semidet.

%------------------------------------------------------------------------------%
% Calls the predicate once for each triangle in the polyhedron
:- pred fold_triangles(pred(polyhedron(T), key(T), key(T), key(T), A, A),
    polyhedron(T),
    A, A).
:- mode fold_triangles(pred(in, in, in, in, in, out) is det, in, in, out) is det.
:- mode fold_triangles(pred(in, in, in, in, in, out) is semidet, in, in, out) is semidet.
:- mode fold_triangles(pred(in, in, in, in, di, uo) is det, in, di, uo) is det.
:- mode fold_triangles(pred(in, in, in, in, mdi, muo) is det, in, mdi, muo) is det.
:- mode fold_triangles(pred(in, in, in, in, mdi, muo) is semidet, in, mdi, muo) is semidet.

%==============================================================================%
:- implementation.
%==============================================================================%

:- use_module bt_array.
:- use_module maybe.
:- import_module int.
:- import_module exception.

%------------------------------------------------------------------------------%

:- type key == int.

%------------------------------------------------------------------------------%

:- type key(T) ---> key(key).

%------------------------------------------------------------------------------%

:- type vertex(T) ---> vertex(v::T, edges::list.list(key)).

%------------------------------------------------------------------------------%

:- type polyhedron(T) ---> polyhedron(vertices::bt_array.bt_array(vertex(T))).

%------------------------------------------------------------------------------%

:- func mk_key(key) = key(_T).
mk_key(K) = key(K).

:- pragma inline(mk_key/1).

%------------------------------------------------------------------------------%

:- pred bt_elem(bt_array.bt_array(T)::in, key::in, T::out) is det.
bt_elem(B, K, bt_array.elem(K, B)).

:- pragma inline(bt_elem/3).

%------------------------------------------------------------------------------%

:- pred match_vertex(T::in, vertex(T)::in, list(key)::out) is semidet.

match_vertex(V ^ v, V, V ^ edges).

:- pragma inline(match_vertex/3).

%------------------------------------------------------------------------------%

:- pred search(pred(T, A), bt_array.bt_array(T), key, A, key).
:- mode search(pred(in, out) is semidet, in, in, out, out) is semidet.

search(Pred, B, N, Out, NOut) :-
    bt_array.max(B) =< N,
    bt_array.elem(N, B) = T,
    ( Pred(T, O) -> O = Out, N = NOut ; search(Pred, B, N+1, Out, NOut) ).

%------------------------------------------------------------------------------%

:- pred search(pred(vertex(T), A), polyhedron(T), A, key).
:- mode search(pred(in, out) is semidet, in, out, out) is semidet.

search(Pred, polyhedron(B), Out, NOut) :-
    search(Pred, B, bt_array.min(B), Out, NOut).

%------------------------------------------------------------------------------%

:- pred insert(polyhedron(T)::in, T::in, polyhedron(T)::out, key::uo) is det.
insert(In, T, polyhedron(Out), Max+0) :-
    Max = bt_array.max(In ^ vertices),
    bt_array.resize(
        In ^ vertices,
        0,
        Max,
        vertex(T, [])
    ) = Out.

:- pragma inline(insert/4).

%------------------------------------------------------------------------------%

init = polyhedron(bt_array.make_empty_array(0)).

%------------------------------------------------------------------------------%

add_vertex(T, In, Out, Key) :-
    ( if
        value(T, In, SemiDetKey)
    then
        In = Out, SemiDetKey = Key
    else
        det_insert_vertex(T, In, Out, Key)
    ).

%------------------------------------------------------------------------------%

insert_vertex(T, In, Out, key(Key)) :-
    insert(In, T, Out, Key),
    % True iff there is no matching vertex
    (not value(T, In, _)).

%------------------------------------------------------------------------------%

det_insert_vertex(T, In, Out, key(Key)) :-
    insert(In, T, Out, Key),
    % Throw if there is a matching vertex
    ( value(T, In, _) -> throw(software_error("Inserting duplicate vertex")) ; true ).

%------------------------------------------------------------------------------%

add_edge(K1, K2, In, Out) :-
    ( insert_edge(K1, K2, In, Mid) -> Mid = Out ; In = Out ).

%------------------------------------------------------------------------------%

insert_edge(key(K1), key(K2), polyhedron(In), polyhedron(Out)) :-
    ( if
        bt_array.semidet_lookup(In, K1, vertex(V1, E1)),
        bt_array.semidet_lookup(In, K2, vertex(V2, E2))
    then
        ( if
            list.member(K2, E1)
        then
            ( if
                list.member(K1, E2)
            then
                false
            else
                throw(software_error("Edge is missing from one of the vertices"))
            )
        else
            bt_array.set(In, K1, vertex(V1, [K2|E1]), Mid),
            bt_array.set(Mid, K2, vertex(V2, [K1|E2]), Out)
        )
    else
        throw(software_error("Key is out of range"))
    ).

%------------------------------------------------------------------------------%

det_insert_edge(K1, K2, In, Out) :-
    ( if
        insert_edge(K1, K2, In, Mid)
    then
        Mid = Out
    else
        throw(software_error("Inserting duplicate edge"))
    ).

%------------------------------------------------------------------------------%

lookup(key(K), P, bt_array.elem(K, P ^ vertices) ^ v).

%------------------------------------------------------------------------------%

value(T, P, key(K)) :-
    search(match_vertex(T), P, _, K).

%------------------------------------------------------------------------------%

det_value(T, P, Key) :-
    ( search(match_vertex(T), P, _, K) -> key(K) = Key
    ; throw(software_error("Missing vertex")) ).

%------------------------------------------------------------------------------%

collect_edges(key(K), polyhedron(B), list.map(mk_key, bt_array.elem(K, B) ^ edges)).

%------------------------------------------------------------------------------%
% Gathers vertices directly connected (one away), forming triangles
:- pred direct_connected_sorted(pred(polyhedron(T), key(T), key(T), key(T), A, A), % Fold call
    polyhedron(T), % Curried in, poly for lookup and calling the pred
    key, % Curried in, key of the original point.
    key, % Curried in, key of the point connected to the original point.
    key, % Passed by foldl
    A, A). % Our fold args
:- mode direct_connected_sorted(pred(in, in, in, in, in, out) is det, in, in, in, in, in, out) is det.
:- mode direct_connected_sorted(pred(in, in, in, in, in, out) is semidet, in, in, in, in, in, out) is semidet.
:- mode direct_connected_sorted(pred(in, in, in, in, di, uo) is det, in, in, in, in, di, uo) is det.
:- mode direct_connected_sorted(pred(in, in, in, in, mdi, muo) is det, in, in, in, in, mdi, muo) is det.
:- mode direct_connected_sorted(pred(in, in, in, in, mdi, muo) is semidet, in, in, in, in, mdi, muo) is semidet.

direct_connected_sorted(Pred, Poly, FirstK, SecondK, ThirdK, In, Out) :-
    ( if
        % First check that the indices are in strictly increasing order.
        % This prevents any duplicate triangles from being found.
        FirstK < SecondK, SecondK < ThirdK,
        % Check connectivity to ensure this is a valid triangle
        bt_array.elem(ThirdK, Poly ^ vertices) ^ edges = Connections,
        list.member(FirstK, Connections)
    then
        Pred(Poly, key(FirstK), key(SecondK), key(ThirdK), In, Out)
    else
        In = Out
    ).

:- pragma inline(direct_connected_sorted/7).

%------------------------------------------------------------------------------%
% Gathers vertices directly connected (one away), forming triangles
:- pred direct_connected(pred(polyhedron(T), key(T), key(T), key(T), A, A), % Fold call
    polyhedron(T), % Curried in, poly for lookup and calling the pred
    key, % Curried in, key of the original point.
    key, % Curried in, key of the point connected to the original point.
    key, % Passed by foldl
    A, A). % Our fold args
:- mode direct_connected(pred(in, in, in, in, in, out) is det, in, in, in, in, in, out) is det.
:- mode direct_connected(pred(in, in, in, in, in, out) is semidet, in, in, in, in, in, out) is semidet.
:- mode direct_connected(pred(in, in, in, in, di, uo) is det, in, in, in, in, di, uo) is det.
:- mode direct_connected(pred(in, in, in, in, mdi, muo) is det, in, in, in, in, mdi, muo) is det.
:- mode direct_connected(pred(in, in, in, in, mdi, muo) is semidet, in, in, in, in, mdi, muo) is semidet.

direct_connected(Pred, Poly, FirstK, SecondK, ThirdK, In, Out) :-
    bt_array.elem(ThirdK, Poly ^ vertices) ^ edges = Connections,
    ( if
        % Check connectivity to ensure this is a valid triangle
        list.member(FirstK, Connections)
    then
        Pred(Poly, key(FirstK), key(SecondK), key(ThirdK), In, Out)
    else
        In = Out
    ).

:- pragma inline(direct_connected/7).

%------------------------------------------------------------------------------%

:- pred indirect_connected_sorted(pred(polyhedron(T), key(T), key(T), key(T), A, A), % Fold call
    polyhedron(T), % Curried in, poly for lookup and calling the pred
    key, % Curried in, key of the original point.
    key, % Passed by foldl
    A, A). % Our fold args
:- mode indirect_connected_sorted(pred(in, in, in, in, in, out) is det, in, in, in, in, out) is det.
:- mode indirect_connected_sorted(pred(in, in, in, in, in, out) is semidet, in, in, in, in, out) is semidet.
:- mode indirect_connected_sorted(pred(in, in, in, in, di, uo) is det, in, in, in, di, uo) is det.
:- mode indirect_connected_sorted(pred(in, in, in, in, mdi, muo) is det, in, in, in, mdi, muo) is det.
:- mode indirect_connected_sorted(pred(in, in, in, in, mdi, muo) is semidet, in, in, in, mdi, muo) is semidet.

indirect_connected_sorted(Pred, Poly, FirstK, SecondK, !A) :-
    bt_array.elem(SecondK, Poly ^ vertices) ^ edges = Connections,
    list.foldl(direct_connected_sorted(Pred, Poly, FirstK, SecondK), Connections, !A).

:- pragma inline(indirect_connected_sorted/6).

%------------------------------------------------------------------------------%

:- pred indirect_connected(pred(polyhedron(T), key(T), key(T), key(T), A, A), % Fold call
    polyhedron(T), % Curried in, poly for lookup and calling the pred
    key, % Curried in, key of the original point.
    key, % Passed by foldl
    A, A). % Our fold args
:- mode indirect_connected(pred(in, in, in, in, in, out) is det, in, in, in, in, out) is det.
:- mode indirect_connected(pred(in, in, in, in, in, out) is semidet, in, in, in, in, out) is semidet.
:- mode indirect_connected(pred(in, in, in, in, di, uo) is det, in, in, in, di, uo) is det.
:- mode indirect_connected(pred(in, in, in, in, mdi, muo) is det, in, in, in, mdi, muo) is det.
:- mode indirect_connected(pred(in, in, in, in, mdi, muo) is semidet, in, in, in, mdi, muo) is semidet.

indirect_connected(Pred, Poly, FirstK, SecondK, !A) :-
    bt_array.elem(SecondK, Poly ^ vertices) ^ edges = Connections,
    list.foldl(direct_connected(Pred, Poly, FirstK, SecondK), Connections, !A).

:- pragma inline(indirect_connected/6).

%------------------------------------------------------------------------------%
% pred(polyhedron(T), key(T), key(T), key(T), A, A)
triangles(Pred, key(Key), Poly, In, Out) :-
    bt_array.elem(Key, Poly ^ vertices) ^ edges = Connections,
    list.foldl(indirect_connected(Pred, Poly, Key), Connections, In, Out).

%------------------------------------------------------------------------------%
% Calls the predicate once for each triangle in the polyhedron
:- pred fold_triangles_between(pred(polyhedron(T), key(T), key(T), key(T), A, A),
    polyhedron(T),
    int, int,
    A, A).
:- mode fold_triangles_between(pred(in, in, in, in, in, out) is det, in, in, in, in, out) is det.
:- mode fold_triangles_between(pred(in, in, in, in, in, out) is semidet, in, in, in, in, out) is semidet.
:- mode fold_triangles_between(pred(in, in, in, in, di, uo) is det, in, in, in, di, uo) is det.
:- mode fold_triangles_between(pred(in, in, in, in, mdi, muo) is det, in, in, in, mdi, muo) is det.
:- mode fold_triangles_between(pred(in, in, in, in, mdi, muo) is semidet, in, in, in, mdi, muo) is semidet.

%------------------------------------------------------------------------------%
% pred(polyhedron(T), key(T), key(T), key(T), A, A)
fold_triangles_between(Pred, Poly, From, To, !A) :-
    ( if
        From < To
    then
        bt_array.elem(From, Poly ^ vertices) ^ edges = Connections,
        list.foldl(indirect_connected(Pred, Poly, From), Connections, !A),
        fold_triangles_between(Pred, Poly, From + 1, To, !A)
    else
        true
    ).

%------------------------------------------------------------------------------%

fold_triangles(Pred, Poly, !A) :-
    Poly ^ vertices = Vertices,
    fold_triangles_between(Pred, Poly, bt_array.min(Vertices), bt_array.max(Vertices), !A).