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.
@@ -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
+ }