Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- void stepPhysicsImmediateModeOneActor(bool interactive){
- if (!ballInstantMode) {
- return;
- }
- PX_UNUSED(interactive);
- gCacheAllocator->reset();
- gConstraintAllocator->release();
- //PxU32 nbStatics = gScene->getNbActors(PxActorTypeFlag::eRIGID_STATIC);
- //PxU32 nbDynamics = gScene->getNbActors(PxActorTypeFlag::eRIGID_DYNAMIC);
- PxU32 nbStatics = 0;
- PxU32 nbDynamics = 1;
- const PxU32 totalActors = nbDynamics + nbStatics;
- Array<ContactPair> activeContactPairs;
- Array<Gu::ContactPoint> contactPoints;
- activeContactPairs.reserve(4 * totalActors);
- Array<PxActor*> actors(totalActors);
- Array<PxBounds3> shapeBounds(totalActors);
- Array<PxSolverBody> bodies(totalActors);
- Array<PxSolverBodyData> bodyData(totalActors);
- Array<PxGeometryHolder> mGeometries;
- // It adds manually the only actor that must be simulated
- actors[0] = ballInstantMode;
- //gScene->getActors(PxActorTypeFlag::eRIGID_DYNAMIC, actors.begin(), nbDynamics);
- //gScene->getActors(PxActorTypeFlag::eRIGID_STATIC, actors.begin() + nbDynamics, nbStatics);
- //Now do collision detection...Brute force every dynamic against every dynamic/static
- for (PxU32 a = 0; a < totalActors; ++a)
- {
- PxRigidActor* actor = actors[a]->is<PxRigidActor>();
- PxShape* shape;
- actor->getShapes(&shape, 1);
- //Compute the AABBs. We inflate these by 2cm margins
- shapeBounds[a] = PxShapeExt::getWorldBounds(*shape, *actor, 1.f);
- shapeBounds[a].minimum -= PxVec3(0.02)*gUnitScale;
- shapeBounds[a].maximum += PxVec3(0.02)*gUnitScale;
- mGeometries.pushBack(shape->getGeometry());
- }
- //Broad phase for active pairs...
- for (PxU32 a = 0; a < nbDynamics; ++a)
- {
- PxRigidDynamic* dyn0 = actors[a]->is<PxRigidDynamic>();
- for (PxU32 b = a + 1; b < totalActors; ++b)
- {
- PxRigidActor* actor1 = actors[b]->is<PxRigidActor>();
- if (shapeBounds[a].intersects(shapeBounds[b]))
- {
- ContactPair pair;
- pair.actor0 = dyn0;
- pair.actor1 = actor1;
- pair.idx0 = a;
- pair.idx1 = b;
- activeContactPairs.pushBack(pair);
- }
- #if WITH_PERSISTENCY
- else
- {
- const PxU32 startIndex = a == 0 ? 0 : (a * totalActors) - (a * (a + 1)) / 2;
- PersistentContactPair& persistentData = (*allContactCache)[startIndex + (b - a - 1)];
- //No collision detection performed at all so clear contact cache and friction data
- persistentData.frictions = NULL;
- persistentData.nbFrictions = 0;
- persistentData.cache = PxCache();
- }
- #endif
- }
- }
- const PxU32 nbActivePairs = activeContactPairs.size();
- ContactPair* activePairs = activeContactPairs.begin();
- activeContactPairs.forceSize_Unsafe(0);
- contactPoints.reserve(4 * nbActivePairs);
- for (PxU32 a = 0; a < nbActivePairs; ++a)
- {
- const ContactPair& pair = activePairs[a];
- PxRigidDynamic* dyn0 = pair.actor0;
- PxGeometryHolder& holder0 = mGeometries[pair.idx0];
- PxRigidActor* actor1 = pair.actor1;
- PxGeometryHolder& holder1 = mGeometries[pair.idx1];
- #if WITH_PERSISTENCY
- const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
- PersistentContactPair& persistentData = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)];
- if (!generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, persistentData.cache))
- {
- //Contact generation run but no touches found so clear cached friction data
- persistentData.frictions = NULL;
- persistentData.nbFrictions = 0;
- }
- #else
- PxCache cache;
- generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, cache);
- #endif
- }
- const PxReal dt = 1.f / 60.f;
- const PxReal invDt = 60.f;
- const PxVec3 gravity(0.f, -9.8f* gUnitScale, 0.f);
- //Construct solver bodies
- for (PxU32 a = 0; a < nbDynamics; ++a)
- {
- PxRigidDynamic* dyn = actors[a]->is<PxRigidDynamic>();
- immediate::PxRigidBodyData data;
- data.linearVelocity = dyn->getLinearVelocity();
- data.angularVelocity = dyn->getAngularVelocity();
- data.invMass = dyn->getInvMass();
- data.invInertia = dyn->getMassSpaceInvInertiaTensor();
- data.body2World = dyn->getGlobalPose();
- data.maxDepenetrationVelocity = dyn->getMaxDepenetrationVelocity();
- data.maxContactImpulse = dyn->getMaxContactImpulse();
- data.linearDamping = dyn->getLinearDamping();
- data.angularDamping = dyn->getAngularDamping();
- data.maxLinearVelocitySq = 100.f*100.f*gUnitScale*gUnitScale;
- data.maxAngularVelocitySq = 7.f*7.f;
- physx::immediate::PxConstructSolverBodies(&data, &bodyData[a], 1, gravity, dt);
- size_t szA = size_t(a);
- dyn->userData = reinterpret_cast<void*>(szA);
- }
- //Construct static bodies
- for (PxU32 a = nbDynamics; a < totalActors; ++a)
- {
- PxRigidStatic* sta = actors[a]->is<PxRigidStatic>();
- physx::immediate::PxConstructStaticSolverBody(sta->getGlobalPose(), bodyData[a]);
- size_t szA = a;
- sta->userData = reinterpret_cast<void*>(szA);
- }
- Array<PxSolverConstraintDesc> descs(activeContactPairs.size() + gConstraints->size());
- for (PxU32 a = 0; a < activeContactPairs.size(); ++a)
- {
- PxSolverConstraintDesc& desc = descs[a];
- ContactPair& pair = activeContactPairs[a];
- //Set body pointers and bodyData idxs
- desc.bodyA = &bodies[pair.idx0];
- desc.bodyB = &bodies[pair.idx1];
- desc.bodyADataIndex = PxU16(pair.idx0);
- desc.bodyBDataIndex = PxU16(pair.idx1);
- desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
- desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
- //Cache pointer to our contact data structure and identify which type of constraint this is. We'll need this later after batching.
- //If we choose not to perform batching and instead just create a single header per-pair, then this would not be necessary because
- //the constraintDescs would not have been reordered
- desc.constraint = reinterpret_cast<PxU8*>(&pair);
- desc.constraintLengthOver16 = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
- }
- for (PxU32 a = 0; a < gConstraints->size(); ++a)
- {
- PxConstraint* constraint = (*gConstraints)[a];
- PxSolverConstraintDesc& desc = descs[activeContactPairs.size() + a];
- PxRigidActor* actor0, *actor1;
- constraint->getActors(actor0, actor1);
- PxU32 id0 = PxU32(size_t(actor0->userData));
- PxU32 id1 = PxU32(size_t(actor1->userData));
- desc.bodyA = &bodies[id0];
- desc.bodyB = &bodies[id1];
- desc.bodyADataIndex = PxU16(id0);
- desc.bodyBDataIndex = PxU16(id1);
- desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
- desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
- desc.constraint = reinterpret_cast<PxU8*>(constraint);
- desc.constraintLengthOver16 = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
- }
- Array<PxConstraintBatchHeader> headers(descs.size());
- Array<PxReal> contactForces(contactPoints.size());
- //Technically, you can batch the contacts and joints all at once using a single call but doing so mixes them in the orderedDescs array, which means that it is impossible to
- //batch all contact or all joint dispatches into a single call. While we don't do this in this snippet (we instead process a single header at a time), our approach could be extended to
- //dispatch all contact headers at once if that was necessary.
- #if BATCH_CONTACTS
- Array<PxSolverConstraintDesc> tempOrderedDescs(descs.size());
- physx::shdfnd::Array<PxSolverConstraintDesc>& orderedDescs = tempOrderedDescs;
- //1 batch the contacts
- const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraints(descs.begin(), activeContactPairs.size(), bodies.begin(), nbDynamics, headers.begin(), orderedDescs.begin());
- //2 batch the joints...
- const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraints(descs.begin() + activeContactPairs.size(), gConstraints->size(), bodies.begin(), nbDynamics, headers.begin() + nbContactHeaders, orderedDescs.begin() + activeContactPairs.size());
- #else
- physx::shdfnd::Array<PxSolverConstraintDesc>& orderedDescs = descs;
- //We are bypassing the constraint batching so we create dummy PxConstraintBatchHeaders
- const PxU32 nbContactHeaders = activeContactPairs.size();
- const PxU32 nbJointHeaders = gConstraints->size();
- for (PxU32 i = 0; i < nbContactHeaders; ++i)
- {
- PxConstraintBatchHeader& hdr = headers[i];
- hdr.mStartIndex = i;
- hdr.mStride = 1;
- hdr.mConstraintType = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
- }
- for (PxU32 i = 0; i < nbJointHeaders; ++i)
- {
- PxConstraintBatchHeader& hdr = headers[nbContactHeaders + i];
- hdr.mStartIndex = i;
- hdr.mStride = 1;
- hdr.mConstraintType = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
- }
- #endif
- const PxU32 totalHeaders = nbContactHeaders + nbJointHeaders;
- headers.forceSize_Unsafe(totalHeaders);
- //1 - Create all the contact constraints. We do this by looping over all the headers and, for each header, constructing the PxSolverContactDesc objects, then creating that contact constraint.
- //We could alternatively create all the PxSolverContactDesc objects in a single pass, then create batch create that constraint
- for (PxU32 i = 0; i < nbContactHeaders; ++i)
- {
- PxConstraintBatchHeader& header = headers[i];
- PX_ASSERT(header.mConstraintType == PxSolverConstraintDesc::eCONTACT_CONSTRAINT);
- PxSolverContactDesc contactDescs[4];
- ContactPair* pairs[4];
- for (PxU32 a = 0; a < header.mStride; ++a)
- {
- PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
- PxSolverContactDesc& contactDesc = contactDescs[a];
- //Extract the contact pair that we saved in this structure earlier.
- ContactPair& pair = *reinterpret_cast<ContactPair*>(constraintDesc.constraint);
- pairs[a] = &pair;
- contactDesc.body0 = constraintDesc.bodyA;
- contactDesc.body1 = constraintDesc.bodyB;
- contactDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
- contactDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];
- //This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
- //example does not use articulations.
- contactDesc.bodyFrame0 = contactDesc.data0->body2World;
- contactDesc.bodyFrame1 = contactDesc.data1->body2World;
- contactDesc.contactForces = &contactForces[pair.startContactIndex];
- contactDesc.contacts = &contactPoints[pair.startContactIndex];
- contactDesc.numContacts = pair.nbContacts;
- #if WITH_PERSISTENCY
- const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
- contactDesc.frictionPtr = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions;
- contactDesc.frictionCount = PxU8((*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions);
- #else
- contactDesc.frictionPtr = NULL;
- contactDesc.frictionCount = 0;
- #endif
- contactDesc.disableStrongFriction = false;
- contactDesc.hasMaxImpulse = false;
- contactDesc.hasForceThresholds = false;
- contactDesc.shapeInteraction = NULL;
- contactDesc.restDistance = 0.f;
- contactDesc.maxCCDSeparation = PX_MAX_F32;
- contactDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
- contactDesc.bodyState1 = pair.actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
- contactDesc.desc = &constraintDesc;
- contactDesc.mInvMassScales.angular0 = contactDesc.mInvMassScales.angular1 = contactDesc.mInvMassScales.linear0 = contactDesc.mInvMassScales.linear1 = 1.f;
- }
- immediate::PxCreateContactConstraints(&header, 1, contactDescs, *gConstraintAllocator, invDt, -2.f * gUnitScale, 0.04f * gUnitScale, 0.025f * gUnitScale);
- #if WITH_PERSISTENCY
- for (PxU32 a = 0; a < header.mStride; ++a)
- {
- //Cache friction information...
- PxSolverContactDesc& contactDesc = contactDescs[a];
- //PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
- ContactPair& pair = *pairs[a];
- const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
- (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions = contactDesc.frictionPtr;
- (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions = contactDesc.frictionCount;
- }
- #endif
- }
- for (PxU32 i = nbContactHeaders; i < totalHeaders; ++i)
- {
- PxConstraintBatchHeader& header = headers[i];
- PX_ASSERT(header.mConstraintType == PxSolverConstraintDesc::eJOINT_CONSTRAINT);
- {
- PxSolverConstraintPrepDesc jointDescs[4];
- PxConstraint* constraints[4];
- header.mStartIndex += activeContactPairs.size();
- for (PxU32 a = 0; a < header.mStride; ++a)
- {
- PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
- //Extract the contact pair that we saved in this structure earlier.
- PxConstraint& constraint = *reinterpret_cast<PxConstraint*>(constraintDesc.constraint);
- constraints[a] = &constraint;
- PxSolverConstraintPrepDesc& jointDesc = jointDescs[a];
- jointDesc.body0 = constraintDesc.bodyA;
- jointDesc.body1 = constraintDesc.bodyB;
- jointDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
- jointDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];
- //This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
- //example does not use articulations.
- jointDesc.bodyFrame0 = jointDesc.data0->body2World;
- jointDesc.bodyFrame1 = jointDesc.data1->body2World;
- PxRigidActor* actor0, *actor1;
- constraint.getActors(actor0, actor1);
- jointDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
- jointDesc.bodyState1 = actor1 == NULL ? PxSolverConstraintPrepDescBase::eSTATIC_BODY : actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
- jointDesc.desc = &constraintDesc;
- jointDesc.mInvMassScales.angular0 = jointDesc.mInvMassScales.angular1 = jointDesc.mInvMassScales.linear0 = jointDesc.mInvMassScales.linear1 = 1.f;
- jointDesc.writeback = NULL;
- constraint.getBreakForce(jointDesc.linBreakForce, jointDesc.angBreakForce);
- jointDesc.minResponseThreshold = constraint.getMinResponseThreshold();
- jointDesc.disablePreprocessing = !!(constraint.getFlags() & PxConstraintFlag::eDISABLE_PREPROCESSING);
- jointDesc.improvedSlerp = !!(constraint.getFlags() & PxConstraintFlag::eIMPROVED_SLERP);
- jointDesc.driveLimitsAreForces = !!(constraint.getFlags() & PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES);
- }
- immediate::PxCreateJointConstraintsWithShaders(&header, 1, constraints, jointDescs, *gConstraintAllocator, dt, invDt);
- }
- }
- //Solve all the constraints produced earlier. Intermediate motion linear/angular velocity buffers are filled in. These contain intermediate delta velocity information that is used
- //the PxIntegrateSolverBody
- Array<PxVec3> motionLinearVelocity(nbDynamics);
- Array<PxVec3> motionAngularVelocity(nbDynamics);
- //Zero the bodies array. This buffer contains the delta velocities and are accumulated during the simulation. For correct behavior, it is vital
- //that this buffer is zeroed.
- PxMemZero(bodies.begin(), bodies.size() * sizeof(PxSolverBody));
- immediate::PxSolveConstraints(headers.begin(), headers.size(), orderedDescs.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, 4, 1);
- immediate::PxIntegrateSolverBodies(bodyData.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, dt);
- for (PxU32 a = 0; a < nbDynamics; ++a)
- {
- PxRigidDynamic* dynamic = actors[a]->is<PxRigidDynamic>();
- PxSolverBodyData& data = bodyData[a];
- dynamic->setLinearVelocity(data.linearVelocity);
- dynamic->setAngularVelocity(data.angularVelocity);
- dynamic->setGlobalPose(data.body2World);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement