aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp')
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp199
1 files changed, 0 insertions, 199 deletions
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
deleted file mode 100644
index b9ea0c0..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
+++ /dev/null
@@ -1,199 +0,0 @@
1// Ripped from Magic Software
2
3#include "Include\dRay.h"
4#include "dxRay.h"
5
6int Find(const dVector3 Origin, dVector3 Direction, dReal Length, const dVector3 CCPos, const dMatrix3 CCRot, dReal CCRadius, dReal CCLength, dReal T[2]){
7 dVector3 U, V, W;
8 Decompose(CCRot, U, V, W);
9
10 dVector3 CCOrigin;
11 CCOrigin[0] = CCPos[0] - (W[0] * CCLength / 2);
12 CCOrigin[1] = CCPos[1] - (W[1] * CCLength / 2);
13 CCOrigin[2] = CCPos[2] - (W[2] * CCLength / 2);
14 CCOrigin[3] = CCPos[3] - (W[3] * CCLength / 2);
15
16 dVector3 D;
17 D[0] = dDOT(U, Direction);
18 D[1] = dDOT(V, Direction);
19 D[2] = dDOT(W, Direction);
20
21 dReal DMag = Length;
22 dReal InvDMag = REAL(1.0) / DMag;
23
24 dVector3 Diff;
25 Diff[0] = Origin[0] - CCOrigin[0];
26 Diff[1] = Origin[1] - CCOrigin[1];
27 Diff[2] = Origin[2] - CCOrigin[2];
28 Diff[3] = Origin[3] - CCOrigin[3];
29
30 dVector3 P;
31 P[0] = dDOT(U, Diff);
32 P[1] = dDOT(V, Diff);
33 P[2] = dDOT(W, Diff);
34
35 dReal CCRadiusSq = CCRadius * CCRadius;
36
37 dReal Epsilon = 1e-12f;
38
39 if (dFabs(D[2]) >= REAL(1.0) - Epsilon){ // line is parallel to capsule axis
40 dReal Discr = CCRadiusSq - P[0] * P[0] - P[1] * P[1];
41
42 if (Discr >= REAL(0.0)){
43 dReal Root = dSqrt(Discr);
44 T[0] = (-P[2] + Root) * InvDMag;
45 T[1] = (CCLength - P[2] + Root) * InvDMag;
46 return 2;
47 }
48 else return 0;
49 }
50
51 // test intersection with infinite cylinder
52 dReal A = D[0] * D[0] + D[1] * D[1];
53 dReal B = P[0] * D[0] + P[1] * D[1];
54 dReal C = P[0] * P[0] + P[1] * P[1] - CCRadiusSq;
55 dReal Discr = B * B - A * C;
56 if (Discr < REAL(0.0)){ // line does not intersect infinite cylinder
57 return 0;
58 }
59
60 int Count = 0;
61
62 if (Discr > REAL(0.0)){ // line intersects infinite cylinder in two places
63 dReal Root = dSqrt(Discr);
64 dReal Inv = REAL(1.0) / A;
65
66 dReal TTemp = (-B - Root) * Inv;
67
68 dReal Tmp = P[2] + TTemp * D[2];
69 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
70 T[Count++] = TTemp * InvDMag;
71 }
72
73
74 TTemp = (-B + Root) * Inv;
75 Tmp = P[2] + TTemp * D[2];
76 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
77 T[Count++] = TTemp * InvDMag;
78 }
79
80 if (Count == 2){ // line intersects capsule wall in two places
81 return 2;
82 }
83 }
84 else{ // line is tangent to infinite cylinder
85 dReal TTemp = -B / A;
86 dReal Tmp = P[2] + TTemp * D[2];
87 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
88 T[0] = TTemp * InvDMag;
89 return 1;
90 }
91 }
92
93 // test intersection with bottom hemisphere
94 // fA = 1
95 B += P[2] * D[2];
96 C += P[2] * P[2];
97 Discr = B * B - C;
98 if (Discr > REAL(0.0)){
99 dReal Root = dSqrt(Discr);
100 dReal TTemp = -B - Root;
101 dReal Tmp = P[2] + TTemp * D[2];
102 if (Tmp <= REAL(0.0)){
103 T[Count++] = TTemp * InvDMag;
104 if (Count == 2){
105 return 2;
106 }
107 }
108
109 TTemp = -B + Root;
110 Tmp = P[2] + TTemp * D[2];
111 if (Tmp <= REAL(0.0)){
112 T[Count++] = TTemp * InvDMag;
113 if (Count == 2){
114 return 2;
115 }
116 }
117 }
118 else if (Discr == REAL(0.0)){
119 dReal TTemp = -B;
120 dReal Tmp = P[2] + TTemp * D[2];
121 if (Tmp <= REAL(0.0)){
122 T[Count++] = TTemp * InvDMag;
123 if (Count == 2){
124 return 2;
125 }
126 }
127 }
128
129 // test intersection with top hemisphere
130 // fA = 1
131 B -= D[2] * CCLength;
132 C += CCLength * (CCLength - REAL(2.0) * P[2]);
133
134 Discr = B * B - C;
135 if (Discr > REAL(0.0)){
136 dReal Root = dSqrt(Discr);
137 dReal TTemp = -B - Root;
138 dReal Tmp = P[2] + TTemp * D[2];
139 if (Tmp >= CCLength){
140
141 T[Count++] = TTemp * InvDMag;
142 if (Count == 2){
143 return 2;
144 }
145 }
146
147 TTemp = -B + Root;
148 Tmp = P[2] + TTemp * D[2];
149 if (Tmp >= CCLength){
150 T[Count++] = TTemp * InvDMag;
151 if (Count == 2){
152 return 2;
153 }
154 }
155 }
156 else if (Discr == REAL(0.0)){
157 dReal TTemp = -B;
158 dReal Tmp = P[2] + TTemp * D[2];
159 if (Tmp >= CCLength){
160 T[Count++] = TTemp * InvDMag;
161 if (Count == 2){
162 return 2;
163 }
164 }
165 }
166 return Count;
167}
168
169int dCollideCCR(dxGeom* RayGeom, dxGeom* CCGeom, int Flags, dContactGeom* Contacts, int Stride){
170 const dVector3& CCPos = *(const dVector3*)dGeomGetPosition(CCGeom);
171 const dMatrix3& CCRot = *(const dMatrix3*)dGeomGetRotation(CCGeom);
172
173 dReal CCRadius, CCLength;
174 dGeomCCylinderGetParams(CCGeom, &CCRadius, &CCLength);
175
176 dVector3 Origin, Direction;
177 dGeomRayGet(RayGeom, Origin, Direction);
178 dReal Length = dGeomRayGetLength(RayGeom);
179
180 dReal T[2];
181 int Count = Find(Origin, Direction, Length, CCPos, CCRot, CCRadius, CCLength, T);
182 int ContactCount = 0;
183 for (int i = 0; i < Count; i++){
184 if (T[i] >= 0.0){
185 dContactGeom* Contact = CONTACT(Flags, Contacts, ContactCount, Stride);
186 Contact->pos[0] = Origin[0] + T[i] * Direction[0] * Length;
187 Contact->pos[1] = Origin[1] + T[i] * Direction[1] * Length;
188 Contact->pos[2] = Origin[2] + T[i] * Direction[2] * Length;
189 Contact->pos[3] = Origin[3] + T[i] * Direction[3] * Length;
190 //Contact->normal = 0;
191 Contact->depth = 0.0f;
192 Contact->g1 = RayGeom;
193 Contact->g2 = CCGeom;
194
195 ContactCount++;
196 }
197 }
198 return ContactCount;
199} \ No newline at end of file
generated by cgit v1.1 at 2024-11-11 04:41:51 +0000