dali-physics/third-party/bullet3/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h

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