nh-rep/code/pre_processing/ParametricSample/Mathematics/GVector.h

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2021
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.13

#pragma once

#include <Mathematics/Logger.h>
#include <Mathematics/Math.h>
#include <vector>

namespace gte
{
    template <typename Real>
    class GVector
    {
    public:
        // The tuple is length zero (uninitialized).
        GVector() = default;

        // The tuple is length 'size' and the elements are uninitialized.
        GVector(int size)
        {
            SetSize(size);
        }

        // For 0 <= d <= size, element d is 1 and all others are zero.  If d
        // is invalid, the zero vector is created.  This is a convenience for
        // creating the standard Euclidean basis vectors; see also
        // MakeUnit(int,int) and Unit(int,int).
        GVector(int size, int d)
        {
            SetSize(size);
            MakeUnit(d);
        }

        // The copy constructor, destructor, and assignment operator are
        // generated by the compiler.

        // Member access.  SetSize(int) does not initialize the tuple.  The
        // first operator[] returns a const reference rather than a Real
        // value.  This supports writing via standard file operations that
        // require a const pointer to data.
        void SetSize(int size)
        {
            LogAssert(size >= 0, "Invalid size.");
            mTuple.resize(size);
        }

        inline int GetSize() const
        {
            return static_cast<int>(mTuple.size());
        }

        inline Real const& operator[](int i) const
        {
            return mTuple[i];
        }

        inline Real& operator[](int i)
        {
            return mTuple[i];
        }

        // Comparison (for use by STL containers).
        inline bool operator==(GVector const& vec) const
        {
            return mTuple == vec.mTuple;
        }

        inline bool operator!=(GVector const& vec) const
        {
            return mTuple != vec.mTuple;
        }

        inline bool operator< (GVector const& vec) const
        {
            return mTuple < vec.mTuple;
        }

        inline bool operator<=(GVector const& vec) const
        {
            return mTuple <= vec.mTuple;
        }

        inline bool operator> (GVector const& vec) const
        {
            return mTuple > vec.mTuple;
        }

        inline bool operator>=(GVector const& vec) const
        {
            return mTuple >= vec.mTuple;
        }

        // Special vectors.

        // All components are 0.
        void MakeZero()
        {
            std::fill(mTuple.begin(), mTuple.end(), (Real)0);
        }

        // Component d is 1, all others are zero.
        void MakeUnit(int d)
        {
            std::fill(mTuple.begin(), mTuple.end(), (Real)0);
            if (0 <= d && d < (int)mTuple.size())
            {
                mTuple[d] = (Real)1;
            }
        }

        static GVector Zero(int size)
        {
            GVector<Real> v(size);
            v.MakeZero();
            return v;
        }

        static GVector Unit(int size, int d)
        {
            GVector<Real> v(size);
            v.MakeUnit(d);
            return v;
        }

    protected:
        // This data structure takes advantage of the built-in operator[],
        // range checking and visualizers in MSVS.
        std::vector<Real> mTuple;
    };

    // Unary operations.
    template <typename Real>
    GVector<Real> operator+(GVector<Real> const& v)
    {
        return v;
    }

    template <typename Real>
    GVector<Real> operator-(GVector<Real> const& v)
    {
        GVector<Real> result(v.GetSize());
        for (int i = 0; i < v.GetSize(); ++i)
        {
            result[i] = -v[i];
        }
        return result;
    }

    // Linear-algebraic operations.
    template <typename Real>
    GVector<Real> operator+(GVector<Real> const& v0, GVector<Real> const& v1)
    {
        GVector<Real> result = v0;
        return result += v1;
    }

    template <typename Real>
    GVector<Real> operator-(GVector<Real> const& v0, GVector<Real> const& v1)
    {
        GVector<Real> result = v0;
        return result -= v1;
    }

    template <typename Real>
    GVector<Real> operator*(GVector<Real> const& v, Real scalar)
    {
        GVector<Real> result = v;
        return result *= scalar;
    }

    template <typename Real>
    GVector<Real> operator*(Real scalar, GVector<Real> const& v)
    {
        GVector<Real> result = v;
        return result *= scalar;
    }

    template <typename Real>
    GVector<Real> operator/(GVector<Real> const& v, Real scalar)
    {
        GVector<Real> result = v;
        return result /= scalar;
    }

    template <typename Real>
    GVector<Real>& operator+=(GVector<Real>& v0, GVector<Real> const& v1)
    {
        if (v0.GetSize() == v1.GetSize())
        {
            for (int i = 0; i < v0.GetSize(); ++i)
            {
                v0[i] += v1[i];
            }
            return v0;
        }
        LogError("Mismatched sizes.");
    }

    template <typename Real>
    GVector<Real>& operator-=(GVector<Real>& v0, GVector<Real> const& v1)
    {
        if (v0.GetSize() == v1.GetSize())
        {
            for (int i = 0; i < v0.GetSize(); ++i)
            {
                v0[i] -= v1[i];
            }
            return v0;
        }
        LogError("Mismatched sizes.");
    }

    template <typename Real>
    GVector<Real>& operator*=(GVector<Real>& v, Real scalar)
    {
        for (int i = 0; i < v.GetSize(); ++i)
        {
            v[i] *= scalar;
        }
        return v;
    }

    template <typename Real>
    GVector<Real>& operator/=(GVector<Real>& v, Real scalar)
    {
        if (scalar != (Real)0)
        {
            Real invScalar = (Real)1 / scalar;
            for (int i = 0; i < v.GetSize(); ++i)
            {
                v[i] *= invScalar;
            }
            return v;
        }
        LogError("Division by zero.");
    }

    // Geometric operations.  The functions with 'robust' set to 'false' use
    // the standard algorithm for normalizing a vector by computing the length
    // as a square root of the squared length and dividing by it.  The results
    // can be infinite (or NaN) if the length is zero.  When 'robust' is set
    // to 'true', the algorithm is designed to avoid floating-point overflow
    // and sets the normalized vector to zero when the length is zero.
    template <typename Real>
    Real Dot(GVector<Real> const& v0, GVector<Real> const& v1)
    {
        if (v0.GetSize() == v1.GetSize())
        {
            Real dot(0);
            for (int i = 0; i < v0.GetSize(); ++i)
            {
                dot += v0[i] * v1[i];
            }
            return dot;
        }
        LogError("Mismatched sizes.");
    }

    template <typename Real>
    Real Length(GVector<Real> const& v, bool robust = false)
    {
        if (robust)
        {
            Real maxAbsComp = std::fabs(v[0]);
            for (int i = 1; i < v.GetSize(); ++i)
            {
                Real absComp = std::fabs(v[i]);
                if (absComp > maxAbsComp)
                {
                    maxAbsComp = absComp;
                }
            }

            Real length;
            if (maxAbsComp > (Real)0)
            {
                GVector<Real> scaled = v / maxAbsComp;
                length = maxAbsComp * std::sqrt(Dot(scaled, scaled));
            }
            else
            {
                length = (Real)0;
            }
            return length;
        }
        else
        {
            return std::sqrt(Dot(v, v));
        }
    }

    template <typename Real>
    Real Normalize(GVector<Real>& v, bool robust = false)
    {
        if (robust)
        {
            Real maxAbsComp = std::fabs(v[0]);
            for (int i = 1; i < v.GetSize(); ++i)
            {
                Real absComp = std::fabs(v[i]);
                if (absComp > maxAbsComp)
                {
                    maxAbsComp = absComp;
                }
            }

            Real length;
            if (maxAbsComp > (Real)0)
            {
                v /= maxAbsComp;
                length = std::sqrt(Dot(v, v));
                v /= length;
                length *= maxAbsComp;
            }
            else
            {
                length = (Real)0;
                for (int i = 0; i < v.GetSize(); ++i)
                {
                    v[i] = (Real)0;
                }
            }
            return length;
        }
        else
        {
            Real length = std::sqrt(Dot(v, v));
            if (length > (Real)0)
            {
                v /= length;
            }
            else
            {
                for (int i = 0; i < v.GetSize(); ++i)
                {
                    v[i] = (Real)0;
                }
            }
            return length;
        }
    }

    // Gram-Schmidt orthonormalization to generate orthonormal vectors from
    // the linearly independent inputs.  The function returns the smallest
    // length of the unnormalized vectors computed during the process.  If
    // this value is nearly zero, it is possible that the inputs are linearly
    // dependent (within numerical round-off errors).  On input,
    // 1 <= numElements <= N and v[0] through v[numElements-1] must be
    // initialized.  On output, the vectors v[0] through v[numElements-1]
    // form an orthonormal set.
    template <typename Real>
    Real Orthonormalize(int numInputs, GVector<Real>* v, bool robust = false)
    {
        if (v && 1 <= numInputs && numInputs <= v[0].GetSize())
        {
            for (int i = 1; i < numInputs; ++i)
            {
                if (v[0].GetSize() != v[i].GetSize())
                {
                    LogError("Mismatched sizes.");
                }
            }

            Real minLength = Normalize(v[0], robust);
            for (int i = 1; i < numInputs; ++i)
            {
                for (int j = 0; j < i; ++j)
                {
                    Real dot = Dot(v[i], v[j]);
                    v[i] -= v[j] * dot;
                }
                Real length = Normalize(v[i], robust);
                if (length < minLength)
                {
                    minLength = length;
                }
            }
            return minLength;
        }
        LogError("Invalid input.");
    }

    // Compute the axis-aligned bounding box of the vectors.  The return value is
    // 'true' iff the inputs are valid, in which case vmin and vmax have valid
    // values.
    template <typename Real>
    bool ComputeExtremes(int numVectors, GVector<Real> const* v,
        GVector<Real>& vmin, GVector<Real>& vmax)
    {
        if (v && numVectors > 0)
        {
            for (int i = 1; i < numVectors; ++i)
            {
                if (v[0].GetSize() != v[i].GetSize())
                {
                    LogError("Mismatched sizes.");
                }
            }

            int const size = v[0].GetSize();
            vmin = v[0];
            vmax = vmin;
            for (int j = 1; j < numVectors; ++j)
            {
                GVector<Real> const& vec = v[j];
                for (int i = 0; i < size; ++i)
                {
                    if (vec[i] < vmin[i])
                    {
                        vmin[i] = vec[i];
                    }
                    else if (vec[i] > vmax[i])
                    {
                        vmax[i] = vec[i];
                    }
                }
            }
            return true;
        }
        LogError("Invalid input.");
    }

    // Lift n-tuple v to homogeneous (n+1)-tuple (v,last).
    template <typename Real>
    GVector<Real> HLift(GVector<Real> const& v, Real last)
    {
        int const size = v.GetSize();
        GVector<Real> result(size + 1);
        for (int i = 0; i < size; ++i)
        {
            result[i] = v[i];
        }
        result[size] = last;
        return result;
    }

    // Project homogeneous n-tuple v = (u,v[n-1]) to (n-1)-tuple u.
    template <typename Real>
    GVector<Real> HProject(GVector<Real> const& v)
    {
        int const size = v.GetSize();
        if (size > 1)
        {
            GVector<Real> result(size - 1);
            for (int i = 0; i < size - 1; ++i)
            {
                result[i] = v[i];
            }
            return result;
        }
        else
        {
            return GVector<Real>();
        }
    }


    // Lift n-tuple v = (w0,w1) to (n+1)-tuple u = (w0,u[inject],w1).  By
    // inference, w0 is a (inject)-tuple [nonexistent when inject=0] and w1
    // is a (n-inject)-tuple [nonexistent when inject=n].
    template <typename Real>
    GVector<Real> Lift(GVector<Real> const& v, int inject, Real value)
    {
        int const size = v.GetSize();
        GVector<Real> result(size + 1);
        int i;
        for (i = 0; i < inject; ++i)
        {
            result[i] = v[i];
        }
        result[i] = value;
        int j = i;
        for (++j; i < size; ++i, ++j)
        {
            result[j] = v[i];
        }
        return result;
    }

    // Project n-tuple v = (w0,v[reject],w1) to (n-1)-tuple u = (w0,w1).  By
    // inference, w0 is a (reject)-tuple [nonexistent when reject=0] and w1
    // is a (n-1-reject)-tuple [nonexistent when reject=n-1].
    template <typename Real>
    GVector<Real> Project(GVector<Real> const& v, int reject)
    {
        int const size = v.GetSize();
        if (size > 1)
        {
            GVector<Real> result(size - 1);
            for (int i = 0, j = 0; i < size - 1; ++i, ++j)
            {
                if (j == reject)
                {
                    ++j;
                }
                result[i] = v[j];
            }
            return result;
        }
        else
        {
            return GVector<Real>();
        }
    }
}