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);
}
// ok..now make sure we didn't prune so many vertices it is now invalid.
// note, that the output vertices are again scaled, we need to scale them back then
local::checkPointsAABBValidity(vcount, vertices, sizeof(PxVec3), distanceEpsilon, resizeValue, center, scale, vcount, vertices, true);
if (quantizer)
quantizer->release();
return true;
}
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);
PxReal oldWc = wc;
wc = factor * 0.5f * wakeCounterResetTime + dt * (sleepClusterFactor - 1.0f);
core.setWakeCounterFromSim(wc);
if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
notifyNotReadyForSleeping();
return wc;
}
}
}
}
else if(wc < wakeCounterResetTime * 0.5f || wc < dt)
{
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);
mSleepLinVelAcc += sleepLinVelAcc;
mSleepAngVelAcc += sleepAngVelAcc;
PxReal invMass = core.getInverseMass();
if(invMass == 0.f)
invMass = 1.f;
const PxReal angular = mSleepAngVelAcc.multiply(mSleepAngVelAcc).dot(inertia) * invMass;
const PxReal linear = mSleepLinVelAcc.magnitudeSquared();
PxReal normalizedEnergy = 0.5f * (angular + linear);
// scale threshold by cluster factor (more contacts => higher sleep threshold)
const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
const PxReal threshold = clusterFactor*energyThreshold;
if (normalizedEnergy >= threshold)
{
PX_ASSERT(isActive());
resetSleepFilter();
const float factor = threshold == 0.f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
PxReal oldWc = wc;
wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
core.setWakeCounterFromSim(wc);
if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
notifyNotReadyForSleeping();
return wc;
}
}
}
wc = PxMax(wc-dt, 0.0f);
core.setWakeCounterFromSim(wc);
return wc;
}
