[216] | 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 | } |
---|