Vector.h

Go to the documentation of this file.

#pragma once
 
 
#include "common/Globals.h"
#include "objects/containers/Tuple.h"
#include "objects/geometry/Generic.h"
#include "objects/wrappers/Interval.h"
#include <immintrin.h>
#include <iomanip>
#include <smmintrin.h>
 
NAMESPACE_SPH_BEGIN
 
enum Coordinate {
    X = 0,
    Y = 1,
    Z = 2,
    H = 3,
};
 
 
template <typename T>
struct IsVectorType {
    static constexpr bool value = false;
};
 
template <typename T>
class BasicVector;
 
template <typename T>
struct IsVectorType<BasicVector<T>> {
    static constexpr bool value = true;
};
 
template <typename T>
constexpr bool IsVector = IsVectorType<T>::value;
 
// test
static_assert(!IsVector<float>, "static test failed");
static_assert(IsVector<BasicVector<float>>, "Static test failed");
 
 
template <typename T>
class BasicVector;
 
 
template <>
class BasicVector<float> {
private:
    __m128 data;
 
public:
    INLINE BasicVector() = default;
 
    INLINE BasicVector(const __m128 data)
        : data(data) {}
 
    INLINE explicit BasicVector(const float f)
        : data(_mm_set1_ps(float(f))) {}
 
    INLINE BasicVector(const float x, const float y, const float z, const float h = 0.f)
        : data(_mm_set_ps(h, z, y, x)) {}
 
    INLINE BasicVector(const BasicVector& v)
        : data(v.data) {}
 
    INLINE const float& operator[](const int i) const {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<const float*>(&data) + i);
    }
 
    INLINE float& operator[](const int i) {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<float*>(&data) + i);
    }
 
    template <int i>
    INLINE const float& get() const {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<const float*>(&data) + i);
    }
 
    template <int i>
    INLINE float& get() {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<float*>(&data) + i);
    }
 
    INLINE const __m128& sse() const {
        return data;
    }
 
    INLINE BasicVector& operator=(const BasicVector& v) {
        data = v.data;
        return *this;
    }
 
    INLINE BasicVector& operator+=(const BasicVector& v) {
        data = _mm_add_ps(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator-=(const BasicVector& v) {
        data = _mm_sub_ps(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator*=(const float f) {
        data = _mm_mul_ps(data, _mm_set1_ps(f));
        return *this;
    }
 
    INLINE BasicVector& operator/=(const float f) {
        data = _mm_div_ps(data, _mm_set1_ps(f));
        return *this;
    }
 
    INLINE BasicVector& operator*=(const BasicVector& v) {
        data = _mm_mul_ps(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator/=(const BasicVector& v) {
        data = _mm_div_ps(data, v.data);
        return *this;
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v) {
        return _mm_xor_ps(v.data, _mm_set1_ps(-0.f));
    }
 
    INLINE friend BasicVector operator+(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_add_ps(v1.data, v2.data));
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_sub_ps(v1.data, v2.data));
    }
 
    INLINE friend auto operator*(const BasicVector& v, const float f) {
        return BasicVector(_mm_mul_ps(v.data, _mm_set1_ps(f)));
    }
 
    INLINE friend auto operator*(const float f, const BasicVector& v) {
        return BasicVector(_mm_mul_ps(v.data, _mm_set1_ps(f)));
    }
 
    INLINE friend auto operator*(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_mul_ps(v1.data, v2.data));
    }
 
    INLINE friend auto operator/(const BasicVector& v, const float f) {
        SPH_ASSERT(f != 0.f);
        return BasicVector(_mm_div_ps(v.data, _mm_set1_ps(f)));
    }
 
    INLINE friend auto operator/(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_div_ps(v1.data, v2.data));
    }
 
    INLINE friend bool operator==(const BasicVector& v1, const BasicVector& v2) {
        constexpr int d = 3;
        const int mask = (1 << d) - 0x01;
        return (_mm_movemask_ps(_mm_cmpeq_ps(v1.data, v2.data)) & mask) == mask;
    }
 
    INLINE friend bool operator!=(const BasicVector& v1, const BasicVector& v2) {
        return !(v1 == v2);
    }
 
    friend std::ostream& operator<<(std::ostream& stream, const BasicVector& v) {
        constexpr int digits = 6;
        stream << std::setprecision(digits);
        for (int i = 0; i < 3; ++i) {
            stream << std::setw(20) << v[i];
        }
        return stream;
    }
};
 
 
 
// #define SPH_VECTOR_AVX
 
#ifdef SPH_VECTOR_AVX
template <>
class BasicVector<double> {
private:
    __m256d data;
 
public:
    INLINE BasicVector() = default;
 
    INLINE BasicVector(const __m256d data)
        : data(data) {}
 
    INLINE explicit BasicVector(const double f)
        : data(_mm256_set1_pd(f)) {}
 
    INLINE BasicVector(const double x, const double y, const double z = 0., const double h = 0.)
        : data(_mm256_set_pd(h, z, y, x)) {}
 
    INLINE BasicVector(const BasicVector& v)
        : data(v.data) {}
 
    INLINE const double& operator[](const int i) const {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<const double*>(&data) + i);
    }
 
    INLINE double& operator[](const int i) {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<double*>(&data) + i);
    }
 
    template <int i>
    INLINE const double& get() const {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<const double*>(&data) + i);
    }
 
    template <int i>
    INLINE double& get() {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<double*>(&data) + i);
    }
 
    INLINE BasicVector& operator=(const BasicVector& v) {
        data = v.data;
        return *this;
    }
 
    INLINE BasicVector& operator+=(const BasicVector& v) {
        data = _mm256_add_pd(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator-=(const BasicVector& v) {
        data = _mm256_sub_pd(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator*=(const double f) {
        data = _mm256_mul_pd(data, _mm256_set1_pd(f));
        return *this;
    }
 
    INLINE BasicVector& operator/=(const double f) {
        data = _mm256_div_pd(data, _mm256_set1_pd(f));
        return *this;
    }
 
    INLINE BasicVector& operator*=(const BasicVector& v) {
        data = _mm256_mul_pd(data, v.data);
        return *this;
    }
 
    INLINE BasicVector& operator/=(const BasicVector& v) {
        data = _mm256_div_pd(data, v.data);
        return *this;
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v) {
        return _mm256_xor_pd(v.data, _mm256_set1_pd(-0.));
    }
 
    INLINE friend BasicVector operator+(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm256_add_pd(v1.data, v2.data));
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm256_sub_pd(v1.data, v2.data));
    }
 
    INLINE friend auto operator*(const BasicVector& v, const double f) {
        return BasicVector(_mm256_mul_pd(v.data, _mm256_set1_pd(f)));
    }
 
    INLINE friend auto operator*(const double f, const BasicVector& v) {
        return BasicVector(_mm256_mul_pd(v.data, _mm256_set1_pd(f)));
    }
 
    INLINE friend auto operator*(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm256_mul_pd(v1.data, v2.data));
    }
 
    INLINE friend auto operator/(const BasicVector& v, const double f) {
        SPH_ASSERT(f != 0.);
        return BasicVector(_mm256_div_pd(v.data, _mm256_set1_pd(f)));
    }
 
    INLINE friend auto operator/(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm256_div_pd(v1.data, v2.data));
    }
 
    INLINE friend bool operator==(const BasicVector& v1, const BasicVector& v2) {
        return v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z];
    }
 
    INLINE friend bool operator!=(const BasicVector& v1, const BasicVector& v2) {
        return !(v1 == v2);
    }
 
    INLINE auto dot(const BasicVector& other) const {
        return (*this)[X] * other[X] + (*this)[Y] * other[Y] + (*this)[Z] * other[Z];
    }
 
    // component-wise minimum
    INLINE BasicVector min(const BasicVector& other) const {
        return BasicVector(_mm256_min_pd(data, other.data));
    }
 
    // component-wise maximum
    INLINE BasicVector max(const BasicVector& other) const {
        return BasicVector(_mm256_max_pd(data, other.data));
    }
 
    INLINE const __m256d& sse() const {
        return data;
    }
 
    friend std::ostream& operator<<(std::ostream& stream, const BasicVector& v) {
        constexpr int digits = PRECISION;
        stream << std::setprecision(digits);
        for (int i = 0; i < 3; ++i) {
            stream << std::setw(20) << v[i];
        }
        return stream;
    }
};
 
#else
 
// also align to 32 to have the same memory layout
template <>
class alignas(32) BasicVector<double> {
private:
    __m128d data[2];
 
public:
    INLINE BasicVector() = default;
 
    INLINE BasicVector(const __m128d data1, const __m128d data2)
        : data{ data1, data2 } {}
 
    INLINE explicit BasicVector(const double f)
        : data{ _mm_set1_pd(f), _mm_set1_pd(f) } {}
 
    INLINE BasicVector(const double x, const double y, const double z, const double h = 0.)
        : data{ _mm_set_pd(y, x), _mm_set_pd(h, z) } {}
 
    INLINE BasicVector(const BasicVector& v)
        : data{ v.data[0], v.data[1] } {}
 
    INLINE const double& operator[](const int i) const {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<const double*>(&data) + i);
    }
 
    INLINE double& operator[](const int i) {
        SPH_ASSERT(unsigned(i) < 4);
        return *(reinterpret_cast<double*>(&data) + i);
    }
 
    template <int i>
    INLINE const double& get() const {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<const double*>(&data) + i);
    }
 
    template <int i>
    INLINE double& get() {
        static_assert(unsigned(i) < 4, "Invalid index");
        return *(reinterpret_cast<double*>(&data) + i);
    }
 
    template <int i>
    INLINE const __m128d& sse() const {
        static_assert(unsigned(i) < 2, "Invalid index");
        return data[i];
    }
 
    INLINE BasicVector& operator=(const BasicVector& v) {
        data[0] = v.data[0];
        data[1] = v.data[1];
        return *this;
    }
 
    INLINE BasicVector& operator+=(const BasicVector& v) {
        data[0] = _mm_add_pd(data[0], v.data[0]);
        data[1] = _mm_add_pd(data[1], v.data[1]);
        return *this;
    }
 
    INLINE BasicVector& operator-=(const BasicVector& v) {
        data[0] = _mm_sub_pd(data[0], v.data[0]);
        data[1] = _mm_sub_pd(data[1], v.data[1]);
        return *this;
    }
 
    INLINE BasicVector& operator*=(const double f) {
        const __m128d value = _mm_set1_pd(f);
        data[0] = _mm_mul_pd(data[0], value);
        data[1] = _mm_mul_pd(data[1], value);
        return *this;
    }
 
    INLINE BasicVector& operator/=(const double f) {
        const __m128d value = _mm_set1_pd(f);
        data[0] = _mm_div_pd(data[0], value);
        data[1] = _mm_div_pd(data[1], value);
        return *this;
    }
 
    INLINE BasicVector& operator*=(const BasicVector& v) {
        data[0] = _mm_mul_pd(data[0], v.data[0]);
        data[1] = _mm_mul_pd(data[1], v.data[1]);
        return *this;
    }
 
    INLINE BasicVector& operator/=(const BasicVector& v) {
        data[0] = _mm_div_pd(data[0], v.data[0]);
        data[1] = _mm_div_pd(data[1], v.data[1]);
        return *this;
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v) {
        const __m128d value = _mm_set1_pd(-0.);
        return { _mm_xor_pd(v.data[0], value), _mm_xor_pd(v.data[1], value) };
    }
 
    INLINE friend BasicVector operator+(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_add_pd(v1.data[0], v2.data[0]), _mm_add_pd(v1.data[1], v2.data[1]));
    }
 
    INLINE friend BasicVector operator-(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_sub_pd(v1.data[0], v2.data[0]), _mm_sub_pd(v1.data[1], v2.data[1]));
    }
 
    INLINE friend auto operator*(const BasicVector& v, const double f) {
        const __m128d value = _mm_set1_pd(f);
        return BasicVector(_mm_mul_pd(v.data[0], value), _mm_mul_pd(v.data[1], value));
    }
 
    INLINE friend auto operator*(const double f, const BasicVector& v) {
        const __m128d value = _mm_set1_pd(f);
        return BasicVector(_mm_mul_pd(v.data[0], value), _mm_mul_pd(v.data[1], value));
    }
 
    INLINE friend auto operator*(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_mul_pd(v1.data[0], v2.data[0]), _mm_mul_pd(v1.data[1], v2.data[1]));
    }
 
    INLINE friend auto operator/(const BasicVector& v, const double f) {
        SPH_ASSERT(f != 0.);
        const __m128d value = _mm_set1_pd(f);
        return BasicVector(_mm_div_pd(v.data[0], value), _mm_div_pd(v.data[1], value));
    }
 
    INLINE friend auto operator/(const BasicVector& v1, const BasicVector& v2) {
        return BasicVector(_mm_div_pd(v1.data[0], v2.data[0]), _mm_div_pd(v1.data[1], v2.data[1]));
    }
 
    INLINE friend bool operator==(const BasicVector& v1, const BasicVector& v2) {
        constexpr int d = 3;
        const int r1 = _mm_movemask_pd(_mm_cmpeq_pd(v1.data[0], v2.data[0]));
        const int r2 = _mm_movemask_pd(_mm_cmpeq_pd(v1.data[1], v2.data[1]));
        const int mask1 = (1 << 2) - 0x01;
        const int mask2 = (1 << (d - 2)) - 0x01;
        return (r1 & mask1) == mask1 && (r2 & mask2) == mask2;
    }
 
    INLINE friend bool operator!=(const BasicVector& v1, const BasicVector& v2) {
        return !(v1 == v2);
    }
 
    friend std::ostream& operator<<(std::ostream& stream, const BasicVector& v) {
        constexpr int digits = PRECISION;
        stream << std::setprecision(digits);
        for (int i = 0; i < 3; ++i) {
            stream << std::setw(20) << v[i];
        }
        return stream;
    }
};
 
#endif
 
static_assert(alignof(BasicVector<double>) == 32, "Incorrect alignment of Vector");
 
 
using Vector = BasicVector<Float>;
 
static_assert(std::is_trivially_default_constructible<Vector>::value, "must be trivially construtible");
static_assert(std::is_trivially_destructible<Vector>::value, "must be trivially destructible");
 
 
INLINE float dot(const BasicVector<float>& v1, const BasicVector<float>& v2) {
    constexpr int d = 3;
    return _mm_cvtss_f32(_mm_dp_ps(v1.sse(), v2.sse(), (1 << (d + 4)) - 0x0F));
}
 
INLINE double dot(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return v1[X] * v2[X] + v1[Y] * v2[Y] + v1[Z] * v2[Z];
}
 
INLINE BasicVector<float> cross(const BasicVector<float>& v1, const BasicVector<float>& v2) {
    return _mm_sub_ps(_mm_mul_ps(_mm_shuffle_ps(v1.sse(), v1.sse(), _MM_SHUFFLE(3, 0, 2, 1)),
                          _mm_shuffle_ps(v2.sse(), v2.sse(), _MM_SHUFFLE(3, 1, 0, 2))),
        _mm_mul_ps(_mm_shuffle_ps(v1.sse(), v1.sse(), _MM_SHUFFLE(3, 1, 0, 2)),
            _mm_shuffle_ps(v2.sse(), v2.sse(), _MM_SHUFFLE(3, 0, 2, 1))));
}
 
INLINE BasicVector<double> cross(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return BasicVector<double>(
        v1[Y] * v2[Z] - v1[Z] * v2[Y], v1[Z] * v2[X] - v1[X] * v2[Z], v1[X] * v2[Y] - v1[Y] * v2[X]);
}
 
INLINE Float getSqrLength(const Vector& v) {
    return dot(v, v);
}
 
INLINE Float getLength(const Vector& v) {
    return sqrt(dot(v, v));
}
 
INLINE Float getLengthApprox(const Vector& v) {
    return sqrtApprox(dot(v, v));
}
 
INLINE Vector getNormalized(const Vector& v) {
    const Float length = getLength(v);
    SPH_ASSERT(length != 0._f);
    return v / length;
}
 
INLINE Tuple<Vector, Float> getNormalizedWithLength(const Vector& v) {
    const Float length = getLength(v);
    SPH_ASSERT(length != 0._f);
    return { v / length, length };
}
 
 
template <>
INLINE BasicVector<float> min(const BasicVector<float>& v1, const BasicVector<float>& v2) {
    return _mm_min_ps(v1.sse(), v2.sse());
}
 
template <>
INLINE BasicVector<float> max(const BasicVector<float>& v1, const BasicVector<float>& v2) {
    return _mm_max_ps(v1.sse(), v2.sse());
}
 
#ifdef SPH_VECTOR_AVX
template <>
INLINE BasicVector<double> min(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return v1.min(v2);
}
 
INLINE BasicVector<double> max(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return v1.max(v2);
}
#else
template <>
INLINE BasicVector<double> min(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return { _mm_min_pd(v1.sse<0>(), v2.sse<0>()), _mm_min_pd(v1.sse<1>(), v2.sse<1>()) };
}
 
template <>
INLINE BasicVector<double> max(const BasicVector<double>& v1, const BasicVector<double>& v2) {
    return { _mm_max_pd(v1.sse<0>(), v2.sse<0>()), _mm_max_pd(v1.sse<1>(), v2.sse<1>()) };
}
#endif
 
 
template <>
INLINE Vector clamp(const Vector& v, const Vector& v1, const Vector& v2) {
    return max(v1, min(v, v2));
}
 
template <>
INLINE Vector clamp(const Vector& v, const Interval& range) {
    return Vector(range.clamp(v[0]), range.clamp(v[1]), range.clamp(v[2]), range.clamp(v[3]));
}
 
INLINE bool almostEqual(const Vector& v1, const Vector& v2, const Float eps = EPS) {
    return getSqrLength(v1 - v2) <= sqr(eps) * (1._f + max(getSqrLength(v1), getSqrLength(v2)));
}
 
template <>
INLINE Float norm(const Vector& v) {
    const Float result = getLengthApprox(v);
    SPH_ASSERT(isReal(result));
    return result;
}
 
template <>
INLINE Float normSqr(const Vector& v) {
    const Float result = getSqrLength(v);
    SPH_ASSERT(isReal(result));
    return result;
}
 
template <>
INLINE Float minElement(const Vector& v) {
    return min(v[0], v[1], v[2]);
}
 
template <>
INLINE Float maxElement(const Vector& v) {
    return max(v[0], v[1], v[2]);
}
 
INLINE Size argMin(const Vector& v) {
    Size minIdx = 0;
    if (v[1] < v[minIdx]) {
        minIdx = 1;
    }
    if (v[2] < v[minIdx]) {
        minIdx = 2;
    }
    return minIdx;
}
 
INLINE Size argMax(const Vector& v) {
    Size maxIdx = 0;
    if (v[1] > v[maxIdx]) {
        maxIdx = 1;
    }
    if (v[2] > v[maxIdx]) {
        maxIdx = 2;
    }
    return maxIdx;
}
 
template <>
INLINE auto abs(const BasicVector<float>& v) {
    return BasicVector<float>(_mm_andnot_ps(_mm_set1_ps(-0.f), v.sse()));
}
 
#ifdef SPH_VECTOR_AVX
template <>
INLINE auto abs(const BasicVector<double>& v) {
    return BasicVector<double>(abs(v[X]), abs(v[Y]), abs(v[Z]), abs(v[H]));
}
#else
template <>
INLINE auto abs(const BasicVector<double>& v) {
    return BasicVector<double>(
        _mm_andnot_pd(_mm_set1_pd(-0.), v.sse<0>()), _mm_andnot_pd(_mm_set1_pd(-0.), v.sse<1>()));
}
#endif
 
INLINE Float l1Norm(const Vector& v) {
    const Vector absV = abs(v);
    return absV[X] + absV[Y] + absV[Z];
}
 
 
/*template <>
INLINE Vector sqrtInv(const Vector& v) {
    return _mm_rsqrt_ss(v.sse());
}*/
 
template <>
INLINE bool isReal(const BasicVector<float>& v) {
    return isReal(v[0]) && isReal(v[1]) && isReal(v[2]);
}
 
template <>
INLINE bool isReal(const BasicVector<double>& v) {
    return isReal(v[0]) && isReal(v[1]) && isReal(v[2]);
}
 
template <>
INLINE auto less(const Vector& v1, const Vector& v2) {
    return Vector(Float(v1[X] < v2[X]), Float(v1[Y] < v2[Y]), Float(v1[Z] < v2[Z]), Float(v1[H] < v2[H]));
}
 
template <>
INLINE StaticArray<Float, 6> getComponents<Vector>(const Vector& v) {
    return { v[X], v[Y], v[Z] };
}
 
 
template <typename T1, typename T2>
INLINE BasicVector<T1> vectorCast(const BasicVector<T2>& v) {
    return BasicVector<T1>(T1(v[X]), T1(v[Y]), T1(v[Z]), T1(v[H]));
}
template <>
INLINE BasicVector<Float> vectorCast(const BasicVector<Float>& v) {
    return v;
}
 
INLINE Vector cos(const Vector& v) {
    return Vector(cos(v[X]), cos(v[Y]), cos(v[Z]));
}
 
INLINE Vector sphericalToCartesian(const Float r, const Float theta, const Float phi) {
    const Float s = sin(theta);
    const Float c = cos(theta);
    return r * Vector(s * cos(phi), s * sin(phi), c);
}
 
INLINE Vector cylindricalToCartesian(const Float r, const Float phi, const Float z) {
    return Vector(r * cos(phi), r * sin(phi), z);
}
 
struct SphericalCoords {
    Float r;     
    Float theta; 
    Float phi;   
};
 
INLINE SphericalCoords cartensianToSpherical(const Vector& v) {
    const Float r = getLength(v);
    const Float phi = atan2(v[Y], v[X]);
    const Float theta = acos(v[Z] / r); // atan2(sqrt(sqr(v[X]) + sqr(v[Y])), v[Z]);
    return { r, theta, phi };
}
 
 
INLINE Vector sphericalInversion(const Vector& v, const Vector& center, const Float radius) {
    const Vector diff = v - center;
    const Float lSqr = getSqrLength(diff);
    return center + diff * radius * radius / lSqr;
}
 
INLINE Float distance(const Vector& r, const Vector& axis) {
    SPH_ASSERT(almostEqual(getSqrLength(axis), 1._f));
    return getLength(r - dot(r, axis) * axis);
}
 
INLINE bool lexicographicalLess(const Vector& v1, const Vector& v2) {
    if (v1[Z] < v2[Z]) {
        return true;
    } else if (v1[Z] > v2[Z]) {
        return false;
    } else if (v1[Y] < v2[Y]) {
        return true;
    } else if (v1[Y] > v2[Y]) {
        return false;
    } else if (v1[X] < v2[X]) {
        return true;
    } else {
        return false;
    }
}
 
NAMESPACE_SPH_END