SimpleTriangleMesh.java

/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.apache.commons.geometry.euclidean.threed.mesh;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
import java.util.TreeMap;
import java.util.function.Function;
import java.util.stream.Stream;
import java.util.stream.StreamSupport;

import org.apache.commons.geometry.core.Transform;
import org.apache.commons.geometry.euclidean.internal.EuclideanUtils;
import org.apache.commons.geometry.euclidean.threed.BoundarySource3D;
import org.apache.commons.geometry.euclidean.threed.Bounds3D;
import org.apache.commons.geometry.euclidean.threed.PlaneConvexSubset;
import org.apache.commons.geometry.euclidean.threed.Planes;
import org.apache.commons.geometry.euclidean.threed.Triangle3D;
import org.apache.commons.geometry.euclidean.threed.Vector3D;
import org.apache.commons.numbers.core.Precision;

/** A simple implementation of the {@link TriangleMesh} interface. This class ensures that
 * faces always contain 3 valid references into the vertex list but does not enforce that
 * the referenced vertices are unique or that they define a triangle with non-zero size. For
 * example, a mesh could contain a face with 3 vertices that are considered equivalent by the
 * configured precision context. Attempting to call the {@link TriangleMesh.Face#getPolygon()}
 * method on such a face results in an exception. The
 * {@link TriangleMesh.Face#definesPolygon()} method can be used to determine if a face defines
 * a valid triangle.
 *
 * <p>Instances of this class are guaranteed to be immutable.</p>
 */
public final class SimpleTriangleMesh implements TriangleMesh {

    /** Vertices in the mesh. */
    private final List<Vector3D> vertices;

    /** Faces in the mesh. */
    private final List<int[]> faces;

    /** The bounds of the mesh. */
    private final Bounds3D bounds;

    /** Object used for floating point comparisons. */
    private final Precision.DoubleEquivalence precision;

    /** Construct a new instance from a vertex list and set of faces. No validation is
     * performed on the input.
     * @param vertices vertex list
     * @param faces face indices list
     * @param bounds mesh bounds
     * @param precision precision context used when creating face polygons
     */
    private SimpleTriangleMesh(final List<Vector3D> vertices, final List<int[]> faces, final Bounds3D bounds,
            final Precision.DoubleEquivalence precision) {
        this.vertices = Collections.unmodifiableList(vertices);
        this.faces = Collections.unmodifiableList(faces);
        this.bounds = bounds;
        this.precision = precision;
    }

    /** {@inheritDoc} */
    @Override
    public Iterable<Vector3D> vertices() {
        return getVertices();
    }

    /** {@inheritDoc} */
    @Override
    public List<Vector3D> getVertices() {
        return vertices;
    }

    /** {@inheritDoc} */
    @Override
    public int getVertexCount() {
        return vertices.size();
    }

    /** {@inheritDoc} */
    @Override
    public Iterable<TriangleMesh.Face> faces() {
        return () -> new FaceIterator<>(Function.identity());
    }

    /** {@inheritDoc} */
    @Override
    public List<TriangleMesh.Face> getFaces() {
        final int count = getFaceCount();

        final List<Face> faceList = new ArrayList<>(count);
        for (int i = 0; i < count; ++i) {
            faceList.add(getFace(i));
        }

        return faceList;
    }

    /** {@inheritDoc} */
    @Override
    public int getFaceCount() {
        return faces.size();
    }

    /** {@inheritDoc} */
    @Override
    public TriangleMesh.Face getFace(final int index) {
        return new SimpleTriangleFace(index, faces.get(index));
    }

    /** {@inheritDoc} */
    @Override
    public Bounds3D getBounds() {
        return bounds;
    }

    /** Get the precision context for the mesh. This context is used during construction of
     * face {@link Triangle3D} instances.
     * @return the precision context for the mesh
     */
    public Precision.DoubleEquivalence getPrecision() {
        return precision;
    }

    /** {@inheritDoc} */
    @Override
    public Stream<PlaneConvexSubset> boundaryStream() {
        return createFaceStream(Face::getPolygon);
    }

    /** {@inheritDoc} */
    @Override
    public Stream<Triangle3D> triangleStream() {
        return createFaceStream(Face::getPolygon);
    }

    /** {@inheritDoc} */
    @Override
    public SimpleTriangleMesh transform(final Transform<Vector3D> transform) {
        // only the vertices and bounds are modified; the faces are the same
        final Bounds3D.Builder boundsBuilder = Bounds3D.builder();
        final List<Vector3D> tVertices = new ArrayList<>(vertices.size());

        Vector3D tVertex;
        for (final Vector3D vertex : vertices) {
            tVertex = transform.apply(vertex);

            boundsBuilder.add(tVertex);
            tVertices.add(tVertex);
        }

        final Bounds3D tBounds = boundsBuilder.hasBounds() ?
                boundsBuilder.build() :
                null;

        return new SimpleTriangleMesh(tVertices, faces, tBounds, precision);
    }

    /** Return this instance if the given precision context is equal to the current precision context.
     * Otherwise, create a new mesh with the given precision context but the same vertices, faces, and
     * bounds.
     * @param meshPrecision precision context to use when generating face polygons
     * @return a mesh instance with the given precision context and the same mesh structure as the current
     *      instance
     */
    @Override
    public SimpleTriangleMesh toTriangleMesh(final Precision.DoubleEquivalence meshPrecision) {
        if (this.precision.equals(meshPrecision)) {
            return this;
        }

        return new SimpleTriangleMesh(vertices, faces, bounds, meshPrecision);
    }

    /** {@inheritDoc} */
    @Override
    public String toString() {
        final StringBuilder sb = new StringBuilder();
        sb.append(getClass().getSimpleName())
            .append("[vertexCount= ")
            .append(getVertexCount())
            .append(", faceCount= ")
            .append(getFaceCount())
            .append(", bounds= ")
            .append(getBounds())
            .append(']');

        return sb.toString();
    }

    /** Create a stream containing the results of applying {@code fn} to each face in
     * the mesh.
     * @param <T> Stream element type
     * @param fn function used to extract the stream values from each face
     * @return a stream containing the results of applying {@code fn} to each face in
     *      the mesh
     */
    private <T> Stream<T> createFaceStream(final Function<TriangleMesh.Face, T> fn) {
        final Iterable<T> iterable = () -> new FaceIterator<>(fn);
        return StreamSupport.stream(iterable.spliterator(), false);
    }

    /** Return a builder for creating new triangle mesh objects.
     * @param precision precision object used for floating point comparisons
     * @return a builder for creating new triangle mesh objects
     */
    public static Builder builder(final Precision.DoubleEquivalence precision) {
        return new Builder(precision);
    }

    /** Construct a new triangle mesh from the given vertices and face indices.
     * @param vertices vertices for the mesh
     * @param faces face indices for the mesh
     * @param precision precision context used for floating point comparisons
     * @return a new triangle mesh instance
     * @throws IllegalArgumentException if any of the face index arrays does not have exactly 3 elements or
     *       if any index is not a valid index into the vertex list
     */
    public static SimpleTriangleMesh from(final Vector3D[] vertices, final int[][] faces,
                                          final Precision.DoubleEquivalence precision) {
        return from(Arrays.asList(vertices), Arrays.asList(faces), precision);
    }

    /** Construct a new triangle mesh from the given vertices and face indices.
     * @param vertices vertices for the mesh
     * @param faces face indices for the mesh
     * @param precision precision context used for floating point comparisons
     * @return a new triangle mesh instance
     * @throws IllegalArgumentException if any of the face index arrays does not have exactly 3 elements or
     *       if any index is not a valid index into the vertex list
     */
    public static SimpleTriangleMesh from(final Collection<Vector3D> vertices, final Collection<int[]> faces,
                                          final Precision.DoubleEquivalence precision) {
        final Builder builder = builder(precision);

        return builder.addVertices(vertices)
                .addFaces(faces)
                .build();
    }

    /** Construct a new mesh instance containing all triangles from the given boundary
     * source. Equivalent vertices are reused wherever possible.
     * @param boundarySrc boundary source to construct a mesh from
     * @param precision precision context used for floating point comparisons
     * @return new mesh instance containing all triangles from the given boundary
     *      source
     * @throws IllegalStateException if any boundary in the boundary source has infinite size and cannot
     *      be converted to triangles
     */
    public static SimpleTriangleMesh from(final BoundarySource3D boundarySrc,
            final Precision.DoubleEquivalence precision) {
        final Builder builder = builder(precision);
        try (Stream<Triangle3D> stream = boundarySrc.triangleStream()) {
            stream.forEach(tri -> builder.addFaceUsingVertices(
                tri.getPoint1(),
                tri.getPoint2(),
                tri.getPoint3()));
        }

        return builder.build();
    }

    /** Internal implementation of {@link TriangleMesh.Face}.
     */
    private final class SimpleTriangleFace implements TriangleMesh.Face {

        /** The index of the face in the mesh. */
        private final int index;

        /** Vertex indices for the face. */
        private final int[] vertexIndices;

        SimpleTriangleFace(final int index, final int[] vertexIndices) {
            this.index = index;
            this.vertexIndices = vertexIndices;
        }

        /** {@inheritDoc} */
        @Override
        public int getIndex() {
            return index;
        }

        /** {@inheritDoc} */
        @Override
        public int[] getVertexIndices() {
            return vertexIndices.clone();
        }

        /** {@inheritDoc} */
        @Override
        public List<Vector3D> getVertices() {
            return Arrays.asList(
                    getPoint1(),
                    getPoint2(),
                    getPoint3());
        }

        /** {@inheritDoc} */
        @Override
        public Vector3D getPoint1() {
            return vertices.get(vertexIndices[0]);
        }

        /** {@inheritDoc} */
        @Override
        public Vector3D getPoint2() {
            return vertices.get(vertexIndices[1]);
        }

        /** {@inheritDoc} */
        @Override
        public Vector3D getPoint3() {
            return vertices.get(vertexIndices[2]);
        }

        /** {@inheritDoc} */
        @Override
        public boolean definesPolygon() {
            final Vector3D p1 = getPoint1();
            final Vector3D v1 = p1.vectorTo(getPoint2());
            final Vector3D v2 = p1.vectorTo(getPoint3());

            return !precision.eqZero(v1.cross(v2).norm());
        }

        /** {@inheritDoc} */
        @Override
        public Triangle3D getPolygon() {
            return Planes.triangleFromVertices(
                    getPoint1(),
                    getPoint2(),
                    getPoint3(),
                    precision);
        }

        /** {@inheritDoc} */
        @Override
        public String toString() {
            final StringBuilder sb = new StringBuilder();
            sb.append(getClass().getSimpleName())
                .append("[index= ")
                .append(getIndex())
                .append(", vertexIndices= ")
                .append(Arrays.toString(getVertexIndices()))
                .append(", vertices= ")
                .append(getVertices())
                .append(']');

            return sb.toString();
        }
    }

    /** Internal class for iterating through the mesh faces and extracting a value from each.
     * @param <T> Type returned by the iterator
     */
    private final class FaceIterator<T> implements Iterator<T> {

        /** The current index of the iterator. */
        private int index;

        /** Function to apply to each face in the mesh. */
        private final Function<? super TriangleMesh.Face, T> fn;

        /** Construct a new instance for iterating through the mesh faces and extracting
         * a value from each.
         * @param fn function to apply to each face in order to obtain the iterated value
         */
        FaceIterator(final Function<? super TriangleMesh.Face, T> fn) {
            this.fn = fn;
        }

        /** {@inheritDoc} */
        @Override
        public boolean hasNext() {
            return index < faces.size();
        }

        /** {@inheritDoc} */
        @Override
        public T next() {
            if (hasNext()) {
                final Face face = getFace(index++);
                return fn.apply(face);
            }
            throw new NoSuchElementException();
        }
    }

    /** Builder class for creating mesh instances.
     */
    public static final class Builder {

        /** List of vertices. */
        private final ArrayList<Vector3D> vertices = new ArrayList<>();

        /** Map of vertices to their first occurrence in the vertex list. */
        private Map<Vector3D, Integer> vertexIndexMap;

        /** List of face vertex indices. */
        private final ArrayList<int[]> faces = new ArrayList<>();

        /** Object used to construct the 3D bounds of the vertex list. */
        private final Bounds3D.Builder boundsBuilder = Bounds3D.builder();

        /** Precision context used for floating point comparisons; this value may be null
         * if vertices are not to be combined in this builder.
         */
        private final Precision.DoubleEquivalence precision;

        /** Flag set to true once a mesh is constructed from this builder. */
        private boolean built;

        /** Construct a new builder.
         * @param precision precision context used for floating point comparisons; may
         *      be null if vertices are not to be combined in this builder.
         */
        private Builder(final Precision.DoubleEquivalence precision) {
            Objects.requireNonNull(precision, "Precision context must not be null");

            this.precision = precision;
        }

        /** Use a vertex in the constructed mesh. If an equivalent vertex already exist, as determined
         * by the configured {@link Precision.DoubleEquivalence}, then the index of the previously added
         * vertex is returned. Otherwise, the given vertex is added to the vertex list and the index
         * of the new entry is returned. This is in contrast with the {@link #addVertex(Vector3D)},
         * which always adds a new entry to the vertex list.
         * @param vertex vertex to use
         * @return the index of the added vertex or an equivalent vertex that was added previously
         * @see #addVertex(Vector3D)
         */
        public int useVertex(final Vector3D vertex) {
            final int nextIdx = vertices.size();
            final int actualIdx = addToVertexIndexMap(vertex, nextIdx, getVertexIndexMap());

            // add to the vertex list if not already present
            if (actualIdx == nextIdx) {
                addToVertexList(vertex);
            }

            return actualIdx;
        }

        /** Add a vertex directly to the vertex list, returning the index of the added vertex.
         * The vertex is added regardless of whether or not an equivalent vertex already
         * exists in the list. This is in contrast with the {@link #useVertex(Vector3D)} method,
         * which only adds a new entry to the vertex list if an equivalent one does not
         * already exist.
         * @param vertex the vertex to append
         * @return the index of the appended vertex in the vertex list
         */
        public int addVertex(final Vector3D vertex) {
            final int idx = addToVertexList(vertex);

            if (vertexIndexMap != null) {
                // add to the map in order to keep it in sync
                addToVertexIndexMap(vertex, idx, vertexIndexMap);
            }

            return idx;
        }

        /** Add a group of vertices directly to the vertex list. No equivalent vertices are reused.
         * @param newVertices vertices to append
         * @return this instance
         * @see #addVertex(Vector3D)
         */
        public Builder addVertices(final Vector3D[] newVertices) {
            return addVertices(Arrays.asList(newVertices));
        }

        /** Add a group of vertices directly to the vertex list. No equivalent vertices are reused.
         * @param newVertices vertices to append
         * @return this instance
         * @see #addVertex(Vector3D)
         */
        public Builder addVertices(final Collection<? extends Vector3D> newVertices) {
            final int newSize = vertices.size() + newVertices.size();
            ensureVertexCapacity(newSize);

            for (final Vector3D vertex : newVertices) {
                addVertex(vertex);
            }

            return this;
        }

        /** Ensure that this instance has enough capacity to store at least {@code numVertices}
         * number of vertices without reallocating space. This can be used to help improve performance
         * and memory usage when creating meshes with large numbers of vertices.
         * @param numVertices the number of vertices to ensure that this instance can contain
         * @return this instance
         */
        public Builder ensureVertexCapacity(final int numVertices) {
            vertices.ensureCapacity(numVertices);
            return this;
        }

        /** Get the current number of vertices in this mesh.
         * @return the current number of vertices in this mesh
         */
        public int getVertexCount() {
            return vertices.size();
        }

        /** Get the vertex at the given index.
         * @param index index of the vertex to retrieve
         * @return vertex at the given index
         * @throws IndexOutOfBoundsException if the index is out of bounds of the mesh vertex list
         */
        public Vector3D getVertex(final int index) {
            return vertices.get(index);
        }

        /** Append a face to this mesh.
         * @param index1 index of the first vertex in the face
         * @param index2 index of the second vertex in the face
         * @param index3 index of the third vertex in the face
         * @return this instance
         * @throws IllegalArgumentException if any of the arguments is not a valid index into
         *      the current vertex list
         */
        public Builder addFace(final int index1, final int index2, final int index3) {
            validateCanModify();

            final int[] indices = {
                validateVertexIndex(index1),
                validateVertexIndex(index2),
                validateVertexIndex(index3)
            };

            faces.add(indices);

            return this;
        }

        /** Append a face to this mesh.
         * @param face array containing the 3 vertex indices defining the face
         * @return this instance
         * @throws IllegalArgumentException if {@code face} does not contain exactly 3 elements
         *      or if any of the vertex indices is not a valid index into the current vertex list
         */
        public Builder addFace(final int[] face) {
            if (face.length != EuclideanUtils.TRIANGLE_VERTEX_COUNT) {
                throw new IllegalArgumentException("Face must contain " + EuclideanUtils.TRIANGLE_VERTEX_COUNT +
                        " vertex indices; found " + face.length);
            }

            addFace(face[0], face[1], face[2]);

            return this;
        }

        /** Append a group of faces to this mesh.
         * @param faceIndices faces to append
         * @return this instance
         * @throws IllegalArgumentException if any of the face index arrays does not have exactly 3 elements or
         *       if any index is not a valid index into the current vertex list
         */
        public Builder addFaces(final int[][] faceIndices) {
            return addFaces(Arrays.asList(faceIndices));
        }

        /** Append a group of faces to this mesh.
         * @param faceIndices faces to append
         * @return this instance
         * @throws IllegalArgumentException if any of the face index arrays does not have exactly 3 elements or
         *       if any index is not a valid index into the current vertex list
         */
        public Builder addFaces(final Collection<int[]> faceIndices) {
            final int newSize = faces.size() + faceIndices.size();
            ensureFaceCapacity(newSize);

            for (final int[] face : faceIndices) {
                addFace(face);
            }

            return this;
        }

        /** Add a face to this mesh, only adding vertices to the vertex list if equivalent vertices are
         * not found.
         * @param p1 first face vertex
         * @param p2 second face vertex
         * @param p3 third face vertex
         * @return this instance
         * @see #useVertex(Vector3D)
         */
        public Builder addFaceUsingVertices(final Vector3D p1, final Vector3D p2, final Vector3D p3) {
            return addFace(
                        useVertex(p1),
                        useVertex(p2),
                        useVertex(p3)
                    );
        }

        /** Add a face and its vertices to this mesh. The vertices are always added to the vertex list,
         * regardless of whether or not equivalent vertices exist in the vertex list.
         * @param p1 first face vertex
         * @param p2 second face vertex
         * @param p3 third face vertex
         * @return this instance
         * @see #addVertex(Vector3D)
         */
        public Builder addFaceAndVertices(final Vector3D p1, final Vector3D p2, final Vector3D p3) {
            return addFace(
                        addVertex(p1),
                        addVertex(p2),
                        addVertex(p3)
                    );
        }

        /** Ensure that this instance has enough capacity to store at least {@code numFaces}
         * number of faces without reallocating space. This can be used to help improve performance
         * and memory usage when creating meshes with large numbers of faces.
         * @param numFaces the number of faces to ensure that this instance can contain
         * @return this instance
         */
        public Builder ensureFaceCapacity(final int numFaces) {
            faces.ensureCapacity(numFaces);
            return this;
        }

        /** Get the current number of faces in this mesh.
         * @return the current number of faces in this meshr
         */
        public int getFaceCount() {
            return faces.size();
        }

        /** Build a triangle mesh containing the vertices and faces in this builder.
         * @return a triangle mesh containing the vertices and faces in this builder
         */
        public SimpleTriangleMesh build() {
            built = true;

            final Bounds3D bounds = boundsBuilder.hasBounds() ?
                    boundsBuilder.build() :
                    null;

            vertices.trimToSize();
            faces.trimToSize();

            return new SimpleTriangleMesh(
                    vertices,
                    faces,
                    bounds,
                    precision);
        }

        /** Get the vertex index map, creating and initializing it if needed.
         * @return the vertex index map
         */
        private Map<Vector3D, Integer> getVertexIndexMap() {
            if (vertexIndexMap == null) {
                vertexIndexMap = new TreeMap<>(new FuzzyVectorComparator(precision));

                // populate the index map
                final int size = vertices.size();
                for (int i = 0; i < size; ++i) {
                    addToVertexIndexMap(vertices.get(i), i, vertexIndexMap);
                }
            }
            return vertexIndexMap;
        }

        /** Add a vertex to the given vertex index map. The vertex is inserted and mapped to {@code targetidx}
         *  if an equivalent vertex does not already exist. The index now associated with the given vertex
         *  or its equivalent is returned.
         * @param vertex vertex to add
         * @param targetIdx the index to associate with the vertex if no equivalent vertex has already been
         *      mapped
         * @param map vertex index map
         * @return the index now associated with the given vertex or its equivalent
         */
        private int addToVertexIndexMap(final Vector3D vertex, final int targetIdx,
                final Map<? super Vector3D, Integer> map) {
            validateCanModify();

            final Integer actualIdx = map.putIfAbsent(vertex, targetIdx);

            return actualIdx != null ?
                    actualIdx :
                    targetIdx;
        }

        /** Append the given vertex to the end of the vertex list. The index of the vertex is returned.
         * @param vertex the vertex to append
         * @return the index of the appended vertex
         */
        private int addToVertexList(final Vector3D vertex) {
            validateCanModify();

            boundsBuilder.add(vertex);

            final int idx = vertices.size();
            vertices.add(vertex);

            return idx;
        }

        /** Throw an exception if the given vertex index is not valid.
         * @param idx vertex index to validate
         * @return the validated index
         * @throws IllegalArgumentException if the given index is not a valid index into
         *      the vertices list
         */
        private int validateVertexIndex(final int idx) {
            if (idx < 0 || idx >= vertices.size()) {
                throw new IllegalArgumentException("Invalid vertex index: " + idx);
            }

            return idx;
        }

        /** Throw an exception if the builder has been used to construct a mesh instance
         * and can no longer be modified.
         */
        private void validateCanModify() {
            if (built) {
                throw new IllegalStateException("Builder instance cannot be modified: mesh construction is complete");
            }
        }
    }

    /** Comparator used to sort vectors using non-strict ("fuzzy") comparisons.
     * Vectors are considered equal if their values in all coordinate dimensions
     * are equivalent as evaluated by the precision context.
     */
    private static final class FuzzyVectorComparator implements Comparator<Vector3D> {
        /** Precision context to determine floating-point equality. */
        private final Precision.DoubleEquivalence precision;

        /** Construct a new instance that uses the given precision context for
         * floating point comparisons.
         * @param precision precision context used for floating point comparisons
         */
        FuzzyVectorComparator(final Precision.DoubleEquivalence precision) {
            this.precision = precision;
        }

        /** {@inheritDoc} */
        @Override
        public int compare(final Vector3D a, final Vector3D b) {
            int result = precision.compare(a.getX(), b.getX());
            if (result == 0) {
                result = precision.compare(a.getY(), b.getY());
                if (result == 0) {
                    result = precision.compare(a.getZ(), b.getZ());
                }
            }

            return result;
        }
    }
}