void physx::Pt::collideCellsWithStaticMesh(ParticleCollData* collData, const LocalCellHash& localCellHash,
const GeometryUnion& meshShape, const PxTransform& world2Shape,
const PxTransform& shape2World, PxReal /*cellSize*/,
PxReal /*collisionRange*/, PxReal proxRadius, const PxVec3& /*packetCorner*/)
{
PX_ASSERT(collData);
PX_ASSERT(localCellHash.isHashValid);
PX_ASSERT(localCellHash.numParticles <= PT_SUBPACKET_PARTICLE_LIMIT_COLLISION);
PX_ASSERT(localCellHash.numHashEntries <= PT_LOCAL_HASH_SIZE_MESH_COLLISION);
const PxTriangleMeshGeometryLL& meshShapeData = meshShape.get<const PxTriangleMeshGeometryLL>();
const TriangleMesh* meshData = meshShapeData.meshData;
PX_ASSERT(meshData);
// mesh bounds in world space (conservative)
const PxBounds3 shapeBounds =
meshData->getLocalBoundsFast().transformSafe(world2Shape.getInverse() * meshShapeData.scale);
const bool idtScaleMesh = meshShapeData.scale.isIdentity();
Cm::FastVertex2ShapeScaling meshScaling;
if(!idtScaleMesh)
meshScaling.init(meshShapeData.scale);
// process the particle cells
for(PxU32 c = 0; c < localCellHash.numHashEntries; c++)
{
const ParticleCell& cell = localCellHash.hashEntries[c];
if(cell.numParticles == PX_INVALID_U32)
continue;
PxBounds3 cellBounds;
cellBounds.setEmpty();
PxBounds3 cellBoundsNew(PxBounds3::empty());
PxU32* it = localCellHash.particleIndices + cell.firstParticle;
const PxU32* end = it + cell.numParticles;
for(; it != end; it++)
{
const ParticleCollData& particle = collData[*it];
cellBounds.include(particle.oldPos);
cellBoundsNew.include(particle.newPos);
}
PX_ASSERT(!cellBoundsNew.isEmpty());
cellBoundsNew.fattenFast(proxRadius);
cellBounds.include(cellBoundsNew);
if(!cellBounds.intersects(shapeBounds))
continue; // early out if (inflated) cell doesn't intersect mesh bounds
// opcode query: cell bounds against shape bounds in unscaled mesh space
PxcContactCellMeshCallback callback(collData, &(localCellHash.particleIndices[cell.firstParticle]),
cell.numParticles, *meshData, meshScaling, proxRadius, NULL, shape2World);
testBoundsMesh(*meshData, world2Shape, meshScaling, idtScaleMesh, cellBounds, callback);
}
}
PxBounds3 RTree::refitAll(CallbackRefit& cb)
{
PxU8* treeNodes8 = PX_IS_X64 ? CAST_U8(get64BitBasePage()) : CAST_U8((mFlags & IS_DYNAMIC) ? NULL : mPages);
PxBounds3 meshBounds = refitRecursive(treeNodes8, 0, NULL, 0, cb);
for (PxU32 j = 1; j<mNumRootPages; j++)
meshBounds.include( refitRecursive(treeNodes8, j, NULL, 0, cb) );
#ifdef PX_CHECKED
validate(&cb);
#endif
return meshBounds;
}
PxBounds3 NpArticulation::getWorldBounds(float inflation) const
{
NP_READ_CHECK(getOwnerScene());
PxBounds3 bounds = PxBounds3::empty();
for(PxU32 i=0; i < mArticulationLinks.size(); i++)
{
bounds.include(mArticulationLinks[i]->getWorldBounds());
}
PX_ASSERT(bounds.isValid());
// PT: unfortunately we can't just scale the min/max vectors, we need to go through center/extents.
const PxVec3 center = bounds.getCenter();
const PxVec3 inflatedExtents = bounds.getExtents() * inflation;
return PxBounds3::centerExtents(center, inflatedExtents);
}
bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const PxU32* primitives, PxU32 nb_prims, PxBounds3& global_box) const
{
// Checkings
if(!primitives || !nb_prims) return false;
// Initialize global box
global_box = mAABBArray[primitives[0]];
// Loop through boxes
for(PxU32 i=1;i<nb_prims;i++)
{
// Update global box
global_box.include(mAABBArray[primitives[i]]);
}
return true;
}
bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const PxU32* primitives, PxU32 nb_prims, PxBounds3& global_box) const
{
// Checkings
if(!primitives || !nb_prims) return false;
// Initialize global box
global_box.setEmpty();
// Loop through vertices
for(PxU32 i=0;i<nb_prims;i++)
{
// Update global box
global_box.include(mVertexArray[primitives[i]]);
}
return true;
}
void EmitterGeomSphereShellImpl::computeFillPositions(physx::Array<PxVec3>& positions,
physx::Array<PxVec3>& velocities,
const PxTransform& pose,
const PxVec3& scale,
float objRadius,
PxBounds3& outBounds,
QDSRand&) const
{
PX_UNUSED(scale);
const float bigRadius = *mRadius + *mShellThickness;
const float radiusSquared = bigRadius * bigRadius;
const float hemisphere = *mHemisphere;
const float sphereCapBaseHeight = -bigRadius + 2 * bigRadius * hemisphere;
const float sphereCapBaseRadius = PxSqrt(radiusSquared - sphereCapBaseHeight * sphereCapBaseHeight);
const float horizontalExtents = hemisphere < 0.5f ? bigRadius : sphereCapBaseRadius;
// we're not doing anything with the velocities array
PX_UNUSED(velocities);
// we don't want anything outside the emitter
uint32_t numX = (uint32_t)PxFloor(horizontalExtents / objRadius);
numX -= numX % 2;
uint32_t numY = (uint32_t)PxFloor((bigRadius - sphereCapBaseHeight) / objRadius);
numY -= numY % 2;
uint32_t numZ = (uint32_t)PxFloor(horizontalExtents / objRadius);
numZ -= numZ % 2;
for (float x = -(numX * objRadius); x <= bigRadius - objRadius; x += 2 * objRadius)
{
for (float y = -(numY * objRadius); y <= bigRadius - objRadius; y += 2 * objRadius)
{
for (float z = -(numZ * objRadius); z <= bigRadius - objRadius; z += 2 * objRadius)
{
const float magnitudeSquare = PxVec3(x, y, z).magnitudeSquared();
if ((magnitudeSquare > (*mRadius + objRadius) * (*mRadius + objRadius)) &&
(magnitudeSquare < (bigRadius - objRadius) * (bigRadius - objRadius)))
{
positions.pushBack(pose.transform(PxVec3(x, y, z)));
outBounds.include(positions.back());
}
}
}
}
}
PxBounds3 RTree::refitRecursive(PxU8* treeNodes8, PxU32 top, RTreePage* parentPage, PxU32 parentSubIndex, CallbackRefit& cb)
{
const PxReal eps = RTREE_INFLATION_EPSILON;
RTreePage* tn = (RTreePage*)(treeNodes8 + top);
PxBounds3 pageBound;
for (PxU32 i = 0; i < RTREE_PAGE_SIZE; i++)
{
if (tn->isEmpty(i))
continue;
PxU32 ptr = tn->ptrs[i];
Vec3V childMn, childMx;
PxBounds3 childBound;
if (ptr & 1)
{
// (ptr-1) clears the isLeaf bit (lowest bit)
cb.recomputeBounds(ptr-1, childMn, childMx); // compute the bound around triangles
V3StoreU(childMn, childBound.minimum);
V3StoreU(childMx, childBound.maximum);
// AP: doesn't seem worth vectorizing because of transposed layout
tn->minx[i] = childBound.minimum.x - eps; // update page bounds for this leaf
tn->miny[i] = childBound.minimum.y - eps;
tn->minz[i] = childBound.minimum.z - eps;
tn->maxx[i] = childBound.maximum.x + eps;
tn->maxy[i] = childBound.maximum.y + eps;
tn->maxz[i] = childBound.maximum.z + eps;
} else
childBound = refitRecursive(treeNodes8, ptr, tn, i, cb);
if (i == 0)
pageBound = childBound;
else
pageBound.include(childBound);
}
if (parentPage)
{
parentPage->minx[parentSubIndex] = pageBound.minimum.x - eps;
parentPage->miny[parentSubIndex] = pageBound.minimum.y - eps;
parentPage->minz[parentSubIndex] = pageBound.minimum.z - eps;
parentPage->maxx[parentSubIndex] = pageBound.maximum.x + eps;
parentPage->maxy[parentSubIndex] = pageBound.maximum.y + eps;
parentPage->maxz[parentSubIndex] = pageBound.maximum.z + eps;
}
return pageBound;
}
bool Character::buildMotion(Acclaim::AMCData &amcData, Motion &motion, PxU32 start, PxU32 end)
{
using namespace Acclaim;
if (mASFData == NULL)
return false;
motion.mNbFrames = end - start + 1;
motion.mMotionData = SAMPLE_NEW(MotionData)[motion.mNbFrames];
// compute bounds of all the motion data on normalized frame
PxBounds3 bounds = PxBounds3::empty();
for (PxU32 i = start; i < end; i++)
{
Acclaim::FrameData &frameData = amcData.mFrameData[i];
PxTransform rootTransform(PxVec3(0.0f), EulerAngleToQuat(frameData.mRootOrientation));
for (PxU32 j = 0; j < mASFData->mNbBones; j++)
{
PxTransform t = computeBoneTransform(rootTransform, mASFData->mBones[j], frameData.mBoneFrameData);
bounds.include(t.p);
}
}
Acclaim::FrameData& firstFrame = amcData.mFrameData[0];
Acclaim::FrameData& lastFrame = amcData.mFrameData[amcData.mNbFrames-1];
// compute direction vector
motion.mDistance = mCharacterScale * (lastFrame.mRootPosition - firstFrame.mRootPosition).magnitude();
PxVec3 firstPosition = firstFrame.mRootPosition;
PX_UNUSED(firstPosition);
PxVec3 firstAngles = firstFrame.mRootOrientation;
PxQuat firstOrientation = EulerAngleToQuat(PxVec3(0, firstAngles.y, 0));
for (PxU32 i = 0; i < motion.mNbFrames; i++)
{
Acclaim::FrameData& frameData = amcData.mFrameData[i+start];
MotionData &motionData = motion.mMotionData[i];
// normalize y-rot by computing inverse quat from first frame
// this makes all the motion aligned in the same (+ z) direction.
PxQuat currentOrientation = EulerAngleToQuat(frameData.mRootOrientation);
PxQuat qdel = firstOrientation.getConjugate() * currentOrientation;
PxTransform rootTransform(PxVec3(0.0f), qdel);
for (PxU32 j = 0; j < mNbBones; j++)
{
PxTransform boneTransform = computeBoneTransform(rootTransform, mASFData->mBones[j], frameData.mBoneFrameData);
motionData.mBoneTransform[j] = boneTransform;
}
//PxReal y = mCharacterScale * (frameData.mRootPosition.y - firstPosition.y) - bounds.minimum.y;
motionData.mRootTransform = PxTransform(PxVec3(0.0f, -bounds.minimum.y, 0.0f), PxQuat(PxIdentity));
}
// now make the motion cyclic by linear interpolating root position and joint angles
const PxU32 windowSize = 10;
for (PxU32 i = 0; i <= windowSize; i++)
{
PxU32 j = motion.mNbFrames - 1 - windowSize + i;
PxReal t = PxReal(i) / PxReal(windowSize);
MotionData& motion_i = motion.mMotionData[0];
MotionData& motion_j = motion.mMotionData[j];
// lerp root translation
PxVec3 blendedRootPos = (1.0f - t) * motion_j.mRootTransform.p + t * motion_i.mRootTransform.p;
for (PxU32 k = 0; k < mNbBones; k++)
{
PxVec3 pj = motion_j.mRootTransform.p + motion_j.mBoneTransform[k].p;
PxVec3 pi = motion_i.mRootTransform.p + motion_i.mBoneTransform[k].p;
PxVec3 p = (1.0f - t) * pj + t * pi;
motion_j.mBoneTransform[k].p = p - blendedRootPos;
}
motion_j.mRootTransform.p = blendedRootPos;
}
return true;
}
void physx::collideWithStaticMesh(PxU32 numParticles, PxsParticleCollData* collData,
PxsFluidParticleOpcodeCache* opcodeCaches, const Gu::GeometryUnion& meshShape,
const PxTransform& world2Shape, const PxTransform& shape2World, PxReal /*cellSize*/, PxReal collisionRange, PxReal proxRadius)
{
PX_ASSERT(collData);
PX_ASSERT(opcodeCaches);
const PxTriangleMeshGeometryLL& meshShapeData = meshShape.get<const PxTriangleMeshGeometryLL>();
const bool idtScaleMesh = meshShapeData.scale.isIdentity();
Cm::FastVertex2ShapeScaling meshScaling;
if(!idtScaleMesh)
meshScaling.init(meshShapeData.scale);
const PxF32 maxCacheBoundsExtent = 4*collisionRange + proxRadius;
const PxsFluidParticleOpcodeCache::QuantizationParams quantizationParams =
PxsFluidParticleOpcodeCache::getQuantizationParams(maxCacheBoundsExtent);
const Gu::InternalTriangleMeshData* meshData = meshShapeData.meshData;
PX_ASSERT(meshData);
bool isSmallMesh = meshData->mNumTriangles <= 0xffff;
PxU32 cachedTriangleBuffer[PxsFluidParticleOpcodeCache::sMaxCachedTriangles];
PxVec3 extent(proxRadius);
for (PxU32 i = 0; i < numParticles; ++i)
{
//had to make this non-const to be able to update cache bits
PxsParticleCollData& particle = collData[i];
PxsFluidParticleOpcodeCache& cache = opcodeCaches[i];
PxBounds3 bounds;
{
bounds = PxBounds3(particle.newPos - extent, particle.newPos + extent);
bounds.include(particle.oldPos);
}
PxU32 numTriangles = 0;
const PxU32* triangles = NULL;
bool isCached = cache.read(particle.particleFlags.low, numTriangles, cachedTriangleBuffer, bounds, quantizationParams, &meshShape, isSmallMesh);
if (isCached)
{
triangles = cachedTriangleBuffer;
if (numTriangles > 0)
{
PxVec3 triangleVerts[PxsFluidParticleOpcodeCache::sMaxCachedTriangles*3];
const PxU32* triangleIndexIt = triangles;
for (PxU32 j = 0; j < numTriangles; ++j, ++triangleIndexIt)
{
Gu::MeshInterface::GetTriangleVerts((PxU32)isSmallMesh, (Cm::MemFetchPtr)meshData->mTriangles, (Cm::MemFetchPtr)meshData->mVertices, *triangleIndexIt,
triangleVerts[j*3], triangleVerts[j*3 + 1], triangleVerts[j*3 + 2]);
}
collData[i].localDcNum = 0.0f;
collData[i].localSurfaceNormal = PxVec3(0);
collData[i].localSurfacePos = PxVec3(0);
collideWithMeshTriangles(collData[i], *meshData, meshScaling, triangleVerts, numTriangles, proxRadius, shape2World);
}
}
else if ((particle.particleFlags.low & PxvInternalParticleFlag::eGEOM_CACHE_BIT_0) != 0 && (particle.particleFlags.low & PxvInternalParticleFlag::eGEOM_CACHE_BIT_1) != 0)
{
// don't update the cache since it's already successfully in use
PxcContactCellMeshCallback callback(collData, &i, 1, *meshData, meshScaling, proxRadius, NULL, shape2World);
testBoundsMesh(*meshData, world2Shape, shape2World, meshScaling, idtScaleMesh, bounds, callback);
}
else
{
// compute new conservative bounds for cache
PxBounds3 cachedBounds;
{
PxVec3 predictedExtent(proxRadius*1.5f);
//add future newpos + extent
PxVec3 newPosPredicted = particle.newPos + 3.f*(particle.newPos - particle.oldPos);
cachedBounds = PxBounds3(newPosPredicted - predictedExtent, newPosPredicted + predictedExtent);
//add next oldpos + extent
cachedBounds.include(PxBounds3(particle.newPos - predictedExtent, particle.newPos + predictedExtent));
//add old pos
cachedBounds.include(particle.oldPos);
}
cache.init(cachedTriangleBuffer);
//the callback function will call collideWithMeshTriangles()
PxcContactCellMeshCallback callback(collData, &i, 1, *meshData, meshScaling, proxRadius, &cache, shape2World);
// opcode query: cache bounds against shape bounds in unscaled mesh space
testBoundsMesh(*meshData, world2Shape, shape2World, meshScaling, idtScaleMesh, cachedBounds, callback);
//update cache
cache.write(particle.particleFlags.low, cachedBounds, quantizationParams, meshShape, isSmallMesh);
}
}
}
//////////////////////////////////////////////////////////////////////////
// checks if points form a valid AABB cube, if not construct a default CUBE
static bool checkPointsAABBValidity(PxU32 numPoints, const PxVec3* points, PxU32 stride , float distanceEpsilon,
float resizeValue, PxVec3& center, PxVec3& scale, PxU32& vcount, PxVec3* vertices, bool fCheck = false)
{
const char* vtx = reinterpret_cast<const char *> (points);
PxBounds3 bounds;
bounds.setEmpty();
// get the bounding box
for (PxU32 i = 0; i < numPoints; i++)
{
const PxVec3& p = *reinterpret_cast<const PxVec3 *> (vtx);
vtx += stride;
bounds.include(p);
}
PxVec3 dim = bounds.getDimensions();
center = bounds.getCenter();
// special case, the AABB is very thin or user provided us with only input 2 points
// we construct an AABB cube and return it
if ( dim.x < distanceEpsilon || dim.y < distanceEpsilon || dim.z < distanceEpsilon || numPoints < 3 )
{
float len = FLT_MAX;
// pick the shortest size bigger than the distance epsilon
if ( dim.x > distanceEpsilon && dim.x < len )
len = dim.x;
if ( dim.y > distanceEpsilon && dim.y < len )
len = dim.y;
if ( dim.z > distanceEpsilon && dim.z < len )
len = dim.z;
// if the AABB is small in all dimensions, resize it
if ( len == FLT_MAX )
{
dim = PxVec3(resizeValue);
}
// if one edge is small, set to 1/5th the shortest non-zero edge.
else
{
if ( dim.x < distanceEpsilon )
dim.x = len * 0.05f;
else
dim.x *= 0.5f;
if ( dim.y < distanceEpsilon )
dim.y = len * 0.05f;
else
dim.y *= 0.5f;
if ( dim.z < distanceEpsilon )
dim.z = len * 0.05f;
else
dim.z *= 0.5f;
}
// construct the AABB
const PxVec3 extPos = center + dim;
const PxVec3 extNeg = center - dim;
if(fCheck)
vcount = 0;
vertices[vcount++] = extNeg;
vertices[vcount++] = PxVec3(extPos.x,extNeg.y,extNeg.z);
vertices[vcount++] = PxVec3(extPos.x,extPos.y,extNeg.z);
vertices[vcount++] = PxVec3(extNeg.x,extPos.y,extNeg.z);
vertices[vcount++] = PxVec3(extNeg.x,extNeg.y,extPos.z);
vertices[vcount++] = PxVec3(extPos.x,extNeg.y,extPos.z);
vertices[vcount++] = extPos;
vertices[vcount++] = PxVec3(extNeg.x,extPos.y,extPos.z);
return true; // return cube
}
else
{
scale = dim;
}
return false;
}