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

	bool ParticleChain::checkPenetration(const Ogre::Vector3 &position, Ogre::Vector3 &closestSurfacePos, Ogre::Vector3 &collisionNormal)
	{
		PxShape *hit = nullptr;
		PxVec3 pxPos = Convert::toPx(position);
		if (mPhysXScene->getPxScene()->overlapAny(PxSphereGeometry(0.05f), PxTransform(pxPos), hit))
		{
			PxVec3 actorCenter = hit->getActor().getWorldBounds().getCenter();
			PxVec3 rayDir = actorCenter - pxPos;
			rayDir.normalize();
			PxVec3 rayOrigin = pxPos - rayDir.multiply(PxVec3(0.2f, 0.2f, 0.2f));
			if (mPhysXScene->getPxScene()->overlapAny(PxSphereGeometry(0.05f), PxTransform(rayOrigin), hit)) return false;
			PxRaycastHit rayHit;
			if (mPhysXScene->getPxScene()->raycastSingle(rayOrigin, rayDir,  0.2f, PxSceneQueryFlag::eIMPACT|PxSceneQueryFlag::eNORMAL|PxSceneQueryFlag::eDISTANCE, rayHit))
			{
				closestSurfacePos = Convert::toOgre(rayHit.impact);
				collisionNormal = Convert::toOgre(rayHit.normal);
				return true;
			}
		}
		return false;
	}

//////////////////////////////////////////////////////////////////////////
// normalize point cloud, remove duplicates!
bool ConvexHullLib::cleanupVertices(PxU32 svcount, const PxVec3* svertices, PxU32 stride,
	PxU32& vcount, PxVec3* vertices, PxVec3& scale, PxVec3& center)
{
	if ( svcount == 0 ) 
		return false;

	const PxVec3* verticesToClean = svertices;
	PxU32 numVerticesToClean = svcount;
	Quantizer* quantizer = NULL;

	// if quantization is enabled, parse the input vertices and produce new qantized vertices, 
	// that will be then cleaned the same way
	if (mConvexMeshDesc.flags & PxConvexFlag::eQUANTIZE_INPUT)
	{
		quantizer = createQuantizer();
		PxU32 vertsOutCount;
		const PxVec3* vertsOut = quantizer->kmeansQuantize3D(svcount, svertices, stride,true, mConvexMeshDesc.quantizedCount, vertsOutCount);

		if (vertsOut)
		{
			numVerticesToClean = vertsOutCount;
			verticesToClean = vertsOut;
		}		
	}

	const float distanceEpsilon = local::DISTANCE_EPSILON * mCookingParams.scale.length;
	const float resizeValue = local::RESIZE_VALUE * mCookingParams.scale.length;
	const float normalEpsilon = local::NORMAL_DISTANCE_EPSILON; // used to determine if 2 points are the same

	vcount = 0;
	PxVec3 recip;

	scale = PxVec3(1.0f);

	// check for the AABB from points, if its very tiny return a resized CUBE
	if (local::checkPointsAABBValidity(numVerticesToClean, verticesToClean, stride, distanceEpsilon, resizeValue, center, scale, vcount, vertices, false))
	{
		if (quantizer)
			quantizer->release();
		return true;
	}

	recip[0] = 1 / scale[0];
	recip[1] = 1 / scale[1];
	recip[2] = 1 / scale[2];

	center = center.multiply(recip);

	// normalize the point cloud
	const char * vtx = reinterpret_cast<const char *> (verticesToClean);
	for (PxU32 i = 0; i<numVerticesToClean; i++)
	{
		const PxVec3& p = *reinterpret_cast<const PxVec3 *>(vtx);
		vtx+=stride;

		PxVec3 normalizedP = p.multiply(recip); // normalize

		PxU32 j;

		// parse the already stored vertices and check the distance
		for (j=0; j<vcount; j++)
		{
			PxVec3& v = vertices[j];

			const float dx = fabsf(normalizedP[0] - v[0] );
			const float dy = fabsf(normalizedP[1] - v[1] );
			const float dz = fabsf(normalizedP[2] - v[2] );

			if ( dx < normalEpsilon && dy < normalEpsilon && dz < normalEpsilon )
			{
				// ok, it is close enough to the old one
				// now let us see if it is further from the center of the point cloud than the one we already recorded.
				// in which case we keep this one instead.
				const float dist1 = (normalizedP - center).magnitudeSquared();
				const float dist2 = (v - center).magnitudeSquared();

				if ( dist1 > dist2 )
				{
					v = normalizedP;
				}
				break;
			}
		}

		// we dont have that vertex in the output, add it
		if ( j == vcount )
		{
			vertices[vcount] = normalizedP;
			vcount++;
		}
	}

	// scale the verts back
	for (PxU32 i = 0; i < vcount; i++)
	{
		vertices[i] = vertices[i].multiply(scale);
	}

//.........这里部分代码省略.........

void CreateCloth()
{

	PxTransform gPose = PxTransform(PxVec3(0, 1, 0));

	// create regular mesh
	PxU32 resolution = 20;
	PxU32 numParticles = resolution*resolution;
	PxU32 numTriangles = 2 * (resolution - 1)*(resolution - 1);

	// create cloth particles
	PxClothParticle* particles = new PxClothParticle[numParticles];
	PxVec3 center(0.5f, 0.3f, 0.0f);
	PxVec3 delta = 1.0f / (resolution - 1) * PxVec3(15.0f, 15.0f, 15.0f);
	PxClothParticle* pIt = particles;
	for (PxU32 i = 0; i < resolution; ++i)
	{
		for (PxU32 j = 0; j < resolution; ++j, ++pIt)
		{
			pIt->invWeight = j + 1 < resolution ? 1.0f : 0.0f;
			pIt->pos = delta.multiply(PxVec3(PxReal(i),
				PxReal(j), -PxReal(j))) - center;
		}
	}

	// create triangles
	PxU32* triangles = new PxU32[3 * numTriangles];
	PxU32* iIt = triangles;
	for (PxU32 i = 0; i < resolution - 1; ++i)
	{
		for (PxU32 j = 0; j < resolution - 1; ++j)
		{
			PxU32 odd = j & 1u, even = 1 - odd;
			*iIt++ = i*resolution + (j + odd);
			*iIt++ = (i + odd)*resolution + (j + 1);
			*iIt++ = (i + 1)*resolution + (j + even);
			*iIt++ = (i + 1)*resolution + (j + even);
			*iIt++ = (i + even)*resolution + j;
			*iIt++ = i*resolution + (j + odd);
		}
	}

	// create fabric from mesh
	PxClothMeshDesc meshDesc;
	meshDesc.points.count = numParticles;
	meshDesc.points.stride = sizeof(PxClothParticle);
	meshDesc.points.data = particles;

	meshDesc.invMasses.count = numParticles;
	meshDesc.invMasses.stride = sizeof(PxClothParticle);
	meshDesc.invMasses.data = &particles->invWeight;

	meshDesc.triangles.count = numTriangles;
	meshDesc.triangles.stride = 3 * sizeof(PxU32);
	meshDesc.triangles.data = triangles;


	// cook fabric
	PxClothFabric* fabric = PxClothFabricCreate(*gPhysicsSDK, meshDesc, PxVec3(0, 1, 0));

	delete[] triangles;

	// create cloth
	gCloth = gPhysicsSDK->createCloth(gPose, *fabric, particles, PxClothFlags(0));

	fabric->release();
	delete[] particles;

	// 240 iterations per/second (4 per-60hz frame)
	gCloth->setSolverFrequency(240.0f);

	gScene->addActor(*gCloth);


}

PxReal Sc::BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, PxReal freezeThreshold, PxReal invDt, bool enableStabilization)
{
    // update the body's sleep state and
    BodyCore& core = getBodyCore();

    PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;

    PxReal wc = core.getWakeCounter();

    {
        if(enableStabilization)
        {
            bool isFrozen = false;
            const PxTransform& body2World = getBody2World();

            // calculate normalized energy: kinetic energy divided by mass

            const PxVec3 t = core.getInverseInertia();
            const PxVec3 inertia(t.x > 0.f ? 1.0f/t.x : 1.f, t.y > 0.f ? 1.0f/t.y : 1.f, t.z > 0.f ? 1.0f/t.z : 1.f);


            PxVec3 sleepLinVelAcc = mLLBody.mAcceleration.linear;
            PxVec3 sleepAngVelAcc = body2World.q.rotateInv(mLLBody.mAcceleration.angular);

            // scale threshold by cluster factor (more contacts => higher sleep threshold)
            //const PxReal clusterFactor = PxReal(1u + getNumUniqueInteractions());
            const PxU32 clusterFactor = getNumUniqueInteractions();

            PxReal invMass = core.getInverseMass();
            if(invMass == 0.f)
                invMass = 1.f;

            const PxReal angular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
            const PxReal linear = sleepLinVelAcc.magnitudeSquared();
            PxReal frameNormalizedEnergy = 0.5f * (angular + linear);

            const PxReal cf = readInternalFlag(BF_HAS_STATIC_TOUCH) && clusterFactor > 1 ? clusterFactor : 0.f;
            const PxReal freezeThresh = cf*freezeThreshold;

            mFreezeCount = PxMax(mFreezeCount-dt, 0.0f);
            bool settled = true;
            if (frameNormalizedEnergy >= freezeThresh)
            {
                settled = false;
                mFreezeCount = PX_FREEZE_INTERVAL;
                if(frameNormalizedEnergy >= (freezeThresh * cf))
                {
                    mAccelScale = 0.f;
                }
            }

            if(settled || mAccelScale > 0.f)
            {
                //Dampen bodies that are just about to go to sleep
                const PxReal sleepDamping = PX_SLEEP_DAMPING;
                const PxReal sleepDampingTimesDT=sleepDamping*dt;
                const PxReal d=1.0f-sleepDampingTimesDT;
                core.setLinearVelocity(core.getLinearVelocity()*d);
                core.setAngularVelocity(core.getAngularVelocity()*d);
                mAccelScale = invDt * PX_FREEZE_SCALE;
                isFrozen = mFreezeCount == 0.f && frameNormalizedEnergy < freezeThreshold;
            }
            if(isFrozen)
            {
                getBodyCore().getCore().mInternalFlags |= PxsRigidCore::eFROZEN;
                core.getCore().body2World = mLLBody.getLastCCDTransform();
            }
            else
                getBodyCore().getCore().mInternalFlags &= (~PxsRigidCore::eFROZEN);

            /*KS: New algorithm for sleeping when using stabilization:
            * Energy *this frame* must be higher than sleep threshold and accumulated energy over previous frames
            * must be higher than clusterFactor*energyThreshold.
            */
            if(wc < wakeCounterResetTime * 0.5f || wc < dt)
            {
                //Accumulate energy
                mSleepLinVelAcc += sleepLinVelAcc;
                mSleepAngVelAcc += sleepAngVelAcc;

                //If energy this frame is high
                if (frameNormalizedEnergy >= energyThreshold)
                {
                    //Compute energy over sleep preparation time
                    const PxReal sleepAngular = mSleepAngVelAcc.multiply(mSleepAngVelAcc).dot(inertia) * invMass;
                    const PxReal sleepLinear = mSleepLinVelAcc.magnitudeSquared();
                    PxReal normalizedEnergy = 0.5f * (sleepAngular + sleepLinear);
                    PxReal sleepClusterFactor = clusterFactor+1.f;

                    // scale threshold by cluster factor (more contacts => higher sleep threshold)
                    const PxReal threshold = sleepClusterFactor*energyThreshold;

                    //If energy over sleep preparation time is high
                    if(normalizedEnergy >= threshold)
                    {
                        //Wake up
                        PX_ASSERT(isActive());

                        resetSleepFilter();
                        const float factor = energyThreshold == 0.f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
//.........这里部分代码省略.........