| 1 | // Ripped from Magic Software |
|---|
| 2 | |
|---|
| 3 | #include "Include\dRay.h" |
|---|
| 4 | #include "dxRay.h" |
|---|
| 5 | |
|---|
| 6 | int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ |
|---|
| 7 | const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); |
|---|
| 8 | dReal Radius = dGeomSphereGetRadius(SphereGeom); |
|---|
| 9 | |
|---|
| 10 | dVector3 Origin, Direction; |
|---|
| 11 | dGeomRayGet(RayGeom, Origin, Direction); |
|---|
| 12 | dReal Length = dGeomRayGetLength(RayGeom); |
|---|
| 13 | |
|---|
| 14 | dVector3 Diff; |
|---|
| 15 | Diff[0] = Origin[0] - Position[0]; |
|---|
| 16 | Diff[1] = Origin[1] - Position[1]; |
|---|
| 17 | Diff[2] = Origin[2] - Position[2]; |
|---|
| 18 | Diff[3] = Origin[3] - Position[3]; |
|---|
| 19 | |
|---|
| 20 | Direction[0] *= Length; |
|---|
| 21 | Direction[1] *= Length; |
|---|
| 22 | Direction[2] *= Length; |
|---|
| 23 | Direction[3] *= Length; |
|---|
| 24 | |
|---|
| 25 | dReal A = Length * Length; |
|---|
| 26 | dReal B = dDOT(Diff, Direction); |
|---|
| 27 | dReal C = dDOT(Diff, Diff) - (Radius * Radius); |
|---|
| 28 | |
|---|
| 29 | dReal Discr = B * B - A * C; |
|---|
| 30 | if (Discr < REAL(0.0)){ |
|---|
| 31 | return 0; |
|---|
| 32 | } |
|---|
| 33 | else if (Discr > REAL(0.0)){ |
|---|
| 34 | dReal T[2]; |
|---|
| 35 | dReal Root = dSqrt(Discr); |
|---|
| 36 | dReal InvA = REAL(1.0) / A; |
|---|
| 37 | T[0] = (-B - Root) * InvA; |
|---|
| 38 | T[1] = (-B + Root) * InvA; |
|---|
| 39 | |
|---|
| 40 | if (T[0] >= REAL(0.0)){ |
|---|
| 41 | dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride); |
|---|
| 42 | Contact0->pos[0] = Origin[0] + T[0] * Direction[0]; |
|---|
| 43 | Contact0->pos[1] = Origin[1] + T[0] * Direction[1]; |
|---|
| 44 | Contact0->pos[2] = Origin[2] + T[0] * Direction[2]; |
|---|
| 45 | Contact0->pos[3] = Origin[3] + T[0] * Direction[3]; |
|---|
| 46 | //Contact0->normal = 0; |
|---|
| 47 | Contact0->depth = 0.0f; |
|---|
| 48 | Contact0->g1 = RayGeom; |
|---|
| 49 | Contact0->g2 = SphereGeom; |
|---|
| 50 | |
|---|
| 51 | dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride); |
|---|
| 52 | Contact1->pos[0] = Origin[0] + T[1] * Direction[0]; |
|---|
| 53 | Contact1->pos[1] = Origin[1] + T[1] * Direction[1]; |
|---|
| 54 | Contact1->pos[2] = Origin[2] + T[1] * Direction[2]; |
|---|
| 55 | Contact1->pos[3] = Origin[3] + T[1] * Direction[3]; |
|---|
| 56 | //Contact1->normal = 0; |
|---|
| 57 | Contact1->depth = 0.0f; |
|---|
| 58 | Contact1->g1 = RayGeom; |
|---|
| 59 | Contact1->g2 = SphereGeom; |
|---|
| 60 | |
|---|
| 61 | return 2; |
|---|
| 62 | } |
|---|
| 63 | else if (T[1] >= REAL(0.0)){ |
|---|
| 64 | dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride); |
|---|
| 65 | Contact->pos[0] = Origin[0] + T[1] * Direction[0]; |
|---|
| 66 | Contact->pos[1] = Origin[1] + T[1] * Direction[1]; |
|---|
| 67 | Contact->pos[2] = Origin[2] + T[1] * Direction[2]; |
|---|
| 68 | Contact->pos[3] = Origin[3] + T[1] * Direction[3]; |
|---|
| 69 | //Contact->normal = 0; |
|---|
| 70 | Contact->depth = 0.0f; |
|---|
| 71 | Contact->g1 = RayGeom; |
|---|
| 72 | Contact->g2 = SphereGeom; |
|---|
| 73 | |
|---|
| 74 | return 1; |
|---|
| 75 | } |
|---|
| 76 | else return 0; |
|---|
| 77 | } |
|---|
| 78 | else{ |
|---|
| 79 | dReal T; |
|---|
| 80 | T = -B / A; |
|---|
| 81 | if (T >= REAL(0.0)){ |
|---|
| 82 | dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride); |
|---|
| 83 | Contact->pos[0] = Origin[0] + T * Direction[0]; |
|---|
| 84 | Contact->pos[1] = Origin[1] + T * Direction[1]; |
|---|
| 85 | Contact->pos[2] = Origin[2] + T * Direction[2]; |
|---|
| 86 | Contact->pos[3] = Origin[3] + T * Direction[3]; |
|---|
| 87 | //Contact->normal = 0; |
|---|
| 88 | Contact->depth = 0.0f; |
|---|
| 89 | Contact->g1 = RayGeom; |
|---|
| 90 | Contact->g2 = SphereGeom; |
|---|
| 91 | return 1; |
|---|
| 92 | } |
|---|
| 93 | else return 0; |
|---|
| 94 | } |
|---|
| 95 | } |
|---|
