Advertisement
Guest User

Untitled

a guest
Mar 30th, 2017
386
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 15.75 KB | None | 0 0
  1. void stepPhysicsImmediateModeOneActor(bool interactive){
  2.  
  3. if (!ballInstantMode) {
  4. return;
  5. }
  6.  
  7. PX_UNUSED(interactive);
  8.  
  9. gCacheAllocator->reset();
  10. gConstraintAllocator->release();
  11.  
  12. //PxU32 nbStatics = gScene->getNbActors(PxActorTypeFlag::eRIGID_STATIC);
  13. //PxU32 nbDynamics = gScene->getNbActors(PxActorTypeFlag::eRIGID_DYNAMIC);
  14. PxU32 nbStatics = 0;
  15. PxU32 nbDynamics = 1;
  16.  
  17. const PxU32 totalActors = nbDynamics + nbStatics;
  18.  
  19. Array<ContactPair> activeContactPairs;
  20. Array<Gu::ContactPoint> contactPoints;
  21.  
  22. activeContactPairs.reserve(4 * totalActors);
  23.  
  24.  
  25. Array<PxActor*> actors(totalActors);
  26. Array<PxBounds3> shapeBounds(totalActors);
  27. Array<PxSolverBody> bodies(totalActors);
  28. Array<PxSolverBodyData> bodyData(totalActors);
  29. Array<PxGeometryHolder> mGeometries;
  30.  
  31. // It adds manually the only actor that must be simulated
  32. actors[0] = ballInstantMode;
  33.  
  34. //gScene->getActors(PxActorTypeFlag::eRIGID_DYNAMIC, actors.begin(), nbDynamics);
  35. //gScene->getActors(PxActorTypeFlag::eRIGID_STATIC, actors.begin() + nbDynamics, nbStatics);
  36.  
  37.  
  38. //Now do collision detection...Brute force every dynamic against every dynamic/static
  39. for (PxU32 a = 0; a < totalActors; ++a)
  40. {
  41. PxRigidActor* actor = actors[a]->is<PxRigidActor>();
  42.  
  43. PxShape* shape;
  44. actor->getShapes(&shape, 1);
  45.  
  46.  
  47. //Compute the AABBs. We inflate these by 2cm margins
  48. shapeBounds[a] = PxShapeExt::getWorldBounds(*shape, *actor, 1.f);
  49. shapeBounds[a].minimum -= PxVec3(0.02)*gUnitScale;
  50. shapeBounds[a].maximum += PxVec3(0.02)*gUnitScale;
  51.  
  52. mGeometries.pushBack(shape->getGeometry());
  53. }
  54.  
  55. //Broad phase for active pairs...
  56. for (PxU32 a = 0; a < nbDynamics; ++a)
  57. {
  58. PxRigidDynamic* dyn0 = actors[a]->is<PxRigidDynamic>();
  59. for (PxU32 b = a + 1; b < totalActors; ++b)
  60. {
  61. PxRigidActor* actor1 = actors[b]->is<PxRigidActor>();
  62.  
  63. if (shapeBounds[a].intersects(shapeBounds[b]))
  64. {
  65. ContactPair pair;
  66. pair.actor0 = dyn0;
  67. pair.actor1 = actor1;
  68. pair.idx0 = a;
  69. pair.idx1 = b;
  70.  
  71. activeContactPairs.pushBack(pair);
  72. }
  73. #if WITH_PERSISTENCY
  74. else
  75. {
  76. const PxU32 startIndex = a == 0 ? 0 : (a * totalActors) - (a * (a + 1)) / 2;
  77.  
  78. PersistentContactPair& persistentData = (*allContactCache)[startIndex + (b - a - 1)];
  79.  
  80. //No collision detection performed at all so clear contact cache and friction data
  81. persistentData.frictions = NULL;
  82. persistentData.nbFrictions = 0;
  83. persistentData.cache = PxCache();
  84. }
  85. #endif
  86. }
  87. }
  88.  
  89. const PxU32 nbActivePairs = activeContactPairs.size();
  90. ContactPair* activePairs = activeContactPairs.begin();
  91.  
  92. activeContactPairs.forceSize_Unsafe(0);
  93.  
  94. contactPoints.reserve(4 * nbActivePairs);
  95.  
  96. for (PxU32 a = 0; a < nbActivePairs; ++a)
  97. {
  98. const ContactPair& pair = activePairs[a];
  99.  
  100. PxRigidDynamic* dyn0 = pair.actor0;
  101. PxGeometryHolder& holder0 = mGeometries[pair.idx0];
  102.  
  103. PxRigidActor* actor1 = pair.actor1;
  104. PxGeometryHolder& holder1 = mGeometries[pair.idx1];
  105.  
  106. #if WITH_PERSISTENCY
  107. const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
  108.  
  109. PersistentContactPair& persistentData = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)];
  110.  
  111. if (!generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, persistentData.cache))
  112. {
  113. //Contact generation run but no touches found so clear cached friction data
  114. persistentData.frictions = NULL;
  115. persistentData.nbFrictions = 0;
  116. }
  117. #else
  118. PxCache cache;
  119. generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, cache);
  120. #endif
  121. }
  122.  
  123.  
  124. const PxReal dt = 1.f / 60.f;
  125. const PxReal invDt = 60.f;
  126.  
  127. const PxVec3 gravity(0.f, -9.8f* gUnitScale, 0.f);
  128.  
  129. //Construct solver bodies
  130. for (PxU32 a = 0; a < nbDynamics; ++a)
  131. {
  132. PxRigidDynamic* dyn = actors[a]->is<PxRigidDynamic>();
  133.  
  134. immediate::PxRigidBodyData data;
  135. data.linearVelocity = dyn->getLinearVelocity();
  136. data.angularVelocity = dyn->getAngularVelocity();
  137. data.invMass = dyn->getInvMass();
  138. data.invInertia = dyn->getMassSpaceInvInertiaTensor();
  139. data.body2World = dyn->getGlobalPose();
  140. data.maxDepenetrationVelocity = dyn->getMaxDepenetrationVelocity();
  141. data.maxContactImpulse = dyn->getMaxContactImpulse();
  142. data.linearDamping = dyn->getLinearDamping();
  143. data.angularDamping = dyn->getAngularDamping();
  144. data.maxLinearVelocitySq = 100.f*100.f*gUnitScale*gUnitScale;
  145. data.maxAngularVelocitySq = 7.f*7.f;
  146.  
  147.  
  148. physx::immediate::PxConstructSolverBodies(&data, &bodyData[a], 1, gravity, dt);
  149.  
  150. size_t szA = size_t(a);
  151. dyn->userData = reinterpret_cast<void*>(szA);
  152. }
  153.  
  154. //Construct static bodies
  155. for (PxU32 a = nbDynamics; a < totalActors; ++a)
  156. {
  157. PxRigidStatic* sta = actors[a]->is<PxRigidStatic>();
  158. physx::immediate::PxConstructStaticSolverBody(sta->getGlobalPose(), bodyData[a]);
  159. size_t szA = a;
  160. sta->userData = reinterpret_cast<void*>(szA);
  161. }
  162.  
  163. Array<PxSolverConstraintDesc> descs(activeContactPairs.size() + gConstraints->size());
  164.  
  165. for (PxU32 a = 0; a < activeContactPairs.size(); ++a)
  166. {
  167. PxSolverConstraintDesc& desc = descs[a];
  168. ContactPair& pair = activeContactPairs[a];
  169.  
  170. //Set body pointers and bodyData idxs
  171. desc.bodyA = &bodies[pair.idx0];
  172. desc.bodyB = &bodies[pair.idx1];
  173.  
  174. desc.bodyADataIndex = PxU16(pair.idx0);
  175. desc.bodyBDataIndex = PxU16(pair.idx1);
  176. desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
  177. desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
  178.  
  179.  
  180. //Cache pointer to our contact data structure and identify which type of constraint this is. We'll need this later after batching.
  181. //If we choose not to perform batching and instead just create a single header per-pair, then this would not be necessary because
  182. //the constraintDescs would not have been reordered
  183. desc.constraint = reinterpret_cast<PxU8*>(&pair);
  184. desc.constraintLengthOver16 = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
  185. }
  186.  
  187. for (PxU32 a = 0; a < gConstraints->size(); ++a)
  188. {
  189. PxConstraint* constraint = (*gConstraints)[a];
  190.  
  191. PxSolverConstraintDesc& desc = descs[activeContactPairs.size() + a];
  192.  
  193. PxRigidActor* actor0, *actor1;
  194. constraint->getActors(actor0, actor1);
  195.  
  196. PxU32 id0 = PxU32(size_t(actor0->userData));
  197. PxU32 id1 = PxU32(size_t(actor1->userData));
  198.  
  199. desc.bodyA = &bodies[id0];
  200. desc.bodyB = &bodies[id1];
  201.  
  202. desc.bodyADataIndex = PxU16(id0);
  203. desc.bodyBDataIndex = PxU16(id1);
  204. desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
  205. desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
  206.  
  207. desc.constraint = reinterpret_cast<PxU8*>(constraint);
  208. desc.constraintLengthOver16 = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
  209.  
  210. }
  211.  
  212. Array<PxConstraintBatchHeader> headers(descs.size());
  213. Array<PxReal> contactForces(contactPoints.size());
  214.  
  215. //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
  216. //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
  217. //dispatch all contact headers at once if that was necessary.
  218.  
  219. #if BATCH_CONTACTS
  220. Array<PxSolverConstraintDesc> tempOrderedDescs(descs.size());
  221. physx::shdfnd::Array<PxSolverConstraintDesc>& orderedDescs = tempOrderedDescs;
  222. //1 batch the contacts
  223. const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraints(descs.begin(), activeContactPairs.size(), bodies.begin(), nbDynamics, headers.begin(), orderedDescs.begin());
  224.  
  225. //2 batch the joints...
  226. const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraints(descs.begin() + activeContactPairs.size(), gConstraints->size(), bodies.begin(), nbDynamics, headers.begin() + nbContactHeaders, orderedDescs.begin() + activeContactPairs.size());
  227.  
  228. #else
  229.  
  230. physx::shdfnd::Array<PxSolverConstraintDesc>& orderedDescs = descs;
  231.  
  232. //We are bypassing the constraint batching so we create dummy PxConstraintBatchHeaders
  233. const PxU32 nbContactHeaders = activeContactPairs.size();
  234. const PxU32 nbJointHeaders = gConstraints->size();
  235.  
  236. for (PxU32 i = 0; i < nbContactHeaders; ++i)
  237. {
  238. PxConstraintBatchHeader& hdr = headers[i];
  239. hdr.mStartIndex = i;
  240. hdr.mStride = 1;
  241. hdr.mConstraintType = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
  242. }
  243.  
  244. for (PxU32 i = 0; i < nbJointHeaders; ++i)
  245. {
  246. PxConstraintBatchHeader& hdr = headers[nbContactHeaders + i];
  247. hdr.mStartIndex = i;
  248. hdr.mStride = 1;
  249. hdr.mConstraintType = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
  250. }
  251.  
  252.  
  253.  
  254. #endif
  255.  
  256. const PxU32 totalHeaders = nbContactHeaders + nbJointHeaders;
  257.  
  258.  
  259. headers.forceSize_Unsafe(totalHeaders);
  260.  
  261. //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.
  262. //We could alternatively create all the PxSolverContactDesc objects in a single pass, then create batch create that constraint
  263. for (PxU32 i = 0; i < nbContactHeaders; ++i)
  264. {
  265. PxConstraintBatchHeader& header = headers[i];
  266.  
  267. PX_ASSERT(header.mConstraintType == PxSolverConstraintDesc::eCONTACT_CONSTRAINT);
  268. PxSolverContactDesc contactDescs[4];
  269.  
  270. ContactPair* pairs[4];
  271.  
  272. for (PxU32 a = 0; a < header.mStride; ++a)
  273. {
  274. PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
  275. PxSolverContactDesc& contactDesc = contactDescs[a];
  276. //Extract the contact pair that we saved in this structure earlier.
  277. ContactPair& pair = *reinterpret_cast<ContactPair*>(constraintDesc.constraint);
  278.  
  279. pairs[a] = &pair;
  280.  
  281. contactDesc.body0 = constraintDesc.bodyA;
  282. contactDesc.body1 = constraintDesc.bodyB;
  283. contactDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
  284. contactDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];
  285.  
  286. //This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
  287. //example does not use articulations.
  288. contactDesc.bodyFrame0 = contactDesc.data0->body2World;
  289. contactDesc.bodyFrame1 = contactDesc.data1->body2World;
  290.  
  291. contactDesc.contactForces = &contactForces[pair.startContactIndex];
  292. contactDesc.contacts = &contactPoints[pair.startContactIndex];
  293. contactDesc.numContacts = pair.nbContacts;
  294.  
  295. #if WITH_PERSISTENCY
  296. const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
  297. contactDesc.frictionPtr = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions;
  298. contactDesc.frictionCount = PxU8((*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions);
  299. #else
  300. contactDesc.frictionPtr = NULL;
  301. contactDesc.frictionCount = 0;
  302. #endif
  303. contactDesc.disableStrongFriction = false;
  304. contactDesc.hasMaxImpulse = false;
  305. contactDesc.hasForceThresholds = false;
  306. contactDesc.shapeInteraction = NULL;
  307. contactDesc.restDistance = 0.f;
  308. contactDesc.maxCCDSeparation = PX_MAX_F32;
  309.  
  310. contactDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
  311. contactDesc.bodyState1 = pair.actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
  312. contactDesc.desc = &constraintDesc;
  313. contactDesc.mInvMassScales.angular0 = contactDesc.mInvMassScales.angular1 = contactDesc.mInvMassScales.linear0 = contactDesc.mInvMassScales.linear1 = 1.f;
  314. }
  315.  
  316. immediate::PxCreateContactConstraints(&header, 1, contactDescs, *gConstraintAllocator, invDt, -2.f * gUnitScale, 0.04f * gUnitScale, 0.025f * gUnitScale);
  317.  
  318. #if WITH_PERSISTENCY
  319. for (PxU32 a = 0; a < header.mStride; ++a)
  320. {
  321. //Cache friction information...
  322. PxSolverContactDesc& contactDesc = contactDescs[a];
  323. //PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
  324. ContactPair& pair = *pairs[a];
  325.  
  326. const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
  327.  
  328. (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions = contactDesc.frictionPtr;
  329. (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions = contactDesc.frictionCount;
  330. }
  331. #endif
  332. }
  333.  
  334. for (PxU32 i = nbContactHeaders; i < totalHeaders; ++i)
  335. {
  336. PxConstraintBatchHeader& header = headers[i];
  337.  
  338. PX_ASSERT(header.mConstraintType == PxSolverConstraintDesc::eJOINT_CONSTRAINT);
  339.  
  340. {
  341. PxSolverConstraintPrepDesc jointDescs[4];
  342.  
  343. PxConstraint* constraints[4];
  344.  
  345. header.mStartIndex += activeContactPairs.size();
  346.  
  347. for (PxU32 a = 0; a < header.mStride; ++a)
  348. {
  349. PxSolverConstraintDesc& constraintDesc = orderedDescs[header.mStartIndex + a];
  350. //Extract the contact pair that we saved in this structure earlier.
  351. PxConstraint& constraint = *reinterpret_cast<PxConstraint*>(constraintDesc.constraint);
  352.  
  353. constraints[a] = &constraint;
  354.  
  355. PxSolverConstraintPrepDesc& jointDesc = jointDescs[a];
  356.  
  357. jointDesc.body0 = constraintDesc.bodyA;
  358. jointDesc.body1 = constraintDesc.bodyB;
  359. jointDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
  360. jointDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];
  361.  
  362. //This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
  363. //example does not use articulations.
  364. jointDesc.bodyFrame0 = jointDesc.data0->body2World;
  365. jointDesc.bodyFrame1 = jointDesc.data1->body2World;
  366.  
  367. PxRigidActor* actor0, *actor1;
  368.  
  369. constraint.getActors(actor0, actor1);
  370.  
  371. jointDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
  372. jointDesc.bodyState1 = actor1 == NULL ? PxSolverConstraintPrepDescBase::eSTATIC_BODY : actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
  373. jointDesc.desc = &constraintDesc;
  374. jointDesc.mInvMassScales.angular0 = jointDesc.mInvMassScales.angular1 = jointDesc.mInvMassScales.linear0 = jointDesc.mInvMassScales.linear1 = 1.f;
  375. jointDesc.writeback = NULL;
  376. constraint.getBreakForce(jointDesc.linBreakForce, jointDesc.angBreakForce);
  377. jointDesc.minResponseThreshold = constraint.getMinResponseThreshold();
  378. jointDesc.disablePreprocessing = !!(constraint.getFlags() & PxConstraintFlag::eDISABLE_PREPROCESSING);
  379. jointDesc.improvedSlerp = !!(constraint.getFlags() & PxConstraintFlag::eIMPROVED_SLERP);
  380. jointDesc.driveLimitsAreForces = !!(constraint.getFlags() & PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES);
  381. }
  382.  
  383. immediate::PxCreateJointConstraintsWithShaders(&header, 1, constraints, jointDescs, *gConstraintAllocator, dt, invDt);
  384.  
  385. }
  386. }
  387.  
  388.  
  389. //Solve all the constraints produced earlier. Intermediate motion linear/angular velocity buffers are filled in. These contain intermediate delta velocity information that is used
  390. //the PxIntegrateSolverBody
  391. Array<PxVec3> motionLinearVelocity(nbDynamics);
  392. Array<PxVec3> motionAngularVelocity(nbDynamics);
  393.  
  394. //Zero the bodies array. This buffer contains the delta velocities and are accumulated during the simulation. For correct behavior, it is vital
  395. //that this buffer is zeroed.
  396. PxMemZero(bodies.begin(), bodies.size() * sizeof(PxSolverBody));
  397.  
  398. immediate::PxSolveConstraints(headers.begin(), headers.size(), orderedDescs.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, 4, 1);
  399.  
  400. immediate::PxIntegrateSolverBodies(bodyData.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, dt);
  401.  
  402. for (PxU32 a = 0; a < nbDynamics; ++a)
  403. {
  404. PxRigidDynamic* dynamic = actors[a]->is<PxRigidDynamic>();
  405.  
  406. PxSolverBodyData& data = bodyData[a];
  407.  
  408. dynamic->setLinearVelocity(data.linearVelocity);
  409. dynamic->setAngularVelocity(data.angularVelocity);
  410. dynamic->setGlobalPose(data.body2World);
  411. }
  412. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement