b3ConvertConstraint4.h 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148
  1. #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
  2. #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
  3. #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
  4. void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
  5. void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
  6. {
  7. if (b3Fabs(n.z) > 0.70710678f)
  8. {
  9. // choose p in y-z plane
  10. float a = n.y * n.y + n.z * n.z;
  11. float k = 1.f / sqrt(a);
  12. p[0].x = 0;
  13. p[0].y = -n.z * k;
  14. p[0].z = n.y * k;
  15. // set q = n x p
  16. q[0].x = a * k;
  17. q[0].y = -n.x * p[0].z;
  18. q[0].z = n.x * p[0].y;
  19. }
  20. else
  21. {
  22. // choose p in x-y plane
  23. float a = n.x * n.x + n.y * n.y;
  24. float k = 1.f / sqrt(a);
  25. p[0].x = -n.y * k;
  26. p[0].y = n.x * k;
  27. p[0].z = 0;
  28. // set q = n x p
  29. q[0].x = -n.z * p[0].y;
  30. q[0].y = n.z * p[0].x;
  31. q[0].z = a * k;
  32. }
  33. }
  34. void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
  35. {
  36. *linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
  37. *angular0 = b3Cross3(r0, n);
  38. *angular1 = -b3Cross3(r1, n);
  39. }
  40. float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
  41. b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
  42. {
  43. return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
  44. }
  45. float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
  46. float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
  47. {
  48. // linear0,1 are normlized
  49. float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0;
  50. float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
  51. float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1;
  52. float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
  53. return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
  54. }
  55. void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
  56. b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
  57. __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
  58. b3ContactConstraint4_t* dstC)
  59. {
  60. dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
  61. dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
  62. float dtInv = 1.f / dt;
  63. for (int ic = 0; ic < 4; ic++)
  64. {
  65. dstC->m_appliedRambdaDt[ic] = 0.f;
  66. }
  67. dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
  68. dstC->m_linear = src->m_worldNormalOnB;
  69. dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
  70. for (int ic = 0; ic < 4; ic++)
  71. {
  72. b3Float4 r0 = src->m_worldPosB[ic] - posA;
  73. b3Float4 r1 = src->m_worldPosB[ic] - posB;
  74. if (ic >= src->m_worldNormalOnB.w) //npoints
  75. {
  76. dstC->m_jacCoeffInv[ic] = 0.f;
  77. continue;
  78. }
  79. float relVelN;
  80. {
  81. b3Float4 linear, angular0, angular1;
  82. setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
  83. dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
  84. invMassA, &invInertiaA, invMassB, &invInertiaB);
  85. relVelN = calcRelVel(linear, -linear, angular0, angular1,
  86. linVelA, angVelA, linVelB, angVelB);
  87. float e = 0.f; //src->getRestituitionCoeff();
  88. if (relVelN * relVelN < 0.004f) e = 0.f;
  89. dstC->m_b[ic] = e * relVelN;
  90. //float penetration = src->m_worldPosB[ic].w;
  91. dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
  92. dstC->m_appliedRambdaDt[ic] = 0.f;
  93. }
  94. }
  95. if (src->m_worldNormalOnB.w > 0) //npoints
  96. { // prepare friction
  97. b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
  98. for (int i = 0; i < src->m_worldNormalOnB.w; i++)
  99. center += src->m_worldPosB[i];
  100. center /= (float)src->m_worldNormalOnB.w;
  101. b3Float4 tangent[2];
  102. b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
  103. b3Float4 r[2];
  104. r[0] = center - posA;
  105. r[1] = center - posB;
  106. for (int i = 0; i < 2; i++)
  107. {
  108. b3Float4 linear, angular0, angular1;
  109. setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
  110. dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
  111. invMassA, &invInertiaA, invMassB, &invInertiaB);
  112. dstC->m_fAppliedRambdaDt[i] = 0.f;
  113. }
  114. dstC->m_center = center;
  115. }
  116. for (int i = 0; i < 4; i++)
  117. {
  118. if (i < src->m_worldNormalOnB.w)
  119. {
  120. dstC->m_worldPos[i] = src->m_worldPosB[i];
  121. }
  122. else
  123. {
  124. dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
  125. }
  126. }
  127. }