1 | #include "Include\dRay.h" |
---|
2 | #include "dxRay.h" |
---|
3 | |
---|
4 | int dRayClass = -1; |
---|
5 | |
---|
6 | void dAABBRay(dxGeom* Ray, dReal AABB[6]){ |
---|
7 | dVector3 Start, End; |
---|
8 | dGeomRayGet(Ray, Start, End); |
---|
9 | dReal Length = dGeomRayGetLength(Ray); |
---|
10 | |
---|
11 | End[0] = Start[0] + End[0] * Length; |
---|
12 | End[1] = Start[1] + End[1] * Length; |
---|
13 | End[2] = Start[2] + End[2] * Length; |
---|
14 | End[3] = Start[3] + End[3] * Length; |
---|
15 | |
---|
16 | if (Start[0] < End[0]){ |
---|
17 | AABB[0] = Start[0]; |
---|
18 | AABB[1] = End[0]; |
---|
19 | } |
---|
20 | else{ |
---|
21 | AABB[0] = End[0]; |
---|
22 | AABB[1] = Start[0]; |
---|
23 | } |
---|
24 | |
---|
25 | if (Start[1] < End[1]){ |
---|
26 | AABB[2] = Start[1]; |
---|
27 | AABB[3] = End[1]; |
---|
28 | } |
---|
29 | else{ |
---|
30 | AABB[2] = End[1]; |
---|
31 | AABB[3] = Start[1]; |
---|
32 | } |
---|
33 | |
---|
34 | if (Start[2] < End[2]){ |
---|
35 | AABB[4] = Start[2]; |
---|
36 | AABB[5] = End[2]; |
---|
37 | } |
---|
38 | else{ |
---|
39 | AABB[4] = End[2]; |
---|
40 | AABB[5] = Start[2]; |
---|
41 | } |
---|
42 | // Should we tweak the box to have a minimum size for axis aligned lines? How small should it be? |
---|
43 | } |
---|
44 | |
---|
45 | dColliderFn* dRayColliderFn(int num){ |
---|
46 | if (num == dPlaneClass) return (dColliderFn*)&dCollidePR; |
---|
47 | if (num == dSphereClass) return (dColliderFn*)&dCollideSR; |
---|
48 | if (num == dBoxClass) return (dColliderFn*)&dCollideBR; |
---|
49 | if (num == dCCylinderClass) return (dColliderFn*)&dCollideCCR; |
---|
50 | return 0; |
---|
51 | } |
---|
52 | |
---|
53 | dxGeom* dGeomCreateRay(dSpaceID space, dReal Length){ |
---|
54 | if (dRayClass == -1){ |
---|
55 | dGeomClass c; |
---|
56 | c.bytes = sizeof(dxRay); |
---|
57 | c.collider = &dRayColliderFn; |
---|
58 | c.aabb = &dAABBRay; |
---|
59 | c.aabb_test = 0; |
---|
60 | c.dtor = 0; |
---|
61 | |
---|
62 | dRayClass = dCreateGeomClass(&c); |
---|
63 | } |
---|
64 | |
---|
65 | dxGeom* g = dCreateGeom(dRayClass); |
---|
66 | if (space) dSpaceAdd(space, g); |
---|
67 | |
---|
68 | dGeomRaySetLength(g, Length); |
---|
69 | return g; |
---|
70 | } |
---|
71 | |
---|
72 | void dGeomRaySetLength(dxGeom* g, dReal Length){ |
---|
73 | ((dxRay*)dGeomGetClassData(g))->Length = Length; |
---|
74 | } |
---|
75 | |
---|
76 | dReal dGeomRayGetLength(dxGeom* g){ |
---|
77 | return ((dxRay*)dGeomGetClassData(g))->Length; |
---|
78 | } |
---|
79 | |
---|
80 | void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction){ |
---|
81 | dGeomSetPosition(g, Origin[0], Origin[1], Origin[2]); |
---|
82 | |
---|
83 | dVector3 Up, Right; |
---|
84 | dPlaneSpace(Direction, Up, Right); |
---|
85 | |
---|
86 | Origin[3] = Up[3] = Right[3] = REAL(0.0); |
---|
87 | |
---|
88 | dMatrix3 Rotation; |
---|
89 | Rotation[0 * 4 + 0] = Right[0]; |
---|
90 | Rotation[1 * 4 + 0] = Right[1]; |
---|
91 | Rotation[2 * 4 + 0] = Right[2]; |
---|
92 | Rotation[3 * 4 + 0] = Right[3]; |
---|
93 | |
---|
94 | Rotation[0 * 4 + 1] = Up[0]; |
---|
95 | Rotation[1 * 4 + 1] = Up[1]; |
---|
96 | Rotation[2 * 4 + 1] = Up[2]; |
---|
97 | Rotation[3 * 4 + 1] = Up[3]; |
---|
98 | |
---|
99 | Rotation[0 * 4 + 2] = Direction[0]; |
---|
100 | Rotation[1 * 4 + 2] = Direction[1]; |
---|
101 | Rotation[2 * 4 + 2] = Direction[2]; |
---|
102 | Rotation[3 * 4 + 2] = Direction[3]; |
---|
103 | |
---|
104 | dGeomSetRotation(g, Rotation); |
---|
105 | } |
---|
106 | |
---|
107 | void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction){ |
---|
108 | const dReal* Position = dGeomGetPosition(g); |
---|
109 | Origin[0] = Position[0]; |
---|
110 | Origin[1] = Position[1]; |
---|
111 | Origin[2] = Position[2]; |
---|
112 | Origin[3] = Position[3]; |
---|
113 | |
---|
114 | const dReal* Rotation = dGeomGetRotation(g); |
---|
115 | Direction[0] = Rotation[0 * 4 + 2]; |
---|
116 | Direction[1] = Rotation[1 * 4 + 2]; |
---|
117 | Direction[2] = Rotation[2 * 4 + 2]; |
---|
118 | Direction[3] = Rotation[3 * 4 + 2]; |
---|
119 | } |
---|