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
|
#include "MinkowskiPenetrationDepthSolver.h"
#include "CollisionShapes/MinkowskiSumShape.h"
#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
#include "NarrowPhaseCollision/GjkPairDetector.h"
struct MyResult : public DiscreteCollisionDetectorInterface::Result
{
MyResult():m_hasResult(false)
{
}
SimdVector3 m_normalOnBInWorld;
SimdVector3 m_pointInWorld;
float m_depth;
bool m_hasResult;
void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
m_hasResult = true;
}
};
bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transA,const SimdTransform& transB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
//just take fixed number of orientation, and sample the penetration depth in that direction
int N = 3;
float minProj = 1e30f;
SimdVector3 minNorm;
SimdVector3 minVertex;
SimdVector3 minA,minB;
//not so good, lots of directions overlap, better to use gauss map
for (int i=-N;i<N;i++)
{
for (int j = -N;j<N;j++)
{
for (int k=-N;k<N;k++)
{
if (i | j | k)
{
SimdVector3 norm(i,j,k);
norm.normalize();
{
SimdVector3 seperatingAxisInA = (-norm)* transA.getBasis();
SimdVector3 seperatingAxisInB = norm* transB.getBasis();
SimdVector3 pInA = convexA->LocalGetSupportingVertex(seperatingAxisInA);
SimdVector3 qInB = convexB->LocalGetSupportingVertex(seperatingAxisInB);
SimdPoint3 pWorld = transA(pInA);
SimdPoint3 qWorld = transB(qInB);
SimdVector3 w = qWorld - pWorld;
float delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
{
SimdVector3 seperatingAxisInA = (norm)* transA.getBasis();
SimdVector3 seperatingAxisInB = -norm* transB.getBasis();
SimdVector3 pInA = convexA->LocalGetSupportingVertex(seperatingAxisInA);
SimdVector3 qInB = convexB->LocalGetSupportingVertex(seperatingAxisInB);
SimdPoint3 pWorld = transA(pInA);
SimdPoint3 qWorld = transB(qInB);
SimdVector3 w = qWorld - pWorld;
float delta = (-norm).dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta ;
minNorm = -norm;
minA = pWorld;
minB = qWorld;
}
}
}
}
}
}
SimdTransform ident;
ident.setIdentity();
GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
v = minNorm * minProj;
GjkPairDetector::ClosestPointInput input;
SimdVector3 newOrg = transA.getOrigin() + v + v;
SimdTransform displacedTrans = transA;
displacedTrans.setOrigin(newOrg);
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = 1e30f;
MyResult res;
gjkdet.GetClosestPoints(input,res);
if (res.m_hasResult)
{
pa = res.m_pointInWorld - res.m_normalOnBInWorld*res.m_depth;
pb = res.m_pointInWorld;
}
return res.m_hasResult;
}
|
