static PxMat44 convertViewMatrix(const PxTransform& eye)
{
PxTransform viewMatrix = eye.getInverse();
PxMat44 mat44 = PxMat44(viewMatrix).getTranspose();
float m[16];
memcpy(m, mat44.front(), sizeof m);
PxMat44 view44;
view44.column0.x = m[0];
view44.column0.y = m[1];
view44.column0.z = m[2];
view44.column0.w = m[3];
view44.column1.x = m[4];
view44.column1.y = m[5];
view44.column1.z = m[6];
view44.column1.w = m[7];
view44.column2.x = m[8];
view44.column2.y = m[9];
view44.column2.z = m[10];
view44.column2.w = m[11];
view44.column3.x = m[12];
view44.column3.y = m[13];
view44.column3.z = m[14];
view44.column3.w = m[15];
PxMat44 tmpmat = view44.getTranspose(); view44 = tmpmat;
return view44;
}
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);
}
}
static bool computeMassAndDiagInertia(Ext::InertiaTensorComputer& inertiaComp,
PxVec3& diagTensor, PxQuat& orient, PxReal& massOut, PxVec3& coM, bool lockCOM, const PxRigidBody& body, const char* errorStr)
{
// The inertia tensor and center of mass is relative to the actor at this point. Transform to the
// body frame directly if CoM is specified, else use computed center of mass
if (lockCOM)
{
inertiaComp.translate(-coM); // base the tensor on user's desired center of mass.
}
else
{
//get center of mass - has to be done BEFORE centering.
coM = inertiaComp.getCenterOfMass();
//the computed result now needs to be centered around the computed center of mass:
inertiaComp.center();
}
// The inertia matrix is now based on the body's center of mass desc.massLocalPose.p
massOut = inertiaComp.getMass();
diagTensor = PxDiagonalize(inertiaComp.getInertia(), orient);
if ((diagTensor.x > 0.0f) && (diagTensor.y > 0.0f) && (diagTensor.z > 0.0f))
return true;
else
{
Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__,
"%s: inertia tensor has negative components (ill-conditioned input expected). Approximation for inertia tensor will be used instead.", errorStr);
// keep center of mass but use the AABB as a crude approximation for the inertia tensor
PxBounds3 bounds = body.getWorldBounds();
PxTransform pose = body.getGlobalPose();
bounds = PxBounds3::transformFast(pose.getInverse(), bounds);
Ext::InertiaTensorComputer it(false);
it.setBox(bounds.getExtents());
it.scaleDensity(massOut / it.getMass());
PxMat33 inertia = it.getInertia();
diagTensor = PxVec3(inertia.column0.x, inertia.column1.y, inertia.column2.z);
orient = PxQuat(PxIdentity);
return true;
}
}
bool
Skin::bindToCharacter(Character &character, SampleArray<PxVec4> &positions)
{
// currently we just bind everything to the 'thorax' (between neck and clavicles).
// Modify this if you need to do more elaborate skin binding
mBindPos.resize(positions.size());
Acclaim::Bone* bone = getBoneFromName(*character.mASFData, "thorax");
if (bone == NULL)
return false;
PxTransform boneTransform = computeBoneTransformRest(*bone);
PxTransform boneTransformInv = boneTransform.getInverse();
mBoneID = bone->mID - 1;
for (PxU32 i = 0; i < positions.size(); i++)
{
mBindPos[i] = boneTransformInv.transform(
reinterpret_cast<const PxVec3&>(positions[i]));
}
return true;
}
bool doRaycastCCD(PxShape* shape, PxTransform& newPose, PxVec3& newShapeCenter, const PxVec3& ccdWitness, const PxVec3& ccdWitnessOffset)
{
PxRigidDynamic* dyna = canDoCCD(shape);
if(!dyna)
return true;
bool updateCCDWitness = true;
const PxVec3 offset = newPose.p - newShapeCenter;
//printf("CCD0: %f | %f | %f\n", newShapeCenter.x, newShapeCenter.y, newShapeCenter.z);
const PxVec3& origin = ccdWitness;
// const PxVec3& dest = newPose.p;
const PxVec3& dest = newShapeCenter;
PxVec3 dir = dest - origin;
const PxReal length = dir.magnitude();
if(length!=0.0f)
{
dir /= length;
// Compute internal radius
// PxVec3 localCenter;
const PxReal internalRadius = computeInternalRadius(shape, dir, /*localCenter,*/ ccdWitnessOffset);
// Compute distance to impact
PxRaycastHit hit;
// if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getActiveScene(), origin + localCenter, dir, length, hit))
if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getScene(), origin, dir, length, hit))
{
#ifdef RAYCAST_CCD_PRINT_DEBUG
static int count=0;
printf("CCD hit %d\n", count++);
#endif
updateCCDWitness = false;
const PxReal radiusLimit = internalRadius * 0.75f;
if(hit.distance>radiusLimit)
{
// newPose.p = origin + dir * (hit.distance - radiusLimit);
newShapeCenter = origin + dir * (hit.distance - radiusLimit);
#ifdef RAYCAST_CCD_PRINT_DEBUG
printf(" Path0: %f | %f\n", hit.distance, radiusLimit);
#endif
}
else
{
// newPose.p = origin;
newShapeCenter = origin;
// newShapeCenter = origin + hit.normal * (radiusLimit - hit.distance);
#ifdef RAYCAST_CCD_PRINT_DEBUG
printf(" Path1: %f\n", hit.distance);
#endif
}
{
newPose.p = offset + newShapeCenter;
//newPose.p.y += 10.0f;
//printf("%f | %f | %f\n", newPose.p.x, newPose.p.y, newPose.p.z);
// dyna->setGlobalPose(newPose);
// newPose = actorGlobalPose * shapeLocalPose
// newPose * inverse(shapeLocalPose) = actorGlobalPose
const PxTransform shapeLocalPose = shape->getLocalPose();
const PxTransform inverseShapeLocalPose = shapeLocalPose.getInverse();
PxTransform newGlobalPose = newPose * inverseShapeLocalPose;
dyna->setGlobalPose(newGlobalPose);
//dyna->setGlobalPose(newPose);
//printf("%f | %f | %f\n", newGlobalPose.p.x, newGlobalPose.p.y, newGlobalPose.p.z);
//printf("%f | %f | %f\n", shapeLocalPose.p.x, shapeLocalPose.p.y, shapeLocalPose.p.z);
/*PX_INLINE PxTransform PxShapeExt::getGlobalPose(const PxShape& shape)
{
PxRigidActor& ra = shape.getActor();
return ra.getGlobalPose() * shape.getLocalPose();
}*/
const PxVec3 testShapeCenter = getShapeCenter(shape, ccdWitnessOffset);
float d = (testShapeCenter - newShapeCenter).magnitude();
//printf("%f\n", d);
//printf("CCD1: %f | %f | %f\n", testShapeCenter.x, testShapeCenter.y, testShapeCenter.z);
//dyna->clearForce(PxForceMode::eFORCE);
//dyna->clearForce(PxForceMode::eIMPULSE);
//dyna->setLinearVelocity(PxVec3(0)); // PT: this helps the CCT but stops small objects dead, which doesn't look great
}
}
}
return updateCCDWitness;
}
bool physx::Gu::computeConvex_HeightFieldMTD(const PxHeightFieldGeometry& heightFieldGeom, const PxTransform& pose, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, const PxReal inflation,
const PxReal distance, const bool isDoubleSided, const PxU32 flags, PxSweepHit& hit)
{
using namespace Ps::aos;
const Gu::HeightFieldUtil hfUtil(heightFieldGeom);
const Vec3V zeroV = V3Zero();
Gu::MeshPersistentContact manifoldContacts[64];
PxU32 numContacts = 0;
FloatV distV = FLoad(distance);
bool foundInitial = false;
static PxU32 iterations = 2;
Gu::ConvexMesh* cm = static_cast<Gu::ConvexMesh*>(convexGeom.convexMesh);
Gu::ConvexHullData* hullData = &cm->getHull();
const bool idtScaleConvex = convexGeom.scale.isIdentity();
Cm::FastVertex2ShapeScaling convexScaling;
if(!idtScaleConvex)
convexScaling.init(convexGeom.scale);
const PxVec3 _shapeSpaceCenterOfMass = convexScaling * hullData->mCenterOfMass;
const Vec3V shapeSpaceCenterOfMass = V3LoadU(_shapeSpaceCenterOfMass);
const QuatV q0 = QuatVLoadU(&convexPose.q.x);
const Vec3V p0 = V3LoadU(&convexPose.p.x);
PsTransformV convexTransformV(p0, q0);
const Vec3V vScale = V3LoadU(convexGeom.scale.scale);
const QuatV vQuat = QuatVLoadU(&convexGeom.scale.rotation.x);
Gu::ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
PX_ALIGN(16, PxU8 convexBuff[sizeof(SupportLocalImpl<ConvexHullV>)]);
const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
const FloatV inflationV = FAdd(FLoad(inflation), convexMargin);
PxReal boundInflation;
FStore(inflationV, &boundInflation);
LocalContainer(tempContainer, 128);
Vec3V closestA = zeroV, closestB = zeroV, normal = zeroV;
Vec3V worldNormal=zeroV, worldContactA=zeroV;//, worldContactB=zeroV;
PxU32 triangleIndex = 0xfffffff;
Vec3V translation = zeroV;
Gu::PolygonalData polyData;
getPCMConvexData(convexHull, idtScaleConvex, polyData);
FloatV mtd;
Vec3V center = p0;
PxTransform tempConvexPose = convexPose;
Cm::Matrix34 meshToWorldSkew(pose);
for(PxU32 i=0; i<iterations; ++i)
{
tempContainer.Reset();
//ML:: construct convex hull data
V3StoreU(center, tempConvexPose.p);
convexTransformV.p = center;
SupportLocal* convexMap = (idtScaleConvex ? (SupportLocal*)PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullNoScaleV>)((ConvexHullNoScaleV&)convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex) :
(SupportLocal*)PX_PLACEMENT_NEW(convexBuff, SupportLocalImpl<ConvexHullV>)(convexHull, convexTransformV, convexHull.vertex2Shape, convexHull.shape2Vertex, idtScaleConvex));
convexMap->setShapeSpaceCenterofMass(shapeSpaceCenterOfMass);
Gu::Box hullOBB;
Gu::computeOBBAroundConvex(hullOBB, convexGeom, cm, tempConvexPose);
hullOBB.extents.x += boundInflation;
hullOBB.extents.y += boundInflation;
hullOBB.extents.z += boundInflation;
PxBounds3 bounds = PxBounds3::basisExtent(hullOBB.center, hullOBB.rot, hullOBB.extents);
midPhaseQueryHF(hfUtil, pose, bounds, tempContainer, flags);
// Get results
PxU32 nbTriangles = tempContainer.GetNbEntries();
if(!nbTriangles)
break;
// Move to AABB space
Cm::Matrix34 worldToConvex(tempConvexPose.getInverse());
const Cm::Matrix34 meshToConvex = worldToConvex*meshToWorldSkew;
Ps::aos::Mat33V rot(V3LoadU(meshToConvex.base0), V3LoadU(meshToConvex.base1), V3LoadU(meshToConvex.base2));
Ps::aos::PsMatTransformV meshToConvexV(V3LoadU(meshToConvex.base3), rot);
PxU32* indices = tempContainer.GetEntries();
bool hadContacts = false;
PxU32 nbBatches = (nbTriangles + BATCH_TRIANGLE_NUMBER-1)/BATCH_TRIANGLE_NUMBER;
mtd = FMax();
MTDTriangle triangles[BATCH_TRIANGLE_NUMBER];
for(PxU32 a = 0; a < nbBatches; ++a)
{
PxU32 startIndex = a * BATCH_TRIANGLE_NUMBER;
PxU32 nbTrigs = PxMin(nbTriangles - startIndex, BATCH_TRIANGLE_NUMBER);
for(PxU32 k=0; k<nbTrigs; k++)
{
//triangle vertext space
const PxU32 triangleIndex1 = indices[startIndex+k];
hfUtil.getTriangle(pose, triangles[k], NULL, NULL, triangleIndex1, false, false);
triangles[k].extraTriData = 0x38;
}
//ML: mtd has back face culling, so if the capsule's center is below the triangle, we won't generate any contacts
hadContacts = calculateMTD(polyData, convexMap, convexTransformV, meshToConvexV, isDoubleSided, inflationV, triangles, nbTrigs, startIndex, manifoldContacts, numContacts, normal, closestA, closestB, triangleIndex, mtd) || hadContacts;
}
if(!hadContacts)
break;
triangleIndex = indices[triangleIndex];
foundInitial = true;
distV =mtd;
worldNormal = convexTransformV.rotate(normal);
worldContactA = convexTransformV.transform(closestA);
if(FAllGrtrOrEq(FZero(), distV))
{
Vec3V t = V3Scale(worldNormal, mtd);
translation = V3Sub(translation, t);
center = V3Sub(center, t);
}
else
{
if(i == 0)
{
//First iteration so keep this normal
hit.distance = 0.f;
V3StoreU(worldContactA, hit.position);
V3StoreU(worldNormal, hit.normal);
hit.faceIndex = triangleIndex;
return true;
}
break;
}
}
worldNormal = V3Normalize(translation);
distV = FNeg(V3Length(translation));
if(foundInitial)
{
//transform closestA to world space
FStore(distV, &hit.distance);
V3StoreU(worldContactA, hit.position);
V3StoreU(worldNormal, hit.normal);
hit.faceIndex = triangleIndex;
}
return foundInitial;
}
