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);
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
setLinearAndAngular(b3Float4ConstArg n,b3Float4ConstArg r0,b3Float4ConstArg r1,b3Float4 * linear,b3Float4 * angular0,b3Float4 * angular1)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
calcRelVel(b3Float4ConstArg l0,b3Float4ConstArg l1,b3Float4ConstArg a0,b3Float4ConstArg a1,b3Float4ConstArg linVel0,b3Float4ConstArg angVel0,b3Float4ConstArg linVel1,b3Float4ConstArg angVel1)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
calcJacCoeff(b3Float4ConstArg linear0,b3Float4ConstArg linear1,b3Float4ConstArg angular0,b3Float4ConstArg angular1,float invMass0,const b3Mat3x3 * invInertia0,float invMass1,const b3Mat3x3 * invInertia1)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
setConstraint4(b3Float4ConstArg posA,b3Float4ConstArg linVelA,b3Float4ConstArg angVelA,float invMassA,b3Mat3x3ConstArg invInertiaA,b3Float4ConstArg posB,b3Float4ConstArg linVelB,b3Float4ConstArg angVelB,float invMassB,b3Mat3x3ConstArg invInertiaB,__global struct b3Contact4Data * src,float dt,float positionDrift,float positionConstraintCoeff,b3ContactConstraint4_t * dstC)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 }
149