diff options
Diffstat (limited to 'PxShared/src/foundation/include/PsMathUtils.h')
| -rw-r--r-- | PxShared/src/foundation/include/PsMathUtils.h | 694 |
1 files changed, 0 insertions, 694 deletions
diff --git a/PxShared/src/foundation/include/PsMathUtils.h b/PxShared/src/foundation/include/PsMathUtils.h deleted file mode 100644 index 789cf3f..0000000 --- a/PxShared/src/foundation/include/PsMathUtils.h +++ /dev/null @@ -1,694 +0,0 @@ -// This code contains NVIDIA Confidential Information and is disclosed to you -// under a form of NVIDIA software license agreement provided separately to you. -// -// Notice -// NVIDIA Corporation and its licensors retain all intellectual property and -// proprietary rights in and to this software and related documentation and -// any modifications thereto. Any use, reproduction, disclosure, or -// distribution of this software and related documentation without an express -// license agreement from NVIDIA Corporation is strictly prohibited. -// -// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES -// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO -// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, -// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. -// -// Information and code furnished is believed to be accurate and reliable. -// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such -// information or for any infringement of patents or other rights of third parties that may -// result from its use. No license is granted by implication or otherwise under any patent -// or patent rights of NVIDIA Corporation. Details are subject to change without notice. -// This code supersedes and replaces all information previously supplied. -// NVIDIA Corporation products are not authorized for use as critical -// components in life support devices or systems without express written approval of -// NVIDIA Corporation. -// -// Copyright (c) 2008-2017 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(); -} - -// 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_INLINE PxReal tanHalf(PxReal sin, PxReal cos) -{ - return sin / (1 + 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<<i) & 3; } -PX_INLINE PxU32 getNextIndex3(PxU32 i) -{ - return (i + 1 + (i >> 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 |