//=================================================================================
// Single closest hit compound sweep
bool PxRigidBodyExt::linearSweepSingle(
	PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance,
	PxHitFlags outputFlags, PxSweepHit& closestHit, PxU32& shapeIndex,
	const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
	const PxQueryCache* cache, const PxReal inflation)
{
	shapeIndex = 0xFFFFffff;
	PxReal closestDist = distance;
	PxU32 nbShapes = body.getNbShapes();
	for(PxU32 i=0; i < nbShapes; i++)
	{
		PxShape* shape = NULL;
		body.getShapes(&shape, 1, i);
		PX_ASSERT(shape != NULL);
		PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
		PxQueryFilterData fd;
		fd.flags = filterData.flags;
		PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
		fd.data = or4 ? filterData.data : shape->getSimulationFilterData();
		PxGeometryHolder anyGeom = shape->getGeometry();

		PxSweepBuffer subHit; // touching hits are not allowed to be returned from the filters
		scene.sweep(anyGeom.any(), pose, unitDir, distance, subHit, outputFlags, fd, filterCall, cache, inflation);
		if (subHit.hasBlock && subHit.block.distance < closestDist)
		{
			closestDist = subHit.block.distance;
			closestHit = subHit.block;
			shapeIndex = i;
		}
	}

	return (shapeIndex != 0xFFFFffff);
}
Beispiel #2
0
static void sweepRigidBody(PxRigidBody& body, PxBatchQuery& batchQuery, bool closestObject, const PxVec3& unitDir, const PxReal distance, PxSceneQueryFilterFlags filterFlags, 
		bool useShapeFilterData, PxFilterData* filterDataList, PxU32 filterDataCount, void* userData, const PxSweepCache* sweepCache)
{
	if (body.getNbShapes() == 0)
		return;

	const char* outOfMemMsg = "PxRigidBodyExt: LinearSweep: Out of memory, call failed.";
	bool succeeded = true;

	PX_ALLOCA(shapes, PxShape*, body.getNbShapes());
	if (!shapes)
	{
		Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
		succeeded = false;
	}
	PxU32 nbShapes = body.getShapes(shapes, body.getNbShapes());
	
	PX_ALLOCA(geoms, const PxGeometry*, nbShapes);
	if (!geoms)
	{
		Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
		succeeded = false;
	}

	PX_ALLOCA(poses, PxTransform, nbShapes);
	if (!poses)
	{
		Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
		succeeded = false;
	}

	PxFilterData* filterData = NULL;
	PX_ALLOCA(filterDataBuffer, PxFilterData, nbShapes);
	if (useShapeFilterData)
	{
		filterData = filterDataBuffer;

		if (!filterDataBuffer)
		{
			Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, outOfMemMsg);
			succeeded = false;
		}
	}
	else if (filterDataList)
	{
		if (filterDataCount == nbShapes)
		{
			filterData = filterDataList;
		}
		else
		{
			Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "PxRigidBodyExt: LinearSweep: Number of filter data entries does not match number of shapes, call failed.");
			succeeded = false;
		}
	}

	if (succeeded)
	{
		PxU32 geomByteSize = 0;

		for(PxU32 i=0; i < nbShapes; i++)
		{
			poses[i] = PxShapeExt::getGlobalPose(*shapes[i]);

			if (useShapeFilterData)
				filterData[i] = shapes[i]->getSimulationFilterData();

			// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
			switch(shapes[i]->getGeometryType())
			{
				case PxGeometryType::eSPHERE : 
				{
					geomByteSize += sizeof(PxSphereGeometry);
				}
				break;

				case PxGeometryType::eBOX : 
				{
					geomByteSize += sizeof(PxBoxGeometry);
				}
				break;

				case PxGeometryType::eCAPSULE : 
				{
					geomByteSize += sizeof(PxCapsuleGeometry);
				}
				break;

				case PxGeometryType::eCONVEXMESH : 
				{
					geomByteSize += sizeof(PxConvexMeshGeometry);
				}
				break;

				case PxGeometryType::ePLANE : 
				{
					geomByteSize += sizeof(PxPlaneGeometry);
				}
				break;

				case PxGeometryType::eTRIANGLEMESH : 
				{
					geomByteSize += sizeof(PxTriangleMeshGeometry);
				}
				break;

				case PxGeometryType::eHEIGHTFIELD : 
				{
					geomByteSize += sizeof(PxHeightFieldGeometry);
				}
				break;
				default:
				break;
			}
		}

		PX_ALLOCA(geomBuffer, PxU8, geomByteSize);

		if (geomBuffer)
		{
			PxU8* currBufferPos = geomBuffer;

			for(PxU32 i=0; i < nbShapes; i++)
			{
				// Copy the non-supported geometry types too, to make sure the closest geometry index maps to the shapes
				switch(shapes[i]->getGeometryType())
				{
					case PxGeometryType::eSPHERE : 
					{
						PxSphereGeometry* g = reinterpret_cast<PxSphereGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getSphereGeometry(*g);
						currBufferPos += sizeof(PxSphereGeometry);
					}
					break;

					case PxGeometryType::eBOX : 
					{
						PxBoxGeometry* g = reinterpret_cast<PxBoxGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getBoxGeometry(*g);
						currBufferPos += sizeof(PxBoxGeometry);
					}
					break;

					case PxGeometryType::eCAPSULE : 
					{
						PxCapsuleGeometry* g = reinterpret_cast<PxCapsuleGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getCapsuleGeometry(*g);
						currBufferPos += sizeof(PxCapsuleGeometry);
					}
					break;

					case PxGeometryType::eCONVEXMESH : 
					{
						PxConvexMeshGeometry* g = reinterpret_cast<PxConvexMeshGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getConvexMeshGeometry(*g);
						currBufferPos += sizeof(PxConvexMeshGeometry);
					}
					break;

					case PxGeometryType::ePLANE : 
					{
						PxPlaneGeometry* g = reinterpret_cast<PxPlaneGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getPlaneGeometry(*g);
						currBufferPos += sizeof(PxPlaneGeometry);
					}
					break;

					case PxGeometryType::eTRIANGLEMESH : 
					{
						PxTriangleMeshGeometry* g = reinterpret_cast<PxTriangleMeshGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getTriangleMeshGeometry(*g);
						currBufferPos += sizeof(PxTriangleMeshGeometry);
					}
					break;

					case PxGeometryType::eHEIGHTFIELD : 
					{
						PxHeightFieldGeometry* g = reinterpret_cast<PxHeightFieldGeometry*>(currBufferPos);
						geoms[i] = g;
						shapes[i]->getHeightFieldGeometry(*g);
						currBufferPos += sizeof(PxHeightFieldGeometry);
					}
					break;
					default:
					break;
				}
			}

			if (closestObject)
				batchQuery.linearCompoundGeometrySweepSingle(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
			else
				batchQuery.linearCompoundGeometrySweepMultiple(geoms, poses, filterData, nbShapes, unitDir, distance, filterFlags, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE|PxSceneQueryFlag::eUV, userData, sweepCache);
		}
	}
}
Beispiel #3
0
static bool computeMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, const PxReal* masses, PxU32 densityOrMassCount, Ext::InertiaTensorComputer& computer)
{
	PX_ASSERT(!densities || !masses);
	PX_ASSERT((densities || masses) && (densityOrMassCount > 0));

	Ext::InertiaTensorComputer inertiaComp(true);

	Ps::InlineArray<PxShape*, 16> shapes("PxShape*"); shapes.resize(body.getNbShapes());

	body.getShapes(shapes.begin(), shapes.size());

	PxU32 validShapeIndex = 0;
	PxReal currentMassOrDensity;
	const PxReal* massOrDensityArray;
	if (densities)
	{
		massOrDensityArray = densities;
		currentMassOrDensity = densities[0];
	}
	else
	{
		massOrDensityArray = masses;
		currentMassOrDensity = masses[0];
	}
	if (!PxIsFinite(currentMassOrDensity))
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
			"computeMassAndInertia: Provided mass or density has no valid value");
		return false;
	}

	for(PxU32 i=0; i < shapes.size(); i++)
	{
		if (!(shapes[i]->getFlags() & PxShapeFlag::eSIMULATION_SHAPE))
			continue; 

		if (multipleMassOrDensity)
		{
			if (validShapeIndex < densityOrMassCount)
			{
				currentMassOrDensity = massOrDensityArray[validShapeIndex];

				if (!PxIsFinite(currentMassOrDensity))
				{
					Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
						"computeMassAndInertia: Provided mass or density has no valid value");
					return false;
				}
			}
			else
			{
				Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
					"computeMassAndInertia: Not enough mass/density values provided for all simulation shapes");
				return false;
			}
		}

		Ext::InertiaTensorComputer it(false);

		switch(shapes[i]->getGeometryType())
		{
		case PxGeometryType::eSPHERE : 
			{
				PxSphereGeometry g;
				bool ok = shapes[i]->getSphereGeometry(g);
				PX_ASSERT(ok);
				PX_UNUSED(ok);
				PxTransform temp(shapes[i]->getLocalPose());

				it.setSphere(g.radius, &temp);
			}
			break;

		case PxGeometryType::eBOX : 
			{
				PxBoxGeometry g;
				bool ok = shapes[i]->getBoxGeometry(g);
				PX_ASSERT(ok);
				PX_UNUSED(ok);
				PxTransform temp(shapes[i]->getLocalPose());

				it.setBox(g.halfExtents, &temp);
			}
			break;

		case PxGeometryType::eCAPSULE : 
			{
				PxCapsuleGeometry g;
				bool ok = shapes[i]->getCapsuleGeometry(g);
				PX_ASSERT(ok);
				PX_UNUSED(ok);
				PxTransform temp(shapes[i]->getLocalPose());

				it.setCapsule(0, g.radius, g.halfHeight, &temp);
			}
			break;

		case PxGeometryType::eCONVEXMESH : 
			{
				PxConvexMeshGeometry g;
				bool ok = shapes[i]->getConvexMeshGeometry(g);
				PX_ASSERT(ok);
				PX_UNUSED(ok);
				PxConvexMesh& convMesh = *g.convexMesh;

				PxReal convMass;
				PxMat33 convInertia;
				PxVec3 convCoM;
				convMesh.getMassInformation(convMass, reinterpret_cast<PxMat33&>(convInertia), convCoM);

				//scale the mass:
				convMass *= (g.scale.scale.x * g.scale.scale.y * g.scale.scale.z);
				convCoM = g.scale.rotation.rotateInv(g.scale.scale.multiply(g.scale.rotation.rotate(convCoM)));
				convInertia = Ext::MassProps::scaleInertia(convInertia, g.scale.rotation, g.scale.scale);

				it = Ext::InertiaTensorComputer(convInertia, convCoM, convMass);
				it.transform(shapes[i]->getLocalPose());
			}
			break;

		default :
			{

				Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
					"computeMassAndInertia: Dynamic actor with illegal collision shapes");
				return false;
			}
		}

		if (densities)
			it.scaleDensity(currentMassOrDensity);
		else if (multipleMassOrDensity)  // mass per shape -> need to scale density per shape
			it.scaleDensity(currentMassOrDensity / it.getMass());

		inertiaComp.add(it);

		validShapeIndex++;
	}

	if (validShapeIndex && masses && (!multipleMassOrDensity))  // at least one simulation shape and single mass for all shapes -> scale density at the end
	{
		inertiaComp.scaleDensity(currentMassOrDensity / inertiaComp.getMass());
	}

	computer = inertiaComp;
	return true;
}
//=================================================================================
// Multiple hits compound sweep
// AP: we might be able to improve the return results API but no time for it in 3.3
PxU32 PxRigidBodyExt::linearSweepMultiple(
	PxRigidBody& body, PxScene& scene, const PxVec3& unitDir, const PxReal distance, PxHitFlags outputFlags,
	PxSweepHit* hitBuffer, PxU32* hitShapeIndices, PxU32 hitBufferSize, PxSweepHit& block, PxI32& blockingHitShapeIndex,
	bool& overflow, const PxQueryFilterData& filterData, PxQueryFilterCallback* filterCall,
	const PxQueryCache* cache, const PxReal inflation)
{
	overflow = false;
	blockingHitShapeIndex = -1;

	for (PxU32 i = 0; i < hitBufferSize; i++)
		hitShapeIndices[i] = 0xFFFFffff;

	PxI32 sumNbResults = 0;

	PxU32 nbShapes = body.getNbShapes();
	PxF32 shrunkMaxDistance = distance;
	for(PxU32 i=0; i < nbShapes; i++)
	{
		PxShape* shape = NULL;
		body.getShapes(&shape, 1, i);
		PX_ASSERT(shape != NULL);
		PxTransform pose = PxShapeExt::getGlobalPose(*shape, body);
		PxQueryFilterData fd;
		fd.flags = filterData.flags;
		PxU32 or4 = (filterData.data.word0 | filterData.data.word1 | filterData.data.word2 | filterData.data.word3);
		fd.data = or4 ? filterData.data : shape->getSimulationFilterData();
		PxGeometryHolder anyGeom = shape->getGeometry();

		PxU32 bufSizeLeft = hitBufferSize-sumNbResults;
		PxSweepHit extraHit;
		PxSweepBuffer buffer(bufSizeLeft == 0 ? &extraHit : hitBuffer+sumNbResults, bufSizeLeft == 0 ? 1 : hitBufferSize-sumNbResults);
		scene.sweep(anyGeom.any(), pose, unitDir, shrunkMaxDistance, buffer, outputFlags, fd, filterCall, cache, inflation);

		// Check and abort on overflow. Assume overflow if result count is bufSize.
		PxU32 nbNewResults = buffer.getNbTouches();
		overflow |= (nbNewResults >= bufSizeLeft);
		if (bufSizeLeft == 0) // this is for when we used the extraHit buffer
			nbNewResults = 0;

		// set hitShapeIndices for each new non-blocking hit
		for (PxU32 j = 0; j < nbNewResults; j++)
			if (sumNbResults + PxU32(j) < hitBufferSize)
				hitShapeIndices[sumNbResults+j] = i;

		if (buffer.hasBlock) // there's a blocking hit in the most recent sweepMultiple results
		{
			// overwrite the return result blocking hit with the new blocking hit if under
			if (blockingHitShapeIndex == -1 || buffer.block.distance < block.distance)
			{
				blockingHitShapeIndex = (PxI32)i;
				block = buffer.block;
			}

			// Remove all the old touching hits below the new maxDist
			// sumNbResults is not updated yet at this point
			//   and represents the count accumulated so far excluding the very last query
			PxI32 nbNewResultsSigned = PxI32(nbNewResults); // need a signed version, see nbNewResultsSigned-- below
			for (PxI32 j = sumNbResults-1; j >= 0; j--) // iterate over "old" hits (up to shapeIndex-1)
				if (buffer.block.distance < hitBuffer[j].distance)
				{
					// overwrite with last "new" hit
					PxI32 sourceIndex = PxI32(sumNbResults)+nbNewResultsSigned-1; PX_ASSERT(sourceIndex >= j);
					hitBuffer[j] = hitBuffer[sourceIndex];
					hitShapeIndices[j] = hitShapeIndices[sourceIndex];
					nbNewResultsSigned--; // can get negative, that means we are shifting the last results array
				}

			sumNbResults += nbNewResultsSigned;
		} else // if there was no new blocking hit we don't need to do anything special, simply append all results to touch array
			sumNbResults += nbNewResults;

		PX_ASSERT(sumNbResults >= 0 && sumNbResults <= PxI32(hitBufferSize));
	}

	return (PxU32)sumNbResults;
}