geometry.h

Go to the documentation of this file.

/*
 *  Copyright (c) 2000-2022 Inria
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *  this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *  this list of conditions and the following disclaimer in the documentation
 *  and/or other materials provided with the distribution.
 *  * Neither the name of the ALICE Project-Team nor the names of its
 *  contributors may be used to endorse or promote products derived from this
 *  software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Contact: Bruno Levy
 *
 *     https://www.inria.fr/fr/bruno-levy
 *
 *     Inria,
 *     Domaine de Voluceau,
 *     78150 Le Chesnay - Rocquencourt
 *     FRANCE
 *
 */
 
#ifndef GEOGRAM_BASIC_GEOMETRY
#define GEOGRAM_BASIC_GEOMETRY
 
#include <geogram/basic/common.h>
#include <geogram/basic/matrix.h>
 
namespace GEO {
 
    /************************************************************************/
 
    typedef vecng<2, Numeric::float64> vec2;
 
    typedef vecng<3, Numeric::float64> vec3;
 
    typedef vecng<4, Numeric::float64> vec4;
 
    typedef vecng<2, Numeric::float32> vec2f;
 
    typedef vecng<3, Numeric::float32> vec3f;
 
    typedef vecng<4, Numeric::float32> vec4f;
 
 
    typedef vecng<2, Numeric::int32> vec2i;
 
    typedef vecng<3, Numeric::int32> vec3i;
 
    typedef vecng<4, Numeric::int32> vec4i;
 
 
    typedef Matrix<2, Numeric::float64> mat2;
 
    typedef Matrix<3, Numeric::float64> mat3;
 
    typedef Matrix<4, Numeric::float64> mat4;
 
    /************************************************************************/
 
    inline double det(const mat2& M) {
        return det2x2(
            M(0,0), M(0,1),
            M(1,0), M(1,1)
        );
    }
 
    inline double det(const mat3& M) {
        return det3x3(
            M(0,0), M(0,1), M(0,2),
            M(1,0), M(1,1), M(1,2),
            M(2,0), M(2,1), M(2,2)
        );
    }
 
    inline double det(const mat4& M) {
        return det4x4(
            M(0,0), M(0,1), M(0,2), M(0,3),
            M(1,0), M(1,1), M(1,2), M(1,3),
            M(2,0), M(2,1), M(2,2), M(2,3),
            M(3,0), M(3,1), M(3,2), M(3,3)
        );
    }
 
    /************************************************************************/
 
    namespace Geom {
 
        inline vec3 barycenter(const vec3& p1, const vec3& p2) {
            return vec3(
                0.5 * (p1.x + p2.x),
                0.5 * (p1.y + p2.y),
                0.5 * (p1.z + p2.z)
            );
        }
 
        inline vec2 barycenter(const vec2& p1, const vec2& p2) {
            return vec2(
                0.5 * (p1.x + p2.x),
                0.5 * (p1.y + p2.y)
            );
        }
 
        inline vec3 barycenter(
            const vec3& p1, const vec3& p2, const vec3& p3
        ) {
            return vec3(
                (p1.x + p2.x + p3.x) / 3.0,
                (p1.y + p2.y + p3.y) / 3.0,
                (p1.z + p2.z + p3.z) / 3.0
            );
        }
 
        inline vec2 barycenter(
            const vec2& p1, const vec2& p2, const vec2& p3
        ) {
            return vec2(
                (p1.x + p2.x + p3.x) / 3.0,
                (p1.y + p2.y + p3.y) / 3.0
            );
        }
 
        inline double cos_angle(const vec3& a, const vec3& b) {
            double lab = ::sqrt(length2(a)*length2(b));
            double result = (lab > 1e-50) ? (dot(a, b) / lab) : 1.0;
            // Numerical precision problem may occur, and generate
            // normalized dot products that are outside the valid
            // range of acos.
            geo_clamp(result, -1.0, 1.0);
            return result;
        }
 
        inline double angle(const vec3& a, const vec3& b) {
            return ::acos(cos_angle(a, b));
        }
 
        inline double cos_angle(const vec2& a, const vec2& b) {
            double lab = ::sqrt(length2(a)*length2(b));
            double result = (lab > 1e-20) ? (dot(a, b) / lab) : 1.0;
            // Numerical precision problem may occur, and generate
            // normalized dot products that are outside the valid
            // range of acos.
            geo_clamp(result, -1.0, 1.0);
            return result;
        }
 
        inline double det(const vec2& a, const vec2& b) {
            return a.x * b.y - a.y * b.x;
        }
 
        inline double angle(const vec2& a, const vec2& b) {
            return det(a, b) > 0 ?
                ::acos(cos_angle(a, b)) :
                -::acos(cos_angle(a, b));
        }
 
        inline vec3 triangle_normal(
            const vec3& p1, const vec3& p2, const vec3& p3
        ) {
            return cross(p2 - p1, p3 - p1);
        }
 
        inline double triangle_area_3d(
            const double* p1, const double* p2, const double* p3
        ) {
            double Ux = p2[0] - p1[0];
            double Uy = p2[1] - p1[1];
            double Uz = p2[2] - p1[2];
 
            double Vx = p3[0] - p1[0];
            double Vy = p3[1] - p1[1];
            double Vz = p3[2] - p1[2];
 
            double Nx = Uy*Vz - Uz*Vy;
            double Ny = Uz*Vx - Ux*Vz;
            double Nz = Ux*Vy - Uy*Vx;
            return 0.5 * ::sqrt(Nx*Nx+Ny*Ny+Nz*Nz);
        }
 
        inline double triangle_area(
            const vec3& p1, const vec3& p2, const vec3& p3
        ) {
            return triangle_area_3d(p1.data(), p2.data(), p3.data());
        }
 
        inline double triangle_signed_area_2d(
            const double* p1, const double* p2, const double* p3
        ) {
            double a = p2[0]-p1[0];
            double b = p3[0]-p1[0];
            double c = p2[1]-p1[1];
            double d = p3[1]-p1[1];
            return 0.5*(a*d-b*c);
        }
 
        inline double triangle_signed_area(
            const vec2& p1, const vec2& p2, const vec2& p3
        ) {
            return 0.5 * det(p2 - p1, p3 - p1);
        }
 
        inline double triangle_area(
            const vec2& p1, const vec2& p2, const vec2& p3
        ) {
            return ::fabs(triangle_signed_area(p1, p2, p3));
        }
 
        inline double triangle_area_2d(
            const double* p1, const double* p2, const double* p3
        ) {
            return ::fabs(triangle_signed_area_2d(p1,p2,p3));
        }
 
        vec2 GEOGRAM_API triangle_circumcenter(
            const vec2& p1, const vec2& p2, const vec2& p3
        );
 
        inline bool has_nan(const vec3& v) {
            return
                Numeric::is_nan(v.x) ||
                Numeric::is_nan(v.y) ||
                Numeric::is_nan(v.z);
        }
 
        inline bool has_nan(const vec2& v) {
            return
                Numeric::is_nan(v.x) ||
                Numeric::is_nan(v.y);
        }
 
        vec3 GEOGRAM_API perpendicular(const vec3& V);
 
        inline double tetra_signed_volume(
            const vec3& p1, const vec3& p2,
            const vec3& p3, const vec3& p4
        ) {
            return dot(p2 - p1, cross(p3 - p1, p4 - p1)) / 6.0;
        }
 
        inline double tetra_signed_volume(
            const double* p1, const double* p2,
            const double* p3, const double* p4
        ) {
            return tetra_signed_volume(
                *reinterpret_cast<const vec3*>(p1),
                *reinterpret_cast<const vec3*>(p2),
                *reinterpret_cast<const vec3*>(p3),
                *reinterpret_cast<const vec3*>(p4)
            );
        }
 
        inline double tetra_volume(
            const vec3& p1, const vec3& p2,
            const vec3& p3, const vec3& p4
        ) {
            return ::fabs(tetra_signed_volume(p1, p2, p3, p4));
        }
 
        vec3 GEOGRAM_API tetra_circum_center(
            const vec3& p1, const vec3& p2,
            const vec3& p3, const vec3& p4
        );
 
        inline void triangle_centroid(
            const vec3& p, const vec3& q, const vec3& r,
            double a, double b, double c,
            vec3& Vg, double& V
        ) {
            double abc = a + b + c;
            double area = Geom::triangle_area(p, q, r);
            V = area / 3.0 * abc;
            double wp = a + abc;
            double wq = b + abc;
            double wr = c + abc;
            double s = area / 12.0;
            Vg.x = s * (wp * p.x + wq * q.x + wr * r.x);
            Vg.y = s * (wp * p.y + wq * q.y + wr * r.y);
            Vg.z = s * (wp * p.z + wq * q.z + wr * r.z);
        }
 
        inline double triangle_mass(
            const vec3& p, const vec3& q, const vec3& r,
            double a, double b, double c
        ) {
            return Geom::triangle_area(p, q, r) / 3.0 * (
                sqrt(::fabs(a)) + sqrt(::fabs(b)) + sqrt(::fabs(c))
            );
        }
 
        inline vec3 random_point_in_triangle(
            const vec3& p1,
            const vec3& p2,
            const vec3& p3
        ) {
            double s = Numeric::random_float64();
            double t = Numeric::random_float64();
            if(s + t > 1) {
                s = 1.0 - s;
                t = 1.0 - t;
            }
            double u = 1.0 - s - t;
            return vec3(
                u * p1.x + s * p2.x + t * p3.x,
                u * p1.y + s * p2.y + t * p3.y,
                u * p1.z + s * p2.z + t * p3.z
            );
        }
    }
 
    struct Plane {
 
        Plane(const vec3& p1, const vec3& p2, const vec3& p3) {
            vec3 n = cross(p2 - p1, p3 - p1);
            a = n.x;
            b = n.y;
            c = n.z;
            d = -(a * p1.x + b * p1.y + c * p1.z);
        }
 
        Plane(const vec3& p, const vec3& n) {
            a = n.x;
            b = n.y;
            c = n.z;
            d = -(a * p.x + b * p.y + c * p.z);
        }
 
        Plane(
            double a_in, double b_in, double c_in, double d_in
        ) :
            a(a_in),
            b(b_in),
            c(c_in),
            d(d_in) {
        }
 
        Plane() {
        }
 
        vec3 normal() const {
            return vec3(a, b, c);
        }
 
        double a, b, c, d;
    };
 
    /*******************************************************************/
 
    class Box {
    public:
        double xyz_min[3];
        double xyz_max[3];
 
        bool contains(const vec3& b) const {
            for(coord_index_t c = 0; c < 3; ++c) {
                if(b[c] < xyz_min[c]) {
                    return false;
                }
                if(b[c] > xyz_max[c]) {
                    return false;
                }
            }
            return true;
        }
    };
 
    typedef Box Box3d;
 
    inline bool bboxes_overlap(const Box& B1, const Box& B2) {
        for(coord_index_t c = 0; c < 3; ++c) {
            if(B1.xyz_max[c] < B2.xyz_min[c]) {
                return false;
            }
            if(B1.xyz_min[c] > B2.xyz_max[c]) {
                return false;
            }
        }
        return true;
    }
 
    inline void bbox_union(Box& target, const Box& B1, const Box& B2) {
        for(coord_index_t c = 0; c < 3; ++c) {
            target.xyz_min[c] = std::min(B1.xyz_min[c], B2.xyz_min[c]);
            target.xyz_max[c] = std::max(B1.xyz_max[c], B2.xyz_max[c]);
        }
    }
 
    /*******************************************************************/
 
    class Box2d {
    public:
        double xy_min[2];
        double xy_max[2];
 
        bool contains(const vec2& b) const {
            for(coord_index_t c = 0; c < 2; ++c) {
                if(b[c] < xy_min[c]) {
                    return false;
                }
                if(b[c] > xy_max[c]) {
                    return false;
                }
            }
            return true;
        }
    };
 
 
    inline bool bboxes_overlap(const Box2d& B1, const Box2d& B2) {
        for(coord_index_t c = 0; c < 2; ++c) {
            if(B1.xy_max[c] < B2.xy_min[c]) {
                return false;
            }
            if(B1.xy_min[c] > B2.xy_max[c]) {
                return false;
            }
        }
        return true;
    }
 
    inline void bbox_union(Box2d& target, const Box2d& B1, const Box2d& B2) {
        for(coord_index_t c = 0; c < 2; ++c) {
            target.xy_min[c] = std::min(B1.xy_min[c], B2.xy_min[c]);
            target.xy_max[c] = std::max(B1.xy_max[c], B2.xy_max[c]);
        }
    }
 
    /*******************************************************************/
 
    template <class FT> vecng<3,FT> transform_vector(
        const vecng<3,FT>& v,
        const Matrix<4,FT>& m
    ){
        index_t i,j ;
        FT result[4] ;
 
        for(i=0; i<4; i++) {
            result[i] = 0 ;
        }
        for(i=0; i<4; i++) {
            for(j=0; j<3; j++) {
                result[i] += v[j] * m(j,i) ;
            }
        }
 
        return vecng<3,FT>(
            result[0], result[1], result[2]
        ) ;
    }
 
    template <class FT> vecng<3,FT> transform_point(
        const vecng<3,FT>& v,
        const Matrix<4,FT>& m
    ){
        index_t i,j ;
        FT result[4] ;
 
        for(i=0; i<4; i++) {
            result[i] = 0 ;
        }
        for(i=0; i<4; i++) {
            for(j=0; j<3; j++) {
                result[i] += v[j] * m(j,i) ;
            }
            result[i] += m(3,i);
        }
 
        return vecng<3,FT>(
            result[0] / result[3],
            result[1] / result[3],
            result[2] / result[3]
        ) ;
    }
 
 
    template <class FT> vecng<3,FT> transform_point(
        const Matrix<4,FT>& m,
        const vecng<3,FT>& v
    ){
        index_t i,j ;
        FT result[4] ;
 
        for(i=0; i<4; i++) {
            result[i] = 0 ;
        }
        for(i=0; i<4; i++) {
            for(j=0; j<3; j++) {
                result[i] += v[j] * m(i,j) ;
            }
            result[i] += m(i,3);
        }
 
        return vecng<3,FT>(
            result[0] / result[3],
            result[1] / result[3],
            result[2] / result[3]
        ) ;
    }
 
    template <class FT> vecng<4,FT> transform_vector(
        const vecng<4,FT>& v,
        const Matrix<4,FT>& m
    ) {
        index_t i,j ;
        FT res[4] = {FT(0), FT(0), FT(0), FT(0)};
 
        for(i=0; i<4; i++) {
            for(j=0; j<4; j++) {
                res[i] += v[j] * m(j,i) ;
            }
        }
 
        return vecng<4,FT>(res[0], res[1], res[2], res[3]) ;
    }
 
    /******************************************************************/
 
    inline mat4 create_translation_matrix(const vec3& T) {
        mat4 result;
        result.load_identity();
        result(3,0) = T.x;
        result(3,1) = T.y;
        result(3,2) = T.z;
        return result;
    }
 
    inline mat4 create_scaling_matrix(double s) {
        mat4 result;
        result.load_identity();
        result(0,0) = s;
        result(1,1) = s;
        result(2,2) = s;
        return result;
    }
 
    /******************************************************************/
 
    struct Ray {
        Ray(vec3 O, vec3 D) : origin(O), direction(D) {
        }
        Ray() {
        }
        vec3 origin;
        vec3 direction;
    };
 
    /******************************************************************/
 
}
 
#endif