1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
#ifndef B3_CONTACT_SPHERE_SPHERE_H
#define B3_CONTACT_SPHERE_SPHERE_H
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronData* convexShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
const b3GpuFace* faces,
b3Contact4* globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
float4 pos = rigidBodies[bodyIndexB].m_pos;
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
b3Transform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(quat);
b3Transform trInv = tr.inverse();
float4 spherePos = trInv(spherePos1);
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
bool bCollide = true;
int region = -1;
float4 localHitNormal;
for ( int f = 0; f < numFaces; f++ )
{
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
float4 planeEqn;
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane.w;
float4 pntReturn;
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
if ( dist > radius)
{
bCollide = false;
break;
}
if ( dist > 0 )
{
//might hit an edge or vertex
b3Vector3 out;
bool isInPoly = IsPointInPolygon(spherePos,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
&out);
if (isInPoly)
{
if (dist>minDist)
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
region=1;
}
} else
{
b3Vector3 tmp = spherePos-out;
b3Scalar l2 = tmp.length2();
if (l2<radius*radius)
{
dist = b3Sqrt(l2);
if (dist>minDist)
{
minDist = dist;
closestPnt = out;
localHitNormal = tmp/dist;
region=2;
}
} else
{
bCollide = false;
break;
}
}
}
else
{
if ( dist > minDist )
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
region=3;
}
}
}
static int numChecks = 0;
numChecks++;
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
float4 pOnB1 = tr(closestPnt);
//printf("dist ,%f,",minDist);
float actualDepth = minDist-radius;
if (actualDepth<0)
{
//printf("actualDepth = ,%f,", actualDepth);
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
//printf("region=,%d,\n", region);
pOnB1[3] = actualDepth;
int dstIdx;
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
if (nGlobalContactsOut < maxContactCapacity)
{
dstIdx=nGlobalContactsOut;
nGlobalContactsOut++;
b3Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormalOnB = normalOnSurfaceB1;
c->setFrictionCoeff(0.7);
c->setRestituitionCoeff(0.f);
c->m_batchIdx = pairIndex;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPosB[0] = pOnB1;
int numPoints = 1;
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
}//if (dstIdx < numPairs)
}
}//if (hasCollision)
}
#endif //B3_CONTACT_SPHERE_SPHERE_H
|