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

void RendererDirectionalLight::setDirection(const PxVec3 &dir)
{
	RENDERER_ASSERT(dir.magnitudeSquared() >= 0.1f, "Trying to give Direction Light invalid Direction value.");
	if(dir.magnitudeSquared() >= 0.1f)
	{
		m_direction = dir;
		m_direction.normalize();
	}
}

bool Actor::patternFracture(const PxVec3& hitLocation, const PxVec3& dirIn, float scale, float vel, float radiusIn)
{
	int compoundNr = -1;
	int convexNr = -1;
	PxVec3 normal;
	float dist;
	bool ret = false;
	PxVec3 dir = dirIn;

	mScene->getScene()->lockWrite();

	bool hit = false;
	if (dir.magnitudeSquared() < 0.5f)
	{
		dir = PxVec3(1.f,0.f,0.f);
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
		if(!hit)
		{
			dir = PxVec3(0.f,1.f,0.f);
			hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			if(!hit)
			{
				dir = PxVec3(0.f,0.f,1.f);
				hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			}
		}
	}
	else
	{
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
	}
	if (hit)
	{
		float radius = mMinRadius + mRadiusMultiplier*radiusIn;
		float impulseMagn = scale*vel*mImpulseScale;

		if (mSheetFracture)
		{
			normal = ((Compound*)mCompounds[(uint32_t)compoundNr])->mNormal;
		}
		PxVec3 a(0.f,0.f,1.f);
		normal.normalize();
		a -= a.dot(normal)*normal;
		if( a.magnitudeSquared() < 0.1f )
		{
			a = PxVec3(0.f,1.f,0.f);
			a -= a.dot(normal)*normal;
		}
		a.normalize();
		PxVec3 b(normal.cross(a));
		PxMat33 trans(a,b,normal);
		ret = base::Actor::patternFracture(hitLocation,dir,compoundNr,trans,radius,impulseMagn,impulseMagn);
	}

	mScene->getScene()->unlockWrite();

	mRenderResourcesDirty = true;

	return ret;
}

// Each frame, we do a shape query for static and dynamic objects
// If this is the first time the synchronize has been called, then we create
// a trigger actor with two spheres in the primary scene.  This trigger
// actor is used to detect when objects move in and outside of the static and dynamic
// mirror range specified.
void MirrorScene::synchronizePrimaryScene(const physx::PxVec3 &cameraPos)
{
	PxVec3 diff = cameraPos - mLastCameraLocation;
	PxF32 dist = diff.magnitudeSquared();
	if ( dist > mMirrorDistanceThreshold )
	{
		mLastCameraLocation = cameraPos;
		if ( mTriggerActor == NULL )
		{
			createTriggerActor(cameraPos);	// Create the scene mirroring trigger actor 
		}
		if ( mTriggerActor )
		{
			mPrimaryScene.lockWrite(__FILE__,__LINE__);
			mTriggerActor->setKinematicTarget( PxTransform(cameraPos) );	// Update the position of the trigger actor to be the current camera location
			mPrimaryScene.unlockWrite();
		}
	}
	// Now, iterate on all of the current actors which are being mirrored
	// Only the primary scene after modifies this hash, so it is safe to do this
	// without any concerns of thread locking.
	// The mirrored scene thread does access the contents of this hash (MirrorActor)
	{
		mPrimaryScene.lockRead(__FILE__,__LINE__);
		for (ActorHash::Iterator i=mActors.getIterator(); !i.done(); ++i)
		{
			MirrorActor *ma = i->second;
			ma->synchronizePose();	// check to see if the position of this object in the primary
			// scene has changed.  If it has, then we create a command for the mirror scene to update
			// it's mirror actor to that new position.
		}
		mPrimaryScene.unlockRead();
	}
}

	void integrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel, PxReal timeStep, PxTransform& result)
	{
		result.p = curTrans.p + linvel * timeStep;

		//from void PxsDynamicsContext::integrateAtomPose(PxsRigidBody* atom, Cm::BitMap &shapeChangedMap) const:
		// Integrate the rotation using closed form quaternion integrator
		PxReal w = angvel.magnitudeSquared();

		if(w != 0.0f)
		{
			w = PxSqrt(w);
			if (w != 0.0f)
			{
				const PxReal v = timeStep * w * 0.5f;
				const PxReal q = PxCos(v);
				const PxReal s = PxSin(v) / w;

				const PxVec3 pqr = angvel * s;
				const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
				PxQuat out;		//need to have temporary, otherwise we may overwrite input if &curTrans == &result.
				out = quatVel * curTrans.q;
				out.x += curTrans.q.x * q;
				out.y += curTrans.q.y * q;
				out.z += curTrans.q.z * q;
				out.w += curTrans.q.w * q;
				result.q = out;
				return;
			}
		}
		//orientation stays the same - convert from quat to matrix:
		result.q = curTrans.q;
	}

void SampleVehicleWayPoints::update(const PxTransform& playerTransform, const PxF32 timestep)
{
	//Increment the elapsed time
	mTimeElapsed+=timestep;

	//Work out the point on the crossing line of the next way-point that is closest to the player.
	const PxTransform& nextWayPoint=mWayPoints[mProgress+1];
	const PxVec3 v=nextWayPoint.p;
	const PxVec3 w=nextWayPoint.q.getBasisVector0();
	const PxVec3 p=playerTransform.p;
	const PxVec3 pv=p-v;
	const PxF32 t=pv.dot(w);

	//Test if the player's position is inside the width of the line crossing the next way-point.
	if(PxAbs(t) < LINEWIDTH)
	{
		//Now test if the shortest distance to the next crossing line is smaller than a threshold.
		const PxVec3 linePos=v+w*t;
		const PxVec3 diff=p-linePos;
		const PxF32 dist2=diff.magnitudeSquared();
		if(dist2<LINEDISTANCE2)
		{
			mProgress++;
		}
	}

	if(mProgress == mNumWayPoints-1)
	{
		mMinTimeElapsed=PxMin(mTimeElapsed, mMinTimeElapsed);
		mTimeElapsed=0;
		mProgress=0;
	}
}

bool Gu::intersectSphereSphere(const Gu::Sphere& sphere0, const Gu::Sphere& sphere1)
{
	const PxVec3 delta = sphere1.center - sphere0.center;

	const PxReal distanceSq = delta.magnitudeSquared();

	const PxReal radSum = sphere0.radius + sphere1.radius;

	return distanceSq <= radSum * radSum;	// PT: objects are defined as closed, so we return 'true' in case of equality
}

bool Gu::intersectSphereBox(const Gu::Sphere& sphere, const Gu::Box& box)
{
	const PxVec3 delta = sphere.center - box.center;
	PxVec3 dRot = box.rot.transformTranspose(delta);	//transform delta into OBB body coords. (use method call!)

	//check if delta is outside ABB - and clip the vector to the ABB.
	bool outside = false;

	if(dRot.x < -box.extents.x)
	{ 
		outside = true; 
		dRot.x = -box.extents.x;
	}
	else if(dRot.x >  box.extents.x)
	{ 
		outside = true; 
		dRot.x = box.extents.x;
	}

	if(dRot.y < -box.extents.y)
	{ 
		outside = true; 
		dRot.y = -box.extents.y;
	}
	else if(dRot.y >  box.extents.y)
	{ 
		outside = true; 
		dRot.y = box.extents.y;
	}

	if(dRot.z < -box.extents.z)
	{ 
		outside = true; 
		dRot.z = -box.extents.z;
	}
	else if(dRot.z >  box.extents.z)
	{ 
		outside = true; 
		dRot.z = box.extents.z;
	}

	if(outside)	//if clipping was done, sphere center is outside of box.
	{
		const PxVec3 clippedDelta = box.rot.transform(dRot);	//get clipped delta back in world coords.

		const PxVec3 clippedVec = delta - clippedDelta;			  //what we clipped away.	
		const PxReal lenSquared = clippedVec.magnitudeSquared();
		const PxReal radius = sphere.radius;
		if(lenSquared > radius * radius)	// PT: objects are defined as closed, so we return 'true' in case of equality
			return false;	//disjoint
	}
	return true;
}

static bool GeomOverlapCallback_SphereSphere(GEOM_OVERLAP_CALLBACK_PARAMS)
{
	PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
	PX_ASSERT(geom1.getType()==PxGeometryType::eSPHERE);
	PX_UNUSED(cache);

	const PxSphereGeometry& sphereGeom0 = static_cast<const PxSphereGeometry&>(geom0);
	const PxSphereGeometry& sphereGeom1 = static_cast<const PxSphereGeometry&>(geom1);

	const PxVec3 delta = transform1.p - transform0.p;
	return delta.magnitudeSquared() <= Ps::sqr(sphereGeom0.radius + sphereGeom1.radius);	// PT: objects are defined as closed, so we return 'true' in case of equality
}

bool physx::Gu::computeSphere_SphereMTD(const Sphere& sphere0, const Sphere& sphere1, PxSweepHit& hit)
{
	const PxVec3 delta = sphere1.center - sphere0.center;
	const PxReal d2 = delta.magnitudeSquared();
	const PxReal radiusSum = sphere0.radius + sphere1.radius;

	const PxReal d = PxSqrt(d2);
	hit.normal = delta / d;
	hit.distance = d - radiusSum ;
	hit.position = sphere0.center + hit.normal * sphere0.radius;
	return true;
}

PX_INLINE void computeFrictionTangents(const PxVec3& vrel,const PxVec3& unitNormal, PxVec3& t0, PxVec3& t1)
{
	PX_ASSERT(PxAbs(unitNormal.magnitude()-1)<1e-3f);

	t0 = vrel - unitNormal * unitNormal.dot(vrel);
	PxReal ll = t0.magnitudeSquared();

	if (ll > 0.1f)										//can set as low as 0.
	{
		t0 *= PxRecipSqrt(ll);
		t1 = unitNormal.cross(t0);
	}
	else
		Ps::normalToTangents(unitNormal, t0, t1);		//fallback
}

void ClothImpl<SwCloth>::setVirtualParticles(Range<const uint32_t[4]> indices, Range<const PxVec3> weights)
{
	mCloth.mNumVirtualParticles = 0;

	// shuffle indices to form independent SIMD sets
	uint16_t numParticles = uint16_t(mCloth.mCurParticles.size());
	TripletScheduler scheduler(indices);
	scheduler.simd(numParticles, 4);

	// convert indices to byte offset
	Vec4us dummy(numParticles, uint16_t(numParticles+1), uint16_t(numParticles+2), 0); 
	Vector<uint32_t>::Type::ConstIterator sIt = scheduler.mSetSizes.begin();
	Vector<uint32_t>::Type::ConstIterator sEnd = scheduler.mSetSizes.end();
	TripletScheduler::ConstTripletIter tIt = scheduler.mTriplets.begin(), tLast;
	mCloth.mVirtualParticleIndices.resize(0);
	mCloth.mVirtualParticleIndices.reserve(indices.size() + 3 * uint32_t(sEnd - sIt));
	for(; sIt != sEnd; ++sIt)
	{
		uint32_t setSize = *sIt;
		for(tLast = tIt + setSize; tIt != tLast; ++tIt, ++mCloth.mNumVirtualParticles)
			mCloth.mVirtualParticleIndices.pushBack(Vec4us(*tIt));
		mCloth.mVirtualParticleIndices.resize(
			(mCloth.mVirtualParticleIndices.size() + 3) & ~3, dummy);
	}
	Vector<Vec4us>::Type(mCloth.mVirtualParticleIndices.begin(), 
		mCloth.mVirtualParticleIndices.end()).swap(mCloth.mVirtualParticleIndices);

	// precompute 1/dot(w,w)
	Vec4fAlignedVector().swap(mCloth.mVirtualParticleWeights);
	mCloth.mVirtualParticleWeights.reserve(weights.size());
	for(; !weights.empty(); weights.popFront())
	{
		PxVec3 w = reinterpret_cast<const PxVec3&>(weights.front());
		PxReal scale = 1 / w.magnitudeSquared();
		mCloth.mVirtualParticleWeights.pushBack(PxVec4(w.x, w.y, w.z, scale));
	}

	mCloth.notifyChanged();
}

void ApplyInverseSquareGravity(PxRigidActor* actor, PxVec3 source, PxReal power)
{
    PxVec3 dir;
    PxReal distSquared;
    PxVec3 norm;
    PxVec3 force;
    int objectNum = boxes.size();

    for(int i = 0; i < objectNum; i++)
    {
        //Disables the scene gravity so we can apply our own
        DisableGravity(boxes[i]->actor);

        dir = source - boxes[i]->actor->getGlobalPose().p;

        distSquared = dir.magnitudeSquared();
        distSquared = (distSquared < 10) ? 10000 : distSquared;

        norm = dir.getNormalized();
        force = (norm * power) / distSquared;

        boxes[i]->actor->isRigidBody()->addForce(force, PxForceMode::eACCELERATION);
    }
}

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);
//.........这里部分代码省略.........

static void tessellateTriangle(TessParams* tp, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2)
{
	tp->nbTessellation++;

	if(!Gu::intersectTriangleBox(tp->cullingBoxCenter, tp->cullingBoxExtents, v0, v1, v2))
		return;

	PxU32 code;
	{
		const PxVec3 edge0 = v0 - v1;
		const PxVec3 edge1 = v1 - v2;
		const PxVec3 edge2 = v2 - v0;
		const float maxEdgeLength2 = tp->maxEdgeLength2;
		const bool split0 = edge0.magnitudeSquared()>maxEdgeLength2;
		const bool split1 = edge1.magnitudeSquared()>maxEdgeLength2;
		const bool split2 = edge2.magnitudeSquared()>maxEdgeLength2;
		code = (PxU32(split2)<<2)|(PxU32(split1)<<1)|PxU32(split0);
	}

	const PxVec3 m0 = (v0 + v1)*0.5f;
	const PxVec3 m1 = (v1 + v2)*0.5f;
	const PxVec3 m2 = (v2 + v0)*0.5f;

	switch(code)
	{
		case 0:     // 000: no split
		{
			tp->worldTriangles->pushBack(PxTriangle(v0, v1, v2));
			tp->triIndicesArray->pushBack(tp->index);
			tp->nbNewTris++;
		}
		break;
		case 1:     // 001: split edge0
		{
			tessellateTriangle(tp, v0, m0, v2);
			tessellateTriangle(tp, m0, v1, v2);
		}
		break;
		case 2:     // 010: split edge1
		{
			tessellateTriangle(tp, v0, v1, m1);
			tessellateTriangle(tp, v0, m1, v2);
		}
		break;
		case 3:     // 011: split edge0/edge1
		{
			tessellateTriangle(tp, v0, m0, m1);
			tessellateTriangle(tp, v0, m1, v2);
			tessellateTriangle(tp, m0, v1, m1);
		}
		break;
		case 4:     // 100: split edge2
		{
			tessellateTriangle(tp, v0, v1, m2);
			tessellateTriangle(tp, v1, v2, m2);
		}
		break;
		case 5:     // 101: split edge0/edge2
		{
			tessellateTriangle(tp, v0, m0, m2);
			tessellateTriangle(tp, m0, v1, m2);
			tessellateTriangle(tp, m2, v1, v2);
		}
		break;
		case 6:     // 110: split edge1/edge2
		{
			tessellateTriangle(tp, v0, v1, m1);
			tessellateTriangle(tp, v0, m1, m2);
			tessellateTriangle(tp, m2, m1, v2);
		}
		break;
		case 7:     // 111: split edge0/edge1/edge2
		{
			tessellateTriangle(tp, v0, m0, m2);
			tessellateTriangle(tp, m0, v1, m1);
			tessellateTriangle(tp, m2, m1, v2);
			tessellateTriangle(tp, m0, m1, m2);
		}
		break;
	};
}

PX_FORCE_INLINE void collideWithSphere(PxsParticleCollData& collData, const PxSphereGeometry& sphereShapeData,
									   PxReal proxRadius)
{
	PxVec3& oldPos = collData.localOldPos;
	PxVec3& newPos = collData.localNewPos;

	PxReal radius = sphereShapeData.radius;

	PxReal oldPosDist2 = oldPos.magnitudeSquared();
	PxReal radius2 = radius * radius;

	bool oldInSphere = (oldPosDist2 < radius2);

	if(oldInSphere)
	{
		// old position inside the skeleton
		// add ccd with time 0.0

		collData.localSurfaceNormal = oldPos;
		if (oldPosDist2 > 0.0f)
			collData.localSurfaceNormal *= PxRecipSqrt(oldPosDist2);
		else
			collData.localSurfaceNormal = PxVec3(0,1.0f,0);

		// Push particle to surface such that the distance to the surface is equal to the collision radius
		collData.localSurfacePos = collData.localSurfaceNormal * (radius + collData.restOffset);
		collData.ccTime = 0.0;
		collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
	}
	else
	{
		// old position is outside of the skeleton
		
		PxVec3 motion = newPos - oldPos;

		// Discriminant
		PxReal b = motion.dot(oldPos) * 2.0f;
		PxReal a2 = 2.0f * motion.magnitudeSquared();
		PxReal disc = (b*b) - (2.0f * a2 * (oldPosDist2 - radius2));

		bool intersection = disc > 0.0f;

		if ((!intersection) || (a2 == 0.0f))
		{
			// the ray does not intersect the sphere
			collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
		}
		else
		{
			// the ray intersects the sphere
			PxReal t = -(b + PxSqrt(disc)) / a2;	// Compute intersection point
			
			if (t < 0.0f || t > 1.0f)
			{
				// intersection point lies outside motion vector
				collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
			}
			else if(t < collData.ccTime)
			{
				// intersection point lies on sphere, add lcc
				//collData.localSurfacePos = oldPos + (motion * t);
				//collData.localSurfaceNormal = collData.localSurfacePos;
				//collData.localSurfaceNormal *= (1.0f / radius);
				//collData.localSurfacePos += (collData.localSurfaceNormal * collData.restOffset);
				PxVec3 relativeImpact = motion*t;
				collData.localSurfaceNormal = oldPos + relativeImpact;
				collData.localSurfaceNormal *= (1.0f / radius);		
				computeContinuousTargetPosition(collData.localSurfacePos, collData.localOldPos, relativeImpact, collData.localSurfaceNormal, collData.restOffset);
				
				collData.ccTime = t;
				collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
			}
		}
	}
}