Revision control
++ // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
++ // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
++ // SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
++ // SPDX-License-Identifier: Apache-2.0
++
++ #include "foundation/PxSIMDHelpers.h"
++ #include "foundation/PxMathUtils.h"
++ #include "foundation/PxVec4.h"
++ #include "foundation/PxAssert.h"
++ #include "foundation/PxBasicTemplates.h"
++ #include "foundation/PxUtilities.h"
++ #include "foundation/PxTransform.h"
++
++ using namespace physx;
++ using namespace physx::intrinsics;
++
++ PX_FOUNDATION_API PxTransform physx::PxTransformFromPlaneEquation(const PxPlane& plane)
++ {
++ PxPlane p = plane;
++ p.normalize();
++
++ // special case handling for axis aligned planes
++ const PxReal halfsqrt2 = 0.707106781f;
++ PxQuat q;
++ if(2 == (p.n.x == 0.0f) + (p.n.y == 0.0f) + (p.n.z == 0.0f)) // special handling for axis aligned planes
++ {
++ if(p.n.x > 0) q = PxQuat(PxIdentity);
++ else if(p.n.x < 0) q = PxQuat(0, 0, 1.0f, 0);
++ else q = PxQuat(0.0f, -p.n.z, p.n.y, 1.0f) * halfsqrt2;
++ }
++ else q = PxShortestRotation(PxVec3(1.f,0,0), p.n);
++
++ return PxTransform(-p.n * p.d, q);
++ }
++
++ PX_FOUNDATION_API PxTransform physx::PxTransformFromSegment(const PxVec3& p0, const PxVec3& p1, PxReal* halfHeight)
++ {
++ const PxVec3 axis = p1-p0;
++ const PxReal height = axis.magnitude();
++ if(halfHeight)
++ *halfHeight = height/2;
++
++ return PxTransform((p1+p0) * 0.5f,
++ height<1e-6f ? PxQuat(PxIdentity) : PxShortestRotation(PxVec3(1.f,0,0), axis/height));
++ }
++
++ PX_FOUNDATION_API PxQuat physx::PxShortestRotation(const PxVec3& v0, const PxVec3& v1)
++ {
++ const PxReal d = v0.dot(v1);
++ const PxVec3 cross = v0.cross(v1);
++
++ const PxQuat q = d > -1 ? PxQuat(cross.x, cross.y, cross.z, 1 + d) : PxAbs(v0.x) < 0.1f ? PxQuat(0.0f, v0.z, -v0.y, 0.0f)
++ : PxQuat(v0.y, -v0.x, 0.0f, 0.0f);
++
++ return q.getNormalized();
++ }
++
++ // indexed rotation around axis, with sine and cosine of half-angle
++ static PxQuat indexedRotation(PxU32 axis, PxReal s, PxReal c)
++ {
++ PxReal v[3] = { 0, 0, 0 };
++ v[axis] = s;
++ return PxQuat(v[0], v[1], v[2], c);
++ }
++
++ PX_FOUNDATION_API PxVec3 physx::PxDiagonalize(const PxMat33& m, PxQuat& massFrame)
++ {
++ // jacobi rotation using quaternions (from an idea of Stan Melax, with fix for precision issues)
++
++ const PxU32 MAX_ITERS = 24;
++
++ PxQuat q(PxIdentity);
++
++ PxMat33 d;
++ for(PxU32 i = 0; i < MAX_ITERS; i++)
++ {
++ // PT: removed for now, it makes one UT fail because the error is slightly above the threshold
++ //const PxMat33Padded axes(q);
++ const PxMat33 axes(q);
++ d = axes.getTranspose() * m * axes;
++
++ const PxReal d0 = PxAbs(d[1][2]), d1 = PxAbs(d[0][2]), d2 = PxAbs(d[0][1]);
++ const PxU32 a = PxU32(d0 > d1 && d0 > d2 ? 0 : d1 > d2 ? 1 : 2); // rotation axis index, from largest off-diagonal element
++
++ const PxU32 a1 = PxGetNextIndex3(a), a2 = PxGetNextIndex3(a1);
++ if(d[a1][a2] == 0.0f || PxAbs(d[a1][a1] - d[a2][a2]) > 2e6f * PxAbs(2.0f * d[a1][a2]))
++ break;
++
++ const PxReal w = (d[a1][a1] - d[a2][a2]) / (2.0f * d[a1][a2]); // cot(2 * phi), where phi is the rotation angle
++ const PxReal absw = PxAbs(w);
++
++ PxQuat r;
++ if(absw > 1000)
++ r = indexedRotation(a, 1.0f / (4.0f * w), 1.0f); // h will be very close to 1, so use small angle approx instead
++ else
++ {
++ const PxReal t = 1.0f / (absw + PxSqrt(w * w + 1.0f)); // absolute value of tan phi
++ const PxReal h = 1.0f / PxSqrt(t * t + 1.0f); // absolute value of cos phi
++
++ PX_ASSERT(h != 1); // |w|<1000 guarantees this with typical IEEE754 machine eps (approx 6e-8)
++ r = indexedRotation(a, PxSqrt((1.0f - h) / 2.0f) * PxSign(w), PxSqrt((1.0f + h) / 2.0f));
++ }
++
++ q = (q * r).getNormalized();
++ }
++
++ massFrame = q;
++ return PxVec3(d.column0.x, d.column1.y, d.column2.z);
++ }
++
++ /**
++ \brief computes a oriented bounding box around the scaled basis.
++ \param basis Input = skewed basis, Output = (normalized) orthogonal basis.
++ \return Bounding box extent.
++ */
++ PxVec3 physx::PxOptimizeBoundingBox(PxMat33& basis)
++ {
++ PxVec3* PX_RESTRICT vec = &basis[0]; // PT: don't copy vectors if not needed...
++
++ // PT: since we store the magnitudes to memory, we can avoid the FCMPs afterwards
++ PxVec3 magnitude(vec[0].magnitudeSquared(), vec[1].magnitudeSquared(), vec[2].magnitudeSquared());
++
++ // find indices sorted by magnitude
++ unsigned int i = magnitude[1] > magnitude[0] ? 1 : 0u;
++ unsigned int j = magnitude[2] > magnitude[1 - i] ? 2 : 1 - i;
++ const unsigned int k = 3 - i - j;
++
++ if(magnitude[i] < magnitude[j])
++ PxSwap(i, j);
++
++ PX_ASSERT(magnitude[i] >= magnitude[j] && magnitude[i] >= magnitude[k] && magnitude[j] >= magnitude[k]);
++
++ // ortho-normalize basis
++
++ const PxReal invSqrt = magnitude[i] != 0.0f ? PxRecipSqrt(magnitude[i]) : 0.0f;
++ magnitude[i] *= invSqrt;
++ vec[i] *= invSqrt; // normalize the first axis
++ PxReal dotij = vec[i].dot(vec[j]);
++ PxReal dotik = vec[i].dot(vec[k]);
++ magnitude[i] += PxAbs(dotij) + PxAbs(dotik); // elongate the axis by projection of the other two
++ vec[j] -= vec[i] * dotij; // orthogonize the two remaining axii relative to vec[i]
++ vec[k] -= vec[i] * dotik;
++
++ magnitude[j] = vec[j].normalize();
++ PxReal dotjk = vec[j].dot(vec[k]);
++ magnitude[j] += PxAbs(dotjk); // elongate the axis by projection of the other one
++ vec[k] -= vec[j] * dotjk; // orthogonize vec[k] relative to vec[j]
++
++ magnitude[k] = vec[k].normalize();
++
++ return magnitude;
++ }
++
++ void physx::PxIntegrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel,
++ PxReal timeStep, PxTransform& result)
++ {
++ result.p = curTrans.p + linvel * timeStep;
++
++ // from void DynamicsContext::integrateAtomPose(PxsRigidBody* atom, Cm::BitMap &shapeChangedMap) const:
++ // Integrate the rotation using closed form quaternion integrator
++ PxReal w = angvel.magnitudeSquared();
++
++ if (w != 0.0f)
++ {
++ w = PxSqrt(w);
++ if (w != 0.0f)
++ {
++ const PxReal v = timeStep * w * 0.5f;
++ const PxReal q = PxCos(v);
++ const PxReal s = PxSin(v) / w;
++
++ const PxVec3 pqr = angvel * s;
++ const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
++ PxQuat out; // need to have temporary, otherwise we may overwrite input if &curTrans == &result.
++ out = quatVel * curTrans.q;
++ out.x += curTrans.q.x * q;
++ out.y += curTrans.q.y * q;
++ out.z += curTrans.q.z * q;
++ out.w += curTrans.q.w * q;
++ result.q = out;
++ return;
++ }
++ }
++ // orientation stays the same - convert from quat to matrix:
++ result.q = curTrans.q;
++ }