C++ PxVec3::isFinite方法代码示例

void D6Joint::setDriveVelocity(const PxVec3& linear,
									 const PxVec3& angular)
{	
	PX_CHECK_AND_RETURN(linear.isFinite() && angular.isFinite(), "PxD6Joint::setDriveVelocity: velocity invalid");
	data().driveLinearVelocity = linear; 
	data().driveAngularVelocity = angular; 
	markDirty();
}

PxU32 physx::PxFindFaceIndex(const PxConvexMeshGeometry& convexGeom, const PxTransform& pose, 
	const PxVec3& impactPos, const PxVec3& unitDir)
{
	PX_ASSERT(unitDir.isFinite());
	PX_ASSERT(unitDir.isNormalized());
	PX_ASSERT(impactPos.isFinite());
	PX_ASSERT(pose.isFinite());

	const PxVec3 impact = impactPos - unitDir * gEpsilon;

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

	// Create shape to vertex scale transformation matrix
	const PxMeshScale& meshScale = convexGeom.scale;
	const PxMat33 rot(meshScale.rotation);
	PxMat33 shape2VertexSkew = rot.getTranspose();
	const PxMat33 diagonal = PxMat33::createDiagonal(PxVec3(1.0f / meshScale.scale.x, 1.0f / meshScale.scale.y, 1.0f / meshScale.scale.z));
	shape2VertexSkew = shape2VertexSkew * diagonal;
	shape2VertexSkew = shape2VertexSkew * rot;

	const PxU32 nbPolys = convexGeom.convexMesh->getNbPolygons();
	PxU32 minIndex = 0;
	PxReal minD = PX_MAX_REAL;
	for (PxU32 j = 0; j < nbPolys; j++)
	{
		PxHullPolygon hullPolygon;
		convexGeom.convexMesh->getPolygonData(j, hullPolygon);
		
		// transform hull plane into shape space
		PxPlane plane;
		const PxVec3 tmp = shape2VertexSkew.transformTranspose(PxVec3(hullPolygon.mPlane[0],hullPolygon.mPlane[1],hullPolygon.mPlane[2]));
		const PxReal denom = 1.0f / tmp.magnitude();
		plane.n = tmp * denom;
		plane.d = hullPolygon.mPlane[3] * denom;

		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;
}

void NpSpatialIndex::raycast(const PxVec3& origin, 
							 const PxVec3& unitDir, 
							 PxReal maxDist, 
							 PxSpatialLocationCallback& callback) const
{
	PX_SIMD_GUARD;

	PX_CHECK_AND_RETURN(origin.isFinite(),								"PxSpatialIndex::raycast: origin is not valid.");
	PX_CHECK_AND_RETURN(unitDir.isFinite() && unitDir.isNormalized(),	"PxSpatialIndex::raycast: unitDir is not valid.");
	PX_CHECK_AND_RETURN(maxDist > 0.0f,									"PxSpatialIndex::raycast: distance must be positive");

	flushUpdates();
	LocationCallback cb(callback);
	mPruner->raycast(origin, unitDir, maxDist, cb);
}

void NpArticulationJoint::setTargetVelocity(const PxVec3& v)
{
	PX_CHECK_AND_RETURN(v.isFinite(), "NpArticulationJoint::setTargetVelocity v is not valid.");

	NP_WRITE_CHECK(getOwnerScene());

	mJoint.setTargetVelocity(v);
}

void NpCloth::setExternalAcceleration(PxVec3 acceleration)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));

	PX_CHECK_AND_RETURN(acceleration.isFinite(), "PxCloth::setExternalAcceleration: invalid values!");

	mCloth.setExternalAcceleration(acceleration);
	sendPvdSimpleProperties();
}

void NpArticulation::computeImpulseResponse(PxArticulationLink* link,
											PxVec3& linearResponse, 
											PxVec3& angularResponse,
											const PxArticulationDriveCache& driveCache,
											const PxVec3& force,
											const PxVec3& torque) const
{

	PX_CHECK_AND_RETURN(getAPIScene(), "PxArticulation::computeImpulseResponse: object must be in a scene");
	PX_CHECK_AND_RETURN(force.isFinite() && torque.isFinite(), "PxArticulation::computeImpulseResponse: invalid force/torque");
	NP_READ_CHECK(getOwnerScene());

	const Sc::ArticulationDriveCache& c = reinterpret_cast<const  Sc::ArticulationDriveCache&>(driveCache);
	PX_CHECK_AND_RETURN(mArticulation.getScArticulation().getCacheLinkCount(c) == mArticulationLinks.size(), "PxArticulation::computeImpulseResponse: Articulation size has changed; drive cache is invalid");
	PX_UNUSED(&c);

	mArticulation.getScArticulation().computeImpulseResponse(static_cast<NpArticulationLink*>(link)->getScbBodyFast().getScBody(),
															 linearResponse, angularResponse,
															 reinterpret_cast<const Sc::ArticulationDriveCache&>(driveCache),
															 force, torque);
}

void NpArticulationLink::setAngularVelocity(const PxVec3& velocity, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);

	PX_CHECK_AND_RETURN(velocity.isFinite(), "NpArticulationLink::setAngularVelocity velocity is not valid.");

	NP_WRITE_CHECK(scene);

	getScbBodyFast().setAngularVelocity(velocity);

	if (scene)
		mRoot->wakeUpInternal((!velocity.isZero()), autowake);
}

void NpArticulationLink::addTorque(const PxVec3& torque, PxForceMode::Enum mode, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);

	PX_CHECK_AND_RETURN(torque.isFinite(), "NpArticulationLink::addTorque: force is not valid.");
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::addTorque: articulation link must be in a scene!");

	addSpatialForce(0, &torque, mode);

	mRoot->wakeUpInternal((!torque.isZero()), autowake);
}

void NpRigidDynamic::addTorque(const PxVec3& torque, PxForceMode::Enum mode, bool autowake)
{
    Scb::Body& b = getScbBodyFast();

    PX_CHECK_AND_RETURN(torque.isFinite(), "NpRigidDynamic::addTorque: torque is not valid.");
    NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
    PX_CHECK_AND_RETURN(NpActor::getAPIScene(*this), "RigidDynamic::addTorque: Body must be in a scene!");
    PX_CHECK_AND_RETURN(!(b.getFlags() & PxRigidBodyFlag::eKINEMATIC), "RigidDynamic::addTorque: Body must be non-kinematic!");
    PX_CHECK_AND_RETURN(!(b.getActorFlags() & PxActorFlag::eDISABLE_SIMULATION), "RigidDynamic::addTorque: Not allowed if PxActorFlag::eDISABLE_SIMULATION is set!");

    addSpatialForce(0, &torque, mode);

    wakeUpInternalNoKinematicTest(b, (!torque.isZero()), autowake);
}

void NpBatchQuery::sweep(
	const PxGeometry& geometry, const PxTransform& pose, const PxVec3& unitDir, const PxReal distance, PxU16 maxTouchHits,
	PxHitFlags hitFlags, const PxQueryFilterData& fd, void* userData, const PxQueryCache* cache, const PxReal inflation)
{
	PX_CHECK_AND_RETURN(pose.isValid(), "Batch sweep input check: pose is not valid.");
	PX_CHECK_AND_RETURN(unitDir.isFinite(), "Batch sweep input check: unitDir is not valid.");
	PX_CHECK_AND_RETURN(unitDir.isNormalized(), "Batch sweep input check: direction must be normalized");
	PX_CHECK_AND_RETURN(distance >= 0.0f, "Batch sweep input check: distance cannot be negative");
	PX_CHECK_AND_RETURN(distance != 0.0f || !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP),
		"Batch sweep input check: zero-length sweep only valid without the PxHitFlag::eASSUME_NO_INITIAL_OVERLAP flag");

	if (mNbSweeps >= mDesc.queryMemory.getMaxSweepsPerExecute())
	{
		PX_CHECK_AND_RETURN(mNbSweeps < mDesc.queryMemory.getMaxSweepsPerExecute(),
			"PxBatchQuery: number of sweep() calls exceeds PxBatchQueryMemory::sweepResultBufferSize, query discarded");
		return;
	}

	
	CHECK_RUNNING("PxBatchQuery::sweep: This batch is still executing, skipping query.")
	mNbSweeps++;

	writeBatchHeader(BatchStreamHeader(hitFlags, cache, fd, userData, maxTouchHits, QTypeROS::eSWEEP));

	//set the MTD flag
	mHasMtdSweep |= !!(hitFlags & PxHitFlag::eMTD);

	if((hitFlags & PxHitFlag::ePRECISE_SWEEP) && (hitFlags & PxHitFlag::eMTD))
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " Precise sweep doesn't support MTD. Perform MTD with default sweep");
		hitFlags &= ~PxHitFlag::ePRECISE_SWEEP;
	}

	if((hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP) && (hitFlags & PxHitFlag::eMTD))
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " eMTD cannot be used in conjunction with eASSUME_NO_INITIAL_OVERLAP. eASSUME_NO_INITIAL_OVERLAP will be ignored");
		hitFlags &= ~PxHitFlag::eASSUME_NO_INITIAL_OVERLAP;
	}

	PxReal realInflation = inflation;
	if((hitFlags & PxHitFlag::ePRECISE_SWEEP)&& inflation > 0.f)
	{
		realInflation = 0.f;
		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, " Precise sweep doesn't support inflation, inflation will be overwritten to be zero");
	}
	
	writeQueryInput(mStream, MultiQueryInput(&geometry, &pose, unitDir, distance, realInflation));
	
	Ps::atomicExchange(&mBatchQueryIsRunning, 0);
}

void NpArticulation::applyImpulse(PxArticulationLink* link,
								  const PxArticulationDriveCache& driveCache,
								  const PxVec3& force,
								  const PxVec3& torque)
{
	PX_CHECK_AND_RETURN(getAPIScene(), "PxArticulation::applyImpulse: object must be in a scene");
	PX_CHECK_AND_RETURN(force.isFinite() && torque.isFinite(), "PxArticulation::applyImpulse: invalid force/torque");
	const Sc::ArticulationDriveCache& c = reinterpret_cast<const Sc::ArticulationDriveCache&>(driveCache);
	PX_CHECK_AND_RETURN(mArticulation.getScArticulation().getCacheLinkCount(c) == mArticulationLinks.size(), "PxArticulation::applyImpulse: Articulation size has changed; drive cache is invalid");

	NP_WRITE_CHECK(getOwnerScene());

	if(isSleeping())
		wakeUp();

	mArticulation.getScArticulation().applyImpulse(static_cast<NpArticulationLink*>(link)->getScbBodyFast().getScBody(), c,force, torque);
	for(PxU32 i=0;i<mArticulationLinks.size();i++)
	{
		PxVec3 lv = mArticulationLinks[i]->getScbBodyFast().getScBody().getLinearVelocity(),
			   av = mArticulationLinks[i]->getScbBodyFast().getScBody().getAngularVelocity();
		mArticulationLinks[i]->setLinearVelocity(lv);
		mArticulationLinks[i]->setAngularVelocity(av);
	}
}

void NpRigidDynamic::setAngularVelocity(const PxVec3& velocity, bool autowake)
{
    NpScene* scene = NpActor::getAPIScene(*this);

    NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
    PX_CHECK_AND_RETURN(velocity.isFinite(), "NpRigidDynamic::setAngularVelocity: velocity is not valid.");
    PX_CHECK_AND_RETURN(!(getScbBodyFast().getFlags() & PxRigidBodyFlag::eKINEMATIC), "RigidDynamic::setAngularVelocity: Body must be non-kinematic!");
    PX_CHECK_AND_RETURN(!(getScbBodyFast().getActorFlags() & PxActorFlag::eDISABLE_SIMULATION), "RigidDynamic::setAngularVelocity: Not allowed if PxActorFlag::eDISABLE_SIMULATION is set!");

    Scb::Body& b = getScbBodyFast();
    b.setAngularVelocity(velocity);

    if (scene)
        wakeUpInternalNoKinematicTest(b, (!velocity.isZero()), autowake);
}

void NpSpatialIndex::sweep(const PxBounds3& aabb, 
						   const PxVec3& unitDir, 
						   PxReal maxDist, 
						   PxSpatialLocationCallback& callback) const
{
	PX_SIMD_GUARD;

	PX_CHECK_AND_RETURN(aabb.isValid(),									"PxSpatialIndex::sweep: aabb is not valid.");
	PX_CHECK_AND_RETURN(unitDir.isFinite() && unitDir.isNormalized(),	"PxSpatialIndex::sweep: unitDir is not valid.");
	PX_CHECK_AND_RETURN(maxDist > 0.0f,									"PxSpatialIndex::sweep: distance must be positive");

	flushUpdates();
	LocationCallback cb(callback);
	PxBoxGeometry boxGeom(aabb.getExtents());
	PxTransform xf(aabb.getCenter());
	Sq::ShapeData shapeData(boxGeom, xf, 0.0f); // temporary rvalue not compatible with PX_NOCOPY 
	mPruner->sweep(shapeData, unitDir, maxDist, cb);
}

void NpBatchQuery::raycast(
	const PxVec3& origin, const PxVec3& unitDir, PxReal distance, PxU16 maxTouchHits,
	PxHitFlags hitFlags, const PxQueryFilterData& fd, void* userData, const PxQueryCache* cache)
{
	PX_CHECK_AND_RETURN(distance>0, "PxBatchQuery::raycast: The maximum distance must be greater than zero!");
	PX_CHECK_AND_RETURN(unitDir.isNormalized(), "PxBatchQuery::raycast: Direction must be normalized");
	PX_CHECK_AND_RETURN(origin.isFinite(), "PxBatchQuery::raycast: origin is not valid");
	if (mNbRaycasts >= mDesc.queryMemory.getMaxRaycastsPerExecute())
	{
		PX_CHECK_AND_RETURN(mNbRaycasts < mDesc.queryMemory.getMaxRaycastsPerExecute(),
			"PxBatchQuery: number of raycast() calls exceeds PxBatchQueryMemory::raycastResultBufferSize, query discarded");
		return;
	}
	CHECK_RUNNING("PxBatchQuery::raycast: This batch is still executing, skipping query.");
	mNbRaycasts++;

	writeBatchHeader(BatchStreamHeader(hitFlags, cache, fd, userData, maxTouchHits, QTypeROS::eRAYCAST));
	writeQueryInput(mStream, MultiQueryInput(origin, unitDir, distance));

	Ps::atomicExchange(&mBatchQueryIsRunning, 0);
}

bool physx::PxMeshQuery::sweep(	const PxVec3& unitDir, const PxReal maxDistance,
								const PxGeometry& geom, const PxTransform& pose,
								PxU32 triangleCount, const PxTriangle* triangles,
								PxSweepHit& sweepHit, PxHitFlags hintFlags_,
								const PxU32* cachedIndex, const PxReal inflation, bool doubleSided)
{
	PX_SIMD_GUARD;
	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "Gu::GeometryQuery::sweep(): pose is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(unitDir.isFinite(), "Gu::GeometryQuery::sweep(): unitDir is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(PxIsFinite(maxDistance), "Gu::GeometryQuery::sweep(): distance is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(maxDistance > 0, "Gu::GeometryQuery::sweep(): sweep distance must be greater than 0.", false);

	const PxReal distance = PxMin(maxDistance, PX_MAX_SWEEP_DISTANCE);

	// PT: the doc says that validity flags are not used, but internally some functions still check them. So
	// to make sure the code works even when no validity flags are passed, we set them all here.
	const PxHitFlags hintFlags = hintFlags_ | PxHitFlag::ePOSITION|PxHitFlag::eNORMAL|PxHitFlag::eDISTANCE;

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

			// PT: TODO: technically this capsule with 0.0 half-height is invalid ("isValid" returns false)
			const PxCapsuleGeometry capsuleGeom(sphereGeom.radius, 0.0f);

			return SweepCapsuleTriangles(	triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
											cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
		}

		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);

			return SweepCapsuleTriangles(	triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
											cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
		}

		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);

			if(!PX_IS_SPU && (hintFlags & PxHitFlag::ePRECISE_SWEEP))
			{
				return sweepCCTBoxTriangles(triangleCount, triangles, doubleSided, boxGeom, pose, 
											unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
											sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
			}
			else
			{
				return SweepBoxTriangles(	triangleCount, triangles, doubleSided, boxGeom, pose, 
											unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
											sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
			}
		}	
		case PxGeometryType::ePLANE:
		case PxGeometryType::eCONVEXMESH:
		case PxGeometryType::eTRIANGLEMESH:
		case PxGeometryType::eHEIGHTFIELD:
		case PxGeometryType::eGEOMETRY_COUNT:
		case PxGeometryType::eINVALID:
		default :
			PX_CHECK_MSG(false, "Gu::GeometryQuery::sweep(): geometry object parameter must be sphere, capsule or box geometry.");
	}
	return false;
}