1//
2// Redistribution and use in source and binary forms, with or without
3// modification, are permitted provided that the following conditions
4// are met:
5// * Redistributions of source code must retain the above copyright
6// notice, this list of conditions and the following disclaimer.
7// * Redistributions in binary form must reproduce the above copyright
8// notice, this list of conditions and the following disclaimer in the
9// documentation and/or other materials provided with the distribution.
10// * Neither the name of NVIDIA CORPORATION nor the names of its
11// contributors may be used to endorse or promote products derived
12// from this software without specific prior written permission.
13//
14// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
15// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
17// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
18// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
19// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
22// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25//
26// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
27// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
28// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
29
30#ifndef PSFOUNDATION_PSMATHUTILS_H
31#define PSFOUNDATION_PSMATHUTILS_H
32
33#include "foundation/PxPreprocessor.h"
34#include "foundation/PxTransform.h"
35#include "foundation/PxMat33.h"
36#include "Ps.h"
37#include "PsIntrinsics.h"
38
39// General guideline is: if it's an abstract math function, it belongs here.
40// If it's a math function where the inputs have specific semantics (e.g.
41// separateSwingTwist) it doesn't.
42
43namespace physx
44{
45namespace shdfnd
46{
47/**
48\brief sign returns the sign of its argument. The sign of zero is undefined.
49*/
50PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 sign(const PxF32 a)
51{
52 return intrinsics::sign(a);
53}
54
55/**
56\brief sign returns the sign of its argument. The sign of zero is undefined.
57*/
58PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 sign(const PxF64 a)
59{
60 return (a >= 0.0) ? 1.0 : -1.0;
61}
62
63/**
64\brief sign returns the sign of its argument. The sign of zero is undefined.
65*/
66PX_CUDA_CALLABLE PX_FORCE_INLINE PxI32 sign(const PxI32 a)
67{
68 return (a >= 0) ? 1 : -1;
69}
70
71/**
72\brief Returns true if the two numbers are within eps of each other.
73*/
74PX_CUDA_CALLABLE PX_FORCE_INLINE bool equals(const PxF32 a, const PxF32 b, const PxF32 eps)
75{
76 return (PxAbs(a: a - b) < eps);
77}
78
79/**
80\brief Returns true if the two numbers are within eps of each other.
81*/
82PX_CUDA_CALLABLE PX_FORCE_INLINE bool equals(const PxF64 a, const PxF64 b, const PxF64 eps)
83{
84 return (PxAbs(a: a - b) < eps);
85}
86
87/**
88\brief The floor function returns a floating-point value representing the largest integer that is less than or equal to
89x.
90*/
91PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 floor(const PxF32 a)
92{
93 return floatFloor(x: a);
94}
95
96/**
97\brief The floor function returns a floating-point value representing the largest integer that is less than or equal to
98x.
99*/
100PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 floor(const PxF64 a)
101{
102 return ::floor(x: a);
103}
104
105/**
106\brief The ceil function returns a single value representing the smallest integer that is greater than or equal to x.
107*/
108PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 ceil(const PxF32 a)
109{
110 return ::ceilf(x: a);
111}
112
113/**
114\brief The ceil function returns a double value representing the smallest integer that is greater than or equal to x.
115*/
116PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 ceil(const PxF64 a)
117{
118 return ::ceil(x: a);
119}
120
121/**
122\brief mod returns the floating-point remainder of x / y.
123
124If the value of y is 0.0, mod returns a quiet NaN.
125*/
126PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 mod(const PxF32 x, const PxF32 y)
127{
128 return PxF32(::fmodf(x: x, y: y));
129}
130
131/**
132\brief mod returns the floating-point remainder of x / y.
133
134If the value of y is 0.0, mod returns a quiet NaN.
135*/
136PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 mod(const PxF64 x, const PxF64 y)
137{
138 return ::fmod(x: x, y: y);
139}
140
141/**
142\brief Square.
143*/
144PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 sqr(const PxF32 a)
145{
146 return a * a;
147}
148
149/**
150\brief Square.
151*/
152PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 sqr(const PxF64 a)
153{
154 return a * a;
155}
156
157/**
158\brief Calculates x raised to the power of y.
159*/
160PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 pow(const PxF32 x, const PxF32 y)
161{
162 return ::powf(x: x, y: y);
163}
164
165/**
166\brief Calculates x raised to the power of y.
167*/
168PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 pow(const PxF64 x, const PxF64 y)
169{
170 return ::pow(x: x, y: y);
171}
172
173/**
174\brief Calculates e^n
175*/
176PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 exp(const PxF32 a)
177{
178 return ::expf(x: a);
179}
180/**
181
182\brief Calculates e^n
183*/
184PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 exp(const PxF64 a)
185{
186 return ::exp(x: a);
187}
188
189/**
190\brief Calculates 2^n
191*/
192PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 exp2(const PxF32 a)
193{
194 return ::expf(x: a * 0.693147180559945309417f);
195}
196/**
197
198\brief Calculates 2^n
199*/
200PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 exp2(const PxF64 a)
201{
202 return ::exp(x: a * 0.693147180559945309417);
203}
204
205/**
206\brief Calculates logarithms.
207*/
208PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 logE(const PxF32 a)
209{
210 return ::logf(x: a);
211}
212
213/**
214\brief Calculates logarithms.
215*/
216PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 logE(const PxF64 a)
217{
218 return ::log(x: a);
219}
220
221/**
222\brief Calculates logarithms.
223*/
224PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 log2(const PxF32 a)
225{
226 return ::logf(x: a) / 0.693147180559945309417f;
227}
228
229/**
230\brief Calculates logarithms.
231*/
232PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 log2(const PxF64 a)
233{
234 return ::log(x: a) / 0.693147180559945309417;
235}
236
237/**
238\brief Calculates logarithms.
239*/
240PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 log10(const PxF32 a)
241{
242 return ::log10f(x: a);
243}
244
245/**
246\brief Calculates logarithms.
247*/
248PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 log10(const PxF64 a)
249{
250 return ::log10(x: a);
251}
252
253/**
254\brief Converts degrees to radians.
255*/
256PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 degToRad(const PxF32 a)
257{
258 return 0.01745329251994329547f * a;
259}
260
261/**
262\brief Converts degrees to radians.
263*/
264PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 degToRad(const PxF64 a)
265{
266 return 0.01745329251994329547 * a;
267}
268
269/**
270\brief Converts radians to degrees.
271*/
272PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 radToDeg(const PxF32 a)
273{
274 return 57.29577951308232286465f * a;
275}
276
277/**
278\brief Converts radians to degrees.
279*/
280PX_CUDA_CALLABLE PX_FORCE_INLINE PxF64 radToDeg(const PxF64 a)
281{
282 return 57.29577951308232286465 * a;
283}
284
285//! \brief compute sine and cosine at the same time. There is a 'fsincos' on PC that we probably want to use here
286PX_CUDA_CALLABLE PX_FORCE_INLINE void sincos(const PxF32 radians, PxF32& sin, PxF32& cos)
287{
288 /* something like:
289 _asm fld Local
290 _asm fsincos
291 _asm fstp LocalCos
292 _asm fstp LocalSin
293 */
294 sin = PxSin(a: radians);
295 cos = PxCos(a: radians);
296}
297
298/**
299\brief uniform random number in [a,b]
300*/
301PX_FORCE_INLINE PxI32 rand(const PxI32 a, const PxI32 b)
302{
303 return a + PxI32(::rand() % (b - a + 1));
304}
305
306/**
307\brief uniform random number in [a,b]
308*/
309PX_FORCE_INLINE PxF32 rand(const PxF32 a, const PxF32 b)
310{
311 return a + (b - a) * PxF32(::rand()) / PxF32(RAND_MAX);
312}
313
314//! \brief return angle between two vectors in radians
315PX_CUDA_CALLABLE PX_FORCE_INLINE PxF32 angle(const PxVec3& v0, const PxVec3& v1)
316{
317 const PxF32 cos = v0.dot(v: v1); // |v0|*|v1|*Cos(Angle)
318 const PxF32 sin = (v0.cross(v: v1)).magnitude(); // |v0|*|v1|*Sin(Angle)
319 return PxAtan2(x: sin, y: cos);
320}
321
322//! If possible use instead fsel on the dot product /*fsel(d.dot(p),onething,anotherthing);*/
323//! Compares orientations (more readable, user-friendly function)
324PX_CUDA_CALLABLE PX_FORCE_INLINE bool sameDirection(const PxVec3& d, const PxVec3& p)
325{
326 return d.dot(v: p) >= 0.0f;
327}
328
329//! Checks 2 values have different signs
330PX_CUDA_CALLABLE PX_FORCE_INLINE IntBool differentSign(PxReal f0, PxReal f1)
331{
332#if !PX_EMSCRIPTEN
333 union
334 {
335 PxU32 u;
336 PxReal f;
337 } u1, u2;
338 u1.f = f0;
339 u2.f = f1;
340 return IntBool((u1.u ^ u2.u) & PX_SIGN_BITMASK);
341#else
342 // javascript floats are 64-bits...
343 return IntBool( (f0*f1) < 0.0f );
344#endif
345}
346
347PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 star(const PxVec3& v)
348{
349 return PxMat33(PxVec3(0, v.z, -v.y), PxVec3(-v.z, 0, v.x), PxVec3(v.y, -v.x, 0));
350}
351
352PX_CUDA_CALLABLE PX_INLINE PxVec3 log(const PxQuat& q)
353{
354 const PxReal s = q.getImaginaryPart().magnitude();
355 if(s < 1e-12f)
356 return PxVec3(0.0f);
357 // force the half-angle to have magnitude <= pi/2
358 PxReal halfAngle = q.w < 0 ? PxAtan2(x: -s, y: -q.w) : PxAtan2(x: s, y: q.w);
359 PX_ASSERT(halfAngle >= -PxPi / 2 && halfAngle <= PxPi / 2);
360
361 return q.getImaginaryPart().getNormalized() * 2.f * halfAngle;
362}
363
364PX_CUDA_CALLABLE PX_INLINE PxQuat exp(const PxVec3& v)
365{
366 const PxReal m = v.magnitudeSquared();
367 return m < 1e-24f ? PxQuat(PxIdentity) : PxQuat(PxSqrt(a: m), v * PxRecipSqrt(a: m));
368}
369
370// quat to rotate v0 t0 v1
371PX_CUDA_CALLABLE PX_INLINE PxQuat rotationArc(const PxVec3& v0, const PxVec3& v1)
372{
373 const PxVec3 cross = v0.cross(v: v1);
374 const PxReal d = v0.dot(v: v1);
375 if(d <= -0.99999f)
376 return (PxAbs(a: v0.x) < 0.1f ? PxQuat(0.0f, v0.z, -v0.y, 0.0f) : PxQuat(v0.y, -v0.x, 0.0, 0.0)).getNormalized();
377
378 const PxReal s = PxSqrt(a: (1 + d) * 2), r = 1 / s;
379
380 return PxQuat(cross.x * r, cross.y * r, cross.z * r, s * 0.5f).getNormalized();
381}
382
383/**
384\brief returns largest axis
385*/
386PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 largestAxis(const PxVec3& v)
387{
388 PxU32 m = PxU32(v.y > v.x ? 1 : 0);
389 return v.z > v[m] ? 2 : m;
390}
391
392/**
393\brief returns indices for the largest axis and 2 other axii
394*/
395PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 largestAxis(const PxVec3& v, PxU32& other1, PxU32& other2)
396{
397 if(v.x >= PxMax(a: v.y, b: v.z))
398 {
399 other1 = 1;
400 other2 = 2;
401 return 0;
402 }
403 else if(v.y >= v.z)
404 {
405 other1 = 0;
406 other2 = 2;
407 return 1;
408 }
409 else
410 {
411 other1 = 0;
412 other2 = 1;
413 return 2;
414 }
415}
416
417/**
418\brief returns axis with smallest absolute value
419*/
420PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 closestAxis(const PxVec3& v)
421{
422 PxU32 m = PxU32(PxAbs(a: v.y) > PxAbs(a: v.x) ? 1 : 0);
423 return PxAbs(a: v.z) > PxAbs(a: v[m]) ? 2 : m;
424}
425
426PX_CUDA_CALLABLE PX_INLINE PxU32 closestAxis(const PxVec3& v, PxU32& j, PxU32& k)
427{
428 // find largest 2D plane projection
429 const PxF32 absPx = PxAbs(a: v.x);
430 const PxF32 absNy = PxAbs(a: v.y);
431 const PxF32 absNz = PxAbs(a: v.z);
432
433 PxU32 m = 0; // x biggest axis
434 j = 1;
435 k = 2;
436 if(absNy > absPx && absNy > absNz)
437 {
438 // y biggest
439 j = 2;
440 k = 0;
441 m = 1;
442 }
443 else if(absNz > absPx)
444 {
445 // z biggest
446 j = 0;
447 k = 1;
448 m = 2;
449 }
450 return m;
451}
452
453/*!
454Extend an edge along its length by a factor
455*/
456PX_CUDA_CALLABLE PX_FORCE_INLINE void makeFatEdge(PxVec3& p0, PxVec3& p1, PxReal fatCoeff)
457{
458 PxVec3 delta = p1 - p0;
459
460 const PxReal m = delta.magnitude();
461 if(m > 0.0f)
462 {
463 delta *= fatCoeff / m;
464 p0 -= delta;
465 p1 += delta;
466 }
467}
468
469//! Compute point as combination of barycentric coordinates
470PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3
471computeBarycentricPoint(const PxVec3& p0, const PxVec3& p1, const PxVec3& p2, PxReal u, PxReal v)
472{
473 // This seems to confuse the compiler...
474 // return (1.0f - u - v)*p0 + u*p1 + v*p2;
475 const PxF32 w = 1.0f - u - v;
476 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);
477}
478
479// generates a pair of quaternions (swing, twist) such that in = swing * twist, with
480// swing.x = 0
481// twist.y = twist.z = 0, and twist is a unit quat
482PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& swing, PxQuat& twist)
483{
484 twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity);
485 swing = q * twist.getConjugate();
486}
487
488PX_FORCE_INLINE float computeSwingAngle(float swingYZ, float swingW)
489{
490 return 4.0f * PxAtan2(x: swingYZ, y: 1.0f + swingW); // tan (t/2) = sin(t)/(1+cos t), so this is the quarter angle
491}
492
493// generate two tangent vectors to a given normal
494PX_FORCE_INLINE void normalToTangents(const PxVec3& normal, PxVec3& tangent0, PxVec3& tangent1)
495{
496 tangent0 = PxAbs(a: normal.x) < 0.70710678f ? PxVec3(0, -normal.z, normal.y) : PxVec3(-normal.y, normal.x, 0);
497 tangent0.normalize();
498 tangent1 = normal.cross(v: tangent0);
499}
500
501/**
502\brief computes a oriented bounding box around the scaled basis.
503\param basis Input = skewed basis, Output = (normalized) orthogonal basis.
504\return Bounding box extent.
505*/
506PX_FOUNDATION_API PxVec3 optimizeBoundingBox(PxMat33& basis);
507
508PX_FOUNDATION_API PxQuat slerp(const PxReal t, const PxQuat& left, const PxQuat& right);
509
510PX_CUDA_CALLABLE PX_INLINE PxVec3 ellipseClamp(const PxVec3& point, const PxVec3& radii)
511{
512 // This function need to be implemented in the header file because
513 // it is included in a spu shader program.
514
515 // finds the closest point on the ellipse to a given point
516
517 // (p.y, p.z) is the input point
518 // (e.y, e.z) are the radii of the ellipse
519
520 // lagrange multiplier method with Newton/Halley hybrid root-finder.
521 // see http://www.geometrictools.com/Documentation/DistancePointToEllipse2.pdf
522 // for proof of Newton step robustness and initial estimate.
523 // Halley converges much faster but sometimes overshoots - when that happens we take
524 // a newton step instead
525
526 // converges in 1-2 iterations where D&C works well, and it's good with 4 iterations
527 // with any ellipse that isn't completely crazy
528
529 const PxU32 MAX_ITERATIONS = 20;
530 const PxReal convergenceThreshold = 1e-4f;
531
532 // iteration requires first quadrant but we recover generality later
533
534 PxVec3 q(0, PxAbs(a: point.y), PxAbs(a: point.z));
535 const PxReal tinyEps = 1e-6f; // very close to minor axis is numerically problematic but trivial
536 if(radii.y >= radii.z)
537 {
538 if(q.z < tinyEps)
539 return PxVec3(0, point.y > 0 ? radii.y : -radii.y, 0);
540 }
541 else
542 {
543 if(q.y < tinyEps)
544 return PxVec3(0, 0, point.z > 0 ? radii.z : -radii.z);
545 }
546
547 PxVec3 denom, e2 = radii.multiply(a: radii), eq = radii.multiply(a: q);
548
549 // we can use any initial guess which is > maximum(-e.y^2,-e.z^2) and for which f(t) is > 0.
550 // this guess works well near the axes, but is weak along the diagonals.
551
552 PxReal t = PxMax(a: eq.y - e2.y, b: eq.z - e2.z);
553
554 for(PxU32 i = 0; i < MAX_ITERATIONS; i++)
555 {
556 denom = PxVec3(0, 1 / (t + e2.y), 1 / (t + e2.z));
557 PxVec3 denom2 = eq.multiply(a: denom);
558
559 PxVec3 fv = denom2.multiply(a: denom2);
560 PxReal f = fv.y + fv.z - 1;
561
562 // although in exact arithmetic we are guaranteed f>0, we can get here
563 // on the first iteration via catastrophic cancellation if the point is
564 // very close to the origin. In that case we just behave as if f=0
565
566 if(f < convergenceThreshold)
567 return e2.multiply(a: point).multiply(a: denom);
568
569 PxReal df = fv.dot(v: denom) * -2.0f;
570 t = t - f / df;
571 }
572
573 // we didn't converge, so clamp what we have
574 PxVec3 r = e2.multiply(a: point).multiply(a: denom);
575 return r * PxRecipSqrt(a: sqr(a: r.y / radii.y) + sqr(a: r.z / radii.z));
576}
577
578PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal tanHalf(PxReal sin, PxReal cos)
579{
580 // PT: avoids divide by zero for singularity. We return sqrt(FLT_MAX) instead of FLT_MAX
581 // to make sure the calling code doesn't generate INF values when manipulating the returned value
582 // (some joints multiply it by 4, etc).
583 if(cos==-1.0f)
584 return sin<0.0f ? -sqrtf(FLT_MAX) : sqrtf(FLT_MAX);
585
586 // PT: half-angle formula: tan(a/2) = sin(a)/(1+cos(a))
587 return sin / (1.0f + cos);
588}
589
590PX_INLINE PxQuat quatFromTanQVector(const PxVec3& v)
591{
592 PxReal v2 = v.dot(v);
593 if(v2 < 1e-12f)
594 return PxQuat(PxIdentity);
595 PxReal d = 1 / (1 + v2);
596 return PxQuat(v.x * 2, v.y * 2, v.z * 2, 1 - v2) * d;
597}
598
599PX_FORCE_INLINE PxVec3 cross100(const PxVec3& b)
600{
601 return PxVec3(0.0f, -b.z, b.y);
602}
603PX_FORCE_INLINE PxVec3 cross010(const PxVec3& b)
604{
605 return PxVec3(b.z, 0.0f, -b.x);
606}
607PX_FORCE_INLINE PxVec3 cross001(const PxVec3& b)
608{
609 return PxVec3(-b.y, b.x, 0.0f);
610}
611
612PX_INLINE void decomposeVector(PxVec3& normalCompo, PxVec3& tangentCompo, const PxVec3& outwardDir,
613 const PxVec3& outwardNormal)
614{
615 normalCompo = outwardNormal * (outwardDir.dot(v: outwardNormal));
616 tangentCompo = outwardDir - normalCompo;
617}
618
619//! \brief Return (i+1)%3
620// Avoid variable shift for XBox:
621// PX_INLINE PxU32 Ps::getNextIndex3(PxU32 i) { return (1<<i) & 3; }
622PX_INLINE PxU32 getNextIndex3(PxU32 i)
623{
624 return (i + 1 + (i >> 1)) & 3;
625}
626
627PX_INLINE PxMat33 rotFrom2Vectors(const PxVec3& from, const PxVec3& to)
628{
629 // See bottom of http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/index.htm
630
631 // Early exit if to = from
632 if((from - to).magnitudeSquared() < 1e-4f)
633 return PxMat33(PxIdentity);
634
635 // Early exit if to = -from
636 if((from + to).magnitudeSquared() < 1e-4f)
637 return PxMat33::createDiagonal(d: PxVec3(1.0f, -1.0f, -1.0f));
638
639 PxVec3 n = from.cross(v: to);
640
641 PxReal C = from.dot(v: to), S = PxSqrt(a: 1 - C * C), CC = 1 - C;
642
643 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;
644
645 PxMat33 R;
646
647 R(0, 0) = 1 + CC * (xx - 1);
648 R(0, 1) = -n.z * S + CC * xy;
649 R(0, 2) = n.y * S + CC * xz;
650
651 R(1, 0) = n.z * S + CC * xy;
652 R(1, 1) = 1 + CC * (yy - 1);
653 R(1, 2) = -n.x * S + CC * yz;
654
655 R(2, 0) = -n.y * S + CC * xz;
656 R(2, 1) = n.x * S + CC * yz;
657 R(2, 2) = 1 + CC * (zz - 1);
658
659 return R;
660}
661
662PX_FOUNDATION_API void integrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel,
663 PxReal timeStep, PxTransform& result);
664
665PX_INLINE void computeBasis(const PxVec3& dir, PxVec3& right, PxVec3& up)
666{
667 // Derive two remaining vectors
668 if(PxAbs(a: dir.y) <= 0.9999f)
669 {
670 right = PxVec3(dir.z, 0.0f, -dir.x);
671 right.normalize();
672
673 // PT: normalize not needed for 'up' because dir & right are unit vectors,
674 // and by construction the angle between them is 90 degrees (i.e. sin(angle)=1)
675 up = PxVec3(dir.y * right.z, dir.z * right.x - dir.x * right.z, -dir.y * right.x);
676 }
677 else
678 {
679 right = PxVec3(1.0f, 0.0f, 0.0f);
680
681 up = PxVec3(0.0f, dir.z, -dir.y);
682 up.normalize();
683 }
684}
685
686PX_INLINE void computeBasis(const PxVec3& p0, const PxVec3& p1, PxVec3& dir, PxVec3& right, PxVec3& up)
687{
688 // Compute the new direction vector
689 dir = p1 - p0;
690 dir.normalize();
691
692 // Derive two remaining vectors
693 computeBasis(dir, right, up);
694}
695
696PX_FORCE_INLINE bool isAlmostZero(const PxVec3& v)
697{
698 if(PxAbs(a: v.x) > 1e-6f || PxAbs(a: v.y) > 1e-6f || PxAbs(a: v.z) > 1e-6f)
699 return false;
700 return true;
701}
702
703} // namespace shdfnd
704} // namespace physx
705
706#endif
707

source code of qtquick3dphysics/src/3rdparty/PhysX/source/foundation/include/PsMathUtils.h