// // 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 NVIDIA CORPORATION 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 ``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 OWNER 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. // // Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved. // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. #ifndef PSFOUNDATION_PSMATHUTILS_H #define PSFOUNDATION_PSMATHUTILS_H #include "foundation/PxPreprocessor.h" #include "foundation/PxTransform.h" #include "foundation/PxMat33.h" #include "Ps.h" #include "PsIntrinsics.h" // General guideline is: if it's an abstract math function, it belongs here. // If it's a math function where the inputs have specific semantics (e.g. // separateSwingTwist) it doesn't. namespace physx { namespace shdfnd { /** \brief sign returns the sign of its argument. The sign of zero is undefined. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 sign(const PxF32 a) { return intrinsics::sign(a); } /** \brief sign returns the sign of its argument. The sign of zero is undefined. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 sign(const PxF64 a) { return (a >= 0.0) ? 1.0 : -1.0; } /** \brief sign returns the sign of its argument. The sign of zero is undefined. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxI32 sign(const PxI32 a) { return (a >= 0) ? 1 : -1; } /** \brief Returns true if the two numbers are within eps of each other. */ PX_CUDA_CALLABLE PX_FORCE_INLINE bool equals(const PxF32 a, const PxF32 b, const PxF32 eps) { return (PxAbs(a - b) < eps); } /** \brief Returns true if the two numbers are within eps of each other. */ PX_CUDA_CALLABLE PX_FORCE_INLINE bool equals(const PxF64 a, const PxF64 b, const PxF64 eps) { return (PxAbs(a - b) < eps); } /** \brief The floor function returns a floating-point value representing the largest integer that is less than or equal to x. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 floor(const PxF32 a) { return floatFloor(a); } /** \brief The floor function returns a floating-point value representing the largest integer that is less than or equal to x. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 floor(const PxF64 a) { return ::floor(a); } /** \brief The ceil function returns a single value representing the smallest integer that is greater than or equal to x. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 ceil(const PxF32 a) { return ::ceilf(a); } /** \brief The ceil function returns a double value representing the smallest integer that is greater than or equal to x. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 ceil(const PxF64 a) { return ::ceil(a); } /** \brief mod returns the floating-point remainder of x / y. If the value of y is 0.0, mod returns a quiet NaN. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 mod(const PxF32 x, const PxF32 y) { return PxF32(::fmodf(x, y)); } /** \brief mod returns the floating-point remainder of x / y. If the value of y is 0.0, mod returns a quiet NaN. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 mod(const PxF64 x, const PxF64 y) { return ::fmod(x, y); } /** \brief Square. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 sqr(const PxF32 a) { return a * a; } /** \brief Square. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 sqr(const PxF64 a) { return a * a; } /** \brief Calculates x raised to the power of y. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 pow(const PxF32 x, const PxF32 y) { return ::powf(x, y); } /** \brief Calculates x raised to the power of y. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 pow(const PxF64 x, const PxF64 y) { return ::pow(x, y); } /** \brief Calculates e^n */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 exp(const PxF32 a) { return ::expf(a); } /** \brief Calculates e^n */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 exp(const PxF64 a) { return ::exp(a); } /** \brief Calculates 2^n */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 exp2(const PxF32 a) { return ::expf(a * 0.693147180559945309417f); } /** \brief Calculates 2^n */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 exp2(const PxF64 a) { return ::exp(a * 0.693147180559945309417); } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 logE(const PxF32 a) { return ::logf(a); } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 logE(const PxF64 a) { return ::log(a); } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 log2(const PxF32 a) { return ::logf(a) / 0.693147180559945309417f; } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 log2(const PxF64 a) { return ::log(a) / 0.693147180559945309417; } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 log10(const PxF32 a) { return ::log10f(a); } /** \brief Calculates logarithms. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 log10(const PxF64 a) { return ::log10(a); } /** \brief Converts degrees to radians. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 degToRad(const PxF32 a) { return 0.01745329251994329547f * a; } /** \brief Converts degrees to radians. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 degToRad(const PxF64 a) { return 0.01745329251994329547 * a; } /** \brief Converts radians to degrees. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 radToDeg(const PxF32 a) { return 57.29577951308232286465f * a; } /** \brief Converts radians to degrees. */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 radToDeg(const PxF64 a) { return 57.29577951308232286465 * a; } //! \brief compute sine and cosine at the same time. There is a 'fsincos' on PC that we probably want to use here PX_CUDA_CALLABLE PX_FORCE_INLINE void sincos(const PxF32 radians, PxF32& sin, PxF32& cos) { /* something like: _asm fld Local _asm fsincos _asm fstp LocalCos _asm fstp LocalSin */ sin = PxSin(radians); cos = PxCos(radians); } /** \brief uniform random number in [a,b] */ PX_FORCE_INLINE PxI32 rand(const PxI32 a, const PxI32 b) { return a + PxI32(::rand() % (b - a + 1)); } /** \brief uniform random number in [a,b] */ PX_FORCE_INLINE PxF32 rand(const PxF32 a, const PxF32 b) { return a + (b - a) * ::rand() / RAND_MAX; } //! \brief return angle between two vectors in radians PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 angle(const PxVec3& v0, const PxVec3& v1) { const PxF32 cos = v0.dot(v1); // |v0|*|v1|*Cos(Angle) const PxF32 sin = (v0.cross(v1)).magnitude(); // |v0|*|v1|*Sin(Angle) return PxAtan2(sin, cos); } //! If possible use instead fsel on the dot product /*fsel(d.dot(p),onething,anotherthing);*/ //! Compares orientations (more readable, user-friendly function) PX_CUDA_CALLABLE PX_FORCE_INLINE bool sameDirection(const PxVec3& d, const PxVec3& p) { return d.dot(p) >= 0.0f; } //! Checks 2 values have different signs PX_CUDA_CALLABLE PX_FORCE_INLINE IntBool differentSign(PxReal f0, PxReal f1) { #if !PX_EMSCRIPTEN union { PxU32 u; PxReal f; } u1, u2; u1.f = f0; u2.f = f1; return IntBool((u1.u ^ u2.u) & PX_SIGN_BITMASK); #else // javascript floats are 64-bits... return IntBool( (f0*f1) < 0.0f ); #endif } PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 star(const PxVec3& v) { return PxMat33(PxVec3(0, v.z, -v.y), PxVec3(-v.z, 0, v.x), PxVec3(v.y, -v.x, 0)); } PX_CUDA_CALLABLE PX_INLINE PxVec3 log(const PxQuat& q) { const PxReal s = q.getImaginaryPart().magnitude(); if(s < 1e-12f) return PxVec3(0.0f); // force the half-angle to have magnitude <= pi/2 PxReal halfAngle = q.w < 0 ? PxAtan2(-s, -q.w) : PxAtan2(s, q.w); PX_ASSERT(halfAngle >= -PxPi / 2 && halfAngle <= PxPi / 2); return q.getImaginaryPart().getNormalized() * 2.f * halfAngle; } PX_CUDA_CALLABLE PX_INLINE PxQuat exp(const PxVec3& v) { const PxReal m = v.magnitudeSquared(); return m < 1e-24f ? PxQuat(PxIdentity) : PxQuat(PxSqrt(m), v * PxRecipSqrt(m)); } // quat to rotate v0 t0 v1 PX_CUDA_CALLABLE PX_INLINE PxQuat rotationArc(const PxVec3& v0, const PxVec3& v1) { const PxVec3 cross = v0.cross(v1); const PxReal d = v0.dot(v1); if(d <= -0.99999f) return (PxAbs(v0.x) < 0.1f ? PxQuat(0.0f, v0.z, -v0.y, 0.0f) : PxQuat(v0.y, -v0.x, 0.0, 0.0)).getNormalized(); const PxReal s = PxSqrt((1 + d) * 2), r = 1 / s; return PxQuat(cross.x * r, cross.y * r, cross.z * r, s * 0.5f).getNormalized(); } /** \brief returns largest axis */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 largestAxis(const PxVec3& v) { PxU32 m = PxU32(v.y > v.x ? 1 : 0); return v.z > v[m] ? 2 : m; } /** \brief returns indices for the largest axis and 2 other axii */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 largestAxis(const PxVec3& v, PxU32& other1, PxU32& other2) { if(v.x >= PxMax(v.y, v.z)) { other1 = 1; other2 = 2; return 0; } else if(v.y >= v.z) { other1 = 0; other2 = 2; return 1; } else { other1 = 0; other2 = 1; return 2; } } /** \brief returns axis with smallest absolute value */ PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 closestAxis(const PxVec3& v) { PxU32 m = PxU32(PxAbs(v.y) > PxAbs(v.x) ? 1 : 0); return PxAbs(v.z) > PxAbs(v[m]) ? 2 : m; } PX_CUDA_CALLABLE PX_INLINE PxU32 closestAxis(const PxVec3& v, PxU32& j, PxU32& k) { // find largest 2D plane projection const PxF32 absPx = PxAbs(v.x); const PxF32 absNy = PxAbs(v.y); const PxF32 absNz = PxAbs(v.z); PxU32 m = 0; // x biggest axis j = 1; k = 2; if(absNy > absPx && absNy > absNz) { // y biggest j = 2; k = 0; m = 1; } else if(absNz > absPx) { // z biggest j = 0; k = 1; m = 2; } return m; } /*! Extend an edge along its length by a factor */ PX_CUDA_CALLABLE PX_FORCE_INLINE void makeFatEdge(PxVec3& p0, PxVec3& p1, PxReal fatCoeff) { PxVec3 delta = p1 - p0; const PxReal m = delta.magnitude(); if(m > 0.0f) { delta *= fatCoeff / m; p0 -= delta; p1 += delta; } } //! Compute point as combination of barycentric coordinates PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 computeBarycentricPoint(const PxVec3& p0, const PxVec3& p1, const PxVec3& p2, PxReal u, PxReal v) { // This seems to confuse the compiler... // return (1.0f - u - v)*p0 + u*p1 + v*p2; const PxF32 w = 1.0f - u - v; return PxVec3(w * p0.x + u * p1.x + v * p2.x, w * p0.y + u * p1.y + v * p2.y, w * p0.z + u * p1.z + v * p2.z); } // generates a pair of quaternions (swing, twist) such that in = swing * twist, with // swing.x = 0 // twist.y = twist.z = 0, and twist is a unit quat PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& swing, PxQuat& twist) { twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity); swing = q * twist.getConjugate(); } PX_FORCE_INLINE float computeSwingAngle(float swingYZ, float swingW) { return 4.0f * PxAtan2(swingYZ, 1.0f + swingW); // tan (t/2) = sin(t)/(1+cos t), so this is the quarter angle } // generate two tangent vectors to a given normal PX_FORCE_INLINE void normalToTangents(const PxVec3& normal, PxVec3& tangent0, PxVec3& tangent1) { tangent0 = PxAbs(normal.x) < 0.70710678f ? PxVec3(0, -normal.z, normal.y) : PxVec3(-normal.y, normal.x, 0); tangent0.normalize(); tangent1 = normal.cross(tangent0); } /** \brief computes a oriented bounding box around the scaled basis. \param basis Input = skewed basis, Output = (normalized) orthogonal basis. \return Bounding box extent. */ PX_FOUNDATION_API PxVec3 optimizeBoundingBox(PxMat33& basis); PX_FOUNDATION_API PxQuat slerp(const PxReal t, const PxQuat& left, const PxQuat& right); PX_CUDA_CALLABLE PX_INLINE PxVec3 ellipseClamp(const PxVec3& point, const PxVec3& radii) { // This function need to be implemented in the header file because // it is included in a spu shader program. // finds the closest point on the ellipse to a given point // (p.y, p.z) is the input point // (e.y, e.z) are the radii of the ellipse // lagrange multiplier method with Newton/Halley hybrid root-finder. // see http://www.geometrictools.com/Documentation/DistancePointToEllipse2.pdf // for proof of Newton step robustness and initial estimate. // Halley converges much faster but sometimes overshoots - when that happens we take // a newton step instead // converges in 1-2 iterations where D&C works well, and it's good with 4 iterations // with any ellipse that isn't completely crazy const PxU32 MAX_ITERATIONS = 20; const PxReal convergenceThreshold = 1e-4f; // iteration requires first quadrant but we recover generality later PxVec3 q(0, PxAbs(point.y), PxAbs(point.z)); const PxReal tinyEps = 1e-6f; // very close to minor axis is numerically problematic but trivial if(radii.y >= radii.z) { if(q.z < tinyEps) return PxVec3(0, point.y > 0 ? radii.y : -radii.y, 0); } else { if(q.y < tinyEps) return PxVec3(0, 0, point.z > 0 ? radii.z : -radii.z); } PxVec3 denom, e2 = radii.multiply(radii), eq = radii.multiply(q); // we can use any initial guess which is > maximum(-e.y^2,-e.z^2) and for which f(t) is > 0. // this guess works well near the axes, but is weak along the diagonals. PxReal t = PxMax(eq.y - e2.y, eq.z - e2.z); for(PxU32 i = 0; i < MAX_ITERATIONS; i++) { denom = PxVec3(0, 1 / (t + e2.y), 1 / (t + e2.z)); PxVec3 denom2 = eq.multiply(denom); PxVec3 fv = denom2.multiply(denom2); PxReal f = fv.y + fv.z - 1; // although in exact arithmetic we are guaranteed f>0, we can get here // on the first iteration via catastrophic cancellation if the point is // very close to the origin. In that case we just behave as if f=0 if(f < convergenceThreshold) return e2.multiply(point).multiply(denom); PxReal df = fv.dot(denom) * -2.0f; t = t - f / df; } // we didn't converge, so clamp what we have PxVec3 r = e2.multiply(point).multiply(denom); return r * PxRecipSqrt(sqr(r.y / radii.y) + sqr(r.z / radii.z)); } PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal tanHalf(PxReal sin, PxReal cos) { // PT: avoids divide by zero for singularity. We return sqrt(FLT_MAX) instead of FLT_MAX // to make sure the calling code doesn't generate INF values when manipulating the returned value // (some joints multiply it by 4, etc). if(cos==-1.0f) return sin<0.0f ? -sqrtf(FLT_MAX) : sqrtf(FLT_MAX); // PT: half-angle formula: tan(a/2) = sin(a)/(1+cos(a)) return sin / (1.0f + cos); } PX_INLINE PxQuat quatFromTanQVector(const PxVec3& v) { PxReal v2 = v.dot(v); if(v2 < 1e-12f) return PxQuat(PxIdentity); PxReal d = 1 / (1 + v2); return PxQuat(v.x * 2, v.y * 2, v.z * 2, 1 - v2) * d; } PX_FORCE_INLINE PxVec3 cross100(const PxVec3& b) { return PxVec3(0.0f, -b.z, b.y); } PX_FORCE_INLINE PxVec3 cross010(const PxVec3& b) { return PxVec3(b.z, 0.0f, -b.x); } PX_FORCE_INLINE PxVec3 cross001(const PxVec3& b) { return PxVec3(-b.y, b.x, 0.0f); } PX_INLINE void decomposeVector(PxVec3& normalCompo, PxVec3& tangentCompo, const PxVec3& outwardDir, const PxVec3& outwardNormal) { normalCompo = outwardNormal * (outwardDir.dot(outwardNormal)); tangentCompo = outwardDir - normalCompo; } //! \brief Return (i+1)%3 // Avoid variable shift for XBox: // PX_INLINE PxU32 Ps::getNextIndex3(PxU32 i) { return (1<> 1)) & 3; } PX_INLINE PxMat33 rotFrom2Vectors(const PxVec3& from, const PxVec3& to) { // See bottom of http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/index.htm // Early exit if to = from if((from - to).magnitudeSquared() < 1e-4f) return PxMat33(PxIdentity); // Early exit if to = -from if((from + to).magnitudeSquared() < 1e-4f) return PxMat33::createDiagonal(PxVec3(1.0f, -1.0f, -1.0f)); PxVec3 n = from.cross(to); PxReal C = from.dot(to), S = PxSqrt(1 - C * C), CC = 1 - C; PxReal xx = n.x * n.x, yy = n.y * n.y, zz = n.z * n.z, xy = n.x * n.y, yz = n.y * n.z, xz = n.x * n.z; PxMat33 R; R(0, 0) = 1 + CC * (xx - 1); R(0, 1) = -n.z * S + CC * xy; R(0, 2) = n.y * S + CC * xz; R(1, 0) = n.z * S + CC * xy; R(1, 1) = 1 + CC * (yy - 1); R(1, 2) = -n.x * S + CC * yz; R(2, 0) = -n.y * S + CC * xz; R(2, 1) = n.x * S + CC * yz; R(2, 2) = 1 + CC * (zz - 1); return R; } PX_FOUNDATION_API void integrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel, PxReal timeStep, PxTransform& result); PX_INLINE void computeBasis(const PxVec3& dir, PxVec3& right, PxVec3& up) { // Derive two remaining vectors if(PxAbs(dir.y) <= 0.9999f) { right = PxVec3(dir.z, 0.0f, -dir.x); right.normalize(); // PT: normalize not needed for 'up' because dir & right are unit vectors, // and by construction the angle between them is 90 degrees (i.e. sin(angle)=1) up = PxVec3(dir.y * right.z, dir.z * right.x - dir.x * right.z, -dir.y * right.x); } else { right = PxVec3(1.0f, 0.0f, 0.0f); up = PxVec3(0.0f, dir.z, -dir.y); up.normalize(); } } PX_INLINE void computeBasis(const PxVec3& p0, const PxVec3& p1, PxVec3& dir, PxVec3& right, PxVec3& up) { // Compute the new direction vector dir = p1 - p0; dir.normalize(); // Derive two remaining vectors computeBasis(dir, right, up); } PX_FORCE_INLINE bool isAlmostZero(const PxVec3& v) { if(PxAbs(v.x) > 1e-6f || PxAbs(v.y) > 1e-6f || PxAbs(v.z) > 1e-6f) return false; return true; } } // namespace shdfnd } // namespace physx #endif