teienlib 0.0.1-x86-linux
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- data/Gemfile +4 -0
- data/LICENSE.txt +22 -0
- data/README.md +29 -0
- data/Rakefile +31 -0
- data/ext/teienlib/extconf.rb +35 -0
- data/ext/teienlib/interface/AnimationBlender.i +8 -0
- data/ext/teienlib/interface/CollisionChecker.i +22 -0
- data/ext/teienlib/interface/DebugDrawer.i +8 -0
- data/ext/teienlib/interface/MeshStrider.i +8 -0
- data/ext/teienlib/interface/Rakefile +27 -0
- data/ext/teienlib/interface/SoftBody.i +9 -0
- data/ext/teienlib/interface/teienlib.i +36 -0
- data/ext/teienlib/src/AnimationBlender.cc +141 -0
- data/ext/teienlib/src/AnimationBlender.h +41 -0
- data/ext/teienlib/src/Collision.cc +797 -0
- data/ext/teienlib/src/Collision.h +130 -0
- data/ext/teienlib/src/CollisionChecker.cc +67 -0
- data/ext/teienlib/src/CollisionChecker.h +33 -0
- data/ext/teienlib/src/DebugDrawer.h +66 -0
- data/ext/teienlib/src/MeshStrider.cc +112 -0
- data/ext/teienlib/src/MeshStrider.h +48 -0
- data/ext/teienlib/src/Shape.cc +9 -0
- data/ext/teienlib/src/Shape.h +186 -0
- data/ext/teienlib/src/SimpleCollisionLibrary.h +7 -0
- data/ext/teienlib/src/SoftBody.cc +148 -0
- data/ext/teienlib/src/SoftBody.h +14 -0
- data/ext/teienlib/src/Vector3D.h +92 -0
- data/lib/Teienlib.so +0 -0
- data/lib/teienlib/version.rb +3 -0
- data/lib/teienlib.rb +5 -0
- data/teienlib.gemspec +33 -0
- metadata +98 -0
@@ -0,0 +1,797 @@
|
|
1
|
+
#include "Collision.h"
|
2
|
+
|
3
|
+
#include <math.h>
|
4
|
+
#include <stdio.h>
|
5
|
+
#include <assert.h>
|
6
|
+
#include <memory>
|
7
|
+
|
8
|
+
#define FLT_MAX 10
|
9
|
+
#define EPSILON 1.0e-8
|
10
|
+
|
11
|
+
#define Max(a, b) (a > b) ? a : b
|
12
|
+
#define Min(a, b) (a < b) ? a : b
|
13
|
+
|
14
|
+
namespace scl{
|
15
|
+
|
16
|
+
Collision::Collision()
|
17
|
+
{
|
18
|
+
debug = false;
|
19
|
+
}
|
20
|
+
|
21
|
+
Collision::~Collision()
|
22
|
+
{
|
23
|
+
}
|
24
|
+
|
25
|
+
Collision::ColMap*
|
26
|
+
Collision::initCollisionMap()
|
27
|
+
{
|
28
|
+
ColMap *cm = new ColMap;
|
29
|
+
|
30
|
+
//cout << "Collision:: ColMap new" << endl;
|
31
|
+
|
32
|
+
(*cm)[makeStringPair("Sphere", "Sphere")] = &Collision::intersectMovingSphereSphere;
|
33
|
+
(*cm)[makeStringPair("Sphere", "Plane")] = &Collision::intersectMovingSpherePlane;
|
34
|
+
(*cm)[makeStringPair("Sphere", "AABB")] = &Collision::intersectMovingSphereAABB;
|
35
|
+
|
36
|
+
(*cm)[makeStringPair("AABB", "AABB")] = &Collision::intersectMovingAABBAABB;
|
37
|
+
(*cm)[makeStringPair("AABB", "Plane")] = &Collision::intersectMovingAABBPlane;
|
38
|
+
(*cm)[makeStringPair("AABB", "Sphere")] = &Collision::intersectMovingAABBSphere;
|
39
|
+
|
40
|
+
(*cm)[makeStringPair("Plane", "Plane")] = &Collision::intersectMovingPlanePlane;
|
41
|
+
(*cm)[makeStringPair("Plane", "AABB")] = &Collision::intersectMovingPlaneAABB;
|
42
|
+
(*cm)[makeStringPair("Plane", "Sphere")] = &Collision::intersectMovingPlaneSphere;
|
43
|
+
|
44
|
+
return cm;
|
45
|
+
}
|
46
|
+
|
47
|
+
pair<std::string, std::string>
|
48
|
+
Collision::makeStringPair(const char *s1, const char *s2)
|
49
|
+
{
|
50
|
+
std::string str1(s1);
|
51
|
+
std::string str2(s2);
|
52
|
+
|
53
|
+
pair<std::string, std::string> retPair(str1, str2);
|
54
|
+
|
55
|
+
return retPair;
|
56
|
+
}
|
57
|
+
|
58
|
+
Collision::ColFuncPtr
|
59
|
+
Collision::lookup(const std::string & classA, const std::string & classB)
|
60
|
+
{
|
61
|
+
static auto_ptr<ColMap> colMap(initCollisionMap());
|
62
|
+
|
63
|
+
ColMap::iterator mapEntry = colMap->find(make_pair(classA, classB));
|
64
|
+
|
65
|
+
if (mapEntry == colMap->end())
|
66
|
+
return NULL;
|
67
|
+
|
68
|
+
return (*mapEntry).second;
|
69
|
+
}
|
70
|
+
|
71
|
+
|
72
|
+
bool
|
73
|
+
Collision::intersectMovingShapes(Shape* shapeA, Vector3D<float> velA,
|
74
|
+
Shape* shapeB, Vector3D<float> velB,
|
75
|
+
float delta, float *t)
|
76
|
+
{
|
77
|
+
return intersectMovingShapes(shapeA, velA - velB, shapeB, delta, t);
|
78
|
+
}
|
79
|
+
|
80
|
+
bool
|
81
|
+
Collision::intersectMovingShapes(Shape* shapeA, Vector3D<float> vel,
|
82
|
+
Shape* shapeB,
|
83
|
+
float delta, float *t)
|
84
|
+
{
|
85
|
+
bool judge = false;
|
86
|
+
|
87
|
+
ColFuncPtr cfp = lookup(std::string(shapeA->getClassName()), std::string(shapeB->getClassName()));
|
88
|
+
|
89
|
+
if (cfp){
|
90
|
+
Vector3D<float> moveVector;
|
91
|
+
float aT = 0;
|
92
|
+
|
93
|
+
moveVector = vel * delta; ///< moveVector�ϡ�delta���֤�1�Ȥ����Ȥ���®��
|
94
|
+
|
95
|
+
try {
|
96
|
+
///< aT�ˤϡ�moveVectorʬ�ΰ�ư���֤�1�Ȥ������ξ��ͻ��֤��֤�
|
97
|
+
judge = (this->*cfp)(shapeA, moveVector, shapeB, &aT);
|
98
|
+
}
|
99
|
+
catch (...) {
|
100
|
+
cerr << "Error: exception in intersectMovingShapes()" << endl;
|
101
|
+
}
|
102
|
+
|
103
|
+
*t = aT * delta; ///< �̾���֤ˤ�ɤ�
|
104
|
+
}
|
105
|
+
else {
|
106
|
+
cerr << "Error: This Collision class does not support this pair of Shapes in intersetcMovingShapes" << endl;
|
107
|
+
cerr << "ShapeA: " << shapeA->getClassName() << ", ShapeB: " << shapeB->getClassName() << endl;
|
108
|
+
}
|
109
|
+
|
110
|
+
if (debug) {
|
111
|
+
cout << "judge: ";
|
112
|
+
|
113
|
+
if (judge)
|
114
|
+
cout << "true" << endl;
|
115
|
+
else
|
116
|
+
cout << "false" << endl;
|
117
|
+
}
|
118
|
+
|
119
|
+
return judge;
|
120
|
+
}
|
121
|
+
|
122
|
+
bool
|
123
|
+
Collision::intersectMovingSpherePlane(Shape* sphere, Vector3D<float> vel, Shape* plane, float *t)
|
124
|
+
{
|
125
|
+
Sphere sp = *(dynamic_cast<Sphere*>(sphere));
|
126
|
+
Plane pl = *(dynamic_cast<Plane*>(plane));
|
127
|
+
|
128
|
+
float dist = (pl.n).dot(sp.c) - pl.d;
|
129
|
+
|
130
|
+
if (fabsf(dist) <= sp.r) {
|
131
|
+
*t = 0.0f;
|
132
|
+
return true;
|
133
|
+
}
|
134
|
+
else {
|
135
|
+
float denom = (pl.n).dot(vel);
|
136
|
+
|
137
|
+
if (denom * dist >= 0.0f) {
|
138
|
+
return false;
|
139
|
+
}
|
140
|
+
else {
|
141
|
+
float r = dist > 0.0f ? sp.r : -sp.r;
|
142
|
+
*t = (r - dist) / denom;
|
143
|
+
return true;
|
144
|
+
|
145
|
+
/*
|
146
|
+
if (*t <= 1)
|
147
|
+
return true;
|
148
|
+
else
|
149
|
+
return false;
|
150
|
+
*/
|
151
|
+
}
|
152
|
+
}
|
153
|
+
}
|
154
|
+
|
155
|
+
bool
|
156
|
+
Collision::intersectMovingSphereSphere(Shape* sphereA, Vector3D<float> vel, Shape* sphereB, float *t)
|
157
|
+
{
|
158
|
+
Sphere spA = *(dynamic_cast<Sphere*>(sphereA));
|
159
|
+
Sphere spB = *(dynamic_cast<Sphere*>(sphereB));
|
160
|
+
|
161
|
+
Sphere spBex = spB;
|
162
|
+
|
163
|
+
// �Żߤ����Ƥ���spB��Ⱦ�¤�spA��Ⱦ�¤��ɲä���spA�����ˤ��롣
|
164
|
+
spBex.r += spA.r;
|
165
|
+
|
166
|
+
Vector3D<float> vN = vel;
|
167
|
+
vN.normalize();
|
168
|
+
|
169
|
+
Vector3D<float> colP;
|
170
|
+
|
171
|
+
// ��spA����vN�����ˤǤ������Ⱦ�³�����spBex�Ȥξ���Ƚ��
|
172
|
+
if (intersectRaySphere(spA.c, vN, spBex, t, &colP)) {
|
173
|
+
*t = (*t) / vel.len();
|
174
|
+
|
175
|
+
if (*t <= 1)
|
176
|
+
return true;
|
177
|
+
}
|
178
|
+
return false;
|
179
|
+
}
|
180
|
+
|
181
|
+
// AABB��ĺ����ǥå���n�ˤ���֤�����ؿ�
|
182
|
+
Vector3D<float> Corner(AABB b, int n)
|
183
|
+
{
|
184
|
+
Vector3D<float> p;
|
185
|
+
p.x = ((n & 1) ? b.max.x : b.min.x);
|
186
|
+
p.y = ((n & 2) ? b.max.y : b.min.y);
|
187
|
+
p.z = ((n & 4) ? b.max.z : b.min.z);
|
188
|
+
|
189
|
+
return p;
|
190
|
+
}
|
191
|
+
|
192
|
+
/**
|
193
|
+
* AABB���Ф���ư���Ƥ����θ�
|
194
|
+
*
|
195
|
+
* vel = Sphere.vel - AABB.vel
|
196
|
+
*/
|
197
|
+
|
198
|
+
bool
|
199
|
+
Collision::intersectMovingSphereAABB(Shape* sphere, Vector3D<float> vel, Shape* aabb, float *t)
|
200
|
+
{
|
201
|
+
Sphere sp = *(dynamic_cast<Sphere*>(sphere));
|
202
|
+
AABB box = *(dynamic_cast<AABB*>(aabb));
|
203
|
+
|
204
|
+
// ���Ⱦ��r�ޤ�b���ĥ��������̤Ȥ���������AABB���
|
205
|
+
AABB e = box;
|
206
|
+
e.min.x -= sp.r; e.min.y -= sp.r; e.min.z -= sp.r;
|
207
|
+
e.max.x += sp.r; e.max.y += sp.r; e.max.z += sp.r;
|
208
|
+
|
209
|
+
// ������ĥ������AABB e���Ф��Ƹ��������ɤ��e����硢�ʤ��ǽ�λ
|
210
|
+
// �����Ǥʤ���С�����p����ӻ���t���̤Ȥ�������
|
211
|
+
Vector3D<float> p;
|
212
|
+
if (!intersectRayAABB(sp.c, vel, e, t, &p) || *t > 1.0f)
|
213
|
+
return false;
|
214
|
+
|
215
|
+
// ����p�����γ�¦�˰��֤��Ƥ���b��min�����max���̤��
|
216
|
+
// u�����v�Υ��åȤ���Ƥ���ӥåȤν����Ʊ���ˤϤʤ�ʤ����ȡ������
|
217
|
+
// �����δ֤Ǿ��ʤ��Ȥ��ĤΥӥåȤ����åȤ���ʤ���Фʤ�ʤ�������
|
218
|
+
int u = 0, v = 0;
|
219
|
+
if (p.x < box.min.x) u |= 1;
|
220
|
+
if (p.x > box.max.x) v |= 1;
|
221
|
+
if (p.y < box.min.y) u |= 2;
|
222
|
+
if (p.y > box.max.y) v |= 2;
|
223
|
+
if (p.z < box.min.z) u |= 4;
|
224
|
+
if (p.z > box.max.z) v |= 4;
|
225
|
+
|
226
|
+
// ��Or��- ���٤ƤΥ��åȤ���Ƥ���ӥåȤ�ӥåȤΥޥ����˰��������(���� �����Ǥ� u + v == u | v)
|
227
|
+
int m = u + v;
|
228
|
+
|
229
|
+
// ��α�ư�ˤ�äƵ��Ҥ����ľ������ʬ [sp.c, sp.c+vel] ���������
|
230
|
+
Vector3D<float> lastPt = sp.c + vel;
|
231
|
+
Segment seg(sp.c, lastPt);
|
232
|
+
Capsule cap;
|
233
|
+
|
234
|
+
// ���٤Ƥ�3�ĤΥӥåȤ����å�(m == 7)����Ƥ����硢p��ĺ���ΰ�ˤ���
|
235
|
+
if (m == 7) {
|
236
|
+
// �������ʬ [sp.c, sp.c+vel] ��ĺ���ǽв�äƤ���3�Ĥ��դΥ��ץ�����Ф��Ƹ��Ƥ���
|
237
|
+
// 1�İʾ�θ�����к��ɤλ��֤��֤�
|
238
|
+
float tmin = FLT_MAX;
|
239
|
+
|
240
|
+
cap.seg.a = Corner(box, v);
|
241
|
+
cap.seg.b = Corner(box, v ^ 1);
|
242
|
+
cap.r = sp.r;
|
243
|
+
if (intersectSegmentCapsule(seg, cap, t))
|
244
|
+
tmin = Min(*t, tmin);
|
245
|
+
|
246
|
+
cap.seg.a = Corner(box, v);
|
247
|
+
cap.seg.b = Corner(box, v^2);
|
248
|
+
cap.r = sp.r;
|
249
|
+
if (intersectSegmentCapsule(seg, cap, t))
|
250
|
+
tmin = Min(*t, tmin);
|
251
|
+
|
252
|
+
cap.seg.a = Corner(box, v);
|
253
|
+
cap.seg.b = Corner(box, v ^ 4);
|
254
|
+
cap.r = sp.r;
|
255
|
+
if (intersectSegmentCapsule(seg, cap, t))
|
256
|
+
tmin = Min(*t, tmin);
|
257
|
+
|
258
|
+
if (tmin == FLT_MAX)
|
259
|
+
return false; // �ʤ�
|
260
|
+
|
261
|
+
*t = tmin;
|
262
|
+
return true; // ����t == tmin�ˤ����Ƹ�
|
263
|
+
}
|
264
|
+
// m�˥��åȤ���Ƥ���ӥåȤ�1�Ĥ����ξ�硢���ä�p�����ΰ����ˤ���
|
265
|
+
if ((m & (m - 1)) == 0) {
|
266
|
+
// ���⤷�ʤ�����ĥ���줿Ȣ�Ȥθ������t��
|
267
|
+
// ���������֤Ǥ���
|
268
|
+
return true;
|
269
|
+
}
|
270
|
+
// p�����ΰ�ˤ��롣�դˤ����ƥ��ץ���ȸ��Ƥ���
|
271
|
+
|
272
|
+
cap.seg.a = Corner(box, u^7);
|
273
|
+
cap.seg.b = Corner(box, v);
|
274
|
+
cap.r = sp.r;
|
275
|
+
return intersectSegmentCapsule(seg, cap, t);
|
276
|
+
}
|
277
|
+
|
278
|
+
|
279
|
+
/// �Żߤ��Ƥ���aabbB���Ф���ư���Ƥ���aabbA�θ�
|
280
|
+
bool
|
281
|
+
Collision::intersectMovingAABBAABB(Shape* aabbA, Vector3D<float> vel, Shape* aabbB, float *t)
|
282
|
+
{
|
283
|
+
if (debug)
|
284
|
+
cout << "Debug: intersectMovingAABBAABB" << endl;
|
285
|
+
|
286
|
+
AABB* aBox;
|
287
|
+
assert(aBox = dynamic_cast<AABB*>(aabbA));
|
288
|
+
AABB boxA = *aBox;
|
289
|
+
assert(aBox = dynamic_cast<AABB*>(aabbB));
|
290
|
+
AABB boxB = *aBox;
|
291
|
+
|
292
|
+
/*
|
293
|
+
* ��ͤλ���ˤ�ꡢ®��ȿž
|
294
|
+
* aabbB�ǤϤʤ���aabbA��ߤ��
|
295
|
+
*/
|
296
|
+
Vector3D<float> v = -vel;
|
297
|
+
|
298
|
+
float tfirst, tlast;
|
299
|
+
|
300
|
+
// �ǽ�λ�����'a'�����'b'���ŤʤäƤ����硤����˽�λ
|
301
|
+
if (testAABBAABB(boxA, boxB)) {
|
302
|
+
*t = tfirst = tlast = 0.0f;
|
303
|
+
return true;
|
304
|
+
}
|
305
|
+
|
306
|
+
// �ǽ�λ����ǽŤʤä�̵���ơ�®�٤�0�ʤ齪λ
|
307
|
+
if (v.x == 0.0f && v.y == 0.0f && v.z == 0.0f) {
|
308
|
+
if (debug)
|
309
|
+
cout << "1" << endl;
|
310
|
+
return false;
|
311
|
+
}
|
312
|
+
|
313
|
+
// �ǽ餪��ӺǸ���ܿ����֤�����
|
314
|
+
tfirst = 0.0f;
|
315
|
+
tlast = 1.0f;
|
316
|
+
|
317
|
+
// �Ƽ����Ф��ơ��ǽ餪��ӺǸ���ܿ����֤⤷����з��ꤹ��
|
318
|
+
// X
|
319
|
+
if (v.x == 0.0f)
|
320
|
+
if (boxA.max.x < boxB.min.x || boxA.min.x > boxB.max.x) {
|
321
|
+
if (debug) cout << "2" << endl;
|
322
|
+
return false;
|
323
|
+
}
|
324
|
+
|
325
|
+
if (v.x < 0.0f) {
|
326
|
+
if (boxB.max.x < boxA.min.x) {
|
327
|
+
if (debug) cout << "3" << endl;
|
328
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
329
|
+
}
|
330
|
+
if (boxA.max.x < boxB.min.x) tfirst = Max((boxA.max.x - boxB.min.x) / v.x, tfirst);
|
331
|
+
if (boxB.max.x > boxA.min.x) tlast = Min((boxA.min.x - boxB.max.x) / v.x, tlast);
|
332
|
+
}
|
333
|
+
if (v.x > 0.0f) {
|
334
|
+
if (boxA.max.x < boxB.min.x) {
|
335
|
+
if(debug) cout << "4" << endl;
|
336
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
337
|
+
}
|
338
|
+
if (boxB.max.x < boxA.min.x) tfirst = Max((boxA.min.x - boxB.max.x) / v.x, tfirst);
|
339
|
+
if (boxA.max.x > boxB.min.x) tlast = Min((boxA.max.x - boxB.min.x) / v.x, tlast);
|
340
|
+
}
|
341
|
+
// �ǽ���ܿ����Ǹ���ܿ��θ��ȯ��������ϡ��Ϥ������ʤ�
|
342
|
+
if (tfirst > tlast){
|
343
|
+
if (debug) cout << "4.5" << endl;
|
344
|
+
return false;
|
345
|
+
}
|
346
|
+
|
347
|
+
|
348
|
+
// Y
|
349
|
+
if (v.y == 0.0f)
|
350
|
+
if (boxA.max.y < boxB.min.y || boxA.min.y > boxB.max.y) {
|
351
|
+
if (debug) cout << "5" << endl;
|
352
|
+
return false;
|
353
|
+
}
|
354
|
+
if (v.y < 0.0f) {
|
355
|
+
if (boxB.max.y < boxA.min.y){
|
356
|
+
if (debug) cout << "6" << endl;
|
357
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
358
|
+
}
|
359
|
+
if (boxA.max.y < boxB.min.y) tfirst = Max((boxA.max.y - boxB.min.y) / v.y, tfirst);
|
360
|
+
if (boxB.max.y > boxA.min.y) tlast = Min((boxA.min.y - boxB.max.y) / v.y, tlast);
|
361
|
+
}
|
362
|
+
if (v.y > 0.0f) {
|
363
|
+
if (boxA.max.y < boxB.min.y){
|
364
|
+
if (debug) cout << "7" << endl;
|
365
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
366
|
+
}
|
367
|
+
if (boxB.max.y < boxA.min.y) tfirst = Max((boxA.min.y - boxB.max.y) / v.y, tfirst);
|
368
|
+
if (boxA.max.y > boxB.min.y) tlast = Min((boxA.max.y - boxB.min.y) / v.y, tlast);
|
369
|
+
}
|
370
|
+
// �ǽ���ܿ����Ǹ���ܿ��θ��ȯ��������ϡ��Ϥ������ʤ�
|
371
|
+
if (tfirst > tlast) {
|
372
|
+
if (debug) cout << "7.5" << endl;
|
373
|
+
return false;
|
374
|
+
}
|
375
|
+
|
376
|
+
|
377
|
+
// Z
|
378
|
+
if (v.z == 0.0f)
|
379
|
+
if (boxA.max.z < boxB.min.z || boxA.min.z > boxB.max.z) {
|
380
|
+
if(debug) cout << "8" << endl;
|
381
|
+
return false;
|
382
|
+
}
|
383
|
+
if (v.z < 0.0f) {
|
384
|
+
if (boxB.max.z < boxA.min.z) {
|
385
|
+
if (debug) cout << "9" << endl;
|
386
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
387
|
+
}
|
388
|
+
if (boxA.max.z < boxB.min.z) tfirst = Max((boxA.max.z - boxB.min.z) / v.z, tfirst);
|
389
|
+
if (boxB.max.z > boxA.min.z) tlast = Min((boxA.min.z - boxB.max.z) / v.z, tlast);
|
390
|
+
}
|
391
|
+
if (v.z > 0.0f) {
|
392
|
+
if (boxA.max.z < boxB.min.z){
|
393
|
+
if (debug) cout << "10" << endl;
|
394
|
+
return false; // �Ϥʤ�Υ��Ʊ�ư���Ƥ���
|
395
|
+
}
|
396
|
+
if (boxB.max.z < boxA.min.z) tfirst = Max((boxA.min.z - boxB.max.z) / v.z, tfirst);
|
397
|
+
if (boxA.max.z > boxB.min.z) tlast = Min((boxA.max.z - boxB.min.z) / v.z, tlast);
|
398
|
+
}
|
399
|
+
// �ǽ���ܿ����Ǹ���ܿ��θ��ȯ��������ϡ��Ϥ������ʤ�
|
400
|
+
if (tfirst > tlast){
|
401
|
+
if (debug) cout << "10.5" << endl;
|
402
|
+
return false;
|
403
|
+
}
|
404
|
+
|
405
|
+
*t = tfirst;
|
406
|
+
return true;
|
407
|
+
|
408
|
+
}
|
409
|
+
|
410
|
+
bool
|
411
|
+
Collision::intersectMovingAABBPlane(Shape* aabb, Vector3D<float> vel, Shape* plane, float *t)
|
412
|
+
{
|
413
|
+
AABB box = *(dynamic_cast<AABB*>(aabb));
|
414
|
+
Plane pl = *(dynamic_cast<Plane*>(plane));
|
415
|
+
|
416
|
+
float e0 = (box.max.x - box.min.x) / 2;
|
417
|
+
float e1 = (box.max.y - box.min.y) / 2;
|
418
|
+
float e2 = (box.max.z - box.min.z) / 2;
|
419
|
+
|
420
|
+
float r = fabs(e0 * pl.n.x + e1 * pl.n.y + e2 * pl.n.z);
|
421
|
+
Sphere sp(box.getCenter(), r);
|
422
|
+
|
423
|
+
return intersectMovingSpherePlane(&sp, vel, &pl, t);
|
424
|
+
}
|
425
|
+
|
426
|
+
/// Plane vs Plane
|
427
|
+
/**
|
428
|
+
* ʿ�ʳ����ͤ��Ƥ���
|
429
|
+
*/
|
430
|
+
bool
|
431
|
+
Collision::intersectMovingPlanePlane(Shape* planeA, Vector3D<float> vel, Shape* planeB, float *t)
|
432
|
+
{
|
433
|
+
// �̤˻Ȥ��Ĥ��ʤ��Τǡ���˾��ͤ��Ƥ��뤳�Ȥ�
|
434
|
+
*t = 0;
|
435
|
+
return true;
|
436
|
+
}
|
437
|
+
|
438
|
+
/**
|
439
|
+
* Helper function
|
440
|
+
*/
|
441
|
+
|
442
|
+
// [min, max]���ϰ���ޤ�n����
|
443
|
+
inline float
|
444
|
+
Clamp(float n, float min, float max)
|
445
|
+
{
|
446
|
+
if (n < min) return min;
|
447
|
+
if (n > max) return max;
|
448
|
+
return n;
|
449
|
+
}
|
450
|
+
|
451
|
+
inline void
|
452
|
+
Swap(float *a, float *b)
|
453
|
+
{
|
454
|
+
float tmp;
|
455
|
+
|
456
|
+
tmp = *a;
|
457
|
+
*a = *b;
|
458
|
+
*b = tmp;
|
459
|
+
}
|
460
|
+
|
461
|
+
|
462
|
+
bool
|
463
|
+
Collision::testAABBAABB(AABB a, AABB b)
|
464
|
+
{
|
465
|
+
if (a.max.x < b.min.x || a.min.x > b.max.x) return false;
|
466
|
+
if (a.max.y < b.min.y || a.min.y > b.max.y) return false;
|
467
|
+
if (a.max.z < b.min.z || a.min.z > b.max.z) return false;
|
468
|
+
|
469
|
+
return true;
|
470
|
+
}
|
471
|
+
|
472
|
+
void
|
473
|
+
Collision::closestPtSegment(Segment sg, Vector3D<float> aPos, Vector3D<float> *aXPos)
|
474
|
+
{
|
475
|
+
Vector3D<float> d = sg.b - sg.a;
|
476
|
+
|
477
|
+
float t = (aPos - sg.a).dot(d) / d.dot(d);
|
478
|
+
|
479
|
+
if (t < 0.0) t = 0.0;
|
480
|
+
if (t > 1.0) t = 1.0;
|
481
|
+
|
482
|
+
*aXPos = sg.a + d * t;
|
483
|
+
}
|
484
|
+
|
485
|
+
/**
|
486
|
+
* S1(s)=a1+s*(b1-a1)�����S2(t)=a2+t*(b2-a2)��
|
487
|
+
* �Ƕ�����C1�����C2�����S�����t���֤���
|
488
|
+
* �ؿ��η�̤�S1(s)��S2(t)�δ֤ε�Υ��ʿ��
|
489
|
+
*/
|
490
|
+
float
|
491
|
+
Collision::closestPtSegmentSegment(Segment sg1, Segment sg2,
|
492
|
+
float *s, float *t, Vector3D<float> *c1, Vector3D<float> *c2)
|
493
|
+
{
|
494
|
+
Vector3D<float> d1 = sg1.b - sg1.a; // ��ʬS1�������٥��ȥ�
|
495
|
+
Vector3D<float> d2 = sg2.b - sg2.a; // ��ʬS2�������٥��ȥ�
|
496
|
+
Vector3D<float> r = sg1.a - sg2.a;
|
497
|
+
float a = d1.dot(d1); // ��ʬS1�ε�Υ��ʿ�����������
|
498
|
+
float e = d2.dot(d2); // ��ʬS2�ε�Υ��ʿ�����������
|
499
|
+
float f = d2.dot(r);
|
500
|
+
|
501
|
+
// �������뤤��ξ������ʬ�����˽��ष�Ƥ��뤫�ɤ��������å�
|
502
|
+
if (a <= EPSILON && e <= EPSILON) {
|
503
|
+
// ξ������ʬ�����˽���
|
504
|
+
*s = *t = 0.0f;
|
505
|
+
*c1 = sg1.a;
|
506
|
+
*c2 = sg2.a;
|
507
|
+
return (*c1 - *c2).dot(*c1 - *c2);
|
508
|
+
}
|
509
|
+
if (a <= EPSILON) {
|
510
|
+
// �ǽ����ʬ�����˽���
|
511
|
+
*s = 0.0f;
|
512
|
+
*t = f / e; // s = 0 => t = (b*s + f) / e = f / e
|
513
|
+
*t = Clamp(*t, 0.0f, 1.0f);
|
514
|
+
}
|
515
|
+
else {
|
516
|
+
float c = d1.dot(r);
|
517
|
+
|
518
|
+
if (e <= EPSILON) {
|
519
|
+
// 2���ܤ���ʬ�����˽���
|
520
|
+
*t = 0.0f;
|
521
|
+
*s = Clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
|
522
|
+
}
|
523
|
+
else {
|
524
|
+
// �����������Ū�ʽ���ξ���
|
525
|
+
float b = d1.dot(d2);
|
526
|
+
float denom = a*e-b*b; // �������
|
527
|
+
|
528
|
+
// ��ʬ��ʿ�ԤǤʤ���硢L1���L2���Ф���Ƕ����������������
|
529
|
+
// ��ʬS1���Ф��ƥ����ס������Ǥʤ�����Ǥ��s(�����Ǥ�0)������
|
530
|
+
if (denom != 0.0f) {
|
531
|
+
*s = Clamp((b*f - c*e) / denom, 0.0f, 1.0f);
|
532
|
+
}
|
533
|
+
else
|
534
|
+
*s = 0.0f;
|
535
|
+
|
536
|
+
// L2���S1(s)���Ф���Ƕ�������ʲ����Ѥ��Ʒ�
|
537
|
+
// t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
|
538
|
+
*t = (b * (*s) + f) / e;
|
539
|
+
|
540
|
+
// t��[0,1]����ˤ���н�λ�������Ǥʤ����t���ס�s��t�ο������ͤ��Ф��ưʲ����Ѥ��ƺƷ�
|
541
|
+
// s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
|
542
|
+
// ������s��[0, 1]���Ф��ƥ�����
|
543
|
+
if (*t < 0.0f) {
|
544
|
+
*t = 0.0f;
|
545
|
+
*s = Clamp(-c / a, 0.0f, 1.0f);
|
546
|
+
}
|
547
|
+
else if (*t > 1.0f) {
|
548
|
+
*t = 1.0f;
|
549
|
+
*s = Clamp((b - c) / a, 0.0f, 1.0f);
|
550
|
+
}
|
551
|
+
}
|
552
|
+
}
|
553
|
+
|
554
|
+
*c1 = sg1.a + d1 * (*s);
|
555
|
+
*c2 = sg2.a + d2 * (*t);
|
556
|
+
|
557
|
+
return (*c1 - *c2).dot(*c1 - *c2);
|
558
|
+
}
|
559
|
+
|
560
|
+
// ����R(t) = p + t * dir��Plane a���Ф���ɬ�����Ƥ��롣
|
561
|
+
// ����t����Ӹ��Ƥ�����*colP���֤�
|
562
|
+
bool
|
563
|
+
Collision::intersectRayPlane(Vector3D<float> p, Vector3D<float> dir, Plane pl,
|
564
|
+
float *t, Vector3D<float> *colP)
|
565
|
+
{
|
566
|
+
*t = (pl.d - (pl.n).dot(p)) / (pl.n).dot(dir);
|
567
|
+
*colP = p + dir * (*t);
|
568
|
+
|
569
|
+
return true;
|
570
|
+
}
|
571
|
+
|
572
|
+
|
573
|
+
// ����R(t) = p + t*d��AABB a���Ф��Ƹ��Ƥ��뤫�ɤ��������Ƥ������
|
574
|
+
// ����t����Ӹ��Ƥ�����*colP���֤�
|
575
|
+
bool
|
576
|
+
Collision::intersectRayAABB(Vector3D<float> p, Vector3D<float> dir, AABB box,
|
577
|
+
float *t, Vector3D<float> *colP)
|
578
|
+
{
|
579
|
+
*t = 0.0f; // -FLT_MAX�����ꤷ��ľ���ˤ�����ǽ�θ�����
|
580
|
+
float tmax = 10000.0f; // (��ʬ���Ф���)��������ư���뤳�ȤΤǤ������ε�Υ������
|
581
|
+
|
582
|
+
// 3�ĤΤ��٤ƥ���֤��Ф���
|
583
|
+
// x
|
584
|
+
if (fabsf(dir.x) < EPSILON) {
|
585
|
+
// �����ϥ���֤��Ф���ʿ�ԡ�����������֤���ˤʤ���иʤ�
|
586
|
+
if (p.x < box.min.x || p.x > box.max.x)
|
587
|
+
return false;
|
588
|
+
}
|
589
|
+
else {
|
590
|
+
// ����֤ζᤤʿ�̤���ӱ�ʿ�̤ȸ��������t���ͤ��
|
591
|
+
float ood = 1.0f / dir.x;
|
592
|
+
float t1 = (box.min.x - p.x) * ood;
|
593
|
+
float t2 = (box.max.x - p.x) * ood;
|
594
|
+
// t1���ᤤʿ�̤Ȥθ�t2����ʿ�̤ȤθȤʤ�
|
595
|
+
if (t1 > t2) Swap(&t1, &t2);
|
596
|
+
// ����֤θ��Ƥ���ֳ֤Ȥθ��
|
597
|
+
if (t1 > *t) *t = t1;
|
598
|
+
if (t2 < tmax) tmax = t2;
|
599
|
+
// ����֤˸��ʤ����Ȥ�ʬ����о��ͤϤʤ��ΤǤ����˽�λ
|
600
|
+
if (*t > tmax)
|
601
|
+
return false;
|
602
|
+
}
|
603
|
+
|
604
|
+
// y
|
605
|
+
if (fabsf(dir.y) < EPSILON) {
|
606
|
+
// �����ϥ���֤��Ф���ʿ�ԡ�����������֤���ˤʤ���иʤ�
|
607
|
+
if (p.y < box.min.y || p.y > box.max.y)
|
608
|
+
return false;
|
609
|
+
}
|
610
|
+
else {
|
611
|
+
// ����֤ζᤤʿ�̤���ӱ�ʿ�̤ȸ��������t���ͤ��
|
612
|
+
float ood = 1.0f / dir.y;
|
613
|
+
float t1 = (box.min.y - p.y) * ood;
|
614
|
+
float t2 = (box.max.y - p.y) * ood;
|
615
|
+
// t1���ᤤʿ�̤Ȥθ�t2����ʿ�̤ȤθȤʤ�
|
616
|
+
if (t1 > t2) Swap(&t1, &t2);
|
617
|
+
// ����֤θ��Ƥ���ֳ֤Ȥθ��
|
618
|
+
if (t1 > *t) *t = t1;
|
619
|
+
if (t2 < tmax) tmax = t2;
|
620
|
+
// ����֤˸��ʤ����Ȥ�ʬ����о��ͤϤʤ��ΤǤ����˽�λ
|
621
|
+
if (*t > tmax)
|
622
|
+
return false;
|
623
|
+
}
|
624
|
+
|
625
|
+
// z
|
626
|
+
if (fabsf(dir.z) < EPSILON) {
|
627
|
+
// �����ϥ���֤��Ф���ʿ�ԡ�����������֤���ˤʤ���иʤ�
|
628
|
+
if (p.z < box.min.z || p.z > box.max.z)
|
629
|
+
return false;
|
630
|
+
}
|
631
|
+
else {
|
632
|
+
// ����֤ζᤤʿ�̤���ӱ�ʿ�̤ȸ��������t���ͤ��
|
633
|
+
float ood = 1.0f / dir.z;
|
634
|
+
float t1 = (box.min.z - p.z) * ood;
|
635
|
+
float t2 = (box.max.z - p.z) * ood;
|
636
|
+
// t1���ᤤʿ�̤Ȥθ�t2����ʿ�̤ȤθȤʤ�
|
637
|
+
if (t1 > t2) Swap(&t1, &t2);
|
638
|
+
// ����֤θ��Ƥ���ֳ֤Ȥθ��
|
639
|
+
if (t1 > *t) *t = t1;
|
640
|
+
if (t2 < tmax) tmax = t2;
|
641
|
+
// ����֤˸��ʤ����Ȥ�ʬ����о��ͤϤʤ��ΤǤ����˽�λ
|
642
|
+
if (*t > tmax)
|
643
|
+
return false;
|
644
|
+
}
|
645
|
+
|
646
|
+
// ������3�ĤΥ���֤��٤Ƥ˸��Ƥ��롣��(q)�ȸ�t����(tmin)���֤�
|
647
|
+
*colP = p + dir * (*t);
|
648
|
+
|
649
|
+
return true;
|
650
|
+
}
|
651
|
+
|
652
|
+
// ����R(t) = p + t*dir��Sphere sp���Ф��Ƹ��Ƥ��뤫�ɤ��������Ƥ������
|
653
|
+
// �ε�Υt(*dist)����Ӹ��Ƥ�����*colP���֤�
|
654
|
+
// dir������������
|
655
|
+
bool
|
656
|
+
Collision::intersectRaySphere(Vector3D<float> p, Vector3D<float> dir, Sphere sp,
|
657
|
+
float *t, Vector3D<float> *colP)
|
658
|
+
{
|
659
|
+
Vector3D<float> m = p - sp.c;
|
660
|
+
float b = m.dot(dir);
|
661
|
+
float c = m.dot(m) - sp.r * sp.r;
|
662
|
+
|
663
|
+
// r�θ�����s�γ�¦�ˤ���(c > 0)��r��s����Υ��Ƥ���������ؤ��Ƥ�����(b > 0)�˽�λ
|
664
|
+
if (c > 0.0f && b > 0.0f)
|
665
|
+
return false;
|
666
|
+
|
667
|
+
float discr = b * b - c ;
|
668
|
+
// ���Ƚ�̼��ϸ��������Ƥ��뤳�Ȥ˰���
|
669
|
+
if (discr < 0.0f)
|
670
|
+
return false;
|
671
|
+
|
672
|
+
// ����Ǹ����ϵ�ȸ��Ƥ��뤳�Ȥ�ʬ���ꡢ����Ǿ�����t���
|
673
|
+
*t = -b - sqrtf(discr);
|
674
|
+
|
675
|
+
// t����Ǥ����硢�����ϵ����¦���鳫�Ϥ��Ƥ���Τ�t���˥�����
|
676
|
+
if (*t < 0.0f)
|
677
|
+
*t = 0.0f;
|
678
|
+
|
679
|
+
*colP = p + dir * (*t);
|
680
|
+
|
681
|
+
return true;
|
682
|
+
}
|
683
|
+
|
684
|
+
|
685
|
+
/**
|
686
|
+
* ��ʬS(t)=sa+t(sb-sa), 0<=t<=1�Ρ�����Ф����
|
687
|
+
*
|
688
|
+
*/
|
689
|
+
bool
|
690
|
+
Collision::intersectSegmentSphere(Segment seg, Sphere sp, float *t)
|
691
|
+
{
|
692
|
+
Vector3D<float> d = seg.b - seg.a;
|
693
|
+
Vector3D<float> dir = d;
|
694
|
+
dir.normalize();
|
695
|
+
|
696
|
+
Vector3D<float> colP;
|
697
|
+
if (intersectRaySphere(seg.a, dir, sp, t, &colP)) {
|
698
|
+
*t = (*t) / d.len(); // dir������������Ƥ���Τǡ�(*t)�ϵ�Υ
|
699
|
+
if (*t <= 1)
|
700
|
+
return true;
|
701
|
+
else
|
702
|
+
return false;
|
703
|
+
}
|
704
|
+
else
|
705
|
+
return false;
|
706
|
+
}
|
707
|
+
|
708
|
+
/**
|
709
|
+
* ��ʬS(t)=sa+t(sb-sa), 0<=t<=1�Ρ����ץ�����Ф����
|
710
|
+
*/
|
711
|
+
bool
|
712
|
+
Collision::intersectSegmentCapsule(Segment seg, Capsule cap, float *t)
|
713
|
+
{
|
714
|
+
Vector3D<float> d = cap.seg.b - cap.seg.a;
|
715
|
+
Vector3D<float> m = seg.a - cap.seg.a;
|
716
|
+
Vector3D<float> n = seg.b - seg.a;
|
717
|
+
float md = m.dot(d);
|
718
|
+
float nd = n.dot(d);
|
719
|
+
float dd = d.dot(d);
|
720
|
+
|
721
|
+
// ��ʬ���Τ��ɤ��餫�α�������̤��Ф��ƴ����˳�¦�ˤ��뤫�ɤ�����Ƚ��
|
722
|
+
if (md < 0.0f && md + nd < 0.0f) {
|
723
|
+
// ��ʬ�������'a'��¦�γ�¦�ˤ���
|
724
|
+
|
725
|
+
// ���ץ���ʤΤ�
|
726
|
+
Sphere sp(cap.seg.a, cap.r);
|
727
|
+
if(intersectSegmentSphere(seg, sp, t))
|
728
|
+
return true;
|
729
|
+
return false;
|
730
|
+
}
|
731
|
+
if (md > dd && md + nd > dd) {
|
732
|
+
// ��ʬ�������'b'��¦�γ�¦�ˤ���
|
733
|
+
|
734
|
+
// ���ץ���ʤΤ�
|
735
|
+
Sphere sp(cap.seg.b, cap.r);
|
736
|
+
if(intersectSegmentSphere(seg, sp, t))
|
737
|
+
return true;
|
738
|
+
return false;
|
739
|
+
}
|
740
|
+
|
741
|
+
// ��ʬ�Τɤ��餫��ü�⤷����ξ���α������̶����ˤ���
|
742
|
+
float nn = n.dot(n);
|
743
|
+
float mn = m.dot(n);
|
744
|
+
float a = dd * nn - nd * nd;
|
745
|
+
float k = m.dot(m) - cap.r * cap.r;
|
746
|
+
float c = dd * k - md * md;
|
747
|
+
// ��ʬ������μ����Ф���ʿ�Ԥ����äƤ��뤫�ɤ�����Ƚ��
|
748
|
+
if (fabsf(a) < EPSILON) {
|
749
|
+
if (c > 0.0f) // 'a'�������ʬ�ϱ���γ�¦�ˤ���
|
750
|
+
return false;
|
751
|
+
|
752
|
+
// �������ʬ������ȸ��Ƥ��뤳�Ȥ�ʬ���ä��Τǡ��ɤΤ褦�˸��Ƥ��뤫��Ĵ�٤�
|
753
|
+
if (md < 0.0f){
|
754
|
+
// ��ʬ��'a'��¦�����̤ȸ��Ƥ���
|
755
|
+
// ���ץ���ʤΤ�
|
756
|
+
Sphere sp(cap.seg.a, cap.r);
|
757
|
+
intersectSegmentSphere(seg, sp, t);
|
758
|
+
}
|
759
|
+
else if (md > dd){
|
760
|
+
// ��ʬ��'b'��¦�����̤ȸ��Ƥ���
|
761
|
+
// ���ץ���ʤΤ�
|
762
|
+
Sphere sp(cap.seg.b, cap.r);
|
763
|
+
intersectSegmentSphere(seg, sp, t);
|
764
|
+
}
|
765
|
+
else {
|
766
|
+
// 'a'�ϱ������¦�ˤ���
|
767
|
+
*t = 0.0f;
|
768
|
+
}
|
769
|
+
return true;
|
770
|
+
}
|
771
|
+
|
772
|
+
float b = dd * mn - nd * md;
|
773
|
+
float discr = b * b - a * c;
|
774
|
+
if (discr < 0.0f)
|
775
|
+
return false; // �¿��ʤ��ΤǸϤʤ�
|
776
|
+
|
777
|
+
*t = (-b - sqrtf(discr)) / a;
|
778
|
+
if (*t < 0.0f || *t > 1.0f)
|
779
|
+
return false; // ����ʬ�γ�¦�ˤ���
|
780
|
+
|
781
|
+
if (md + (*t) * nd < 0.0f) {
|
782
|
+
// �����'a'��¦�γ�¦�Ǹ�
|
783
|
+
// ���ץ���ʤΤ�
|
784
|
+
Sphere sp(cap.seg.a, cap.r);
|
785
|
+
return intersectSegmentSphere(seg, sp, t);
|
786
|
+
}
|
787
|
+
else if (md + (*t) * nd > dd) {
|
788
|
+
// �����'q'��¦�γ�¦�Ǹ�
|
789
|
+
// ���ץ���ʤΤ�
|
790
|
+
Sphere sp(cap.seg.b, cap.r);
|
791
|
+
return intersectSegmentSphere(seg, sp, t);
|
792
|
+
}
|
793
|
+
// ��ʬ�����̤δ֤δ֤Ǹ��Ƥ���Τǡ�t��������
|
794
|
+
return true;
|
795
|
+
}
|
796
|
+
|
797
|
+
}
|