1e5ede1942
More accurately normalize 4D vectors, to avoid potential numerical stability issues in Mantaflow. For consistency with changes from #136966. Pull Request: https://projects.blender.org/blender/blender/pulls/137413
544 lines
14 KiB
C++
544 lines
14 KiB
C++
/******************************************************************************
|
|
*
|
|
* MantaFlow fluid solver framework
|
|
* Copyright 2011 Tobias Pfaff, Nils Thuerey
|
|
*
|
|
* This program is free software, distributed under the terms of the
|
|
* Apache License, Version 2.0
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* 4D vector class
|
|
*
|
|
******************************************************************************/
|
|
|
|
#ifndef _VECTOR4D_H
|
|
#define _VECTOR4D_H
|
|
|
|
#include "vectorbase.h"
|
|
|
|
namespace Manta {
|
|
|
|
//! Basic inlined vector class
|
|
template<class S> class Vector4D {
|
|
public:
|
|
//! Constructor
|
|
inline Vector4D() : x(0), y(0), z(0), t(0)
|
|
{
|
|
}
|
|
|
|
//! Copy-Constructor
|
|
inline Vector4D(const Vector4D<S> &v) : x(v.x), y(v.y), z(v.z), t(v.t)
|
|
{
|
|
}
|
|
|
|
//! Copy-Constructor
|
|
inline Vector4D(const float *v) : x((S)v[0]), y((S)v[1]), z((S)v[2]), t((S)v[3])
|
|
{
|
|
}
|
|
|
|
//! Copy-Constructor
|
|
inline Vector4D(const double *v) : x((S)v[0]), y((S)v[1]), z((S)v[2]), t((S)v[3])
|
|
{
|
|
}
|
|
|
|
//! Construct a vector from one S
|
|
inline Vector4D(S v) : x(v), y(v), z(v), t(v)
|
|
{
|
|
}
|
|
|
|
//! Construct a vector from three Ss
|
|
inline Vector4D(S vx, S vy, S vz, S vw) : x(vx), y(vy), z(vz), t(vw)
|
|
{
|
|
}
|
|
|
|
// Operators
|
|
|
|
//! Assignment operator
|
|
inline const Vector4D<S> &operator=(const Vector4D<S> &v)
|
|
{
|
|
x = v.x;
|
|
y = v.y;
|
|
z = v.z;
|
|
t = v.t;
|
|
return *this;
|
|
}
|
|
//! Assignment operator
|
|
inline const Vector4D<S> &operator=(S s)
|
|
{
|
|
x = y = z = t = s;
|
|
return *this;
|
|
}
|
|
//! Assign and add operator
|
|
inline const Vector4D<S> &operator+=(const Vector4D<S> &v)
|
|
{
|
|
x += v.x;
|
|
y += v.y;
|
|
z += v.z;
|
|
t += v.t;
|
|
return *this;
|
|
}
|
|
//! Assign and add operator
|
|
inline const Vector4D<S> &operator+=(S s)
|
|
{
|
|
x += s;
|
|
y += s;
|
|
z += s;
|
|
t += s;
|
|
return *this;
|
|
}
|
|
//! Assign and sub operator
|
|
inline const Vector4D<S> &operator-=(const Vector4D<S> &v)
|
|
{
|
|
x -= v.x;
|
|
y -= v.y;
|
|
z -= v.z;
|
|
t -= v.t;
|
|
return *this;
|
|
}
|
|
//! Assign and sub operator
|
|
inline const Vector4D<S> &operator-=(S s)
|
|
{
|
|
x -= s;
|
|
y -= s;
|
|
z -= s;
|
|
t -= s;
|
|
return *this;
|
|
}
|
|
//! Assign and mult operator
|
|
inline const Vector4D<S> &operator*=(const Vector4D<S> &v)
|
|
{
|
|
x *= v.x;
|
|
y *= v.y;
|
|
z *= v.z;
|
|
t *= v.t;
|
|
return *this;
|
|
}
|
|
//! Assign and mult operator
|
|
inline const Vector4D<S> &operator*=(S s)
|
|
{
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
t *= s;
|
|
return *this;
|
|
}
|
|
//! Assign and div operator
|
|
inline const Vector4D<S> &operator/=(const Vector4D<S> &v)
|
|
{
|
|
x /= v.x;
|
|
y /= v.y;
|
|
z /= v.z;
|
|
t /= v.t;
|
|
return *this;
|
|
}
|
|
//! Assign and div operator
|
|
inline const Vector4D<S> &operator/=(S s)
|
|
{
|
|
x /= s;
|
|
y /= s;
|
|
z /= s;
|
|
t /= s;
|
|
return *this;
|
|
}
|
|
//! Negation operator
|
|
inline Vector4D<S> operator-() const
|
|
{
|
|
return Vector4D<S>(-x, -y, -z, -t);
|
|
}
|
|
|
|
//! Get smallest component
|
|
// inline S min() const { return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z ); }
|
|
//! Get biggest component
|
|
// inline S max() const { return ( x>y ) ? ( ( x>z ) ? x:z ) : ( ( y>z ) ? y:z ); }
|
|
|
|
//! Test if all components are zero
|
|
inline bool empty()
|
|
{
|
|
return x == 0 && y == 0 && z == 0 && t == 0;
|
|
}
|
|
|
|
//! access operator
|
|
inline S &operator[](unsigned int i)
|
|
{
|
|
return value[i];
|
|
}
|
|
//! constant access operator
|
|
inline const S &operator[](unsigned int i) const
|
|
{
|
|
return value[i];
|
|
}
|
|
|
|
//! debug output vector to a string
|
|
std::string toString() const;
|
|
|
|
//! test if nans are present
|
|
bool isValid() const;
|
|
|
|
//! actual values
|
|
union {
|
|
S value[4];
|
|
struct {
|
|
S x;
|
|
S y;
|
|
S z;
|
|
S t;
|
|
};
|
|
struct {
|
|
S X;
|
|
S Y;
|
|
S Z;
|
|
S T;
|
|
};
|
|
};
|
|
|
|
// zero element
|
|
static const Vector4D<S> Zero, Invalid;
|
|
|
|
protected:
|
|
};
|
|
|
|
//************************************************************************
|
|
// Additional operators
|
|
//************************************************************************
|
|
|
|
//! Addition operator
|
|
template<class S> inline Vector4D<S> operator+(const Vector4D<S> &v1, const Vector4D<S> &v2)
|
|
{
|
|
return Vector4D<S>(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z, v1.t + v2.t);
|
|
}
|
|
//! Addition operator
|
|
template<class S, class S2> inline Vector4D<S> operator+(const Vector4D<S> &v, S2 s)
|
|
{
|
|
return Vector4D<S>(v.x + s, v.y + s, v.z + s, v.t + s);
|
|
}
|
|
//! Addition operator
|
|
template<class S, class S2> inline Vector4D<S> operator+(S2 s, const Vector4D<S> &v)
|
|
{
|
|
return Vector4D<S>(v.x + s, v.y + s, v.z + s, v.t + s);
|
|
}
|
|
|
|
//! Subtraction operator
|
|
template<class S> inline Vector4D<S> operator-(const Vector4D<S> &v1, const Vector4D<S> &v2)
|
|
{
|
|
return Vector4D<S>(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z, v1.t - v2.t);
|
|
}
|
|
//! Subtraction operator
|
|
template<class S, class S2> inline Vector4D<S> operator-(const Vector4D<S> &v, S2 s)
|
|
{
|
|
return Vector4D<S>(v.x - s, v.y - s, v.z - s, v.t - s);
|
|
}
|
|
//! Subtraction operator
|
|
template<class S, class S2> inline Vector4D<S> operator-(S2 s, const Vector4D<S> &v)
|
|
{
|
|
return Vector4D<S>(s - v.x, s - v.y, s - v.z, s - v.t);
|
|
}
|
|
|
|
//! Multiplication operator
|
|
template<class S> inline Vector4D<S> operator*(const Vector4D<S> &v1, const Vector4D<S> &v2)
|
|
{
|
|
return Vector4D<S>(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z, v1.t * v2.t);
|
|
}
|
|
//! Multiplication operator
|
|
template<class S, class S2> inline Vector4D<S> operator*(const Vector4D<S> &v, S2 s)
|
|
{
|
|
return Vector4D<S>(v.x * s, v.y * s, v.z * s, v.t * s);
|
|
}
|
|
//! Multiplication operator
|
|
template<class S, class S2> inline Vector4D<S> operator*(S2 s, const Vector4D<S> &v)
|
|
{
|
|
return Vector4D<S>(s * v.x, s * v.y, s * v.z, s * v.t);
|
|
}
|
|
|
|
//! Division operator
|
|
template<class S> inline Vector4D<S> operator/(const Vector4D<S> &v1, const Vector4D<S> &v2)
|
|
{
|
|
return Vector4D<S>(v1.x / v2.x, v1.y / v2.y, v1.z / v2.z, v1.t / v2.t);
|
|
}
|
|
//! Division operator
|
|
template<class S, class S2> inline Vector4D<S> operator/(const Vector4D<S> &v, S2 s)
|
|
{
|
|
return Vector4D<S>(v.x / s, v.y / s, v.z / s, v.t / s);
|
|
}
|
|
//! Division operator
|
|
template<class S, class S2> inline Vector4D<S> operator/(S2 s, const Vector4D<S> &v)
|
|
{
|
|
return Vector4D<S>(s / v.x, s / v.y, s / v.z, s / v.t);
|
|
}
|
|
|
|
//! Comparison operator
|
|
template<class S> inline bool operator==(const Vector4D<S> &s1, const Vector4D<S> &s2)
|
|
{
|
|
return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z && s1.t == s2.t;
|
|
}
|
|
|
|
//! Comparison operator
|
|
template<class S> inline bool operator!=(const Vector4D<S> &s1, const Vector4D<S> &s2)
|
|
{
|
|
return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z || s1.t != s2.t;
|
|
}
|
|
|
|
//************************************************************************
|
|
// External functions
|
|
//************************************************************************
|
|
|
|
//! Dot product
|
|
template<class S> inline S dot(const Vector4D<S> &t, const Vector4D<S> &v)
|
|
{
|
|
return t.x * v.x + t.y * v.y + t.z * v.z + t.t * v.t;
|
|
}
|
|
|
|
/* Based on libstdc++ implementation from:
|
|
https://github.com/gcc-mirror/gcc/blob/master/libstdc%2B%2B-v3/include/c_global/cmath#L3769
|
|
*/
|
|
template<typename _Tp>
|
|
inline _Tp
|
|
__hypot4(_Tp __x, _Tp __y, _Tp __z, _Tp __t)
|
|
{
|
|
__x = std::abs(__x);
|
|
__y = std::abs(__y);
|
|
__z = std::abs(__z);
|
|
__t = std::abs(__t);
|
|
if (_Tp __a = __x < __y ? (__y < __z ? (__z < __t ? __t : __z ) : (__y < __t ? __t : __y )) : __x < __z ? (__z < __t ? __t : __z ) : (__x < __t ? __t : __x ))
|
|
return __a * std::sqrt((__x / __a) * (__x / __a)
|
|
+ (__y / __a) * (__y / __a)
|
|
+ (__z / __a) * (__z / __a)
|
|
+ (__t / __a) * (__t / __a));
|
|
else
|
|
return {};
|
|
}
|
|
|
|
inline float
|
|
hypot4(float __x, float __y, float __z, float __t)
|
|
{ return __hypot4<float>(__x, __y, __z, __t); }
|
|
|
|
inline double
|
|
hypot4(double __x, double __y, double __z, double __t)
|
|
{ return __hypot4<double>(__x, __y, __z, __t); }
|
|
|
|
//! Cross product
|
|
/*template<class S>
|
|
inline Vector4D<S> cross ( const Vector4D<S> &t, const Vector4D<S> &v ) {
|
|
NYI Vector4D<S> cp (
|
|
( ( t.y*v.z ) - ( t.z*v.y ) ),
|
|
( ( t.z*v.x ) - ( t.x*v.z ) ),
|
|
( ( t.x*v.y ) - ( t.y*v.x ) ) );
|
|
return cp;
|
|
}*/
|
|
|
|
//! Compute the magnitude (length) of the vector
|
|
template<class S> inline S norm(const Vector4D<S> &v)
|
|
{
|
|
S l = hypot4(v.x, v.y, v.z, v.t);
|
|
return (fabs(l - 1.) < VECTOR_EPSILON) ? 1. : l;
|
|
}
|
|
|
|
//! Compute squared magnitude
|
|
template<class S> inline S normSquare(const Vector4D<S> &v)
|
|
{
|
|
return v.x * v.x + v.y * v.y + v.z * v.z + v.t * v.t;
|
|
}
|
|
|
|
//! Returns a normalized vector
|
|
template<class S> inline Vector4D<S> getNormalized(const Vector4D<S> &v)
|
|
{
|
|
S l = hypot4(v.x, v.y, v.z, v.t);
|
|
if (fabs(l - 1.) < VECTOR_EPSILON)
|
|
return v; /* normalized "enough"... */
|
|
else if (l > VECTOR_EPSILON) {
|
|
S fac = 1. / l;
|
|
return Vector4D<S>(v.x * fac, v.y * fac, v.z * fac, v.t * fac);
|
|
}
|
|
else
|
|
return Vector4D<S>((S)0);
|
|
}
|
|
|
|
//! Compute the norm of the vector and normalize it.
|
|
/*! \return The value of the norm */
|
|
template<class S> inline S normalize(Vector4D<S> &v)
|
|
{
|
|
S norm;
|
|
S l = hypot4(v.x, v.y, v.z, v.t);
|
|
if (fabs(l - 1.) < VECTOR_EPSILON) {
|
|
norm = 1.;
|
|
}
|
|
else if (l > VECTOR_EPSILON) {
|
|
norm = l;
|
|
v *= 1. / norm;
|
|
}
|
|
else {
|
|
v = Vector4D<S>::Zero;
|
|
norm = 0.;
|
|
}
|
|
return (S)norm;
|
|
}
|
|
|
|
//! Outputs the object in human readable form as string
|
|
template<class S> std::string Vector4D<S>::toString() const
|
|
{
|
|
char buf[256];
|
|
snprintf(buf,
|
|
256,
|
|
"[%+4.6f,%+4.6f,%+4.6f,%+4.6f]",
|
|
(double)(*this)[0],
|
|
(double)(*this)[1],
|
|
(double)(*this)[2],
|
|
(double)(*this)[3]);
|
|
// for debugging, optionally increase precision:
|
|
// snprintf ( buf,256,"[%+4.16f,%+4.16f,%+4.16f,%+4.16f]", ( double ) ( *this ) [0], ( double ) (
|
|
// *this ) [1], ( double ) ( *this ) [2], ( double ) ( *this ) [3] );
|
|
return std::string(buf);
|
|
}
|
|
|
|
template<> std::string Vector4D<int>::toString() const;
|
|
|
|
//! Outputs the object in human readable form to stream
|
|
template<class S> std::ostream &operator<<(std::ostream &os, const Vector4D<S> &i)
|
|
{
|
|
os << i.toString();
|
|
return os;
|
|
}
|
|
|
|
//! Reads the contents of the object from a stream
|
|
template<class S> std::istream &operator>>(std::istream &is, Vector4D<S> &i)
|
|
{
|
|
char c;
|
|
char dummy[4];
|
|
is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> dummy >> i[3] >> c;
|
|
return is;
|
|
}
|
|
|
|
/**************************************************************************/
|
|
// Define default vector alias
|
|
/**************************************************************************/
|
|
|
|
//! 3D vector class of type Real (typically float)
|
|
typedef Vector4D<Real> Vec4;
|
|
|
|
//! 3D vector class of type int
|
|
typedef Vector4D<int> Vec4i;
|
|
|
|
//! convert to Real Vector
|
|
template<class T> inline Vec4 toVec4(T v)
|
|
{
|
|
return Vec4(v[0], v[1], v[2], v[3]);
|
|
}
|
|
template<class T> inline Vec4i toVec4i(T v)
|
|
{
|
|
return Vec4i(v[0], v[1], v[2], v[3]);
|
|
}
|
|
|
|
/**************************************************************************/
|
|
// Specializations for common math functions
|
|
/**************************************************************************/
|
|
|
|
template<> inline Vec4 clamp<Vec4>(const Vec4 &a, const Vec4 &b, const Vec4 &c)
|
|
{
|
|
return Vec4(
|
|
clamp(a.x, b.x, c.x), clamp(a.y, b.y, c.y), clamp(a.z, b.z, c.z), clamp(a.t, b.t, c.t));
|
|
}
|
|
template<> inline Vec4 safeDivide<Vec4>(const Vec4 &a, const Vec4 &b)
|
|
{
|
|
return Vec4(
|
|
safeDivide(a.x, b.x), safeDivide(a.y, b.y), safeDivide(a.z, b.z), safeDivide(a.t, b.t));
|
|
}
|
|
template<> inline Vec4 nmod<Vec4>(const Vec4 &a, const Vec4 &b)
|
|
{
|
|
return Vec4(nmod(a.x, b.x), nmod(a.y, b.y), nmod(a.z, b.z), nmod(a.t, b.t));
|
|
}
|
|
|
|
/**************************************************************************/
|
|
// 4d interpolation (note only 4d here, 2d/3d interpolations are in interpol.h)
|
|
/**************************************************************************/
|
|
|
|
#define BUILD_INDEX_4D \
|
|
Real px = pos.x - 0.5f, py = pos.y - 0.5f, pz = pos.z - 0.5f, pt = pos.t - 0.5f; \
|
|
int xi = (int)px; \
|
|
int yi = (int)py; \
|
|
int zi = (int)pz; \
|
|
int ti = (int)pt; \
|
|
Real s1 = px - (Real)xi, s0 = 1. - s1; \
|
|
Real t1 = py - (Real)yi, t0 = 1. - t1; \
|
|
Real f1 = pz - (Real)zi, f0 = 1. - f1; \
|
|
Real g1 = pt - (Real)ti, g0 = 1. - g1; \
|
|
/* clamp to border */ \
|
|
if (px < 0.) { \
|
|
xi = 0; \
|
|
s0 = 1.0; \
|
|
s1 = 0.0; \
|
|
} \
|
|
if (py < 0.) { \
|
|
yi = 0; \
|
|
t0 = 1.0; \
|
|
t1 = 0.0; \
|
|
} \
|
|
if (pz < 0.) { \
|
|
zi = 0; \
|
|
f0 = 1.0; \
|
|
f1 = 0.0; \
|
|
} \
|
|
if (pt < 0.) { \
|
|
ti = 0; \
|
|
g0 = 1.0; \
|
|
g1 = 0.0; \
|
|
} \
|
|
if (xi >= size.x - 1) { \
|
|
xi = size.x - 2; \
|
|
s0 = 0.0; \
|
|
s1 = 1.0; \
|
|
} \
|
|
if (yi >= size.y - 1) { \
|
|
yi = size.y - 2; \
|
|
t0 = 0.0; \
|
|
t1 = 1.0; \
|
|
} \
|
|
if (zi >= size.z - 1) { \
|
|
zi = size.z - 2; \
|
|
f0 = 0.0; \
|
|
f1 = 1.0; \
|
|
} \
|
|
if (ti >= size.t - 1) { \
|
|
ti = size.t - 2; \
|
|
g0 = 0.0; \
|
|
g1 = 1.0; \
|
|
} \
|
|
const int sX = 1; \
|
|
const int sY = size.x;
|
|
|
|
static inline void checkIndexInterpol4d(const Vec4i &size, int idx)
|
|
{
|
|
if (idx < 0 || idx > size.x * size.y * size.z * size.t) {
|
|
std::ostringstream s;
|
|
s << "Grid interpol4d dim " << size << " : index " << idx << " out of bound ";
|
|
errMsg(s.str());
|
|
}
|
|
}
|
|
|
|
template<class T>
|
|
inline T interpol4d(
|
|
const T *data, const Vec4i &size, const IndexInt sZ, const IndexInt sT, const Vec4 &pos)
|
|
{
|
|
BUILD_INDEX_4D
|
|
IndexInt idx = (IndexInt)xi + sY * (IndexInt)yi + sZ * (IndexInt)zi + sT * (IndexInt)ti;
|
|
DEBUG_ONLY(checkIndexInterpol4d(size, idx));
|
|
DEBUG_ONLY(checkIndexInterpol4d(size, idx + sX + sY + sZ + sT));
|
|
|
|
return (((data[idx] * t0 + data[idx + sY] * t1) * s0 +
|
|
(data[idx + sX] * t0 + data[idx + sX + sY] * t1) * s1) *
|
|
f0 +
|
|
((data[idx + sZ] * t0 + data[idx + sY + sZ] * t1) * s0 +
|
|
(data[idx + sX + sZ] * t0 + data[idx + sX + sY + sZ] * t1) * s1) *
|
|
f1) *
|
|
g0 +
|
|
(((data[idx + sT] * t0 + data[idx + sT + sY] * t1) * s0 +
|
|
(data[idx + sT + sX] * t0 + data[idx + sT + sX + sY] * t1) * s1) *
|
|
f0 +
|
|
((data[idx + sT + sZ] * t0 + data[idx + sT + sY + sZ] * t1) * s0 +
|
|
(data[idx + sT + sX + sZ] * t0 + data[idx + sT + sX + sY + sZ] * t1) * s1) *
|
|
f1) *
|
|
g1;
|
|
}
|
|
|
|
}; // namespace Manta
|
|
|
|
#endif
|