1 | #include <stdio.h> |
---|
2 | #include <glut.h> |
---|
3 | #include "ode.h" |
---|
4 | |
---|
5 | #define NUMBODIES 80 |
---|
6 | |
---|
7 | #define USE_SPHERE 0 |
---|
8 | #define USE_HELIX 1 |
---|
9 | #define USE_TORQUE 1 |
---|
10 | #define USE_WEIRD_MATRIX_OPS 0 |
---|
11 | |
---|
12 | #define CONTACTS 1 |
---|
13 | |
---|
14 | dWorldID aWorld; |
---|
15 | dSpaceID aSpace; |
---|
16 | float cycle = 0, fade; |
---|
17 | dJointGroupID aContactGroup; |
---|
18 | dBodyID bodies[NUMBODIES]; |
---|
19 | dGeomID geoms[NUMBODIES]; |
---|
20 | GLfloat colors[NUMBODIES][4]; |
---|
21 | unsigned int contactsThisFrame; |
---|
22 | |
---|
23 | void kglTransformByODEGeom(dGeomID geom) { |
---|
24 | const dReal *p = dGeomGetPosition(geom); |
---|
25 | const dReal *R = dGeomGetRotation(geom); |
---|
26 | GLdouble glm[16]; |
---|
27 | |
---|
28 | glm[0] = R[0]; glm[1] = R[4]; glm[2] = R[8]; glm[3] = 0; |
---|
29 | glm[4] = R[1]; glm[5] = R[5]; glm[6] = R[9]; glm[7] = 0; |
---|
30 | glm[8] = R[2]; glm[9] = R[6]; glm[10] = R[10];glm[11] = 0; |
---|
31 | glm[12] = p[0]; glm[13] = p[1]; glm[14] = p[2]; glm[15] = 1; |
---|
32 | |
---|
33 | glMultMatrixd(glm); |
---|
34 | } |
---|
35 | |
---|
36 | static void odeNearCallback(void *data, dGeomID g1, dGeomID g2) { |
---|
37 | dBodyID b1 = dGeomGetBody(g1), |
---|
38 | b2 = dGeomGetBody(g2); |
---|
39 | dContact contact[CONTACTS]; |
---|
40 | int contactsUsed, i; |
---|
41 | |
---|
42 | if (b1 && b2 && dAreConnected(b1, b2)) return; |
---|
43 | |
---|
44 | contactsUsed = dCollide(g1, g2, CONTACTS, &contact[0].geom, |
---|
45 | sizeof(dContact)); |
---|
46 | if (contactsUsed > CONTACTS) contactsUsed = CONTACTS; |
---|
47 | |
---|
48 | for (i = 0; i < contactsUsed; i++) { |
---|
49 | contact[i].surface.mode = 0; |
---|
50 | contact[i].surface.mu = 20.0; |
---|
51 | |
---|
52 | dJointAttach(dJointCreateContact(aWorld, aContactGroup, |
---|
53 | &(contact[i])), b1, b2); |
---|
54 | contactsThisFrame++; |
---|
55 | } |
---|
56 | } |
---|
57 | |
---|
58 | void myGlutResize(int w, int h) { |
---|
59 | glViewport(0, 0, w, h); |
---|
60 | glMatrixMode(GL_PROJECTION); |
---|
61 | glLoadIdentity(); |
---|
62 | gluPerspective(45.0, (GLfloat)w / h, 1.0, 120.0); |
---|
63 | glMatrixMode(GL_MODELVIEW); |
---|
64 | glLoadIdentity(); |
---|
65 | glTranslatef(0, -6, -20); |
---|
66 | } |
---|
67 | |
---|
68 | void myGlutIdle(void) { |
---|
69 | const float step = 1.0/120; |
---|
70 | int i; |
---|
71 | |
---|
72 | cycle = fmod(cycle + step / 4, 1); |
---|
73 | fade = fabs(cycle * 2 - 1); |
---|
74 | |
---|
75 | contactsThisFrame = 0; |
---|
76 | dSpaceCollide(aSpace, NULL, &odeNearCallback); |
---|
77 | //printf("%u\n", contactsThisFrame); |
---|
78 | dWorldStep(aWorld, step); |
---|
79 | dJointGroupEmpty(aContactGroup); |
---|
80 | |
---|
81 | for (i = 0; i < NUMBODIES; i++) { |
---|
82 | const dReal *cvel = dBodyGetLinearVel(bodies[i]); |
---|
83 | dBodyAddForce(bodies[i], |
---|
84 | -cvel[0] * 0.5, |
---|
85 | -cvel[1] * 0.5, |
---|
86 | -cvel[2] * 0.5 |
---|
87 | ); |
---|
88 | } |
---|
89 | |
---|
90 | glutPostRedisplay(); |
---|
91 | } |
---|
92 | |
---|
93 | void myGlutDisplay(void) { |
---|
94 | int i; |
---|
95 | |
---|
96 | glClearColor(fade * 0.15, 0, 0, 1); |
---|
97 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); |
---|
98 | |
---|
99 | if (USE_WEIRD_MATRIX_OPS) glPushMatrix(); |
---|
100 | for (i = 0; i < NUMBODIES; i++) { |
---|
101 | if (!USE_WEIRD_MATRIX_OPS) glPushMatrix(); |
---|
102 | kglTransformByODEGeom(geoms[i]); |
---|
103 | glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, colors[i]); |
---|
104 | glColor3f(fade * 1.5, 0, 0); |
---|
105 | #if USE_SPHERE |
---|
106 | glRotatef(90, 1, 0, 0); |
---|
107 | glutSolidSphere(0.5, 9, 6); |
---|
108 | glDisable(GL_LIGHTING); |
---|
109 | glutWireSphere(0.5, 9, 6); |
---|
110 | #else |
---|
111 | glutSolidCube(1); |
---|
112 | glDisable(GL_LIGHTING); |
---|
113 | glutWireCube(1); |
---|
114 | #endif |
---|
115 | glEnable(GL_LIGHTING); |
---|
116 | if (!USE_WEIRD_MATRIX_OPS) glPopMatrix(); |
---|
117 | } |
---|
118 | if (USE_WEIRD_MATRIX_OPS) glPopMatrix(); |
---|
119 | |
---|
120 | glutSwapBuffers(); |
---|
121 | } |
---|
122 | |
---|
123 | int main(int argc, char **argv) { |
---|
124 | printf("Initializing GLUT\n"); |
---|
125 | |
---|
126 | glutInit(&argc, argv); |
---|
127 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); |
---|
128 | glutInitWindowSize(400, 300); |
---|
129 | glutInitWindowPosition(100, 100); |
---|
130 | glutCreateWindow("ODE Crash Test"); |
---|
131 | |
---|
132 | glutDisplayFunc(myGlutDisplay); |
---|
133 | glutReshapeFunc(myGlutResize); |
---|
134 | glutIdleFunc(myGlutIdle); |
---|
135 | |
---|
136 | glPolygonOffset(1, 1); |
---|
137 | glDepthFunc(GL_LEQUAL); |
---|
138 | glEnable(GL_POLYGON_OFFSET_FILL); |
---|
139 | glEnable(GL_DEPTH_TEST); |
---|
140 | glEnable(GL_CULL_FACE); |
---|
141 | glEnable(GL_LIGHTING); |
---|
142 | glEnable(GL_LIGHT0); |
---|
143 | myGlutResize(400, 300); |
---|
144 | |
---|
145 | printf("Creating ODE world\n"); |
---|
146 | aWorld = dWorldCreate(); |
---|
147 | aSpace = dHashSpaceCreate(); |
---|
148 | aContactGroup = dJointGroupCreate(0); |
---|
149 | dCreatePlane(aSpace, 0, 1, 0, 0); |
---|
150 | dWorldSetGravity(aWorld, 0, -9.81, 0); |
---|
151 | dWorldSetERP(aWorld, 0.4); |
---|
152 | dWorldSetCFM(aWorld, 1e-10); |
---|
153 | |
---|
154 | printf("Creating objects\n"); |
---|
155 | { |
---|
156 | int i; |
---|
157 | dMass mass; |
---|
158 | |
---|
159 | dMassSetBox(&mass, 1.0, 1, 1, 1); |
---|
160 | |
---|
161 | for (i = 0; i < NUMBODIES; i++) { |
---|
162 | float fraction = (float)i / NUMBODIES; |
---|
163 | |
---|
164 | bodies[i] = dBodyCreate(aWorld); |
---|
165 | dBodySetMass(bodies[i], &mass); |
---|
166 | #if USE_SPHERE |
---|
167 | geoms[i] = dCreateSphere(aSpace, 0.5); |
---|
168 | #else |
---|
169 | geoms[i] = dCreateBox(aSpace, 1, 1, 1); |
---|
170 | #endif |
---|
171 | dGeomSetBody(geoms[i], bodies[i]); |
---|
172 | |
---|
173 | if (USE_HELIX) { |
---|
174 | float r = (i % 3 - 1) * (1.5+4*(1 - fraction)), |
---|
175 | theta = (float)i / 4; |
---|
176 | dBodySetPosition(bodies[i], |
---|
177 | sin(theta) * r, |
---|
178 | (float)i + 1, |
---|
179 | cos(theta) * r |
---|
180 | ); |
---|
181 | } else { |
---|
182 | dBodySetPosition(bodies[i], 0, (float)i * 2 + 1, 0); |
---|
183 | } |
---|
184 | if (USE_TORQUE) dBodyAddTorque(bodies[i], fraction*10, fraction*20, fraction*30); |
---|
185 | |
---|
186 | colors[i][0] = fraction; |
---|
187 | colors[i][1] = 1 - fraction; |
---|
188 | colors[i][2] = 1 - fabs(fraction * 2 - 1); |
---|
189 | colors[i][3] = 1; |
---|
190 | } |
---|
191 | } |
---|
192 | |
---|
193 | printf("Starting simulation\n"); |
---|
194 | glutMainLoop(); |
---|
195 | |
---|
196 | return 0; |
---|
197 | } |
---|