Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 562 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp b/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp
new file mode 100644
index 00000000..e67ea751
--- /dev/null
+++ b/PhysX_3.4/Source/GeomUtils/src/GuRaycastTests.cpp
@@ -0,0 +1,562 @@
+// 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-2016 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 "GuMidphaseInterface.h"
+#include "GuInternal.h"
+#include "PxSphereGeometry.h"
+#include "PxConvexMeshGeometry.h"
+#include "GuIntersectionRayCapsule.h"
+#include "GuIntersectionRaySphere.h"
+#include "GuIntersectionRayPlane.h"
+#include "GuHeightFieldUtil.h"
+#include "GuDistancePointSegment.h"
+#include "GuConvexMesh.h"
+#include "CmScaling.h"
+
+using namespace physx;
+using namespace Gu;
+
+////////////////////////////////////////////////// raycasts //////////////////////////////////////////////////////////////////
+PxU32 raycast_box(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eBOX);
+	PX_ASSERT(maxHits && hits);
+	const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
+
+	const PxTransform& absPose = pose;
+
+	PxVec3 localOrigin = rayOrigin - absPose.p;
+	localOrigin = absPose.q.rotateInv(localOrigin);
+
+	const PxVec3 localDir = absPose.q.rotateInv(rayDir);
+
+	PxVec3 localImpact;
+	PxReal t;
+	PxU32 rval = rayAABBIntersect2(-boxGeom.halfExtents, boxGeom.halfExtents, localOrigin, localDir, localImpact, t);
+	if(!rval)
+		return 0;
+
+	if(t>maxDist)
+		return 0;
+
+	hits->distance	= t; //worldRay.orig.distance(hit.worldImpact);	//should be the same, assuming ray dir was normalized!!
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	PxHitFlags outFlags = PxHitFlag::eDISTANCE;
+	if((hitFlags & PxHitFlag::ePOSITION))
+	{
+		outFlags |= PxHitFlag::ePOSITION;
+		if(t!=0.0f)
+			hits->position = absPose.transform(localImpact);
+		else
+			hits->position = rayOrigin;
+	}
+
+	// Compute additional information if needed
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		outFlags |= PxHitFlag::eNORMAL;
+
+		//Because rayAABBIntersect2 set t = 0 if start point inside shape
+		if(t == 0)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			//local space normal is:
+			rval--;
+			PxVec3 n(0.0f);
+			n[rval] = PxReal((localImpact[rval] > 0.0f) ? 1.0f : -1.0f);
+			hits->normal = absPose.q.rotate(n);
+		}
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags	= outFlags;
+	return 1;
+}
+
+PxU32 raycast_sphere(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eSPHERE);
+	PX_ASSERT(maxHits && hits);
+
+	const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
+
+	if(!intersectRaySphere(rayOrigin, rayDir, maxDist, pose.p, sphereGeom.radius, hits->distance, &hits->position))
+		return 0;
+
+	/*	// PT: should be useless now
+	hit.distance	= worldRay.orig.distance(hit.worldImpact);
+	if(hit.distance>maxDist)
+	return false;
+	*/
+	// PT: we can't avoid computing the position here since it's needed to compute the normal anyway
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	// Compute additional information if needed
+	PxHitFlags outFlags = PxHitFlag::eDISTANCE|PxHitFlag::ePOSITION;
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		// User requested impact normal
+		//Because intersectRaySphere set distance = 0 if start point inside shape
+		if(hits->distance == 0.0f)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			hits->normal = hits->position - pose.p;
+			hits->normal.normalize();
+		}
+		outFlags |= PxHitFlag::eNORMAL;
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags = outFlags;
+
+	return 1;
+}
+
+PxU32 raycast_capsule(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eCAPSULE);
+	PX_ASSERT(maxHits && hits);
+
+	const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
+
+	// TODO: PT: could we simplify this ?
+	Capsule capsule;
+	getCapsuleSegment(pose, capsuleGeom, capsule);
+	capsule.radius = capsuleGeom.radius;
+
+	PxReal t;
+	if(!intersectRayCapsule(rayOrigin, rayDir, capsule, t))
+		return 0;
+
+	if(t>maxDist)
+		return 0;
+
+	// PT: we can't avoid computing the position here since it's needed to compute the normal anyway
+	hits->position	= rayOrigin + rayDir*t;	// PT: will be rayOrigin for t=0.0f (i.e. what the spec wants)
+	hits->distance	= t;
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+
+	// Compute additional information if needed
+	PxHitFlags outFlags = PxHitFlag::eDISTANCE|PxHitFlag::ePOSITION;
+	if(hitFlags & PxHitFlag::eNORMAL)
+	{
+		outFlags |= PxHitFlag::eNORMAL;
+
+		if(t==0.0f)
+		{
+			hits->normal = -rayDir;
+		}
+		else
+		{
+			PxReal capsuleT;
+			distancePointSegmentSquared(capsule, hits->position, &capsuleT);
+			capsule.computePoint(hits->normal, capsuleT);
+			hits->normal = hits->position - hits->normal;	 //this should never be zero. It should have a magnitude of the capsule radius.
+			hits->normal.normalize();
+		}
+	}
+	else
+	{
+		hits->normal = PxVec3(0.0f);
+	}
+	hits->flags = outFlags;
+
+	return 1;
+}
+
+PxU32 raycast_plane(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::ePLANE);
+	PX_ASSERT(maxHits && hits);
+	PX_UNUSED(geom);
+//	const PxPlaneGeometry& planeGeom = static_cast<const PxPlaneGeometry&>(geom);
+
+	// Perform backface culling so that we can pick objects beyond planes
+	const PxPlane plane = getPlane(pose);
+	if(rayDir.dot(plane.n)>=0.0f)
+		return false;
+
+	PxReal distanceAlongLine;
+	if(!intersectRayPlane(rayOrigin, rayDir, plane, distanceAlongLine, &hits->position))
+		return 0;
+
+	/*
+	PxReal test = worldRay.orig.distance(hit.worldImpact);
+
+	PxReal dd;
+	PxVec3 pp;
+	PxSegmentPlaneIntersect(worldRay.orig, worldRay.orig+worldRay.dir*1000.0f, plane, dd, pp);
+	*/
+
+	if(distanceAlongLine<0.0f)
+		return 0;
+
+	if(distanceAlongLine>maxDist)
+		return 0;
+
+	hits->distance	= distanceAlongLine;
+	hits->faceIndex	= 0xffffffff;
+	hits->u			= 0.0f;
+	hits->v			= 0.0f;
+	hits->flags		= PxHitFlag::eDISTANCE|PxHitFlag::ePOSITION|PxHitFlag::eNORMAL;
+	hits->normal	= plane.n;
+	return 1;
+}
+
+PxU32 raycast_convexMesh(GU_RAY_FUNC_PARAMS)
+{ 
+	PX_UNUSED(maxHits);
+	PX_ASSERT(geom.getType() == PxGeometryType::eCONVEXMESH);
+	PX_ASSERT(maxHits && hits);
+	PX_ASSERT(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f);
+
+	const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
+
+	ConvexMesh* convexMesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+
+	PxRaycastHit& hit = *hits;
+	
+	//scaling: transform the ray to vertex space
+	const Cm::Matrix34 world2vertexSkew = convexGeom.scale.getInverse() * pose.getInverse();	
+
+	//ConvexMesh* cmesh = static_cast<ConvexMesh*>(convexGeom.convexMesh);
+	const PxU32 nPolys = convexMesh->getNbPolygonsFast();
+	const HullPolygonData* PX_RESTRICT polysEA = convexMesh->getPolygons();
+	const HullPolygonData* polys = polysEA;
+
+	const PxVec3 vrayOrig = world2vertexSkew.transform(rayOrigin);
+	const PxVec3 vrayDir = world2vertexSkew.rotate(rayDir);
+
+	/*
+	Purely convex planes based algorithm
+	Iterate all planes of convex, with following rules:
+	* determine of ray origin is inside them all or not.  
+	* planes parallel to ray direction are immediate early out if we're on the outside side (plane normal is sep axis)
+	* else 
+		- for all planes the ray direction "enters" from the front side, track the one furthest along the ray direction (A)
+		- for all planes the ray direction "exits" from the back side, track the one furthest along the negative ray direction (B)
+	if the ray origin is outside the convex and if along the ray, A comes before B, the directed line stabs the convex at A
+	*/
+	bool originInsideAllPlanes = true;
+	PxReal latestEntry = -FLT_MAX;
+	PxReal earliestExit = FLT_MAX;
+//	PxU32 bestPolygonIndex = 0;
+	hit.faceIndex	= 0xffffffff;
+
+	for(PxU32 i=0;i<nPolys;i++)
+	{
+		const HullPolygonData& poly = polys[i];
+		const PxPlane& vertSpacePlane = poly.mPlane;
+
+		const PxReal distToPlane = vertSpacePlane.distance(vrayOrig);
+		const PxReal dn = vertSpacePlane.n.dot(vrayDir);
+		const PxReal distAlongRay = -distToPlane/dn;	// PT: TODO: potential divide by zero here!
+
+		// PT: TODO: this is computed again in the last branch!
+		if(distToPlane > 0.0f)
+			originInsideAllPlanes = false;	//origin not behind plane == ray starts outside the convex.
+
+		if(dn > 1E-7f)	//the ray direction "exits" from the back side
+		{
+			earliestExit = physx::intrinsics::selectMin(earliestExit, distAlongRay);
+		}
+		else if(dn < -1E-7f)	//the ray direction "enters" from the front side
+		{
+			if(distAlongRay > latestEntry)
+			{
+				latestEntry = distAlongRay;
+				hit.faceIndex = i;
+			}
+		}
+		else
+		{
+			//plane normal and ray dir are orthogonal
+			if(distToPlane > 0.0f)	
+				return 0;	//a plane is parallel with ray -- and we're outside the ray -- we definitely miss the entire convex!
+		}
+	}
+
+	if(originInsideAllPlanes)	//ray starts inside convex
+	{
+		hit.distance	= 0.0f;
+		hit.faceIndex	= 0xffffffff;
+		hit.u			= 0.0f;
+		hit.v			= 0.0f;
+		hit.position	= rayOrigin;
+		hit.normal		= -rayDir;
+		hit.flags		= PxHitFlag::eDISTANCE|PxHitFlag::eNORMAL|PxHitFlag::ePOSITION;
+		return 1;
+	}
+
+	// AP: changed to latestEntry < maxDist-1e-5f so that we have a conservatively negative result near end of ray
+	if(latestEntry < earliestExit && latestEntry > 0.0f && latestEntry < maxDist-1e-5f)
+	{
+		PxHitFlags outFlags = PxHitFlag::eDISTANCE | PxHitFlag::eFACE_INDEX;
+		if(hitFlags & PxHitFlag::ePOSITION)
+		{
+			outFlags |= PxHitFlag::ePOSITION;
+			const PxVec3 pointOnPlane = vrayOrig + latestEntry * vrayDir;
+			hit.position = pose.transform(convexGeom.scale.toMat33() * pointOnPlane);
+		}
+		hit.distance	= latestEntry;
+		hit.u			= 0.0f;
+		hit.v			= 0.0f;
+		hit.normal		= PxVec3(0.0f);
+
+		// Compute additional information if needed
+		if(hitFlags & PxHitFlag::eNORMAL)
+		{
+			outFlags |= PxHitFlag::eNORMAL;
+			//when we have nonuniform scaling we actually have to transform by the transpose of the inverse of vertex2worldSkew.M == transpose of world2vertexSkew:
+			hit.normal = world2vertexSkew.rotateTranspose(polys[hit.faceIndex].mPlane.n);
+			hit.normal.normalize();
+		}
+		hit.flags = outFlags;
+		return 1;
+	}
+	return 0;
+}
+
+PxU32 raycast_triangleMesh(GU_RAY_FUNC_PARAMS) 
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eTRIANGLEMESH);
+	PX_ASSERT(PxAbs(rayDir.magnitudeSquared()-1)<1e-4f);
+
+	const PxTriangleMeshGeometry& meshGeom = static_cast<const PxTriangleMeshGeometry&>(geom);
+
+	TriangleMesh* meshData = static_cast<TriangleMesh*>(meshGeom.triangleMesh);
+
+	return Midphase::raycastTriangleMesh(meshData, meshGeom, pose, rayOrigin, rayDir, maxDist, hitFlags, maxHits, hits);
+}
+
+namespace
+{
+	struct HFTraceSegmentCallback 
+	{
+		PX_NOCOPY(HFTraceSegmentCallback)
+	public:
+		PxRaycastHit*			mHits;
+		const PxU32				mMaxHits;
+		PxU32					mNbHits;
+		const HeightFieldUtil&	mUtil;
+		const PxTransform&		mPose;
+		const PxVec3&			mRayDir;
+		const PxVec3&			mLocalRayDir;
+		const PxVec3&			mLocalRayOrig;		
+		const PxHitFlags		mHitFlags;
+		const bool				mIsDoubleSided;
+
+		HFTraceSegmentCallback(	PxRaycastHit* hits, PxU32 maxHits, const PxHitFlags hitFlags, const HeightFieldUtil& hfUtil, const PxTransform& pose,
+								const PxVec3& rayDir, const PxVec3& localRayDir, const PxVec3& localRayOrig,
+								bool isDoubleSided) :
+			mHits			(hits),
+			mMaxHits		(maxHits),
+			mNbHits			(0),
+			mUtil			(hfUtil),
+			mPose			(pose),
+			mRayDir			(rayDir),
+			mLocalRayDir	(localRayDir),
+			mLocalRayOrig	(localRayOrig),
+			mHitFlags		(hitFlags),
+			mIsDoubleSided	(isDoubleSided)
+		{
+			PX_ASSERT(maxHits > 0);
+		}
+
+		PX_FORCE_INLINE bool onEvent(PxU32 , PxU32*)
+		{
+			return true;
+		}
+
+		PX_FORCE_INLINE bool underFaceHit(const HeightFieldUtil&, const PxVec3&, const PxVec3&, PxF32, PxF32, PxF32, PxU32)
+		{
+			return true;	// true means continue traversal
+		}
+
+		PxAgain faceHit(const HeightFieldUtil&, const PxVec3& aHitPoint, PxU32 aTriangleIndex, PxReal u, PxReal v)
+		{
+			// traversal is strictly sorted so there's no need to sort hits
+			if(mNbHits >= mMaxHits)
+				return false; // false = stop traversal
+
+			PxRaycastHit& hit = mHits[mNbHits++];
+			hit.position	= aHitPoint;
+			hit.faceIndex	= aTriangleIndex;
+			hit.u			= u;
+			hit.v			= v;
+			hit.flags		= PxHitFlag::eUV | PxHitFlag::eFACE_INDEX; // UVs and face index are always set
+
+			if(mHitFlags & PxHitFlag::eNORMAL)
+			{
+				// We need the normal for the dot product.
+				PxVec3 normal = mPose.q.rotate(mUtil.getNormalAtShapePoint(hit.position.x, hit.position.z)); 
+				normal.normalize();
+				if(mIsDoubleSided && normal.dot(mRayDir) > 0.0f) // comply with normal spec for double sided (should always face opposite rayDir)
+					hit.normal = -normal;
+				else
+					hit.normal = normal;
+				hit.flags |= PxHitFlag::eNORMAL;
+			}
+
+			if(mHitFlags & PxHitFlag::eDISTANCE)
+			{
+				hit.distance = (hit.position - mLocalRayOrig).dot(mLocalRayDir);
+				hit.flags |= PxHitFlag::eDISTANCE;
+			}
+
+			if(mHitFlags & PxHitFlag::ePOSITION)
+			{
+				hit.position = mPose.transform(hit.position);
+				hit.flags |= PxHitFlag::ePOSITION;
+			}
+			return (mNbHits < mMaxHits); // true = continue traversal, false = stop traversal
+		}
+	};   
+}
+
+PxU32 raycast_heightField(GU_RAY_FUNC_PARAMS)
+{
+	PX_ASSERT(geom.getType() == PxGeometryType::eHEIGHTFIELD);
+	PX_ASSERT(maxHits && hits);
+	PX_UNUSED(maxHits);
+
+	const PxHeightFieldGeometry& hfGeom = static_cast<const PxHeightFieldGeometry&>(geom);
+
+	const PxTransform invAbsPose = pose.getInverse();
+	const PxVec3 localRayOrig = invAbsPose.transform(rayOrigin);
+	const PxVec3 localRayDir = invAbsPose.rotate(rayDir);
+
+	const bool isDoubleSided = hfGeom.heightFieldFlags.isSet(PxMeshGeometryFlag::eDOUBLE_SIDED);
+	const bool bothSides = isDoubleSided || (hitFlags & PxHitFlag::eMESH_BOTH_SIDES);
+
+	const HeightFieldUtil hfUtil(hfGeom);
+
+	PxVec3 normRayDir = localRayDir;
+	normRayDir.normalizeSafe(); // nothing will happen if length is < PX_NORMALIZATION_EPSILON
+	
+	// pretest if we intersect HF bounds. If no early exit, if yes move the origin and shorten the maxDist
+	// to deal with precision issues with large maxDist
+	PxBounds3 hfLocalBounds;
+	hfUtil.computeLocalBounds(hfLocalBounds);
+
+	PxVec3 localImpact;
+	PxReal t;	// closest intersection, t==0 hit inside
+	PxU32 rval = rayAABBIntersect2(hfLocalBounds.minimum, hfLocalBounds.maximum, localRayOrig, localRayDir, localImpact, t);
+	// early exit we miss the AABB
+	if (!rval)
+		return 0;
+	if (t > maxDist)
+		return 0;
+
+	// PT: if eMESH_ANY is used then eMESH_MULTIPLE won't be, and we'll stop the query after 1 hit is found. There is no difference
+	// between 'any hit' and 'closest hit' for HFs since hits are reported in order.
+	HFTraceSegmentCallback callback(hits, hitFlags.isSet(PxHitFlag::eMESH_MULTIPLE) ? maxHits : 1, hitFlags, hfUtil, pose,
+									rayDir, localRayDir, localRayOrig,
+									isDoubleSided); // make sure we return only 1 hit without eMESH_MULTIPLE
+
+	PxReal offset = 0.0f;
+	PxReal maxDistOffset = maxDist;
+	PxVec3 localRayOrigOffset = localRayOrig;
+
+	// if we don't start inside the AABB box, offset the start pos, because of precision issues with large maxDist
+	if(t > 0.0f)
+	{
+		offset = t - GU_RAY_SURFACE_OFFSET;
+		// move the rayOrig to offset start pos
+		localRayOrigOffset = localRayOrig + normRayDir*offset;
+	}
+
+	// shorten the maxDist of the offset that was cut off and clip it
+	// we pick either the original maxDist, if maxDist is huge we clip it
+	maxDistOffset = PxMin(maxDist - offset, GU_RAY_SURFACE_OFFSET + 2.0f * PxMax(hfLocalBounds.maximum.x - hfLocalBounds.minimum.x, PxMax(hfLocalBounds.maximum.y - hfLocalBounds.minimum.y, hfLocalBounds.maximum.z - hfLocalBounds.minimum.z)));
+
+	hfUtil.traceSegment<HFTraceSegmentCallback, false, false>(localRayOrigOffset, normRayDir, maxDistOffset, 
+		&callback, hfLocalBounds, !bothSides);
+	return callback.mNbHits;
+}
+
+static PxU32 raycast_heightField_unregistered(GU_RAY_FUNC_PARAMS)
+{
+	PX_UNUSED(geom);
+	PX_UNUSED(pose);
+	PX_UNUSED(rayOrigin);
+	PX_UNUSED(rayDir);
+	PX_UNUSED(maxDist);
+	PX_UNUSED(hitFlags);
+	PX_UNUSED(maxHits);
+	PX_UNUSED(hits);
+	Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "Height Field Raycast test called with height fields unregistered ");
+	return 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// PT: table is not static because it's accessed as 'extern' within Gu (bypassing the function call).
+RaycastFunc gRaycastMap[PxGeometryType::eGEOMETRY_COUNT] =
+{
+	raycast_sphere,
+	raycast_plane,
+	raycast_capsule,
+	raycast_box,
+	raycast_convexMesh,
+	raycast_triangleMesh,	
+	raycast_heightField_unregistered
+};
+
+// PT: the function is used by external modules (Np, CCT, Sq)
+const Gu::GeomRaycastTable& Gu::getRaycastFuncTable()
+{
+	return gRaycastMap;
+}
+
+void registerHeightFields_Raycasts()
+{
+	gRaycastMap[PxGeometryType::eHEIGHTFIELD] = raycast_heightField;
+}