vecg.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_VECG
#define GEOGRAM_BASIC_VECG
 
#include <geogram/basic/common.h>
#include <geogram/basic/numeric.h>
#include <geogram/basic/determinant.h>
#include <geogram/basic/memory.h>
#include <geogram/basic/assert.h>
#include <initializer_list>
 
#include <iostream>
#include <cfloat>
#include <cmath>
 
namespace GEO {
 
    template <index_t DIM, class T>
    class vecng {
    public:
        static constexpr index_t dim = DIM;
 
        typedef vecng<DIM, T> vector_type;
 
        typedef T value_type;
 
        vecng() {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] = T(0);
            }
        }
 
        // This one should never be called :
        // a template constructor cannot be a copy constructor
 
        template <class T2>
        explicit vecng(const vecng<DIM, T2>& v) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] = T(v[i]);
            }
        }
 
        // to avoid compilation problems
        template <class T2, index_t DIM2>
        explicit vecng(
            const vecng<DIM2, T2>& v
        ) {
            geo_debug_assert(DIM2 == DIM);
            for(index_t i = 0; i < DIM; i++) {
                data_[i] = T(v[i]);
            }
        }
 
        template <class T2>
        explicit vecng(const T2* v) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] = T(v[i]);
            }
        }
 
        vecng(const std::initializer_list<T>& Vi) {
            index_t i = 0;
            for(auto& it: Vi) {
                geo_debug_assert(i < DIM);
                data()[i] = it;
                ++i;
            }
        }
 
        index_t dimension() const {
            return DIM;
        }
 
        T* data() {
            return data_;
        }
 
        const T* data() const {
            return data_;
        }
 
        inline T& operator[] (index_t i) {
            geo_debug_assert(i < DIM);
            return data()[i];
        }
 
        inline const T& operator[] (index_t i) const {
            geo_debug_assert(i < DIM);
            return data()[i];
        }
 
        inline T length2() const {
            T result = T(0);
            for(index_t i = 0; i < DIM; i++) {
                result += data_[i] * data_[i];
            }
            return result;
        }
 
        inline T length() const {
            return sqrt(length2());
        }
 
        inline T distance2(const vector_type& v) const {
            T result(0);
            for(index_t i = 0; i < DIM; i++) {
                result += geo_sqr(v.data_[i] - data_[i]);
            }
            return result;
        }
 
        inline T distance(const vector_type& v) const {
            return sqrt(distance2(v));
        }
 
        // operators
 
        inline vector_type& operator+= (const vector_type& v) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] += v.data_[i];
            }
            return *this;
        }
 
        inline vector_type& operator-= (const vector_type& v) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] -= v.data_[i];
            }
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator*= (T2 s) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] *= T(s);
            }
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator/= (T2 s) {
            for(index_t i = 0; i < DIM; i++) {
                data_[i] /= T(s);
            }
            return *this;
        }
 
        inline vector_type operator+ (const vector_type& v) const {
            vector_type result(*this);
            for(index_t i = 0; i < DIM; i++) {
                result.data_[i] += v.data_[i];
            }
            return result;
        }
 
        inline vector_type operator- (const vector_type& v) const {
            vector_type result(*this);
            for(index_t i = 0; i < DIM; i++) {
                result.data_[i] -= v.data_[i];
            }
            return result;
        }
 
        template <class T2>
        inline vector_type operator* (T2 s) const {
            vector_type result(*this);
            for(index_t i = 0; i < DIM; i++) {
                result.data_[i] *= T(s);
            }
            return result;
        }
 
        template <class T2>
        inline vector_type operator/ (T2 s) const {
            vector_type result(*this);
            for(index_t i = 0; i < DIM; i++) {
                result.data_[i] /= T(s);
            }
            return result;
        }
 
        inline vector_type operator- () const {
            vector_type result;
            for(index_t i = 0; i < DIM; i++) {
                result.data_[i] = -data_[i];
            }
            return result;
        }
 
    private:
        T data_[DIM];
    };
 
    template <index_t DIM, class T>
    inline T dot(
        const vecng<DIM, T>& v1, const vecng<DIM, T>& v2
    ) {
        T result = 0;
        for(index_t i = 0; i < DIM; i++) {
            result += v1[i] * v2[i];
        }
        return result;
    }
 
    template <class T2, index_t DIM, class T>
    inline vecng<DIM, T> operator* (
        T2 s, const vecng<DIM, T>& v
    ) {
        vecng<DIM, T> result;
        for(index_t i = 0; i < DIM; i++) {
            result[i] = T(s) * v[i];
        }
        return result;
    }
 
    // Compatibility with GLSL
 
    template <index_t DIM, class T>
    inline T length(const vecng<DIM, T>& v) {
        return v.length();
    }
 
    template <index_t DIM, class T>
    inline T length2(const vecng<DIM, T>& v) {
        return v.length2();
    }
 
    template <index_t DIM, class T>
    inline T distance2(
        const vecng<DIM, T>& v1, const vecng<DIM, T>& v2
    ) {
        return v2.distance2(v1);
    }
 
    template <index_t DIM, class T>
    inline T distance(
        const vecng<DIM, T>& v1, const vecng<DIM, T>& v2
    ) {
        return v2.distance(v1);
    }
 
    template <index_t DIM, class T>
    inline vecng<DIM, T> normalize(
        const vecng<DIM, T>& v
    ) {
        T s = length(v);
        if(s > 1e-30) {
            s = T(1) / s;
        }
        return s * v;
    }
 
    template <index_t DIM, class T>
    inline vecng<DIM, T> mix(
        const vecng<DIM, T>& v1, const vecng<DIM, T>& v2, T s
    ) {
        return (T(1) - s) * v1 + s * v2;
    }
 
    /************************************************************************/
 
    template <class T>
    class vecng<2, T> {
    public:
        static constexpr index_t dim = 2;
 
        typedef vecng<dim, T> vector_type;
 
        typedef T value_type;
 
        vecng() :
            x(0),
            y(0) {
        }
 
        vecng(const T& x_in, const T& y_in) :
            x(x_in),
            y(y_in) {
        }
 
        vecng(T&& x_in, T&& y_in) :
            x(x_in),
            y(y_in) {
        }
 
        vecng(const vecng<2,T>& rhs) = default;
        vecng(vecng<2,T>&& rhs) = default;
 
        template <class T2>
        explicit vecng(const vecng<dim, T2>& v) :
            x(v.x),
            y(v.y) {
        }
 
        template <class T2>
        explicit vecng(const T2* v) :
            x(v[0]),
            y(v[1]) {
        }
 
        vecng(const std::initializer_list<T>& Vi) {
            index_t i = 0;
            for(auto& it: Vi) {
                geo_debug_assert(i < dim);
                data()[i] = it;
                ++i;
            }
        }
 
        vecng<2,T>& operator=(const vecng<2,T>& rhs) = default;
        vecng<2,T>& operator=(vecng<2,T>&& rhs) = default;
 
        inline T length2() const {
            return x * x + y * y;
        }
 
        inline T length() const {
            return sqrt(x * x + y * y);
        }
 
        inline T distance2(const vector_type& v) const {
            T dx = v.x - x;
            T dy = v.y - y;
            return dx * dx + dy * dy;
        }
 
        inline T distance(const vector_type& v) const {
            return sqrt(distance2(v));
        }
 
        inline vector_type& operator+= (const vector_type& v) {
            x += v.x;
            y += v.y;
            return *this;
        }
 
        inline vector_type& operator-= (const vector_type& v) {
            x -= v.x;
            y -= v.y;
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator*= (T2 s) {
            x *= T(s);
            y *= T(s);
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator/= (T2 s) {
            x /= T(s);
            y /= T(s);
            return *this;
        }
 
        inline vector_type operator+ (const vector_type& v) const {
            return vector_type(x + v.x, y + v.y);
        }
 
        inline vector_type operator- (const vector_type& v) const {
            return vector_type(x - v.x, y - v.y);
        }
 
        template <class T2>
        inline vector_type operator* (T2 s) const {
            return vector_type(x * T(s), y * T(s));
        }
 
        template <class T2>
        inline vector_type operator/ (T2 s) const {
            return vector_type(x / T(s), y / T(s));
        }
 
        inline vector_type operator- () const {
            return vector_type(-x, -y);
        }
 
        index_t dimension() const {
            return dim;
        }
 
        T* data() {
            return &x;
        }
 
        const T* data() const {
            return &x;
        }
 
        inline T& operator[] (index_t i) {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        inline const T& operator[] (index_t i) const {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        void optimize() {
            Numeric::optimize_number_representation(x);
            Numeric::optimize_number_representation(y);
        }
 
        T x;
        T y;
    };
 
    template <class T>
    inline T dot(
        const vecng<2, T>& v1, const vecng<2, T>& v2
    ) {
        return v1.x * v2.x + v1.y * v2.y;
    }
 
    template <class T>
    inline T det(
        const vecng<2, T>& v1, const vecng<2, T>& v2
    ) {
        return v1.x * v2.y - v1.y * v2.x;
    }
 
    template <class T2, class T>
    inline vecng<2, T> operator* (
        T2 s, const vecng<2, T>& v
    ) {
        return vecng<2, T>(T(s) * v.x, T(s) * v.y);
    }
 
    /************************************************************************/
 
    template <class T>
    class vecng<3, T> {
    public:
        static constexpr index_t dim = 3;
 
        typedef vecng<dim, T> vector_type;
 
        typedef T value_type;
 
        vecng() :
            x(T(0.0)),
            y(T(0.0)),
            z(T(0.0)) {
        }
 
        vecng(const T& x_in, const T& y_in, const T& z_in) :
            x(x_in),
            y(y_in),
            z(z_in) {
        }
 
        vecng(T&& x_in, T&& y_in, T&& z_in) :
            x(x_in),
            y(y_in),
            z(z_in) {
        }
 
        vecng(const vecng<3,T>& rhs) = default;
        vecng(vecng<3,T>&& rhs) = default;
 
        template <class T2>
        explicit vecng(const vecng<dim, T2>& v) :
            x(v.x),
            y(v.y),
            z(v.z) {
        }
 
        template <class T2>
        explicit vecng(const T2* v) :
            x(v[0]),
            y(v[1]),
            z(v[2]) {
        }
 
        vecng(const std::initializer_list<T>& Vi) {
            index_t i = 0;
            for(auto& it: Vi) {
                geo_debug_assert(i < dim);
                data()[i] = it;
                ++i;
            }
        }
 
        vecng<3,T>& operator=(const vecng<3,T>& rhs) = default;
        vecng<3,T>& operator=(vecng<3,T>&& rhs) = default;
 
        inline T length2() const {
            return x * x + y * y + z * z;
        }
 
        inline T length() const {
            return sqrt(x * x + y * y + z * z);
        }
 
        inline T distance2(const vector_type& v) const {
            T dx = v.x - x;
            T dy = v.y - y;
            T dz = v.z - z;
            return dx * dx + dy * dy + dz * dz;
        }
 
        inline T distance(const vector_type& v) const {
            return sqrt(distance2(v));
        }
 
        inline vector_type& operator+= (const vector_type& v) {
            x += v.x;
            y += v.y;
            z += v.z;
            return *this;
        }
 
        inline vector_type& operator-= (const vector_type& v) {
            x -= v.x;
            y -= v.y;
            z -= v.z;
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator*= (T2 s) {
            x *= T(s);
            y *= T(s);
            z *= T(s);
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator/= (T2 s) {
            x /= T(s);
            y /= T(s);
            z /= T(s);
            return *this;
        }
 
        inline vector_type operator+ (const vector_type& v) const {
            return vector_type(x + v.x, y + v.y, z + v.z);
        }
 
        inline vector_type operator- (const vector_type& v) const {
            return vector_type(x - v.x, y - v.y, z - v.z);
        }
 
        template <class T2>
        inline vector_type operator* (T2 s) const {
            return vector_type(x * T(s), y * T(s), z * T(s));
        }
 
        template <class T2>
        inline vector_type operator/ (T2 s) const {
            return vector_type(x / T(s), y / T(s), z / T(s));
        }
 
        inline vector_type operator- () const {
            return vector_type(-x, -y, -z);
        }
 
        index_t dimension() const {
            return dim;
        }
 
        T* data() {
            return &x;
        }
 
        const T* data() const {
            return &x;
        }
 
        inline T& operator[] (index_t i) {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        inline const T& operator[] (index_t i) const {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        void optimize() {
            Numeric::optimize_number_representation(x);
            Numeric::optimize_number_representation(y);
            Numeric::optimize_number_representation(z);
        }
 
        T x;
        T y;
        T z;
    };
 
    template <class T>
    inline T dot(
        const vecng<3, T>& v1, const vecng<3, T>& v2
    ) {
        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
    }
 
    template <class T>
    inline vecng<3, T> cross(
        const vecng<3, T>& v1, const vecng<3, T>& v2
    ) {
        return vecng<3, T>(
            det2x2(v1.y, v2.y, v1.z, v2.z),
            det2x2(v1.z, v2.z, v1.x, v2.x),
            det2x2(v1.x, v2.x, v1.y, v2.y)
        );
    }
 
    template <class T2, class T>
    inline vecng<3, T> operator* (
        T2 s, const vecng<3, T>& v
    ) {
        return vecng<3, T>(T(s) * v.x, T(s) * v.y, T(s) * v.z);
    }
 
    /************************************************************************/
 
    template <class T>
    class vecng<4, T> {
    public:
        static constexpr index_t dim = 4;
 
        typedef vecng<dim, T> vector_type;
 
        typedef T value_type;
 
        vecng() :
            x(0),
            y(0),
            z(0),
            w(0) {
        }
 
        vecng(const T& x_in, const T& y_in, const T& z_in, const T& w_in) :
            x(x_in),
            y(y_in),
            z(z_in),
            w(w_in) {
        }
 
        vecng(T&& x_in, T&& y_in, T&& z_in, T&& w_in) :
            x(x_in),
            y(y_in),
            z(z_in),
            w(w_in) {
        }
 
        vecng(const vecng<4,T>& rhs) = default;
        vecng(vecng<4,T>&& rhs) = default;
 
        template <class T2>
        explicit vecng(const vecng<dim, T2>& v) :
            x(v.x),
            y(v.y),
            z(v.z),
            w(v.w) {
        }
 
        template <class T2>
        explicit vecng(const T2* v) :
            x(v[0]),
            y(v[1]),
            z(v[2]),
            w(v[3]) {
        }
 
        vecng(const std::initializer_list<T>& Vi) {
            index_t i = 0;
            for(auto& it: Vi) {
                geo_debug_assert(i < dim);
                data()[i] = it;
                ++i;
            }
        }
 
        vecng<4,T>& operator=(const vecng<4,T>& rhs) = default;
        vecng<4,T>& operator=(vecng<4,T>&& rhs) = default;
 
        inline T length2() const {
            return x * x + y * y + z * z + w * w;
        }
 
        inline T length() const {
            return sqrt(x * x + y * y + z * z + w * w);
        }
 
        inline T distance2(const vector_type& v) const {
            T dx = v.x - x;
            T dy = v.y - y;
            T dz = v.z - z;
            T dw = v.w - w;
            return dx * dx + dy * dy + dz * dz + dw * dw;
        }
 
        inline T distance(const vector_type& v) const {
            return sqrt(distance2(v));
        }
 
        index_t dimension() const {
            return dim;
        }
 
        inline vector_type& operator+= (const vector_type& v) {
            x += v.x;
            y += v.y;
            z += v.z;
            w += v.w;
            return *this;
        }
 
        inline vector_type& operator-= (const vector_type& v) {
            x -= v.x;
            y -= v.y;
            z -= v.z;
            w -= v.w;
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator*= (T2 s) {
            x *= T(s);
            y *= T(s);
            z *= T(s);
            w *= T(s);
            return *this;
        }
 
        template <class T2>
        inline vector_type& operator/= (T2 s) {
            x /= T(s);
            y /= T(s);
            z /= T(s);
            w /= T(s);
            return *this;
        }
 
        inline vector_type operator+ (const vector_type& v) const {
            return vector_type(x + v.x, y + v.y, z + v.z, w + v.w);
        }
 
        inline vector_type operator- (const vector_type& v) const {
            return vector_type(x - v.x, y - v.y, z - v.z, w - v.w);
        }
 
        template <class T2>
        inline vector_type operator* (T2 s) const {
            return vector_type(x * T(s), y * T(s), z * T(s), w * T(s));
        }
 
        template <class T2>
        inline vector_type operator/ (T2 s) const {
            return vector_type(x / T(s), y / T(s), z / T(s), w / T(s));
        }
 
        inline vector_type operator- () const {
            return vector_type(-x, -y, -z, -w);
        }
 
        T* data() {
            return &x;
        }
 
        const T* data() const {
            return &x;
        }
 
        inline T& operator[] (index_t i) {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        inline const T& operator[] (index_t i) const {
            geo_debug_assert(i < dim);
            return data()[i];
        }
 
        T x;
        T y;
        T z;
        T w;
    };
 
    template <class T>
    inline T dot(
        const vecng<4, T>& v1, const vecng<4, T>& v2
    ) {
        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
    }
 
    template <class T2, class T>
    inline vecng<4, T> operator* (
        T2 s, const vecng<4, T>& v
    ) {
        return vecng<4, T>(T(s) * v.x, T(s) * v.y, T(s) * v.z, T(s) * v.w);
    }
 
    template <index_t DIM, class T>
    inline std::ostream& operator<< (
        std::ostream& out, const GEO::vecng<DIM, T>& v
    ) {
        const char* sep = "";
        for(index_t i = 0; i < DIM; i++) {
            out << sep << v[i];
            sep = " ";
        }
        return out;
    }
 
    template <index_t DIM, class T>
    inline std::istream& operator>> (
        std::istream& in, GEO::vecng<DIM, T>& v
    ) {
        char c;
        while(isspace(in.peek())) {
            in.get(c);
        }
        if(in.peek() == '[' || in.peek() == '{') {
            in.get(c);
        }
        while(isspace(in.peek())) {
            in.get(c);
        }
        for(index_t i = 0; i < DIM; i++) {
            in >> v[i];
            while(isspace(in.peek())) {
                in.get(c);
            }
            if(in.peek() == ',') {
                in.get(c);
            }
            while(isspace(in.peek())) {
                in.get(c);
            }
        }
        if(in.peek() == ']' || in.peek() == '}') {
            in.get(c);
        }
        return in;
    }
 
    /************************************************************************/
 
    namespace Numeric {
 
        template<class T>
        inline void optimize_number_representation(
            vecng<2,T>& v
        ) {
            v.optimize();
        }
 
        template<class T>
        inline void optimize_number_representation(
            vecng<3,T>& v
        ) {
            v.optimize();
        }
 
    }
 
    /************************************************************************/
}
 
#endif