1 | // Ripped from Magic Software |
---|
2 | |
---|
3 | #include "Include\dRay.h" |
---|
4 | #include "dxRay.h" |
---|
5 | |
---|
6 | bool Clip(dReal Denom, dReal Numer, dReal& T0, dReal& T1){ |
---|
7 | // Return value is 'true' if line segment intersects the current test |
---|
8 | // plane. Otherwise 'false' is returned in which case the line segment |
---|
9 | // is entirely clipped. |
---|
10 | |
---|
11 | if (Denom > REAL(0.0)){ |
---|
12 | if (Numer > Denom * T1){ |
---|
13 | return false; |
---|
14 | } |
---|
15 | |
---|
16 | if (Numer > Denom * T0){ |
---|
17 | T0 = Numer / Denom; |
---|
18 | } |
---|
19 | return true; |
---|
20 | } |
---|
21 | else if (Denom < REAL(0.0)){ |
---|
22 | if (Numer > Denom * T0){ |
---|
23 | return false; |
---|
24 | } |
---|
25 | |
---|
26 | if (Numer > Denom * T1){ |
---|
27 | T1 = Numer / Denom; |
---|
28 | } |
---|
29 | return true; |
---|
30 | } |
---|
31 | else return Numer <= REAL(0.0); |
---|
32 | } |
---|
33 | |
---|
34 | bool FindIntersection(const dVector3 Origin, const dVector3 Direction, const dVector3 Extents, dReal& T0, dReal& T1){ |
---|
35 | dReal SaveT0 = T0; |
---|
36 | dReal SaveT1 = T1; |
---|
37 | |
---|
38 | bool NotEntirelyClipped = |
---|
39 | Clip(+Direction[0], -Origin[0] - Extents[0], T0, T1) && |
---|
40 | Clip(-Direction[0], +Origin[0] - Extents[0], T0, T1) && |
---|
41 | Clip(+Direction[1], -Origin[1] - Extents[1], T0, T1) && |
---|
42 | Clip(-Direction[1], +Origin[1] - Extents[1], T0, T1) && |
---|
43 | Clip(+Direction[2], -Origin[2] - Extents[2], T0, T1) && |
---|
44 | Clip(-Direction[2], +Origin[2] - Extents[2], T0, T1); |
---|
45 | |
---|
46 | return NotEntirelyClipped && (T0 != SaveT0 || T1 != SaveT1); |
---|
47 | } |
---|
48 | |
---|
49 | int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ |
---|
50 | const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom); |
---|
51 | const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom); |
---|
52 | dVector3 Extents; |
---|
53 | dGeomBoxGetLengths(BoxGeom, Extents); |
---|
54 | Extents[0] /= 2; |
---|
55 | Extents[1] /= 2; |
---|
56 | Extents[2] /= 2; |
---|
57 | Extents[3] /= 2; |
---|
58 | |
---|
59 | dVector3 Origin, Direction; |
---|
60 | dGeomRayGet(RayGeom, Origin, Direction); |
---|
61 | dReal Length = dGeomRayGetLength(RayGeom); |
---|
62 | |
---|
63 | dVector3 Diff; |
---|
64 | Diff[0] = Origin[0] - Position[0]; |
---|
65 | Diff[1] = Origin[1] - Position[1]; |
---|
66 | Diff[2] = Origin[2] - Position[2]; |
---|
67 | Diff[3] = Origin[3] - Position[3]; |
---|
68 | |
---|
69 | Direction[0] *= Length; |
---|
70 | Direction[1] *= Length; |
---|
71 | Direction[2] *= Length; |
---|
72 | Direction[3] *= Length; |
---|
73 | |
---|
74 | dVector3 Rot[3]; |
---|
75 | Decompose(Rotation, Rot); |
---|
76 | |
---|
77 | dVector3 TransOrigin; |
---|
78 | TransOrigin[0] = dDOT(Diff, Rot[0]); |
---|
79 | TransOrigin[1] = dDOT(Diff, Rot[1]); |
---|
80 | TransOrigin[2] = dDOT(Diff, Rot[2]); |
---|
81 | TransOrigin[3] = REAL(0.0); |
---|
82 | |
---|
83 | dVector3 TransDirection; |
---|
84 | TransDirection[0] = dDOT(Direction, Rot[0]); |
---|
85 | TransDirection[1] = dDOT(Direction, Rot[1]); |
---|
86 | TransDirection[2] = dDOT(Direction, Rot[2]); |
---|
87 | TransDirection[3] = REAL(0.0); |
---|
88 | |
---|
89 | dReal T[2]; |
---|
90 | T[0] = 0.0f; |
---|
91 | T[1] = dInfinity; |
---|
92 | |
---|
93 | bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]); |
---|
94 | |
---|
95 | if (Intersect){ |
---|
96 | if (T[0] > REAL(0.0)){ |
---|
97 | dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride); |
---|
98 | Contact0->pos[0] = Origin[0] + T[0] * Direction[0]; |
---|
99 | Contact0->pos[1] = Origin[1] + T[0] * Direction[1]; |
---|
100 | Contact0->pos[2] = Origin[2] + T[0] * Direction[2]; |
---|
101 | Contact0->pos[3] = Origin[3] + T[0] * Direction[3]; |
---|
102 | //Contact0->normal = 0; |
---|
103 | Contact0->depth = 0.0f; |
---|
104 | Contact0->g1 = RayGeom; |
---|
105 | Contact0->g2 = BoxGeom; |
---|
106 | |
---|
107 | dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride); |
---|
108 | Contact1->pos[0] = Origin[0] + T[1] * Direction[0]; |
---|
109 | Contact1->pos[1] = Origin[1] + T[1] * Direction[1]; |
---|
110 | Contact1->pos[2] = Origin[2] + T[1] * Direction[2]; |
---|
111 | Contact1->pos[3] = Origin[3] + T[1] * Direction[3]; |
---|
112 | //Contact1->normal = 0; |
---|
113 | Contact1->depth = 0.0f; |
---|
114 | Contact1->g1 = RayGeom; |
---|
115 | Contact1->g2 = BoxGeom; |
---|
116 | |
---|
117 | return 2; |
---|
118 | } |
---|
119 | else{ |
---|
120 | dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride); |
---|
121 | Contact->pos[0] = Origin[0] + T[1] * Direction[0]; |
---|
122 | Contact->pos[1] = Origin[1] + T[1] * Direction[1]; |
---|
123 | Contact->pos[2] = Origin[2] + T[1] * Direction[2]; |
---|
124 | Contact->pos[3] = Origin[3] + T[1] * Direction[3]; |
---|
125 | //Contact->normal = 0; |
---|
126 | Contact->depth = 0.0f; |
---|
127 | Contact->g1 = RayGeom; |
---|
128 | Contact->g2 = BoxGeom; |
---|
129 | |
---|
130 | return 1; |
---|
131 | } |
---|
132 | } |
---|
133 | else return 0; |
---|
134 | } |
---|