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
|
// Ripped from Magic Software
#include "Include\dRay.h"
#include "dxRay.h"
bool Clip(dReal Denom, dReal Numer, dReal& T0, dReal& T1){
// Return value is 'true' if line segment intersects the current test
// plane. Otherwise 'false' is returned in which case the line segment
// is entirely clipped.
if (Denom > REAL(0.0)){
if (Numer > Denom * T1){
return false;
}
if (Numer > Denom * T0){
T0 = Numer / Denom;
}
return true;
}
else if (Denom < REAL(0.0)){
if (Numer > Denom * T0){
return false;
}
if (Numer > Denom * T1){
T1 = Numer / Denom;
}
return true;
}
else return Numer <= REAL(0.0);
}
bool FindIntersection(const dVector3 Origin, const dVector3 Direction, const dVector3 Extents, dReal& T0, dReal& T1){
dReal SaveT0 = T0;
dReal SaveT1 = T1;
bool NotEntirelyClipped =
Clip(+Direction[0], -Origin[0] - Extents[0], T0, T1) &&
Clip(-Direction[0], +Origin[0] - Extents[0], T0, T1) &&
Clip(+Direction[1], -Origin[1] - Extents[1], T0, T1) &&
Clip(-Direction[1], +Origin[1] - Extents[1], T0, T1) &&
Clip(+Direction[2], -Origin[2] - Extents[2], T0, T1) &&
Clip(-Direction[2], +Origin[2] - Extents[2], T0, T1);
return NotEntirelyClipped && (T0 != SaveT0 || T1 != SaveT1);
}
int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
dVector3 Extents;
dGeomBoxGetLengths(BoxGeom, Extents);
Extents[0] /= 2;
Extents[1] /= 2;
Extents[2] /= 2;
Extents[3] /= 2;
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dVector3 Rot[3];
Decompose(Rotation, Rot);
dVector3 TransOrigin;
TransOrigin[0] = dDOT(Diff, Rot[0]);
TransOrigin[1] = dDOT(Diff, Rot[1]);
TransOrigin[2] = dDOT(Diff, Rot[2]);
TransOrigin[3] = REAL(0.0);
dVector3 TransDirection;
TransDirection[0] = dDOT(Direction, Rot[0]);
TransDirection[1] = dDOT(Direction, Rot[1]);
TransDirection[2] = dDOT(Direction, Rot[2]);
TransDirection[3] = REAL(0.0);
dReal T[2];
T[0] = 0.0f;
T[1] = dInfinity;
bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
if (Intersect){
if (T[0] > REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = BoxGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = BoxGeom;
return 2;
}
else{
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = BoxGeom;
return 1;
}
}
else return 0;
}
|
