//
// 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.  

#include "geometry/PxConvexMeshGeometry.h"
#include "geometry/PxSphereGeometry.h"
#include "GuSweepTests.h"
#include "GuHeightFieldUtil.h"
#include "CmScaling.h"
#include "GuConvexMesh.h"
#include "GuIntersectionRayPlane.h"
#include "GuVecBox.h"
#include "GuVecCapsule.h"
#include "GuVecConvexHull.h"
#include "GuSweepMTD.h"
#include "GuSweepSphereCapsule.h"
#include "GuSweepCapsuleCapsule.h"
#include "GuSweepTriangleUtils.h"
#include "GuSweepCapsuleTriangle.h"
#include "GuInternal.h"
#include "GuGJKRaycast.h"

using namespace physx;
using namespace Gu;
using namespace Cm;
using namespace physx::shdfnd::aos;

static const PxReal gEpsilon = .01f;

static PxU32 computeSweepConvexPlane(
	const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxU32& nbPolys, const PxTransform& pose,
	const PxVec3& impact_, const PxVec3& unitDir)
{
	PX_ASSERT(nbPolys);

	const PxVec3 impact = impact_ - unitDir * gEpsilon;

	const PxVec3 localPoint = pose.transformInv(impact);
	const PxVec3 localDir = pose.rotateInv(unitDir);

	const FastVertex2ShapeScaling scaling(convexGeom.scale);

	PxU32 minIndex = 0;
	PxReal minD = PX_MAX_REAL;
	for(PxU32 j=0; j<nbPolys; j++)
	{
		const PxPlane& pl = hullData->mPolygons[j].mPlane;

		PxPlane plane;
		scaling.transformPlaneToShapeSpace(pl.n, pl.d, plane.n, plane.d);

		PxReal d = plane.distance(localPoint);
		if(d<0.0f)
			continue;

		const PxReal tweak = plane.n.dot(localDir) * gEpsilon;
		d += tweak;

		if(d<minD)
		{
			minIndex = j;
			minD = d;
		}
	}
	return minIndex;
}

static PX_FORCE_INLINE bool computeFaceIndex(PxSweepHit& sweepHit, const PxHitFlags hitFlags, const PxConvexMeshGeometry& convexGeom, ConvexHullData* hullData, const PxTransform& pose, const PxVec3& unitDir)
{
	if(hitFlags & PxHitFlag::eFACE_INDEX)
	{
		// PT: compute closest polygon using the same tweak as in swept-capsule-vs-mesh
		sweepHit.faceIndex = computeSweepConvexPlane(convexGeom, hullData, hullData->mNbPolygons, pose, sweepHit.position, unitDir);
		sweepHit.flags |= PxHitFlag::eFACE_INDEX;
	}
	return true;
}

static PX_FORCE_INLINE bool hasInitialOverlap(PxSweepHit& sweepHit, const PxVec3& unitDir,
											  const FloatVArg toi,
											  const Vec3VArg normal, const Vec3VArg closestA,
											  const PsTransformV& convexPose,
											  const bool isMtd, const bool impactPointOnTheOtherShape)
{
	sweepHit.flags = PxHitFlag::eNORMAL;

	const FloatV zero = FZero();
	if(FAllGrtrOrEq(zero, toi))
	{
		//ML: initial overlap
		if(isMtd)
		{
			sweepHit.flags |= PxHitFlag::ePOSITION;
			const FloatV length = toi;
			const Vec3V worldPointA = convexPose.transform(closestA);
			const Vec3V worldNormal = V3Normalize(convexPose.rotate(normal));
			if(impactPointOnTheOtherShape)
			{
				const Vec3V destWorldPointA = V3NegScaleSub(worldNormal, length, worldPointA);
				V3StoreU(worldNormal, sweepHit.normal);
				V3StoreU(destWorldPointA, sweepHit.position);
			}
			else
			{
				const Vec3V destNormal = V3Neg(worldNormal);
				V3StoreU(destNormal, sweepHit.normal);
				V3StoreU(worldPointA, sweepHit.position);
			}
			FStore(length, &sweepHit.distance);
		}
		else
		{
			sweepHit.distance	= 0.0f;
			sweepHit.normal		= -unitDir;
		}
		sweepHit.faceIndex = 0xffffffff;
		return true;
	}
	return false;
}

/////////////////////////////////////////////////  sweepCapsule/Sphere  //////////////////////////////////////////////////////
bool sweepCapsule_SphereGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
	PX_UNUSED(capsuleGeom_);
	PX_UNUSED(capsulePose_);

	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);

	const Sphere sphere(pose.p, sphereGeom.radius+inflation);

	if(!sweepSphereCapsule(sphere, lss, -unitDir, distance, sweepHit.distance, sweepHit.position, sweepHit.normal, hitFlags))
		return false;

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	if(isMtd)
	{
		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;

		if(sweepHit.distance == 0.f)
		{
			//intialOverlap
			if(lss.p0 == lss.p1)
			{
				//sphere
				return computeSphere_SphereMTD(sphere, Sphere(lss.p0, lss.radius), sweepHit);
			}
			else
			{
				//capsule
				return computeSphere_CapsuleMTD(sphere, lss, sweepHit);
			}
		}
	}
	else
	{
		if(sweepHit.distance!=0.0f)
			sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
		else
			sweepHit.flags = PxHitFlag::eNORMAL;
	}
	return true;
}

bool sweepCapsule_PlaneGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
	PX_UNUSED(capsuleGeom_);
	PX_UNUSED(capsulePose_);

	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
	PX_UNUSED(geom);
//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);

	const PxPlane& worldPlane = getPlane(pose);

	const PxF32 capsuleRadius = lss.radius + inflation;

	PxU32 index = 0;
	PxVec3 pts[2];

	PxReal minDp = PX_MAX_REAL;

	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes

	// Find extreme point on the capsule
	// AP: removed if (lss.p0 == lss.p1 clause because it wasn't properly computing minDp)
	pts[0] = lss.p0;
	pts[1] = lss.p1;
	for(PxU32 i=0; i<2; i++)
	{
		const PxReal dp = pts[i].dot(worldPlane.n);
		if(dp<minDp)
		{
			minDp = dp;
			index = i;
		}
	}

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	if(isMtd)
	{
		//initial overlap with the plane
		if(minDp <= capsuleRadius - worldPlane.d)
		{
			sweepHit.flags = PxHitFlag::eNORMAL| PxHitFlag::ePOSITION;
			return computePlane_CapsuleMTD(worldPlane, lss, sweepHit);
		}
	}
	else
	{
		if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
		{
			// test if the capsule initially overlaps with plane
			if(minDp <= capsuleRadius - worldPlane.d)
			{
				sweepHit.flags		= PxHitFlag::eNORMAL;
				sweepHit.distance	= 0.0f;
				sweepHit.normal		= -unitDir;
				return true;
			}
		}
	}

	const PxVec3 ptOnCapsule = pts[index] - worldPlane.n*capsuleRadius;

	// Raycast extreme vertex against plane
	bool hitPlane = intersectRayPlane(ptOnCapsule, unitDir, worldPlane, sweepHit.distance, &sweepHit.position);
	if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
	{
		sweepHit.normal = worldPlane.n;
		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
		return true;
	}
	return false;
}

bool sweepCapsule_CapsuleGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
	PX_UNUSED(capsuleGeom_);
	PX_UNUSED(capsulePose_);

	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);

	Capsule staticCapsule;
	getCapsule(staticCapsule, capsuleGeom, pose);
	staticCapsule.radius +=inflation;

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	PxU16 outFlags;
	if(!sweepCapsuleCapsule(lss, staticCapsule, -unitDir, distance, sweepHit.distance, sweepHit.position, sweepHit.normal, hitFlags, outFlags))
		return false;

	sweepHit.flags = PxHitFlags(outFlags);
	if(sweepHit.distance == 0.0f)
	{
		//initial overlap
		if(isMtd)
		{
			sweepHit.flags |= PxHitFlag::ePOSITION;
			return computeCapsule_CapsuleMTD(lss, staticCapsule, sweepHit);
		}
	}
	return true;
}

bool sweepCapsule_ConvexGeom(GU_CAPSULE_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);

	using namespace Ps::aos;
	
	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);

	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
	ConvexHullData* hullData = &convexMesh->getHull();

	const Vec3V zeroV = V3Zero();
	const FloatV zero = FZero();
	const FloatV dist = FLoad(distance);
	const Vec3V worldDir = V3LoadU(unitDir);

	const PsTransformV capPose = loadTransformU(capsulePose_);
	const PsTransformV convexPose = loadTransformU(pose);

	const PsMatTransformV aToB(convexPose.transformInv(capPose));

	const FloatV capsuleHalfHeight = FLoad(capsuleGeom_.halfHeight);
	const FloatV capsuleRadius = FLoad(lss.radius);

	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);

	CapsuleV capsule(aToB.p, aToB.rotate( V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());

	const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));

	bool isMtd = hitFlags & PxHitFlag::eMTD;

	FloatV toi;
	Vec3V closestA, normal;//closestA and normal is in the local space of convex hull
	LocalConvex<CapsuleV> convexA(capsule);
	LocalConvex<ConvexHullV> convexB(convexHull);
	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, lss.radius + inflation, isMtd))
		return false;

	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, true))
		return true;

	sweepHit.flags |= PxHitFlag::ePOSITION;
	const Vec3V worldPointA = convexPose.transform(closestA);
	const FloatV length = FMul(dist, toi);
	const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
	const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
	V3StoreU(destNormal, sweepHit.normal);
	V3StoreU(destWorldPointA, sweepHit.position);
	FStore(length, &sweepHit.distance);

	return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
}

/////////////////////////////////////////////////  sweepBox  //////////////////////////////////////////////////////

bool sweepBox_PlaneGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
	PX_UNUSED(geom);
	PX_UNUSED(boxPose_);
	PX_UNUSED(boxGeom_);

//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);

	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes

	PxPlane worldPlane = getPlane(pose);
	worldPlane.d -=inflation;

	// Find extreme point on the box
	PxVec3 boxPts[8];
	box.computeBoxPoints(boxPts);
	PxU32 index = 0;
	PxReal minDp = PX_MAX_REAL;
	for(PxU32 i=0;i<8;i++)
	{
		const PxReal dp = boxPts[i].dot(worldPlane.n);
	
		if(dp<minDp)
		{
			minDp = dp;
			index = i;
		}
	}

	bool isMtd = hitFlags & PxHitFlag::eMTD;

	if(isMtd)
	{
		// test if box initially overlap with plane
		if(minDp <= -worldPlane.d)
		{
			sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
			//compute Mtd;
			return computePlane_BoxMTD(worldPlane, box, sweepHit);
		}
	}
	else
	{
		if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
		{
			// test if box initially overlap with plane
			if(minDp <= -worldPlane.d)
			{
				sweepHit.flags		= PxHitFlag::eNORMAL;
				sweepHit.distance	= 0.0f;
				sweepHit.normal		= -unitDir;
				return true;
			}
		}
	}

	// Raycast extreme vertex against plane
	bool hitPlane = intersectRayPlane(boxPts[index], unitDir, worldPlane, sweepHit.distance, &sweepHit.position);
	if(hitPlane && sweepHit.distance > 0 && sweepHit.distance <= distance)
	{
		sweepHit.normal = worldPlane.n;
		sweepHit.flags = PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
		return true;
	}
	return false;
}

bool sweepBox_ConvexGeom(GU_BOX_SWEEP_FUNC_PARAMS)
{
	PX_UNUSED(boxGeom_);

	using namespace Ps::aos;
	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);

	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
	ConvexHullData* hullData = &convexMesh->getHull();

	const Vec3V zeroV = V3Zero();
	const FloatV zero = FZero();

	const PsTransformV boxPose = loadTransformU(boxPose_);
	const PsTransformV convexPose = loadTransformU(pose);

	const PsMatTransformV aToB(convexPose.transformInv(boxPose));

	const Vec3V boxExtents = V3LoadU(box.extents);

	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
	
	BoxV boxV(zeroV, boxExtents);
	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());

	const Vec3V worldDir = V3LoadU(unitDir);
	const FloatV dist = FLoad(distance);
	const Vec3V dir = convexPose.rotateInv(V3Neg(V3Scale(worldDir, dist)));

	bool isMtd = hitFlags & PxHitFlag::eMTD;

	FloatV toi;
	Vec3V closestA, normal;
	RelativeConvex<BoxV> convexA(boxV, aToB);
	LocalConvex<ConvexHullV> convexB(convexHull);
	if(!gjkRaycastPenetration< RelativeConvex<BoxV>,LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd))
		return false;

	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, true))
		return true;

	sweepHit.flags |= PxHitFlag::ePOSITION;
	const Vec3V destNormal = V3Normalize(convexPose.rotate(normal));
	const FloatV length = FMul(dist, toi);
	const Vec3V worldPointA = convexPose.transform(closestA);
	const Vec3V destWorldPointA = V3ScaleAdd(worldDir, length, worldPointA);
	V3StoreU(destNormal, sweepHit.normal);
	V3StoreU(destWorldPointA, sweepHit.position);
	FStore(length, &sweepHit.distance);

	return computeFaceIndex(sweepHit, hitFlags, convexGeom, hullData, pose, unitDir);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////

bool Gu::sweepCapsuleTriangles(GU_SWEEP_TRIANGLES_FUNC_PARAMS(PxCapsuleGeometry))
{
	Capsule capsule;
	getCapsule(capsule, geom, pose);
	capsule.radius +=inflation;

	// Compute swept box
	Box capsuleBox;
	computeBoxAroundCapsule(capsule, capsuleBox);

	BoxPadded sweptBounds;
	computeSweptBox(sweptBounds, capsuleBox.extents, capsuleBox.center, capsuleBox.rot, unitDir, distance);

	PxVec3 triNormal;
	return sweepCapsuleTriangles_Precise(nbTris, triangles, capsule, unitDir, distance, cachedIndex, hit, triNormal, hitFlags, doubleSided, &sweptBounds);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////

bool sweepConvex_SphereGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);

	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
	ConvexHullData* hullData = &convexMesh->getHull();

	const Vec3V zeroV = V3Zero();
	const FloatV zero= FZero();

	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);

	const FloatV sphereRadius = FLoad(sphereGeom.radius);

	const PsTransformV sphereTransf = loadTransformU(pose);
	const PsTransformV convexTransf = loadTransformU(convexPose);

	const PsMatTransformV aToB(convexTransf.transformInv(sphereTransf));

	const Vec3V worldDir = V3LoadU(unitDir);
	const FloatV dist = FLoad(distance);
	const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));

	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());
	//CapsuleV capsule(zeroV, sphereRadius);
	CapsuleV capsule(aToB.p, sphereRadius);

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	FloatV toi;
	Vec3V closestA, normal;
	LocalConvex<CapsuleV> convexA(capsule);
	LocalConvex<ConvexHullV> convexB(convexHull);
	const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
	if(!gjkRaycastPenetration< LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, zero, zeroV, dir, toi, normal, closestA, sphereGeom.radius+inflation, isMtd))
		return false;

	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, false))
		return true;

	sweepHit.flags |= PxHitFlag::ePOSITION;
	const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
	const FloatV length = FMul(dist, toi);
	const Vec3V destWorldPointA = convexTransf.transform(closestA);
	V3StoreU(destNormal, sweepHit.normal);
	V3StoreU(destWorldPointA, sweepHit.position);
	FStore(length, &sweepHit.distance);
	sweepHit.faceIndex = 0xffffffff;
	return true;
}

bool sweepConvex_PlaneGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
	PX_UNUSED(hitFlags);
	PX_UNUSED(geom);

	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
	ConvexHullData* hullData = &convexMesh->getHull();

	sweepHit.faceIndex	= 0xFFFFffff; // spec says face index is undefined for planes

	const PxVec3* PX_RESTRICT hullVertices = hullData->getHullVertices();
	PxU32 numHullVertices = hullData->mNbHullVertices;

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	const FastVertex2ShapeScaling convexScaling(convexGeom.scale);

	PxPlane plane = getPlane(pose);
	plane.d -=inflation;

	sweepHit.distance	= distance;
	bool status = false;
	bool initialOverlap = false;
	while(numHullVertices--)
	{
		const PxVec3& vertex = *hullVertices++;
		const PxVec3 worldPt = convexPose.transform(convexScaling * vertex);
		float t;
		PxVec3 pointOnPlane;
		if(intersectRayPlane(worldPt, unitDir, plane, t, &pointOnPlane))
		{	
			if(plane.distance(worldPt) <= 0.0f)
			{
				initialOverlap = true;
				break;
				//// Convex touches plane
				//sweepHit.distance		= 0.0f;
				//sweepHit.flags			= PxHitFlag::eNORMAL;
				//sweepHit.normal			= -unitDir;
				//return true;
			}

			if(t > 0.0f && t <= sweepHit.distance)
			{
				sweepHit.distance	= t;
				sweepHit.flags		= PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
				sweepHit.position	= pointOnPlane;
				sweepHit.normal		= plane.n;
				status				= true;
			}
		}
	}

	if(initialOverlap)
	{
		if(isMtd)
		{
			sweepHit.flags		= PxHitFlag::ePOSITION | PxHitFlag::eNORMAL;
			return computePlane_ConvexMTD(plane, convexGeom, convexPose, sweepHit);
		}
		else
		{
			sweepHit.distance	= 0.0f;
			sweepHit.flags		= PxHitFlag::eNORMAL;
			sweepHit.normal		= -unitDir;
			return true;
		}
	}
	return status;
}

bool sweepConvex_CapsuleGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);

	Capsule capsule;
	getCapsule(capsule, capsuleGeom, pose);

	// remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
	PxHitFlags tempHitFlags = hitFlags;
	tempHitFlags &= ~PxHitFlag::eFACE_INDEX;

	if(!sweepCapsule_ConvexGeom(convexGeom, convexPose, capsuleGeom, pose, capsule, -unitDir, distance, sweepHit, tempHitFlags, inflation))
		return false;

	if(sweepHit.flags & PxHitFlag::ePOSITION)
		sweepHit.position += unitDir * sweepHit.distance;

	sweepHit.normal = -sweepHit.normal;
	sweepHit.faceIndex = 0xffffffff;
	return true;
}

bool sweepConvex_BoxGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
{
	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);

	Box box;
	buildFrom(box, pose.p, boxGeom.halfExtents, pose.q);

	// remove PxHitFlag::eFACE_INDEX, not neeeded to compute.
	PxHitFlags tempHitFlags = hitFlags;
	tempHitFlags &= ~PxHitFlag::eFACE_INDEX;

	if(!sweepBox_ConvexGeom(convexGeom, convexPose, boxGeom, pose, box, -unitDir, distance, sweepHit, tempHitFlags, inflation))
		return false;

	if(sweepHit.flags & PxHitFlag::ePOSITION)
		sweepHit.position += unitDir * sweepHit.distance;

	sweepHit.normal = -sweepHit.normal;
	sweepHit.faceIndex = 0xffffffff;
	return true;
}

bool sweepConvex_ConvexGeom(GU_CONVEX_SWEEP_FUNC_PARAMS)
{
	using namespace Ps::aos;
	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
	const PxConvexMeshGeometry& otherConvexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
	ConvexMesh& otherConvexMesh = *static_cast<ConvexMesh*>(otherConvexGeom.convexMesh);

	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
	ConvexHullData* hullData = &convexMesh->getHull();

	ConvexHullData* otherHullData = &otherConvexMesh.getHull();
	
	const Vec3V zeroV = V3Zero();
	const FloatV zero = FZero();

	const Vec3V otherVScale = V3LoadU_SafeReadW(otherConvexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
	const QuatV otherVQuat = QuatVLoadU(&otherConvexGeom.scale.rotation.x);

	const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
	const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);

	const PsTransformV otherTransf = loadTransformU(pose);
	const PsTransformV convexTransf = loadTransformU(convexPose);

	const Vec3V worldDir = V3LoadU(unitDir);
	const FloatV dist = FLoad(distance);
	const Vec3V dir = convexTransf.rotateInv(V3Scale(worldDir, dist));

	const PsMatTransformV aToB(convexTransf.transformInv(otherTransf));
	
	ConvexHullV otherConvexHull(otherHullData, zeroV, otherVScale, otherVQuat, otherConvexGeom.scale.isIdentity());
	ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, convexGeom.scale.isIdentity());

	const bool isMtd = hitFlags & PxHitFlag::eMTD;

	FloatV toi;
	Vec3V closestA, normal;
	RelativeConvex<ConvexHullV> convexA(otherConvexHull, aToB);
	LocalConvex<ConvexHullV> convexB(convexHull);
	if(!gjkRaycastPenetration< RelativeConvex<ConvexHullV>, LocalConvex<ConvexHullV> >(convexA, convexB, aToB.p, zero, zeroV, dir, toi, normal, closestA, inflation, isMtd))
		return false;

	if(hasInitialOverlap(sweepHit, unitDir, toi, normal, closestA, convexPose, isMtd, false))
		return true;

	sweepHit.flags |= PxHitFlag::ePOSITION;
	const Vec3V worldPointA = convexTransf.transform(closestA);
	const Vec3V destNormal = V3Neg(V3Normalize(convexTransf.rotate(normal)));
	const FloatV length = FMul(dist, toi);
	V3StoreU(destNormal, sweepHit.normal);
	V3StoreU(worldPointA, sweepHit.position);
	FStore(length, &sweepHit.distance);

	return computeFaceIndex(sweepHit, hitFlags, otherConvexGeom, otherHullData, pose, unitDir);
}