libcruft-util/geom/plane.hpp

/*
 * Licensed 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.
 *
 * Copyright 2015-2018 Danny Robson <danny@nerdcruft.net>
 */

#pragma once

#include "../point.hpp"
#include "../vector.hpp"
#include "../matrix.hpp"

namespace util::geom {
    /// represents an S dimensional plane in parametric form: ax + by + cz + d = 0
    template <size_t S, typename T>
    struct plane {
        plane () = default;
        plane (util::point<S,T> base, util::vector<S,T> normal);

        explicit plane (util::vector<S+1,T> _coefficients):
            coefficients (_coefficients)
        { ; }

        util::vector<S+1,T> coefficients;
    };


    typedef plane<2,float> plane2f;
    typedef plane<3,float> plane3f;

    inline plane3f
    make_plane (util::point3f a, util::point3f b, util::point3f c)
    {
        return plane3f (a, normalised (cross (b - a, c - a)));
    }


    ///////////////////////////////////////////////////////////////////////////
    /// returns the normal for a plane
    template <size_t S, typename T>
    util::vector<S,T>
    normal (plane<S,T> p)
    {
        return p.coefficients.template redim<S> ();
    }


    ///////////////////////////////////////////////////////////////////////////
    /// normalises a plane's parametric form
    ///
    /// useful only after manually modifying the coefficients (as in default
    /// construction and piecewise initialisation). a plane will otherwise be
    /// assumed to be in normal form.
    template <size_t S, typename T>
    plane<S,T>
    normalised (plane<S,T> p)
    {
        const auto mag = norm (normal (p));
        CHECK_NEZ (mag);
        return plane<S,T> (p.coefficients / mag);
    }


    ///////////////////////////////////////////////////////////////////////////
    /// calculates the distance from a plane to a point.
    ///
    /// positive distances are in front of the plane, negative is behind;
    ///
    /// NOTE: `distance2` is _not_ the squared distance. It is the
    /// unnormalised distance. It needs to be divided by the norm of the
    /// plane normal.
    template <size_t S, typename T>
    T
    distance2 (plane<S,T> p, point<S,T> q)
    {
        return dot (p.coefficients, q.template redim<S+1> (1));
    }


    template <size_t S, typename T>
    T
    distance2 (point<S,T> p, plane<S,T> q)
    {
        return -distance2 (q, p);
    }


    template <size_t S, typename T>
    T
    distance (plane<S,T> p, point<S,T> q)
    {
        return distance2 (p, q) / norm (normal (p));
    }


    template <size_t S, typename T>
    T
    distance (point<S,T> p, plane<S,T> q)
    {
        return -distance (q, p);
    }


    ///////////////////////////////////////////////////////////////////////////
    template <size_t S, typename T>
    auto
    furthest (plane<S,T> p, const std::vector<util::point<S,T>> &cloud)
    {
        T maxd = -INFINITY;
        auto best = cloud.begin ();

        for (auto q = cloud.begin (), last = cloud.end (); q != last; ++q) {
            if (auto d = distance (p, *q); d > maxd) {
                maxd = d;
                best = q;
            }
        }

        return std::make_tuple (best, maxd);
    }

    template <size_t S, typename T>
    auto
    furthest (plane<S,T>, std::vector<util::point<S,T>> &&) = delete;
}