| 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381 |
- /*
- * Copyright (c) Contributors to the Open 3D Engine Project.
- * For complete copyright and license terms please see the LICENSE at the root of this distribution.
- *
- * SPDX-License-Identifier: Apache-2.0 OR MIT
- *
- */
- // Description : Common vector class
- #ifndef CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H
- #define CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H
- #pragma once
- #include "CryEndian.h"
- #include <AzCore/Math/Vector3.h>
- template<typename T>
- struct VecPrecisionValues
- {
- ILINE static bool CheckGreater(const T value)
- {
- return value > 0;
- }
- };
- template<>
- struct VecPrecisionValues<float>
- {
- ILINE static bool CheckGreater(const float value)
- {
- return value > FLT_EPSILON;
- }
- };
- template <typename F>
- struct Vec3s_tpl
- {
- F x, y, z;
- ILINE Vec3s_tpl(F vx, F vy, F vz)
- : x(vx)
- , y(vy)
- , z(vz){}
- ILINE F& operator [] (int32 index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- ILINE F operator [] (int32 index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- };
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- // class Vec3_tpl
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- template <typename F>
- struct Vec3_tpl
- {
- typedef F value_type;
- enum
- {
- component_count = 3
- };
- F x, y, z;
- #if defined(_DEBUG)
- ILINE Vec3_tpl()
- {
- if constexpr (sizeof(F) == 4)
- {
- uint32* p = alias_cast<uint32*>(&x);
- p[0] = F32NAN;
- p[1] = F32NAN;
- p[2] = F32NAN;
- }
- if constexpr (sizeof(F) == 8)
- {
- uint64* p = alias_cast<uint64*>(&x);
- p[0] = F64NAN;
- p[1] = F64NAN;
- p[2] = F64NAN;
- }
- }
- #else
- ILINE Vec3_tpl() {};
- #endif
- /*!
- * template specialization to initialize a vector
- *
- * Example:
- * Vec3 v0=Vec3(ZERO);
- * Vec3 v1=Vec3(MIN);
- * Vec3 v2=Vec3(MAX);
- */
- Vec3_tpl(type_zero)
- : x(0)
- , y(0)
- , z(0) {}
- Vec3_tpl(type_min);
- Vec3_tpl(type_max);
- /*!
- * constructors and bracket-operator to initialize a vector
- *
- * Example:
- * Vec3 v0=Vec3(1,2,3);
- * Vec3 v1(1,2,3);
- * v2.Set(1,2,3);
- */
- ILINE Vec3_tpl(F vx, F vy, F vz)
- : x(vx)
- , y(vy)
- , z(vz){ assert(this->IsValid()); }
- ILINE void operator () (F vx, F vy, F vz) { x = vx; y = vy; z = vz; assert(this->IsValid()); }
- ILINE Vec3_tpl<F>& Set(const F xval, const F yval, const F zval) { x = xval; y = yval; z = zval; assert(this->IsValid()); return *this; }
- explicit ILINE Vec3_tpl(F f)
- : x(f)
- , y(f)
- , z(f) { assert(this->IsValid()); }
- explicit ILINE Vec3_tpl(const AZ::Vector3& v)
- {
- x = v.GetX();
- y = v.GetY();
- z = v.GetZ();
- }
- /*!
- * the copy/casting/assignement constructor
- *
- * Example:
- * Vec3 v0=v1;
- * Vec3 v0=Vec3(angle);
- * Vec3 v0=Vec3(vector4);
- */
- ILINE Vec3_tpl(const Vec3_tpl& v) { x = v.x; y = v.y; z = v.z; }
- template<class F1>
- ILINE Vec3_tpl<F>(const Vec3_tpl<F1>&v) {
- x = F(v.x);
- y = F(v.y);
- z = F(v.z);
- assert(IsValid());
- }
- ILINE Vec3_tpl<F>(const Vec2_tpl<F>&v) {
- x = v.x;
- y = v.y;
- z = 0;
- assert(IsValid());
- }
- template<class T>
- ILINE Vec3_tpl<F>(const Vec2_tpl<T>&v) {
- x = F(v.x);
- y = F(v.y);
- z = 0;
- assert(IsValid());
- }
- explicit ILINE Vec3_tpl<F>(const Ang3_tpl<F>&v) {
- x = v.x;
- y = v.y;
- z = v.z;
- assert(IsValid());
- }
- template<class T>
- explicit ILINE Vec3_tpl<F>(const Ang3_tpl<T>&v) {
- x = v.x;
- y = v.y;
- z = v.z;
- assert(IsValid());
- }
- /*!
- * overloaded arithmetic operator
- *
- * Example:
- * Vec3 v0=v1*4;
- */
- ILINE Vec3_tpl<F> operator * (F k) const
- {
- const Vec3_tpl<F> v = *this;
- return Vec3_tpl<F>(v.x * k, v.y * k, v.z * k);
- }
- ILINE Vec3_tpl<F> operator / (F k) const
- {
- k = (F)1.0 / k;
- return Vec3_tpl<F>(x * k, y * k, z * k);
- }
- ILINE friend Vec3_tpl<F> operator * (F f, const Vec3_tpl& vec)
- {
- return Vec3_tpl((F)(f * vec.x), (F)(f * vec.y), (F)(f * vec.z));
- }
- ILINE Vec3_tpl<F>& operator *= (F k)
- {
- x *= k;
- y *= k;
- z *= k;
- return *this;
- }
- ILINE Vec3_tpl<F>& operator /= (F k)
- {
- k = (F)1.0 / k;
- x *= k;
- y *= k;
- z *= k;
- return *this;
- }
- ILINE Vec3_tpl<F> operator - (void) const
- {
- return Vec3_tpl<F>(-x, -y, -z);
- }
- ILINE Vec3_tpl<F>& Flip()
- {
- x = -x;
- y = -y;
- z = -z;
- return *this;
- }
- /*!
- * bracked-operator
- *
- * Example:
- * uint32 t=v[0];
- */
- ILINE F& operator [] (int32 index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- ILINE F operator [] (int32 index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- /*!
- * functions and operators to compare vector
- *
- * Example:
- * if (v0==v1) dosomething;
- */
- ILINE bool operator==(const Vec3_tpl<F>& vec)
- {
- return x == vec.x && y == vec.y && z == vec.z;
- }
- ILINE bool operator!=(const Vec3_tpl<F>& vec)
- {
- return !(*this == vec);
- }
- ILINE friend bool operator ==(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1)
- {
- return ((v0.x == v1.x) && (v0.y == v1.y) && (v0.z == v1.z));
- }
- ILINE friend bool operator !=(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1)
- {
- return !(v0 == v1);
- }
- ILINE bool IsZero(F e = (F) 0.0) const
- {
- return (fabs_tpl(x) <= e) && (fabs_tpl(y) <= e) && (fabs_tpl(z) <= e);
- }
- ILINE bool IsZeroFast(F e = (F) 0.0003) const
- {
- return (fabs_tpl(x) + fabs_tpl(y) + fabs_tpl(z)) <= e;
- }
- // Chebyshev distance (axis aligned)
- ILINE bool IsEquivalent(const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
- {
- assert(v1.IsValid());
- assert(this->IsValid());
- return ((fabs_tpl(x - v1.x) <= epsilon) && (fabs_tpl(y - v1.y) <= epsilon) && (fabs_tpl(z - v1.z) <= epsilon));
- }
- ILINE static bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON)
- {
- assert(v0.IsValid());
- assert(v1.IsValid());
- return ((fabs_tpl(v0.x - v1.x) <= epsilon) && (fabs_tpl(v0.y - v1.y) <= epsilon) && (fabs_tpl(v0.z - v1.z) <= epsilon));
- }
- // Euclidean distance L2
- ILINE bool IsEquivalentL2(const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
- {
- assert(v1.IsValid());
- assert(this->IsValid());
- return (*this - v1).GetLengthSquared() <= (epsilon * epsilon);
- }
- ILINE static bool IsEquivalentL2(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON)
- {
- assert(v0.IsValid());
- assert(v1.IsValid());
- return (v0 - v1).GetLengthSquared() <= (epsilon * epsilon);
- }
- ILINE bool IsUnit(F epsilon = VEC_EPSILON) const
- {
- return (fabs_tpl(1 - GetLengthSquared()) <= epsilon);
- }
- bool IsValid() const
- {
- if (!NumberValid(x))
- {
- return false;
- }
- if (!NumberValid(y))
- {
- return false;
- }
- if (!NumberValid(z))
- {
- return false;
- }
- return true;
- }
- //! force vector length by normalizing it
- ILINE void SetLength(F fLen)
- {
- F fLenMe = GetLengthSquared();
- if (fLenMe < 0.00001f * 0.00001f)
- {
- return;
- }
- fLenMe = fLen * isqrt_tpl(fLenMe);
- x *= fLenMe;
- y *= fLenMe;
- z *= fLenMe;
- }
- ILINE void ClampLength(F maxLength)
- {
- F sqrLength = GetLengthSquared();
- if (sqrLength > (maxLength * maxLength))
- {
- F scale = maxLength * isqrt_tpl(sqrLength);
- x *= scale;
- y *= scale;
- z *= scale;
- }
- }
- //! calculate the length of the vector
- ILINE F GetLength() const
- {
- return sqrt_tpl(x * x + y * y + z * z);
- }
- ILINE F GetLengthFloat() const
- {
- return GetLength();
- }
- ILINE F GetLengthFast() const
- {
- return sqrt_fast_tpl(x * x + y * y + z * z);
- }
- //! calculate the squared length of the vector
- ILINE F GetLengthSquared() const
- {
- return x * x + y * y + z * z;
- }
- ILINE F GetLengthSquaredFloat() const
- {
- return GetLengthSquared();
- }
- //! calculate the length of the vector ignoring the z component
- ILINE F GetLength2D() const
- {
- return sqrt_tpl(x * x + y * y);
- }
- //! calculate the squared length of the vector ignoring the z component
- ILINE F GetLengthSquared2D() const
- {
- return x * x + y * y;
- }
- ILINE F GetDistance(const Vec3_tpl<F>& vec1) const
- {
- return sqrt_tpl((x - vec1.x) * (x - vec1.x) + (y - vec1.y) * (y - vec1.y) + (z - vec1.z) * (z - vec1.z));
- }
- ILINE F GetSquaredDistance (const Vec3_tpl<F>& v) const
- {
- return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y) + (z - v.z) * (z - v.z);
- }
- ILINE F GetSquaredDistance2D (const Vec3_tpl<F>& v) const
- {
- return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y);
- }
- //! Normalize the vector.
- // The default Normalize function is in fact "safe". 0 vectors remain unchanged.
- ILINE void Normalize()
- {
- assert(this->IsValid());
- F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
- x *= fInvLen;
- y *= fInvLen;
- z *= fInvLen;
- }
- //! May be faster and less accurate.
- ILINE void NormalizeFast()
- {
- assert(this->IsValid());
- F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
- x *= fInvLen;
- y *= fInvLen;
- z *= fInvLen;
- }
- //! Normalize the vector to a scale.
- ILINE void Normalize(F scale)
- {
- assert(this->IsValid());
- F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z) * scale;
- x *= fInvLen;
- y *= fInvLen;
- z *= fInvLen;
- }
- ILINE void NormalizeFast(F scale)
- {
- assert(this->IsValid());
- F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z) * scale;
- x *= fInvLen;
- y *= fInvLen;
- z *= fInvLen;
- }
- //! Normalize the vector.
- // check for null vector - set to the passed in vector (which should be normalised!) if it is null vector
- // returns the original length of the vector
- ILINE F NormalizeSafe(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(0, 0, 0))
- {
- assert(this->IsValid());
- F fLen2 = x * x + y * y + z * z;
- IF (VecPrecisionValues<F>::CheckGreater(fLen2), 1)
- {
- F fInvLen = isqrt_tpl(fLen2);
- x *= fInvLen;
- y *= fInvLen;
- z *= fInvLen;
- return F(1) / fInvLen;
- }
- else
- {
- *this = safe;
- return F(0);
- }
- }
- ILINE Vec3_tpl GetNormalizedFloat() const
- {
- return GetNormalized();
- }
- //! Return a normalized vector.
- ILINE Vec3_tpl GetNormalized() const
- {
- F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
- return *this * fInvLen;
- }
- //! Return a normalized vector.
- ILINE Vec3_tpl GetNormalizedFast() const
- {
- F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
- return *this * fInvLen;
- }
- //! Return a safely normalized vector - returns safe vector (should be normalised) if original is zero length.
- ILINE Vec3_tpl GetNormalizedSafe(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(1, 0, 0)) const
- {
- F fLen2 = x * x + y * y + z * z;
- IF (VecPrecisionValues<F>::CheckGreater(fLen2), 1)
- {
- F fInvLen = isqrt_tpl(fLen2);
- return *this * fInvLen;
- }
- else
- {
- return safe;
- }
- }
- //! Return a safely normalized vector - returns safe vector (should be normalised) if original is zero length.
- ILINE Vec3_tpl GetNormalizedSafeFloat(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(1, 0, 0)) const
- {
- return GetNormalizedSafe(safe);
- }
- //! Return a normalized and scaled vector.
- ILINE Vec3_tpl GetNormalized(F scale) const
- {
- F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
- return *this * (fInvLen * scale);
- }
- ILINE Vec3_tpl GetNormalizedFast(F scale) const
- {
- F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
- return *this * (fInvLen * scale);
- }
- //! Permutate coordinates so that z goes to new_z slot.
- ILINE Vec3_tpl GetPermutated(int new_z) const { return Vec3_tpl(*(&x + inc_mod3[new_z]), *(&x + dec_mod3[new_z]), *(&x + new_z)); }
- //! Returns volume of a box with this vector as diagonal.
- ILINE F GetVolume() const { return x * y * z; }
- //! Returns a vector that consists of absolute values of this one's coordinates.
- ILINE Vec3_tpl<F> abs() const
- {
- return Vec3_tpl(fabs_tpl(x), fabs_tpl(y), fabs_tpl(z));
- }
- //! Check for min bounds.
- ILINE void CheckMin(const Vec3_tpl<F> other)
- {
- x = min(other.x, x);
- y = min(other.y, y);
- z = min(other.z, z);
- }
- //! Check for max bounds.
- ILINE void CheckMax(const Vec3_tpl<F> other)
- {
- x = max(other.x, x);
- y = max(other.y, y);
- z = max(other.z, z);
- }
- /*!
- * Sets a vector orthogonal to the input vector.
- *
- * Example:
- * Vec3 v;
- * v.SetOrthogonal( i );
- */
- ILINE void SetOrthogonal(const Vec3_tpl<F>& v)
- {
- sqr(F(0.9)) * (v | v) - v.x * v.x < 0 ? (x = -v.z, y = 0, z = v.x) : (x = 0, y = v.z, z = -v.y);
- }
- //! Returns a vector orthogonal to this one.
- ILINE Vec3_tpl GetOrthogonal() const
- {
- return sqr(F(0.9)) * (x * x + y * y + z * z) - x * x < 0 ? Vec3_tpl<F>(-z, 0, x) : Vec3_tpl<F>(0, z, -y);
- }
- /*!
- * Project a point/vector on a (virtual) plane
- * Consider we have a plane going through the origin.
- * Because d=0 we need just the normal. The vector n is assumed to be a unit-vector.
- *
- * Example:
- * Vec3 result=Vec3::CreateProjection( incident, normal );
- */
- ILINE void SetProjection(const Vec3_tpl& i, const Vec3_tpl& n)
- {
- *this = i - n * (n | i);
- }
- ILINE static Vec3_tpl<F> CreateProjection(const Vec3_tpl& i, const Vec3_tpl& n)
- {
- return i - n * (n | i);
- }
- /*!
- * Calculate a reflection vector. Vec3 n is assumed to be a unit-vector.
- *
- * Example:
- * Vec3 result=Vec3::CreateReflection( incident, normal );
- */
- ILINE void SetReflection(const Vec3_tpl<F>& i, const Vec3_tpl<F>& n)
- {
- *this = (n * (i | n) * 2) - i;
- }
- ILINE static Vec3_tpl<F> CreateReflection(const Vec3_tpl<F>& i, const Vec3_tpl<F>& n)
- {
- return (n * (i | n) * 2) - i;
- }
- /*!
- * Linear-Interpolation between Vec3 (lerp)
- *
- * Example:
- * Vec3 r=Vec3::CreateLerp( p, q, 0.345f );
- */
- ILINE void SetLerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
- {
- const Vec3_tpl<F> diff = q - p;
- *this = p + (diff * t);
- }
- ILINE static Vec3_tpl<F> CreateLerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
- {
- const Vec3_tpl<F> diff = q - p;
- return p + (diff * t);
- }
- /*!
- * Spherical-Interpolation between 3d-vectors (geometrical slerp)
- * both vectors are assumed to be normalized.
- *
- * Example:
- * Vec3 r=Vec3::CreateSlerp( p, q, 0.5674f );
- */
- void SetSlerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
- {
- assert(p.IsUnit(0.005f));
- assert(q.IsUnit(0.005f));
- // calculate cosine using the "inner product" between two vectors: p*q=cos(radiant)
- F cosine = clamp_tpl((p | q), F(-1), F(1));
- //we explore the special cases where the both vectors are very close together,
- //in which case we approximate using the more economical LERP and avoid "divisions by zero" since sin(Angle) = 0 as Angle=0
- if (cosine >= (F)0.99)
- {
- SetLerp(p, q, t); //perform LERP:
- Normalize();
- }
- else
- {
- //perform SLERP: because of the LERP-check above, a "division by zero" is not possible
- F rad = acos_tpl(cosine);
- F scale_0 = sin_tpl((1 - t) * rad);
- F scale_1 = sin_tpl(t * rad);
- *this = (p * scale_0 + q * scale_1) / sin_tpl(rad);
- Normalize();
- }
- }
- ILINE static Vec3_tpl<F> CreateSlerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
- {
- Vec3_tpl<F> v;
- v.SetSlerp(p, q, t);
- return v;
- }
- /*!
- * Quadratic-Interpolation between vectors v0,v1,v2.
- * This is repeated linear interpolation from 3 points and the resulting curve is a parabola.
- * If t is in the range [0...1], then the curve goes only through v0 and v2.
- *
- * Example:
- * Vec3 ip; ip.SetQuadraticCurve( v0,v1,v2, 0.345f );
- */
- ILINE void SetQuadraticCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t1)
- {
- F t0 = 1.0f - t1;
- *this = t0 * t0 * v0 + t0 * t1 * 2.0f * v1 + t1 * t1 * v2;
- }
- ILINE static Vec3_tpl<F> CreateQuadraticCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
- {
- Vec3_tpl<F> ip;
- ip.SetQuadraticCurve(v0, v1, v2, t);
- return ip;
- }
- /*!
- * Cubic-Interpolation between vectors v0,v1,v2,v3.
- * This is repeated linear interpolation from 4 points.
- * If t is in the range [0...1], then the curve goes only through v0 and v3.
- *
- * Example:
- * Vec3 ip; ip.SetCubicCurve( v0,v1,v2,v3, 0.345f );
- */
- ILINE void SetCubicCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, const Vec3_tpl<F>& v3, F t1)
- {
- F t0 = 1.0f - t1;
- *this = t0 * t0 * t0 * v0 + 3 * t0 * t0 * t1 * v1 + 3 * t0 * t1 * t1 * v2 + t1 * t1 * t1 * v3;
- }
- ILINE static Vec3_tpl<F> CreateCubicCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, const Vec3_tpl<F>& v3, F t)
- {
- Vec3_tpl<F> ip;
- ip.SetCubicCurve(v0, v1, v2, v3, t);
- return ip;
- }
- /*!
- * Spline-Interpolation between vectors v0,v1,v2.
- * This is a variation of a quadratic curve.
- * If t is in the range [0...1], then the spline goes through all 3 points
- *
- * Example:
- * Vec3 ip; ip.SetSplineInterpolation( v0,v1,v2, 0.345f );
- */
- ILINE void SetQuadraticSpline(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
- {
- SetQuadraticCurve(v0, v1 - (v0 * 0.5f + v1 + v2 * 0.5f - v1 * 2.0f), v2, t);
- }
- ILINE static Vec3_tpl<F> CreateQuadraticSpline(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
- {
- Vec3_tpl<F> ip;
- ip.SetQuadraticSpline(v0, v1, v2, t);
- return ip;
- }
- /*!
- * Rotate a vector using angle&axis.
- *
- * Example:
- * Vec3 r=v.GetRotated(axis,angle);
- */
- ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& axis, F angle) const
- {
- return GetRotated(axis, cos_tpl(angle), sin_tpl(angle));
- }
- ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& axis, F cosa, F sina) const
- {
- Vec3_tpl<F> zax = axis * (*this | axis);
- Vec3_tpl<F> xax = *this - zax;
- Vec3_tpl<F> yax = axis % xax;
- return xax * cosa + yax * sina + zax;
- }
- /*!
- * Rotate a vector around a center using angle&axis.
- *
- * Example:
- * Vec3 r=v.GetRotated(axis,angle);
- */
- ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl& center, const Vec3_tpl<F>& axis, F angle) const
- {
- return center + (*this - center).GetRotated(axis, angle);
- }
- ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& center, const Vec3_tpl<F>& axis, F cosa, F sina) const
- {
- return center + (*this - center).GetRotated(axis, cosa, sina);
- }
- /*!
- * Component wise multiplication of two vectors.
- */
- ILINE Vec3_tpl CompMul(const Vec3_tpl<F> rhs) const
- {
- return(Vec3_tpl(x * rhs.x, y * rhs.y, z * rhs.z));
- }
- //! Three methods for a "dot-product" operation.
- ILINE F Dot (const Vec3_tpl<F> v) const
- {
- return x * v.x + y * v.y + z * v.z;
- }
- //! Two methods for a "cross-product" operation.
- ILINE Vec3_tpl<F> Cross (const Vec3_tpl<F> vec2) const
- {
- return Vec3_tpl<F>(y * vec2.z - z * vec2.y, z * vec2.x - x * vec2.z, x * vec2.y - y * vec2.x);
- }
- //f32* fptr=vec;
- DEPRECATED operator F* () { return (F*)this; }
- template <class T>
- explicit DEPRECATED Vec3_tpl(const T* src) { x = F(src[0]); y = F(src[1]); z = F(src[2]); }
- ILINE Vec3_tpl& zero() { x = y = z = 0; return *this; }
- ILINE F len() const
- {
- return sqrt_tpl(x * x + y * y + z * z);
- }
- ILINE F len2() const
- {
- return x * x + y * y + z * z;
- }
- ILINE Vec3_tpl& normalize()
- {
- F len2 = x * x + y * y + z * z;
- if (len2 > (F)1e-20f)
- {
- F rlen = isqrt_tpl(len2);
- x *= rlen;
- y *= rlen;
- z *= rlen;
- }
- else
- {
- Set(0, 0, 1);
- }
- return *this;
- }
- ILINE Vec3_tpl normalized() const
- {
- F len2 = x * x + y * y + z * z;
- if (len2 > (F)1e-20f)
- {
- F rlen = isqrt_tpl(len2);
- return Vec3_tpl(x * rlen, y * rlen, z * rlen);
- }
- else
- {
- return Vec3_tpl(0, 0, 1);
- }
- }
- //vector subtraction
- template<class F1>
- ILINE Vec3_tpl<F1> sub(const Vec3_tpl<F1>& v) const
- {
- return Vec3_tpl<F1>(x - v.x, y - v.y, z - v.z);
- }
- //vector scale
- template<class F1>
- ILINE Vec3_tpl<F1> scale(const F1 k) const
- {
- return Vec3_tpl<F>(x * k, y * k, z * k);
- }
- //vector dot product
- template<class F1>
- ILINE F1 dot(const Vec3_tpl<F1>& v) const
- {
- return (F1)(x * v.x + y * v.y + z * v.z);
- }
- //vector cross product
- template<class F1>
- ILINE Vec3_tpl<F1> cross(const Vec3_tpl<F1>& v) const
- {
- return Vec3_tpl<F1>(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
- }
- };
- using Vec3i = Vec3_tpl<int32>;
- // dot product (2 versions)
- template<class F1, class F2>
- ILINE F1 operator * (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return v0.Dot(v1);
- }
- template<class F1, class F2>
- ILINE F1 operator | (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return v0.Dot(v1);
- }
- // cross product (2 versions)
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator ^ (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return v0.Cross(v1);
- }
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator % (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return v0.Cross(v1);
- }
- //vector addition
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator + (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(static_cast<F1>(v0.x + v1.x), static_cast<F1>(v0.y + v1.y), static_cast<F1>(v0.z + v1.z));
- }
- //vector addition
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator + (const Vec2_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v1.z);
- }
- //vector addition
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator + (const Vec3_tpl<F1>& v0, const Vec2_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v0.z);
- }
- //vector subtraction
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator - (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>((F1)(v0.x - v1.x), (F1)(v0.y - v1.y), (F1)(v0.z - v1.z));
- }
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator - (const Vec2_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, 0.0f - v1.z);
- }
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator - (const Vec3_tpl<F1>& v0, const Vec2_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, v0.z);
- }
- //---------------------------------------------------------------------------
- //vector self-addition
- template<class F1, class F2>
- ILINE Vec3_tpl<F1>& operator += (Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- v0 = v0 + v1;
- return v0;
- }
- //vector self-subtraction
- template<class F1, class F2>
- ILINE Vec3_tpl<F1>& operator -= (Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- v0 = v0 - v1;
- return v0;
- }
- template<class F1, class F2>
- ILINE Vec3_tpl<F1> operator / (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
- {
- return Vec3_tpl<F1>(v0.x / v1.x, v0.y / v1.y, v0.z / v1.z);
- }
- template <class F>
- ILINE bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, f32 epsilon = VEC_EPSILON)
- {
- return ((fabs_tpl(v0.x - v1.x) <= epsilon) && (fabs_tpl(v0.y - v1.y) <= epsilon) && (fabs_tpl(v0.z - v1.z) <= epsilon));
- }
- ///////////////////////////////////////////////////////////////////////////////
- // Typedefs //
- ///////////////////////////////////////////////////////////////////////////////
- typedef Vec3_tpl<f32> Vec3; // always 32 bit
- template<>
- inline Vec3_tpl<f32>::Vec3_tpl(type_min) { x = y = z = -3.3E38f; }
- template<>
- inline Vec3_tpl<f32>::Vec3_tpl(type_max) { x = y = z = 3.3E38f; }
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- // struct Ang3_tpl
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- template <typename F>
- struct Ang3_tpl
- {
- F x, y, z;
- #if defined(_DEBUG)
- ILINE Ang3_tpl()
- {
- if constexpr (sizeof(F) == 4)
- {
- uint32* p = alias_cast<uint32*>(&x);
- p[0] = F32NAN;
- p[1] = F32NAN;
- p[2] = F32NAN;
- }
- if constexpr (sizeof(F) == 8)
- {
- uint64* p = alias_cast<uint64*>(&x);
- p[0] = F64NAN;
- p[1] = F64NAN;
- p[2] = F64NAN;
- }
- }
- #else
- ILINE Ang3_tpl() {};
- #endif
- Ang3_tpl(type_zero) { x = y = z = 0; }
- void operator () (F vx, F vy, F vz) { x = vx; y = vy; z = vz; };
- ILINE Ang3_tpl<F>(F vx, F vy, F vz) {
- x = vx;
- y = vy;
- z = vz;
- }
- explicit ILINE Ang3_tpl(const Vec3_tpl<F>& v)
- : x((F)v.x)
- , y((F)v.y)
- , z((F)v.z) { assert(this->IsValid()); }
- ILINE bool operator==(const Ang3_tpl<F>& vec) { return x == vec.x && y == vec.y && z == vec.z; }
- ILINE bool operator!=(const Ang3_tpl<F>& vec) { return !(*this == vec); }
- ILINE Ang3_tpl<F> operator * (F k) const { return Ang3_tpl<F>(x * k, y * k, z * k); }
- ILINE Ang3_tpl<F> operator / (F k) const { k = (F)1.0 / k; return Ang3_tpl<F>(x * k, y * k, z * k); }
- ILINE Ang3_tpl<F>& operator *= (F k) { x *= k; y *= k; z *= k; return *this; }
- //explicit ILINE Ang3_tpl<F>& operator = (const Vec3_tpl<F>& v) { x=v.x; y=v.y; z=v.z; return *this; }
- ILINE Ang3_tpl<F> operator - (void) const { return Ang3_tpl<F>(-x, -y, -z); }
- ILINE friend bool operator ==(const Ang3_tpl<F>& v0, const Ang3_tpl<F>& v1)
- {
- return ((v0.x == v1.x) && (v0.y == v1.y) && (v0.z == v1.z));
- }
- ILINE void Set(F xval, F yval, F zval) { x = xval; y = yval; z = zval; }
- ILINE bool IsEquivalent(const Ang3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
- {
- return ((fabs_tpl(x - v1.x) <= epsilon) && (fabs_tpl(y - v1.y) <= epsilon) && (fabs_tpl(z - v1.z) <= epsilon));
- }
- ILINE bool IsInRangePI() const
- {
- F pi = (F)(gf_PI + 0.001); //we need this to fix fp-precision problem
- return ((x > -pi) && (x < pi) && (y > -pi) && (y < pi) && (z > -pi) && (z < pi));
- }
- //! Normalize angle to -pi and +pi range.
- ILINE void RangePI()
- {
- const F modX = fmod(x + gf_PI, gf_PI2);
- x = if_neg_else(modX, modX + gf_PI, modX - gf_PI);
- const F modY = fmod(y + gf_PI, gf_PI2);
- y = if_neg_else(modY, modY + gf_PI, modY - gf_PI);
- const F modZ = fmod(z + gf_PI, gf_PI2);
- z = if_neg_else(modZ, modZ + gf_PI, modZ - gf_PI);
- }
- //! Convert unit Quat to Euler Angles (xyz).
- template<class F1>
- explicit Ang3_tpl(const Quat_tpl<F1>& q)
- {
- assert(q.IsValid());
- y = F(asin_tpl(max((F)-1.0, min((F)1.0, -(q.v.x * q.v.z - q.w * q.v.y) * 2))));
- if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
- {
- x = F(0);
- z = F(atan2_tpl(-2 * (q.v.x * q.v.y - q.w * q.v.z), 1 - (q.v.x * q.v.x + q.v.z * q.v.z) * 2));
- }
- else
- {
- x = F(atan2_tpl((q.v.y * q.v.z + q.w * q.v.x) * 2, 1 - (q.v.x * q.v.x + q.v.y * q.v.y) * 2));
- z = F(atan2_tpl((q.v.x * q.v.y + q.w * q.v.z) * 2, 1 - (q.v.z * q.v.z + q.v.y * q.v.y) * 2));
- }
- }
- //! Convert Matrix33 to Euler Angles (xyz).
- template<class F1>
- explicit Ang3_tpl(const Matrix33_tpl<F1>& m)
- {
- assert(m.IsOrthonormalRH(0.001f));
- y = (F)asin_tpl(max((F)-1.0, min((F)1.0, -m.m20)));
- if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
- {
- x = F(0);
- z = F(atan2_tpl(-m.m01, m.m11));
- }
- else
- {
- x = F(atan2_tpl(m.m21, m.m22));
- z = F(atan2_tpl(m.m10, m.m00));
- }
- }
- //! Convert Matrix34 to Euler Angles (xyz).
- template<class F1>
- explicit Ang3_tpl(const Matrix34_tpl<F1>& m)
- {
- assert(m.IsOrthonormalRH(0.001f));
- y = F(asin_tpl(max((F)-1.0, min((F)1.0, -m.m20))));
- if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
- {
- x = F(0);
- z = F(atan2_tpl(-m.m01, m.m11));
- }
- else
- {
- x = F(atan2_tpl(m.m21, m.m22));
- z = F(atan2_tpl(m.m10, m.m00));
- }
- }
- //! Convert Matrix44 to Euler Angles (xyz).
- template<class F1>
- explicit Ang3_tpl(const Matrix44_tpl<F1>& m)
- {
- assert(Matrix33(m).IsOrthonormalRH(0.001f));
- y = F(asin_tpl(max((F)-1.0, min((F)1.0, -m.m20))));
- if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
- {
- x = F(0);
- z = F(atan2_tpl(-m.m01, m.m11));
- }
- else
- {
- x = F(atan2_tpl(m.m21, m.m22));
- z = F(atan2_tpl(m.m10, m.m00));
- }
- }
- template<typename F1>
- static ILINE F CreateRadZ(const Vec2_tpl<F1>& v0, const Vec2_tpl<F1>& v1)
- {
- F cz = v0.x * v1.y - v0.y * v1.x;
- F c = v0.x * v1.x + v0.y * v1.y;
- return F(atan2_tpl(cz, c));
- }
- template<typename F1>
- static ILINE F CreateRadZ(const Vec3_tpl<F1>& v0, const Vec3_tpl<F1>& v1)
- {
- F cz = v0.x * v1.y - v0.y * v1.x;
- F c = v0.x * v1.x + v0.y * v1.y;
- return F(atan2_tpl(cz, c));
- }
- template<typename F1>
- ILINE static Ang3_tpl<F> GetAnglesXYZ(const Quat_tpl<F1>& q) { return Ang3_tpl<F>(q); }
- template<typename F1>
- ILINE void SetAnglesXYZ(const Quat_tpl<F1>& q) { *this = Ang3_tpl<F>(q); }
- template<typename F1>
- ILINE static Ang3_tpl<F> GetAnglesXYZ(const Matrix33_tpl<F1>& m) { return Ang3_tpl<F>(m); }
- template<typename F1>
- ILINE void SetAnglesXYZ(const Matrix33_tpl<F1>& m) { *this = Ang3_tpl<F>(m); }
- template<typename F1>
- ILINE static Ang3_tpl<F> GetAnglesXYZ(const Matrix34_tpl<F1>& m) { return Ang3_tpl<F>(m); }
- template<typename F1>
- ILINE void SetAnglesXYZ(const Matrix34_tpl<F1>& m) { *this = Ang3_tpl<F>(m); }
- //---------------------------------------------------------------
- ILINE F& operator [] (int index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- ILINE F operator [] (int index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
- bool IsValid() const
- {
- if (!NumberValid(x))
- {
- return false;
- }
- if (!NumberValid(y))
- {
- return false;
- }
- if (!NumberValid(z))
- {
- return false;
- }
- return true;
- }
- };
- typedef Ang3_tpl<f32> Ang3;
- //---------------------------------------
- //vector addition
- template<class F1, class F2>
- ILINE Ang3_tpl<F1> operator + (const Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
- {
- return Ang3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v0.z + v1.z);
- }
- //vector subtraction
- template<class F1, class F2>
- ILINE Ang3_tpl<F1> operator - (const Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
- {
- return Ang3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, v0.z - v1.z);
- }
- //---------------------------------------
- //vector self-addition
- template<class F1, class F2>
- ILINE Ang3_tpl<F1>& operator += (Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
- {
- v0 = v0 + v1;
- return v0;
- }
- //vector self-subtraction
- template<class F1, class F2>
- ILINE Ang3_tpl<F1>& operator -= (Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
- {
- v0 = v0 - v1;
- return v0;
- }
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- // struct CAngleAxis
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////
- template <typename F>
- struct AngleAxis_tpl
- {
- //! storage for the Angle&Axis coordinates.
- F angle;
- Vec3_tpl<F> axis;
- // default quaternion constructor
- AngleAxis_tpl(void) { };
- AngleAxis_tpl(F a, F ax, F ay, F az) { angle = a; axis.x = ax; axis.y = ay; axis.z = az; }
- AngleAxis_tpl(F a, const Vec3_tpl<F>& n) { angle = a; axis = n; }
- void operator () (F a, const Vec3_tpl<F>& n) { angle = a; axis = n; }
- AngleAxis_tpl(const AngleAxis_tpl<F>& aa); //CAngleAxis aa=angleaxis
- const Vec3_tpl<F> operator * (const Vec3_tpl<F>& v) const;
- AngleAxis_tpl(const Quat_tpl<F>& q)
- {
- angle = acos_tpl(q.w) * 2;
- axis = q.v;
- axis.Normalize();
- F s = sin_tpl(angle * (F)0.5);
- if (s == 0)
- {
- angle = 0;
- axis.x = 0;
- axis.y = 0;
- axis.z = 1;
- }
- }
- };
- typedef AngleAxis_tpl<f32> AngleAxis;
- template<typename F>
- ILINE const Vec3_tpl<F> AngleAxis_tpl<F>::operator * (const Vec3_tpl<F>& v) const
- {
- Vec3_tpl<F> origin = axis * (axis | v);
- return origin + (v - origin) * cos_tpl(angle) + (axis % v) * sin_tpl(angle);
- }
- //////////////////////////////////////////////////////////////////////
- template<typename F>
- struct Plane_tpl
- {
- //plane-equation: n.x*x + n.y*y + n.z*z + d > 0 is in front of the plane
- Vec3_tpl<F> n; //!< normal
- F d; //!< distance
- //----------------------------------------------------------------
- #if defined(_DEBUG)
- ILINE Plane_tpl()
- {
- if constexpr (sizeof(F) == 4)
- {
- uint32* p = alias_cast<uint32*>(&n.x);
- p[0] = F32NAN;
- p[1] = F32NAN;
- p[2] = F32NAN;
- p[3] = F32NAN;
- }
- if constexpr (sizeof(F) == 8)
- {
- uint64* p = alias_cast<uint64*>(&n.x);
- p[0] = F64NAN;
- p[1] = F64NAN;
- p[2] = F64NAN;
- p[3] = F64NAN;
- }
- }
- #else
- ILINE Plane_tpl() {};
- #endif
- ILINE Plane_tpl(const Plane_tpl<F>& p) { n = p.n; d = p.d; }
- ILINE Plane_tpl(const Vec3_tpl<F>& normal, const F& distance) { n = normal; d = distance; }
- //! set normal and dist for this plane and then calculate plane type
- ILINE void Set(const Vec3_tpl<F>& vNormal, const F fDist)
- {
- n = vNormal;
- d = fDist;
- }
- ILINE void SetPlane(const Vec3_tpl<F>& normal, const Vec3_tpl<F>& point)
- {
- n = normal;
- d = -(point | normal);
- }
- ILINE static Plane_tpl<F> CreatePlane(const Vec3_tpl<F>& normal, const Vec3_tpl<F>& point)
- {
- return Plane_tpl<F>(normal, -(point | normal));
- }
- ILINE Plane_tpl<F> operator - (void) const { return Plane_tpl<F>(-n, -d); }
- /*!
- * Constructs the plane by tree given Vec3s (=triangle) with a right-hand (anti-clockwise) winding
- *
- * Example 1:
- * Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
- * Plane_tpl<F> plane;
- * plane.SetPlane(v0,v1,v2);
- *
- * Example 2:
- * Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
- * Plane_tpl<F> plane=Plane_tpl<F>::CreatePlane(v0,v1,v2);
- */
- ILINE void SetPlane(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2)
- {
- n = ((v1 - v0) % (v2 - v0)).GetNormalized(); //vector cross-product
- d = -(n | v0); //calculate d-value
- }
- ILINE static Plane_tpl<F> CreatePlane(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2)
- {
- Plane_tpl<F> p;
- p.SetPlane(v0, v1, v2);
- return p;
- }
- /*!
- * Computes signed distance from point to plane.
- * This is the standard plane-equation: d=Ax*By*Cz+D.
- * The normal-vector is assumed to be normalized.
- *
- * Example:
- * Vec3 v(1,2,3);
- * Plane_tpl<F> plane=CalculatePlane(v0,v1,v2);
- * f32 distance = plane|v;
- */
- ILINE F operator | (const Vec3_tpl<F>& point) const { return (n | point) + d; }
- ILINE F DistFromPlane(const Vec3_tpl<F>& vPoint) const { return (n * vPoint + d); }
- ILINE Plane_tpl<F> operator - (const Plane_tpl<F>& p) const { return Plane_tpl<F>(n - p.n, d - p.d); }
- ILINE Plane_tpl<F> operator + (const Plane_tpl<F>& p) const { return Plane_tpl<F>(n + p.n, d + p.d); }
- ILINE void operator -= (const Plane_tpl<F>& p) { d -= p.d; n -= p.n; }
- ILINE Plane_tpl<F> operator * (F s) const { return Plane_tpl<F>(n * s, d * s); }
- ILINE Plane_tpl<F> operator / (F s) const { return Plane_tpl<F>(n / s, d / s); }
- //! check for equality between two planes
- friend bool operator ==(const Plane_tpl<F>& p1, const Plane_tpl<F>& p2)
- {
- if (fabsf(p1.n.x - p2.n.x) > 0.0001f)
- {
- return (false);
- }
- if (fabsf(p1.n.y - p2.n.y) > 0.0001f)
- {
- return (false);
- }
- if (fabsf(p1.n.z - p2.n.z) > 0.0001f)
- {
- return (false);
- }
- if (fabsf(p1.d - p2.d) < 0.01f)
- {
- return(true);
- }
- return (false);
- }
- Vec3_tpl<F> MirrorVector(const Vec3_tpl<F>& i) { return n * (2 * (n | i)) - i; }
- Vec3_tpl<F> MirrorPosition(const Vec3_tpl<F>& i) { return i - n * (2 * ((n | i) + d)); }
- ILINE bool IsValid() const { return !n.IsZeroFast(); } //A plane with a zero normal isn't valid.
- };
- typedef Plane_tpl<f32> Plane; //always 32 bit
- // declare common constants. Must be done after the class for compiler conformance
- // (msvc and clang handle instantiation differently)
- const Vec3_tpl<float> Vec3_Zero(0, 0, 0);
- const Vec3_tpl<float> Vec3_OneX(1, 0, 0);
- const Vec3_tpl<float> Vec3_OneY(0, 1, 0);
- const Vec3_tpl<float> Vec3_OneZ(0, 0, 1);
- const Vec3_tpl<float> Vec3_One(1, 1, 1);
- namespace AZ
- {
- AZ_TYPE_INFO_SPECIALIZE(Vec3, "{DFA993FB-4E92-4A13-BDB3-4E9285A5346F}");
- }
- #endif // CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H
|