OPC_SphereTriOverlap.h
Go to the documentation of this file.
1 
2 // This is collision detection. If you do another distance test for collision *response*,
3 // if might be useful to simply *skip* the test below completely, and report a collision.
4 // - if sphere-triangle overlap, result is ok
5 // - if they don't, we'll discard them during collision response with a similar test anyway
6 // Overall this approach should run faster.
7 
8 // Original code by David Eberly in Magic.
9 BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
10 {
11  // Stats
12  mNbVolumePrimTests++;
13 
14  // Early exit if one of the vertices is inside the sphere
15  Point kDiff = vert2 - mCenter;
16  float fC = kDiff.SquareMagnitude();
17  if(fC <= mRadius2) return TRUE;
18 
19  kDiff = vert1 - mCenter;
20  fC = kDiff.SquareMagnitude();
21  if(fC <= mRadius2) return TRUE;
22 
23  kDiff = vert0 - mCenter;
24  fC = kDiff.SquareMagnitude();
25  if(fC <= mRadius2) return TRUE;
26 
27  // Else do the full distance test
28  Point TriEdge0 = vert1 - vert0;
29  Point TriEdge1 = vert2 - vert0;
30 
31 //Point kDiff = vert0 - mCenter;
32  float fA00 = TriEdge0.SquareMagnitude();
33  float fA01 = TriEdge0 | TriEdge1;
34  float fA11 = TriEdge1.SquareMagnitude();
35  float fB0 = kDiff | TriEdge0;
36  float fB1 = kDiff | TriEdge1;
37 //float fC = kDiff.SquareMagnitude();
38  float fDet = fabsf(fA00*fA11 - fA01*fA01);
39  float u = fA01*fB1-fA11*fB0;
40  float v = fA01*fB0-fA00*fB1;
41  float SqrDist;
42 
43  if(u + v <= fDet)
44  {
45  if(u < 0.0f)
46  {
47  if(v < 0.0f) // region 4
48  {
49  if(fB0 < 0.0f)
50  {
51 // v = 0.0f;
52  if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
53  else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
54  }
55  else
56  {
57 // u = 0.0f;
58  if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
59  else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
60  else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
61  }
62  }
63  else // region 3
64  {
65 // u = 0.0f;
66  if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
67  else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
68  else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
69  }
70  }
71  else if(v < 0.0f) // region 5
72  {
73 // v = 0.0f;
74  if(fB0>=0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
75  else if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
76  else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
77  }
78  else // region 0
79  {
80  // minimum at interior point
81  if(fDet==0.0f)
82  {
83 // u = 0.0f;
84 // v = 0.0f;
85  SqrDist = MAX_FLOAT;
86  }
87  else
88  {
89  float fInvDet = 1.0f/fDet;
90  u *= fInvDet;
91  v *= fInvDet;
92  SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
93  }
94  }
95  }
96  else
97  {
98  float fTmp0, fTmp1, fNumer, fDenom;
99 
100  if(u < 0.0f) // region 2
101  {
102  fTmp0 = fA01 + fB0;
103  fTmp1 = fA11 + fB1;
104  if(fTmp1 > fTmp0)
105  {
106  fNumer = fTmp1 - fTmp0;
107  fDenom = fA00-2.0f*fA01+fA11;
108  if(fNumer >= fDenom)
109  {
110 // u = 1.0f;
111 // v = 0.0f;
112  SqrDist = fA00+2.0f*fB0+fC;
113  }
114  else
115  {
116  u = fNumer/fDenom;
117  v = 1.0f - u;
118  SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
119  }
120  }
121  else
122  {
123 // u = 0.0f;
124  if(fTmp1 <= 0.0f) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
125  else if(fB1 >= 0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
126  else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
127  }
128  }
129  else if(v < 0.0f) // region 6
130  {
131  fTmp0 = fA01 + fB1;
132  fTmp1 = fA00 + fB0;
133  if(fTmp1 > fTmp0)
134  {
135  fNumer = fTmp1 - fTmp0;
136  fDenom = fA00-2.0f*fA01+fA11;
137  if(fNumer >= fDenom)
138  {
139 // v = 1.0f;
140 // u = 0.0f;
141  SqrDist = fA11+2.0f*fB1+fC;
142  }
143  else
144  {
145  v = fNumer/fDenom;
146  u = 1.0f - v;
147  SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
148  }
149  }
150  else
151  {
152 // v = 0.0f;
153  if(fTmp1 <= 0.0f) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
154  else if(fB0 >= 0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
155  else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
156  }
157  }
158  else // region 1
159  {
160  fNumer = fA11 + fB1 - fA01 - fB0;
161  if(fNumer <= 0.0f)
162  {
163 // u = 0.0f;
164 // v = 1.0f;
165  SqrDist = fA11+2.0f*fB1+fC;
166  }
167  else
168  {
169  fDenom = fA00-2.0f*fA01+fA11;
170  if(fNumer >= fDenom)
171  {
172 // u = 1.0f;
173 // v = 0.0f;
174  SqrDist = fA00+2.0f*fB0+fC;
175  }
176  else
177  {
178  u = fNumer/fDenom;
179  v = 1.0f - u;
180  SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
181  }
182  }
183  }
184  }
185 
186  return fabsf(SqrDist) < mRadius2;
187 }
Point::SquareMagnitude
inline_ float SquareMagnitude() const
Computes square magnitude.
Definition: IcePoint.h:217
SphereCollider::mRadius2
float mRadius2
Sphere radius squared.
Definition: OPC_SphereCollider.h:65
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
swingTest.f
f
Definition: swingTest.py:6
VolumeCollider::mNbVolumePrimTests
udword mNbVolumePrimTests
Number of Volume-Primitive tests.
Definition: OPC_VolumeCollider.h:100
SphereCollider::SphereTriOverlap
BOOL SphereTriOverlap(const Point &vert0, const Point &vert1, const Point &vert2)
Definition: OPC_SphereTriOverlap.h:9
SphereCollider::mCenter
Point mCenter
Sphere center.
Definition: OPC_SphereCollider.h:64
TRUE
#define TRUE
Definition: OPC_IceHook.h:13
MAX_FLOAT
#define MAX_FLOAT
max possible float value
Definition: IceTypes.h:130
Point
Definition: IcePoint.h:25

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04