// 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-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.

#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

	PxReal invSqrt = PxRecipSqrt(magnitude[i]);
	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;
}