1 files changed, 199 insertions, 0 deletions

diff --git a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
new file mode 100644
index 0000000..b9ea0c0
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
@@ -0,0 +1,199 @@
+// Ripped from Magic Software
+
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+int Find(const dVector3 Origin, dVector3 Direction, dReal Length, const dVector3 CCPos, const dMatrix3 CCRot, dReal CCRadius, dReal CCLength, dReal T[2]){
+        dVector3 U, V, W;
+        Decompose(CCRot, U, V, W);
+
+        dVector3 CCOrigin;
+        CCOrigin[0] = CCPos[0] - (W[0] * CCLength / 2);
+        CCOrigin[1] = CCPos[1] - (W[1] * CCLength / 2);
+        CCOrigin[2] = CCPos[2] - (W[2] * CCLength / 2);
+        CCOrigin[3] = CCPos[3] - (W[3] * CCLength / 2);
+        
+        dVector3 D;
+        D[0] = dDOT(U, Direction);
+        D[1] = dDOT(V, Direction);
+        D[2] = dDOT(W, Direction);
+
+        dReal DMag = Length;
+        dReal InvDMag = REAL(1.0) / DMag;
+
+        dVector3 Diff;
+        Diff[0] = Origin[0] - CCOrigin[0];
+        Diff[1] = Origin[1] - CCOrigin[1];
+        Diff[2] = Origin[2] - CCOrigin[2];
+        Diff[3] = Origin[3] - CCOrigin[3];
+
+        dVector3 P;
+        P[0] = dDOT(U, Diff);
+        P[1] = dDOT(V, Diff);
+        P[2] = dDOT(W, Diff);
+
+        dReal CCRadiusSq = CCRadius * CCRadius;
+
+        dReal Epsilon = 1e-12f;
+
+        if (dFabs(D[2]) >= REAL(1.0) - Epsilon){        // line is parallel to capsule axis
+                dReal Discr =  CCRadiusSq - P[0] * P[0] - P[1] * P[1];
+
+                if (Discr >= REAL(0.0)){
+            dReal Root = dSqrt(Discr);
+            T[0] = (-P[2] + Root) * InvDMag;
+            T[1] = (CCLength - P[2] + Root) * InvDMag;
+            return 2;
+        }
+        else return 0;
+        }
+
+        // test intersection with infinite cylinder
+    dReal A = D[0] * D[0] + D[1] * D[1];
+    dReal B = P[0] * D[0] + P[1] * D[1];
+    dReal C = P[0] * P[0] + P[1] * P[1] - CCRadiusSq;
+    dReal Discr = B * B - A * C;
+    if (Discr < REAL(0.0)){     // line does not intersect infinite cylinder
+        return 0;
+    }
+
+        int Count = 0;
+
+    if (Discr > REAL(0.0)){     // line intersects infinite cylinder in two places
+        dReal Root = dSqrt(Discr);
+        dReal Inv = REAL(1.0) / A;
+
+                dReal TTemp = (-B - Root) * Inv;
+
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+                        T[Count++] = TTemp * InvDMag;
+                }
+                
+                
+        TTemp = (-B + Root) * Inv;
+        Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+                        T[Count++] = TTemp * InvDMag;
+                }
+
+        if (Count == 2){        // line intersects capsule wall in two places
+            return 2;
+        }
+    }
+        else{   // line is tangent to infinite cylinder
+        dReal TTemp = -B / A;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+            T[0] = TTemp * InvDMag;
+            return 1;
+        }
+    }
+
+        // test intersection with bottom hemisphere
+    // fA = 1
+    B += P[2] * D[2];
+    C += P[2] * P[2];
+    Discr = B * B - C;
+    if (Discr > REAL(0.0)){
+        dReal Root = dSqrt(Discr);
+        dReal TTemp = -B - Root;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+
+        TTemp = -B + Root;
+        Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+    else if (Discr == REAL(0.0)){
+        dReal TTemp = -B;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+
+        // test intersection with top hemisphere
+    // fA = 1
+    B -= D[2] * CCLength;
+    C += CCLength * (CCLength - REAL(2.0) * P[2]);
+
+    Discr = B * B - C;
+    if (Discr > REAL(0.0)){
+        dReal Root = dSqrt(Discr);
+        dReal TTemp = -B - Root;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+
+        TTemp = -B + Root;
+        Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+    else if (Discr == REAL(0.0)){
+        dReal TTemp = -B;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+        return Count;
+}
+
+int dCollideCCR(dxGeom* RayGeom, dxGeom* CCGeom, int Flags, dContactGeom* Contacts, int Stride){
+        const dVector3& CCPos = *(const dVector3*)dGeomGetPosition(CCGeom);
+        const dMatrix3& CCRot = *(const dMatrix3*)dGeomGetRotation(CCGeom);
+
+        dReal CCRadius, CCLength;
+        dGeomCCylinderGetParams(CCGeom, &CCRadius, &CCLength);
+
+        dVector3 Origin, Direction;
+        dGeomRayGet(RayGeom, Origin, Direction);
+        dReal Length = dGeomRayGetLength(RayGeom);
+
+        dReal T[2];
+    int Count = Find(Origin, Direction, Length, CCPos, CCRot, CCRadius, CCLength, T);
+        int ContactCount = 0;
+        for (int i = 0; i < Count; i++){
+                if (T[i] >= 0.0){
+                        dContactGeom* Contact = CONTACT(Flags, Contacts, ContactCount, Stride);
+                        Contact->pos[0] = Origin[0] + T[i] * Direction[0] * Length;
+                        Contact->pos[1] = Origin[1] + T[i] * Direction[1] * Length;
+                        Contact->pos[2] = Origin[2] + T[i] * Direction[2] * Length;
+                        Contact->pos[3] = Origin[3] + T[i] * Direction[3] * Length;
+                        //Contact->normal = 0;
+                        Contact->depth = 0.0f;
+                        Contact->g1 = RayGeom;
+                        Contact->g2 = CCGeom;
+
+                        ContactCount++;
+                }
+        }
+    return ContactCount;
+}
\ No newline at end of file