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); } }
/** input scaledPlaneBuf needs a capacity of the number of planes in convexShape */ void physx::collideWithConvex(PxPlane* scaledPlaneBuf, PxsParticleCollData* particleCollData, PxU32 numCollData, const Gu::GeometryUnion& convexShape, const PxReal proxRadius) { PX_ASSERT(scaledPlaneBuf); PX_ASSERT(particleCollData); const PxConvexMeshGeometryLL& convexShapeData = convexShape.get<const PxConvexMeshGeometryLL>(); const Gu::ConvexHullData* convexHullData = convexShapeData.hullData; PX_ASSERT(convexHullData); // convex bounds in local space PxMat33 scaling = convexShapeData.scale.toMat33(), invScaling; invScaling = scaling.getInverse(); PX_ASSERT(!convexHullData->mAABB.isEmpty()); PxBounds3 shapeBounds = PxBounds3::transformFast(scaling, convexHullData->mAABB); PX_ASSERT(!shapeBounds.isEmpty()); shapeBounds.fattenFast(proxRadius); bool scaledPlanes = false; #if PXS_FLUID_USE_SIMD_CONVEX_COLLISION const Vec3V boundMin = V3LoadU(shapeBounds.minimum); const Vec3V boundMax = V3LoadU(shapeBounds.maximum); const Vec4V boundMinX = V4SplatElement<0>(Vec4V_From_Vec3V(boundMin)); const Vec4V boundMinY = V4SplatElement<1>(Vec4V_From_Vec3V(boundMin)); const Vec4V boundMinZ = V4SplatElement<2>(Vec4V_From_Vec3V(boundMin)); const Vec4V boundMaxX = V4SplatElement<0>(Vec4V_From_Vec3V(boundMax)); const Vec4V boundMaxY = V4SplatElement<1>(Vec4V_From_Vec3V(boundMax)); const Vec4V boundMaxZ = V4SplatElement<2>(Vec4V_From_Vec3V(boundMax)); PxsParticleCollDataV4 collDataV4; const VecU32V u4Zero = VecU32VLoadXYZW(0,0,0,0); const VecU32V u4One = VecU32VLoadXYZW(1,1,1,1); PX_ALIGN(16, PxsParticleCollData fakeCsd); fakeCsd.localOldPos = PxVec3(FLT_MAX, FLT_MAX, FLT_MAX); fakeCsd.localNewPos = PxVec3(FLT_MAX, FLT_MAX, FLT_MAX); PX_ALIGN(16, PxU32 overlapArray[128]); PxU32 start = 0; while(start < numCollData) { const PxU32 batchSize = PxMin(numCollData-start, (PxU32)128); PxU32 v4Count = 0; PxsParticleCollData* particleCollDataIt = &particleCollData[start]; for(PxU32 i=0; i<batchSize; i+=4) { PxsParticleCollData* collData[4]; collData[0] = particleCollDataIt++; collData[1] = (i+1 < numCollData) ? particleCollDataIt++ : &fakeCsd; collData[2] = (i+2 < numCollData) ? particleCollDataIt++ : &fakeCsd; collData[3] = (i+3 < numCollData) ? particleCollDataIt++ : &fakeCsd; Vec4V oldPosV0 = V4LoadU((PxF32*)&collData[0]->localOldPos); Vec4V newPosV0 = V4LoadU((PxF32*)&collData[0]->localNewPos); Vec4V oldPosV1 = V4LoadU((PxF32*)&collData[1]->localOldPos); Vec4V newPosV1 = V4LoadU((PxF32*)&collData[1]->localNewPos); Vec4V oldPosV2 = V4LoadU((PxF32*)&collData[2]->localOldPos); Vec4V newPosV2 = V4LoadU((PxF32*)&collData[2]->localNewPos); Vec4V oldPosV3 = V4LoadU((PxF32*)&collData[3]->localOldPos); Vec4V newPosV3 = V4LoadU((PxF32*)&collData[3]->localNewPos); Vec4V particleMin0 = V4Min(oldPosV0, newPosV0); Vec4V particleMax0 = V4Max(oldPosV0, newPosV0); Vec4V particleMin1 = V4Min(oldPosV1, newPosV1); Vec4V particleMax1 = V4Max(oldPosV1, newPosV1); Vec4V particleMin2 = V4Min(oldPosV2, newPosV2); Vec4V particleMax2 = V4Max(oldPosV2, newPosV2); Vec4V particleMin3 = V4Min(oldPosV3, newPosV3); Vec4V particleMax3 = V4Max(oldPosV3, newPosV3); Mat44V particleMin44(particleMin0, particleMin1, particleMin2, particleMin3); const Mat44V particleMinTrans44 = M44Trnsps(particleMin44); Mat44V particleMax44(particleMax0, particleMax1, particleMax2, particleMax3); const Mat44V particleMaxTrans44 = M44Trnsps(particleMax44); BoolV mask = V4IsGrtr(boundMaxX, particleMinTrans44.col0); mask = BAnd(V4IsGrtr(boundMaxY, particleMinTrans44.col1), mask); mask = BAnd(V4IsGrtr(boundMaxZ, particleMinTrans44.col2), mask); mask = BAnd(V4IsGrtr(particleMaxTrans44.col0, boundMinX), mask); mask = BAnd(V4IsGrtr(particleMaxTrans44.col1, boundMinY), mask); mask = BAnd(V4IsGrtr(particleMaxTrans44.col2, boundMinZ), mask); VecU32V overlap4 = V4U32Sel(mask, u4One, u4Zero); V4U32StoreAligned(overlap4,(VecU32V*)&overlapArray[i]); } particleCollDataIt = &particleCollData[start]; for(PxU32 k=0; k<batchSize; k++, ++particleCollDataIt) { if (overlapArray[k]) { if(!scaledPlanes) { scalePlanes(scaledPlaneBuf, convexHullData, invScaling); scaledPlanes = true; } collDataV4.localOldPos[v4Count].v3 = particleCollDataIt->localOldPos; collDataV4.localNewPos[v4Count].v3 = particleCollDataIt->localNewPos; collDataV4.localFlags[v4Count] = particleCollDataIt->localFlags; collDataV4.restOffset[v4Count] = particleCollDataIt->restOffset; collDataV4.ccTime[v4Count] = particleCollDataIt->ccTime; collDataV4.collData[v4Count] = particleCollDataIt; v4Count++; } if(v4Count == 4 || (v4Count > 0 && (k == batchSize-1))) { collideWithConvexPlanesSIMD(collDataV4, scaledPlaneBuf, convexHullData->mNbPolygons, proxRadius); for(PxU32 j =0 ; j < v4Count; j++) { PxsParticleCollData* collData = collDataV4.collData[j]; PxU32 stateFlag = collDataV4.localFlags[j]; if(stateFlag) { collData->localFlags |= stateFlag; collData->ccTime = collDataV4.ccTime[j]; collData->localSurfaceNormal = collDataV4.localSurfaceNormal[j].v3; collData->localSurfacePos = collDataV4.localSurfacePos[j].v3; } } v4Count = 0; } } start += batchSize; } #else PxsParticleCollData* particleCollDataIt = particleCollData; for(PxU32 i=0; i<numCollData; ++i, ++particleCollDataIt) { PxBounds3 particleBounds = PxBounds3::boundsOfPoints(particleCollDataIt->localOldPos, particleCollDataIt->localNewPos); if (particleBounds.intersects(shapeBounds)) { if(!scaledPlanes) { scalePlanes(scaledPlaneBuf, convexHullData, invScaling); scaledPlanes = true; } collideWithConvexPlanes(*particleCollDataIt, scaledPlaneBuf, convexHullData->mNbPolygons, proxRadius); } } #endif }