Extras/PhysicsEffects/src/low_level/solver/pfx_constraint_solver_single.cpp

   1 /*\r
   2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #include "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"\r
  18 #include "../../../include/physics_effects/base_level/solver/pfx_contact_constraint.h"\r
  19 #include "../../../include/physics_effects/low_level/solver/pfx_joint_constraint_func.h"\r
  20 #include "../../../include/physics_effects/low_level/solver/pfx_constraint_solver.h"\r
  21 #include "../../base_level/solver/pfx_check_solver.h"\r
  22 #include "pfx_parallel_group.h"\r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 PfxUInt32 pfxGetWorkBytesOfSolveConstraints(PfxUInt32 numRigidBodies,PfxUInt32 numContactPairs,PfxUInt32 numJointPairs,PfxUInt32 maxTasks)\r
  28 {\r
  29         (void)maxTasks;\r
  30         PfxUInt32 workBytes = SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt8) * numRigidBodies) +\r
  31                 SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxUInt32)*((SCE_PFX_MAX(numContactPairs,numJointPairs)+31)/32));\r
  32 \r
  33         workBytes += 128 + (SCE_PFX_ALLOC_BYTES_ALIGN16(sizeof(PfxParallelGroup)) + \r
  34                  SCE_PFX_ALLOC_BYTES_ALIGN128(sizeof(PfxParallelBatch)*(SCE_PFX_MAX_SOLVER_PHASES*SCE_PFX_MAX_SOLVER_BATCHES))) * 2;\r
  35 \r
  36         return workBytes;\r
  37 }\r
  38 \r
  39 PfxInt32 pfxCheckParamOfSetupSolverBodies(const PfxSetupSolverBodiesParam &param)\r
  40 {\r
  41         if(!param.states || !param.bodies || !param.solverBodies ) return SCE_PFX_ERR_INVALID_VALUE;\r
  42         if(!SCE_PFX_PTR_IS_ALIGNED16(param.states) || !SCE_PFX_PTR_IS_ALIGNED16(param.bodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.solverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;\r
  43         return SCE_PFX_OK;\r
  44 }\r
  45 \r
  46 PfxInt32 pfxCheckParamOfSetupContactConstraints(const PfxSetupContactConstraintsParam &param)\r
  47 {\r
  48         if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || !param.offsetRigidStates || \r
  49                 !param.offsetRigidBodies  || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;\r
  50         if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||\r
  51                 !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;\r
  52         return SCE_PFX_OK;\r
  53 }\r
  54 \r
  55 PfxInt32 pfxCheckParamOfSetupJointConstraints(const PfxSetupJointConstraintsParam &param)\r
  56 {\r
  57         if((param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || \r
  58                 !param.offsetRigidBodies  || !param.offsetSolverBodies || param.timeStep <= 0.0f) return SCE_PFX_ERR_INVALID_VALUE;\r
  59         if(!SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||\r
  60                 !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidBodies) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;\r
  61         return SCE_PFX_OK;\r
  62 }\r
  63 \r
  64 PfxInt32 pfxCheckParamOfSolveConstraints(const PfxSolveConstraintsParam &param)\r
  65 {\r
  66         if((param.numContactPairs>0&&(!param.contactPairs||!param.offsetContactManifolds)) || \r
  67                 (param.numJointPairs>0&&(!param.jointPairs||!param.offsetJoints)) || !param.offsetRigidStates || !param.offsetSolverBodies) return SCE_PFX_ERR_INVALID_VALUE;\r
  68         if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || \r
  69                 !SCE_PFX_PTR_IS_ALIGNED16(param.jointPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetJoints) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) ||\r
  70                 !SCE_PFX_PTR_IS_ALIGNED16(param.offsetSolverBodies)) return SCE_PFX_ERR_INVALID_ALIGN;\r
  71         if(SCE_PFX_AVAILABLE_BYTES_ALIGN16(param.workBuff,param.workBytes) < pfxGetWorkBytesOfSolveConstraints(param.numRigidBodies,param.numContactPairs,param.numJointPairs) ) return SCE_PFX_ERR_OUT_OF_BUFFER;\r
  72         return SCE_PFX_OK;\r
  73 }\r
  74 \r
  75 ///////////////////////////////////////////////////////////////////////////////\r
  76 // SINGLE THREAD\r
  77 \r
  78 PfxInt32 pfxSetupSolverBodies(PfxSetupSolverBodiesParam &param)\r
  79 {\r
  80         PfxInt32 ret = pfxCheckParamOfSetupSolverBodies(param);\r
  81         if(ret != SCE_PFX_OK) return ret;\r
  82         \r
  83         SCE_PFX_PUSH_MARKER("pfxSetupSolverBodies");\r
  84 \r
  85         PfxRigidState *states = param.states;\r
  86         PfxRigidBody *bodies = param.bodies;\r
  87         PfxSolverBody *solverBodies = param.solverBodies;\r
  88         PfxUInt32 numRigidBodies = param.numRigidBodies;\r
  89         \r
  90         for(PfxUInt32 i=0;i<numRigidBodies;i++) {\r
  91                 PfxRigidState &state = states[i];\r
  92                 PfxRigidBody &body = bodies[i];\r
  93                 PfxSolverBody &solverBody = solverBodies[i];\r
  94                 \r
  95                 solverBody.m_orientation = state.getOrientation();\r
  96                 solverBody.m_deltaLinearVelocity = PfxVector3(0.0f);\r
  97                 solverBody.m_deltaAngularVelocity = PfxVector3(0.0f);\r
  98                 solverBody.m_motionType = state.getMotionMask();\r
  99 \r
 100                 if(SCE_PFX_MOTION_MASK_DYNAMIC(state.getMotionType())) {\r
 101                         PfxMatrix3 ori(solverBody.m_orientation);\r
 102                         solverBody.m_massInv = body.getMassInv();\r
 103                         solverBody.m_inertiaInv = ori * body.getInertiaInv() * transpose(ori);\r
 104                 }\r
 105                 else {\r
 106                         solverBody.m_massInv = 0.0f;\r
 107                         solverBody.m_inertiaInv = PfxMatrix3(0.0f);\r
 108                 }\r
 109         }\r
 110 \r
 111         SCE_PFX_POP_MARKER();\r
 112 \r
 113         return SCE_PFX_OK;\r
 114 }\r
 115 \r
 116 PfxInt32 pfxSetupContactConstraints(PfxSetupContactConstraintsParam &param)\r
 117 {\r
 118         PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);\r
 119         if(ret != SCE_PFX_OK) return ret;\r
 120         \r
 121         SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");\r
 122 \r
 123         PfxConstraintPair *contactPairs = param.contactPairs;\r
 124         PfxUInt32 numContactPairs = param.numContactPairs;\r
 125         PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;\r
 126         PfxRigidState *offsetRigidStates = param.offsetRigidStates;\r
 127         PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;\r
 128         PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;\r
 129         \r
 130         for(PfxUInt32 i=0;i<numContactPairs;i++) {\r
 131                 PfxConstraintPair &pair = contactPairs[i];\r
 132                 if(!pfxCheckSolver(pair)) {\r
 133                         continue;\r
 134                 }\r
 135 \r
 136                 PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 137                 PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 138                 PfxUInt32 iConstraint = pfxGetConstraintId(pair);\r
 139 \r
 140                 PfxContactManifold &contact = offsetContactManifolds[iConstraint];\r
 141 \r
 142                 SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());\r
 143                 SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());\r
 144 \r
 145                 PfxRigidState &stateA = offsetRigidStates[iA];\r
 146                 PfxRigidBody &bodyA = offsetRigidBodies[iA];\r
 147                 PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 148 \r
 149                 PfxRigidState &stateB = offsetRigidStates[iB];\r
 150                 PfxRigidBody &bodyB = offsetRigidBodies[iB];\r
 151                 PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 152         \r
 153                 contact.setInternalFlag(0);\r
 154                 \r
 155                 PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());\r
 156                 if(contact.getDuration() > 1) restitution = 0.0f;\r
 157                 \r
 158                 PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());\r
 159                 \r
 160                 for(int j=0;j<contact.getNumContacts();j++) {\r
 161                         PfxContactPoint &cp = contact.getContactPoint(j);\r
 162                         \r
 163                         pfxSetupContactConstraint(\r
 164                                 cp.m_constraintRow[0],\r
 165                                 cp.m_constraintRow[1],\r
 166                                 cp.m_constraintRow[2],\r
 167                                 cp.m_distance,\r
 168                                 restitution,\r
 169                                 friction,\r
 170                                 pfxReadVector3(cp.m_constraintRow[0].m_normal),\r
 171                                 pfxReadVector3(cp.m_localPointA),\r
 172                                 pfxReadVector3(cp.m_localPointB),\r
 173                                 stateA,\r
 174                                 stateB,\r
 175                                 solverBodyA,\r
 176                                 solverBodyB,\r
 177                                 param.separateBias,\r
 178                                 param.timeStep\r
 179                                 );\r
 180                 }\r
 181 \r
 182                 contact.setCompositeFriction(friction);\r
 183         }\r
 184 \r
 185         SCE_PFX_POP_MARKER();\r
 186 \r
 187         return SCE_PFX_OK;\r
 188 }\r
 189 \r
 190 PfxInt32 pfxSetupJointConstraints(PfxSetupJointConstraintsParam &param)\r
 191 {\r
 192         PfxInt32 ret = pfxCheckParamOfSetupJointConstraints(param);\r
 193         if(ret != SCE_PFX_OK) return ret;\r
 194         \r
 195         SCE_PFX_PUSH_MARKER("pfxSetupJointConstraints");\r
 196 \r
 197         PfxConstraintPair *jointPairs = param.jointPairs;\r
 198         PfxUInt32 numJointPairs = param.numJointPairs;\r
 199         PfxJoint *offsetJoints = param.offsetJoints;\r
 200         PfxRigidState *offsetRigidStates = param.offsetRigidStates;\r
 201         PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;\r
 202         \r
 203         for(PfxUInt32 i=0;i<numJointPairs;i++) {\r
 204                 PfxConstraintPair &pair = jointPairs[i];\r
 205                 if(!pfxCheckSolver(pair)) {\r
 206                         continue;\r
 207                 }\r
 208 \r
 209                 PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 210                 PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 211                 PfxUInt32 iConstraint = pfxGetConstraintId(pair);\r
 212                 \r
 213                 PfxJoint &joint = offsetJoints[iConstraint];\r
 214 \r
 215                 SCE_PFX_ALWAYS_ASSERT(iA==joint.m_rigidBodyIdA);\r
 216                 SCE_PFX_ALWAYS_ASSERT(iB==joint.m_rigidBodyIdB);\r
 217 \r
 218                 PfxRigidState &stateA = offsetRigidStates[iA];\r
 219                 PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 220 \r
 221                 PfxRigidState &stateB = offsetRigidStates[iB];\r
 222                 PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 223                 \r
 224                 pfxGetSetupJointConstraintFunc(joint.m_type)(\r
 225                         joint,\r
 226                         stateA,\r
 227                         stateB,\r
 228                         solverBodyA,\r
 229                         solverBodyB,\r
 230                         param.timeStep);\r
 231         }\r
 232 \r
 233         SCE_PFX_POP_MARKER();\r
 234 \r
 235         return SCE_PFX_OK;\r
 236 }\r
 237 \r
 238 PfxInt32 pfxSolveConstraints(PfxSolveConstraintsParam &param)\r
 239 {\r
 240         PfxInt32 ret = pfxCheckParamOfSolveConstraints(param);\r
 241         if(ret != SCE_PFX_OK) return ret;\r
 242 \r
 243         SCE_PFX_PUSH_MARKER("pfxSolveConstraints");\r
 244 \r
 245         PfxConstraintPair *contactPairs = param.contactPairs;\r
 246         PfxUInt32 numContactPairs = param.numContactPairs;\r
 247         PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;\r
 248         PfxConstraintPair *jointPairs = param.jointPairs;\r
 249         PfxUInt32 numJointPairs = param.numJointPairs;\r
 250         PfxJoint *offsetJoints = param.offsetJoints;\r
 251         PfxRigidState *offsetRigidStates = param.offsetRigidStates;\r
 252         PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;\r
 253         PfxUInt32 numRigidBodies = param.numRigidBodies;\r
 254         \r
 255         // Warm Starting\r
 256         {\r
 257                 for(PfxUInt32 i=0;i<numJointPairs;i++) {\r
 258                         PfxConstraintPair &pair = jointPairs[i];\r
 259                         if(!pfxCheckSolver(pair)) {\r
 260                                 continue;\r
 261                         }\r
 262 \r
 263                         PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 264                         PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 265 \r
 266                         PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];\r
 267 \r
 268                         SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);\r
 269                         SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);\r
 270 \r
 271                         PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 272                         PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 273                         \r
 274                         pfxGetWarmStartJointConstraintFunc(joint.m_type)(\r
 275                                 joint,\r
 276                                 solverBodyA,\r
 277                                 solverBodyB);\r
 278                 }\r
 279                 for(PfxUInt32 i=0;i<numContactPairs;i++) {\r
 280                         PfxConstraintPair &pair = contactPairs[i];\r
 281                         if(!pfxCheckSolver(pair)) {\r
 282                                 continue;\r
 283                         }\r
 284 \r
 285                         PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 286                         PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 287 \r
 288                         PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];\r
 289 \r
 290                         SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());\r
 291                         SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());\r
 292 \r
 293                         PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 294                         PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 295                         \r
 296                         PfxFloat massInvA = solverBodyA.m_massInv;\r
 297                         PfxFloat massInvB = solverBodyB.m_massInv;\r
 298                         PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;\r
 299                         PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;\r
 300 \r
 301                         if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {\r
 302                                 massInvB = 0.0f;\r
 303                                 inertiaInvB = PfxMatrix3(0.0f);\r
 304                         }\r
 305                         if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {\r
 306                                 massInvA = 0.0f;\r
 307                                 inertiaInvA = PfxMatrix3(0.0f);\r
 308                         }\r
 309 \r
 310                         for(int j=0;j<contact.getNumContacts();j++) {\r
 311                                 PfxContactPoint &cp = contact.getContactPoint(j);\r
 312                                 \r
 313                                 PfxVector3 rA = rotate(solverBodyA.m_orientation,pfxReadVector3(cp.m_localPointA));\r
 314                                 PfxVector3 rB = rotate(solverBodyB.m_orientation,pfxReadVector3(cp.m_localPointB));\r
 315                                 \r
 316                                 for(int k=0;k<3;k++) {\r
 317                                         PfxVector3 normal = pfxReadVector3(cp.m_constraintRow[k].m_normal);\r
 318                                         PfxFloat deltaImpulse = cp.m_constraintRow[k].m_accumImpulse;\r
 319                                         solverBodyA.m_deltaLinearVelocity += deltaImpulse * massInvA * normal;\r
 320                                         solverBodyA.m_deltaAngularVelocity += deltaImpulse * inertiaInvA * cross(rA,normal);\r
 321                                         solverBodyB.m_deltaLinearVelocity -= deltaImpulse * massInvB * normal;\r
 322                                         solverBodyB.m_deltaAngularVelocity -= deltaImpulse * inertiaInvB * cross(rB,normal);\r
 323                                 }\r
 324                         }\r
 325                 }\r
 326         }\r
 327         \r
 328         // Solver\r
 329         for(PfxUInt32 iteration=0;iteration<param.iteration;iteration++) {\r
 330                 for(PfxUInt32 i=0;i<numJointPairs;i++) {\r
 331                         PfxConstraintPair &pair = jointPairs[i];\r
 332                         if(!pfxCheckSolver(pair)) {\r
 333                                 continue;\r
 334                         }\r
 335 \r
 336                         PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 337                         PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 338 \r
 339                         PfxJoint &joint = offsetJoints[pfxGetConstraintId(pair)];\r
 340 \r
 341                         SCE_PFX_ASSERT(iA==joint.m_rigidBodyIdA);\r
 342                         SCE_PFX_ASSERT(iB==joint.m_rigidBodyIdB);\r
 343 \r
 344                         PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 345                         PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 346                         \r
 347                         pfxGetSolveJointConstraintFunc(joint.m_type)(\r
 348                                 joint,\r
 349                                 solverBodyA,\r
 350                                 solverBodyB);\r
 351                 }\r
 352                 for(PfxUInt32 i=0;i<numContactPairs;i++) {\r
 353                         PfxConstraintPair &pair = contactPairs[i];\r
 354                         if(!pfxCheckSolver(pair)) {\r
 355                                 continue;\r
 356                         }\r
 357 \r
 358                         PfxUInt16 iA = pfxGetObjectIdA(pair);\r
 359                         PfxUInt16 iB = pfxGetObjectIdB(pair);\r
 360 \r
 361                         PfxContactManifold &contact = offsetContactManifolds[pfxGetConstraintId(pair)];\r
 362 \r
 363                         SCE_PFX_ASSERT(iA==contact.getRigidBodyIdA());\r
 364                         SCE_PFX_ASSERT(iB==contact.getRigidBodyIdB());\r
 365 \r
 366                         PfxSolverBody &solverBodyA = offsetSolverBodies[iA];\r
 367                         PfxSolverBody &solverBodyB = offsetSolverBodies[iB];\r
 368                         \r
 369                         for(int j=0;j<contact.getNumContacts();j++) {\r
 370                                 PfxContactPoint &cp = contact.getContactPoint(j);\r
 371                                 \r
 372                                 pfxSolveContactConstraint(\r
 373                                         cp.m_constraintRow[0],\r
 374                                         cp.m_constraintRow[1],\r
 375                                         cp.m_constraintRow[2],\r
 376                                         pfxReadVector3(cp.m_localPointA),\r
 377                                         pfxReadVector3(cp.m_localPointB),\r
 378                                         solverBodyA,\r
 379                                         solverBodyB,\r
 380                                         contact.getCompositeFriction()\r
 381                                         );\r
 382                         }\r
 383                 }\r
 384         }\r
 385 \r
 386         for(PfxUInt32 i=0;i<numRigidBodies;i++) {\r
 387                 offsetRigidStates[i].setLinearVelocity(\r
 388                         offsetRigidStates[i].getLinearVelocity()+offsetSolverBodies[i].m_deltaLinearVelocity);\r
 389                 offsetRigidStates[i].setAngularVelocity(\r
 390                         offsetRigidStates[i].getAngularVelocity()+offsetSolverBodies[i].m_deltaAngularVelocity);\r
 391         }\r
 392 \r
 393         SCE_PFX_POP_MARKER();\r
 394 \r
 395         return SCE_PFX_OK;\r
 396 }\r
 397 \r
 398 } //namespace PhysicsEffects\r
 399 } //namespace sce\r