1 #ifndef B3_REDUCE_CONTACTS_H
2 #define B3_REDUCE_CONTACTS_H
3
b3ReduceContacts(const b3Float4 * p,int nPoints,const b3Float4 & nearNormal,b3Int4 * contactIdx)4 inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
5 {
6 if (nPoints == 0)
7 return 0;
8
9 if (nPoints <= 4)
10 return nPoints;
11
12 if (nPoints > 64)
13 nPoints = 64;
14
15 b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
16 {
17 for (int i = 0; i < nPoints; i++)
18 center += p[i];
19 center /= (float)nPoints;
20 }
21
22 // sample 4 directions
23
24 b3Float4 aVector = p[0] - center;
25 b3Float4 u = b3Cross3(nearNormal, aVector);
26 b3Float4 v = b3Cross3(nearNormal, u);
27 u = b3FastNormalized3(u);
28 v = b3FastNormalized3(v);
29
30 //keep point with deepest penetration
31 float minW = FLT_MAX;
32
33 int minIndex = -1;
34
35 b3Float4 maxDots;
36 maxDots.x = FLT_MIN;
37 maxDots.y = FLT_MIN;
38 maxDots.z = FLT_MIN;
39 maxDots.w = FLT_MIN;
40
41 // idx, distance
42 for (int ie = 0; ie < nPoints; ie++)
43 {
44 if (p[ie].w < minW)
45 {
46 minW = p[ie].w;
47 minIndex = ie;
48 }
49 float f;
50 b3Float4 r = p[ie] - center;
51 f = b3Dot3F4(u, r);
52 if (f < maxDots.x)
53 {
54 maxDots.x = f;
55 contactIdx[0].x = ie;
56 }
57
58 f = b3Dot3F4(-u, r);
59 if (f < maxDots.y)
60 {
61 maxDots.y = f;
62 contactIdx[0].y = ie;
63 }
64
65 f = b3Dot3F4(v, r);
66 if (f < maxDots.z)
67 {
68 maxDots.z = f;
69 contactIdx[0].z = ie;
70 }
71
72 f = b3Dot3F4(-v, r);
73 if (f < maxDots.w)
74 {
75 maxDots.w = f;
76 contactIdx[0].w = ie;
77 }
78 }
79
80 if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
81 {
82 //replace the first contact with minimum (todo: replace contact with least penetration)
83 contactIdx[0].x = minIndex;
84 }
85
86 return 4;
87 }
88
89 #endif //B3_REDUCE_CONTACTS_H
90