#define TPE_LOG puts #include "tinyphysicsengine.h" #include #include #define S3L_RESOLUTION_X 640 #define S3L_RESOLUTION_Y 480 #define S3L_PIXEL_FUNCTION drawPixel #define S3L_Z_BUFFER 1 #include "small3dlib.h" #define PIXELS_SIZE (S3L_RESOLUTION_X * S3L_RESOLUTION_Y * 4) #define FPS 30 #define MSPF (1000 / (FPS)) TPE_World world; TPE_Body bodies[128]; S3L_Unit cubeVertices[] = { S3L_CUBE_VERTICES(1024) }; S3L_Index cubeTriangles[] = { S3L_CUBE_TRIANGLES }; S3L_Model3D cube; uint8_t pixels[PIXELS_SIZE]; uint8_t red = 100; TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit distLim) { TPE_Vec3 r, rMin = p; TPE_Unit d, dMin = 10000000; #define testShape(c) \ r = c; if (p.x == r.x && p.y == r.y && p.z == r.z) return p; \ d = TPE_DISTANCE(p,r); if (d < dMin) { dMin = d; rMin = r; } /* testShape( TPE_envHalfPlane(p,TPE_vec3(-1000,0,0),TPE_vec3(20,20,0)) ) testShape( TPE_envHalfPlane(p,TPE_vec3(+1000,0,0),TPE_vec3(-20,20,0)) ) */ testShape( TPE_envAABoxInside(p,TPE_vec3(0,0,0),TPE_vec3(16000,6000,16000)) ) testShape( TPE_envSphere(p,TPE_vec3(2000,-3500,2000),2000) ) testShape( TPE_envSphere(p,TPE_vec3(-2000,-3500,0),2000) ) #undef testShape return rMin; } void drawPixel(S3L_PixelInfo *p) { uint32_t index = (p->y * S3L_RESOLUTION_X + p->x) * 4; pixels[index + 1] = p->triangleIndex * 16; pixels[index + 2] = 255 - p->triangleIndex * 16; pixels[index + 3] = red; } void draw2DPoint(int x, int y, int r, int g, int b) { if (x < 1 || x > S3L_RESOLUTION_X - 3 || y < 1 || y > S3L_RESOLUTION_Y - 3) return; uint32_t index = ((y - 1) * S3L_RESOLUTION_X + x) * 4; #define d pixels[index] = 0; pixels[index + 1] = b; pixels[index + 2] = g; pixels[index + 3] = r; d index += S3L_RESOLUTION_X * 4 - 4; d index += 4; d index += 4; d index += S3L_RESOLUTION_X * 4 - 4; d #undef d } void debugDrawPixel(uint16_t x, uint16_t y, uint8_t color) { draw2DPoint(x,y,255 - color * 64,color * 128,color * 64); } void drawLine(int x1, int y1, int x2, int y2, int r, int g, int b) { #define STEPS 20 float dx = (x2 - x1) / ((float) STEPS); float dy = (y2 - y1) / ((float) STEPS); for (int i = 0; i < STEPS; ++i) draw2DPoint(x1 + dx * i, y1 + dy * i,r,g,b); #undef STEPS } S3L_Scene sphereScene; void draw3DLine(int x1, int y1, int z1, int x2, int y2, int z2) { S3L_Vec4 p1, p2, r1, r2; S3L_vec4Set(&p1,x1,y1,z1,0); S3L_vec4Set(&p2,x2,y2,z2,0); S3L_project3DPointToScreen(p1,sphereScene.camera,&r1); S3L_project3DPointToScreen(p2,sphereScene.camera,&r2); if (r1.z > 0 && r2.z > 0) drawLine(r1.x,r1.y,r2.x,r2.y,200,100,200); } void drawSphere(S3L_Unit x, S3L_Unit y, S3L_Unit z, S3L_Unit r) { sphereScene.models[0].transform.translation.x = x; sphereScene.models[0].transform.translation.y = y; sphereScene.models[0].transform.translation.z = z; sphereScene.models[0].transform.scale.x = r; sphereScene.models[0].transform.scale.y = r; sphereScene.models[0].transform.scale.z = r; S3L_drawScene(sphereScene); } void drawBody(TPE_Body *body, uint8_t color) { red = color; for (int i = 0; i < body->jointCount; ++i) drawSphere( body->joints[i].position.x, body->joints[i].position.y, body->joints[i].position.z, TPE_JOINT_SIZE(body->joints[i])); for (int i = 0; i < body->connectionCount; ++i) { S3L_Vec4 p1, p2, r1, r2; S3L_vec4Set(&p1, body->joints[body->connections[i].joint1].position.x, body->joints[body->connections[i].joint1].position.y, body->joints[body->connections[i].joint1].position.z,0); S3L_vec4Set(&p2, body->joints[body->connections[i].joint2].position.x, body->joints[body->connections[i].joint2].position.y, body->joints[body->connections[i].joint2].position.z,0); S3L_project3DPointToScreen(p1,sphereScene.camera,&r1); S3L_project3DPointToScreen(p2,sphereScene.camera,&r2); if (r1.z > 0 && r2.z > 0) drawLine(r1.x,r1.y,r2.x,r2.y,100,200,300); } } #define SPHERE_VERTEX_COUNT 42 const S3L_Unit sphereVertices[SPHERE_VERTEX_COUNT * 3] = { 0, -512, 0, // 0 370, -228, -269, // 3 -141, -228, -435, // 6 -457, -228, 0, // 9 -141, -228, 435, // 12 370, -228, 269, // 15 141, 228, -435, // 18 -370, 228, -269, // 21 -370, 228, 269, // 24 141, 228, 435, // 27 457, 228, 0, // 30 0, 512, 0, // 33 -83, -435, -255, // 36 217, -435, -158, // 39 134, -269, -414, // 42 435, -269, 0, // 45 217, -435, 158, // 48 -269, -435, 0, // 51 -352, -269, -255, // 54 -83, -435, 255, // 57 -352, -269, 255, // 60 134, -269, 414, // 63 486, 0, -158, // 66 486, 0, 158, // 69 0, 0, -512, // 72 300, 0, -414, // 75 -486, 0, -158, // 78 -300, 0, -414, // 81 -300, 0, 414, // 84 -486, 0, 158, // 87 300, 0, 414, // 90 0, 0, 512, // 93 352, 269, -255, // 96 -134, 269, -414, // 99 -435, 269, 0, // 102 -134, 269, 414, // 105 352, 269, 255, // 108 83, 435, -255, // 111 269, 435, 0, // 114 -217, 435, -158, // 117 -217, 435, 158, // 120 83, 435, 255 // 123 }; // sphereVertices #define SPHERE_TRIANGLE_COUNT 80 const S3L_Index sphereTriangleIndices[SPHERE_TRIANGLE_COUNT * 3] = { 0, 13, 12, // 0 1, 13, 15, // 3 0, 12, 17, // 6 0, 17, 19, // 9 0, 19, 16, // 12 1, 15, 22, // 15 2, 14, 24, // 18 3, 18, 26, // 21 4, 20, 28, // 24 5, 21, 30, // 27 1, 22, 25, // 30 2, 24, 27, // 33 3, 26, 29, // 36 4, 28, 31, // 39 5, 30, 23, // 42 6, 32, 37, // 45 7, 33, 39, // 48 8, 34, 40, // 51 9, 35, 41, // 54 10, 36, 38, // 57 38, 41, 11, // 60 38, 36, 41, // 63 36, 9, 41, // 66 41, 40, 11, // 69 41, 35, 40, // 72 35, 8, 40, // 75 40, 39, 11, // 78 40, 34, 39, // 81 34, 7, 39, // 84 39, 37, 11, // 87 39, 33, 37, // 90 33, 6, 37, // 93 37, 38, 11, // 96 37, 32, 38, // 99 32, 10, 38, // 102 23, 36, 10, // 105 23, 30, 36, // 108 30, 9, 36, // 111 31, 35, 9, // 114 31, 28, 35, // 117 28, 8, 35, // 120 29, 34, 8, // 123 29, 26, 34, // 126 26, 7, 34, // 129 27, 33, 7, // 132 27, 24, 33, // 135 24, 6, 33, // 138 25, 32, 6, // 141 25, 22, 32, // 144 22, 10, 32, // 147 30, 31, 9, // 150 30, 21, 31, // 153 21, 4, 31, // 156 28, 29, 8, // 159 28, 20, 29, // 162 20, 3, 29, // 165 26, 27, 7, // 168 26, 18, 27, // 171 18, 2, 27, // 174 24, 25, 6, // 177 24, 14, 25, // 180 14, 1, 25, // 183 22, 23, 10, // 186 22, 15, 23, // 189 15, 5, 23, // 192 16, 21, 5, // 195 16, 19, 21, // 198 19, 4, 21, // 201 19, 20, 4, // 204 19, 17, 20, // 207 17, 3, 20, // 210 17, 18, 3, // 213 17, 12, 18, // 216 12, 2, 18, // 219 15, 16, 5, // 222 15, 13, 16, // 225 13, 0, 16, // 228 12, 14, 2, // 231 12, 13, 14, // 234 13, 1, 14 // 237 }; // sphereTriangleIndices int main(void) { SDL_Window *window = SDL_CreateWindow("test", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, S3L_RESOLUTION_X, S3L_RESOLUTION_Y, SDL_WINDOW_SHOWN); SDL_Renderer *renderer = SDL_CreateRenderer(window,-1,0); SDL_Texture *textureSDL = SDL_CreateTexture(renderer,SDL_PIXELFORMAT_RGBX8888, SDL_TEXTUREACCESS_STATIC, S3L_RESOLUTION_X, S3L_RESOLUTION_Y); SDL_Surface *screenSurface = SDL_GetWindowSurface(window); SDL_Event event; int running = 1; S3L_Model3D sphereModel; S3L_model3DInit(sphereVertices,SPHERE_VERTEX_COUNT,sphereTriangleIndices, SPHERE_TRIANGLE_COUNT,&sphereModel); S3L_model3DInit(cubeVertices,S3L_CUBE_VERTEX_COUNT,cubeTriangles, S3L_CUBE_TRIANGLE_COUNT,&cube); S3L_sceneInit(&sphereModel,1,&sphereScene); sphereScene.camera.transform.translation.z = -512 -512 -512; int frame = 0; TPE_Joint joints[100]; TPE_Connection connections[100]; #define MASS 200 switch (1) { case 0: TPE_make2Line(joints,connections,1500,512); TPE_bodyInit(bodies,joints,2,connections,1,MASS); break; case 1: TPE_makeBox(joints,connections,1000,1000,1000,500); // TPE_bodyInit(bodies,joints,8,connections,16,MASS); TPE_bodyInit(bodies,joints,1,connections,0,MASS); break; case 2: TPE_makeCenterRect(joints,connections,1300,1000,512); TPE_bodyInit(bodies,joints,5,connections,8,MASS); break; case 3: TPE_makeCenterBox(joints,connections,1000,1000,1000,512); joints[8].sizeDivided *= 3; joints[8].sizeDivided /= 2; TPE_bodyInit(bodies,joints,9,connections,18,MASS); break; case 4: TPE_makeTriangle(joints,connections,2000,512); TPE_bodyInit(bodies,joints,3,connections,3,MASS); break; case 5: TPE_make2Line(joints,connections,1500,200); TPE_bodyInit(bodies,joints,1,connections,0,MASS); break; default: break; } //TPE_make2Line(joints + 20,connections + 20,1000,200); //TPE_bodyInit(bodies + 1,joints + 20,1,connections + 20,0,MASS); TPE_makeBox(joints + 20,connections + 20,1000,1000,1000,500); TPE_bodyInit(bodies + 1,joints + 20,8,connections + 20,16,100); //bodies[0].flags |= TPE_BODY_FLAG_SOFT; //bodies[0].elasticity = 256; TPE_worldInit(&world,bodies,2,environmentDistance); #define FR 256 world.bodies[0].friction = FR; world.bodies[1].friction = FR; TPE_bodyMove(world.bodies,TPE_vec3(500,-200,400)); TPE_bodyMove(&world.bodies[1],TPE_vec3(-500,800,400)); TPE_bodyStop(world.bodies); TPE_bodyStop(world.bodies + 1); TPE_bodyAccelerate(world.bodies,TPE_vec3(-50,0,0)); TPE_bodyAccelerate(world.bodies + 1,TPE_vec3(50,0,0)); //TPE_bodyRotate(world.bodies,TPE_vec3(0,0,200)); //------- int time; while (running) { time = SDL_GetTicks(); for (uint32_t i = 0; i < PIXELS_SIZE; ++i) pixels[i] = 0; S3L_newFrame(); //TPE_bodiesResolveCollision(world.bodies,world.bodies + 1); for (int i = 0; i < world.bodyCount; ++i) { if (!(world.bodies[i].flags & TPE_BODY_FLAG_DEACTIVATED)) TPE_bodyAccelerate(world.bodies + i,TPE_vec3(0,-6,0)); } TPE_worldStep(&world); while (SDL_PollEvent(&event)) { if (event.type == SDL_QUIT) running = 0; else if (event.type == SDL_KEYDOWN) { if (event.key.keysym.scancode == SDL_SCANCODE_Q || event.key.keysym.scancode == SDL_SCANCODE_ESCAPE) running = 0; } } const uint8_t *state = SDL_GetKeyboardState(NULL); S3L_Vec4 camF, camR; #define SHIFT_STEP 50 #define ROT_STEP 5 S3L_rotationToDirections(sphereScene.camera.transform.rotation,SHIFT_STEP,&camF,&camR,0); TPE_Vec3 forw = TPE_vec3Minus( bodies[0].joints[2].position, bodies[0].joints[0].position); TPE_Vec3 righ = TPE_vec3Minus( bodies[0].joints[0].position, bodies[0].joints[1].position); /* forw = TPE_vec3Plus(forw, TPE_vec3Minus( bodies[0].joints[6].position, bodies[0].joints[4].position) ); righ = TPE_vec3Plus(righ, TPE_vec3Minus( bodies[0].joints[4].position, bodies[0].joints[5].position) ); */ TPE_Vec3 rrrr = TPE_orientationFromVecs(forw,righ); cube.transform.rotation.x = rrrr.x; cube.transform.rotation.y = rrrr.y; cube.transform.rotation.z = rrrr.z; cube.transform.scale.x = 600; cube.transform.scale.y = cube.transform.scale.x; cube.transform.scale.z = cube.transform.scale.x; TPE_Vec3 ppp = TPE_bodyGetCenter(&bodies[0]); cube.transform.translation.x = ppp.x; cube.transform.translation.y = ppp.y; cube.transform.translation.z = ppp.z; TPE_Vec3 camPos = TPE_vec3( sphereScene.camera.transform.translation.x, sphereScene.camera.transform.translation.y, sphereScene.camera.transform.translation.z); TPE_Vec3 camRot = TPE_vec3( sphereScene.camera.transform.rotation.x, sphereScene.camera.transform.rotation.y, sphereScene.camera.transform.rotation.z); //drawEnv(TPE_vec3(-100,-100,-100),100,5); //for (int i = 0; i < world.bodyCount; ++i) // drawBody(&(world.bodies[i]),100 * i); sphereScene.models = &cube; S3L_newFrame(); //S3L_drawScene(sphereScene); sphereScene.models = &sphereModel; TPE_worldDebugDraw(&world,debugDrawPixel, camPos,camRot,TPE_vec3(S3L_RESOLUTION_X,S3L_RESOLUTION_Y,sphereScene.camera.focalLength)); /* draw3DLine(0,0,0,forw.x,forw.y,forw.z); draw3DLine(0,0,0,righ.x,righ.y,righ.z); */ SDL_UpdateTexture(textureSDL,NULL,pixels,S3L_RESOLUTION_X * sizeof(uint32_t)); if (state[SDL_SCANCODE_LSHIFT]) { if (state[SDL_SCANCODE_UP]) S3L_vec3Add(&sphereScene.camera.transform.translation,camF); else if (state[SDL_SCANCODE_DOWN]) S3L_vec3Sub(&sphereScene.camera.transform.translation,camF); else if (state[SDL_SCANCODE_LEFT]) S3L_vec3Sub(&sphereScene.camera.transform.translation,camR); else if (state[SDL_SCANCODE_RIGHT]) S3L_vec3Add(&sphereScene.camera.transform.translation,camR); } else { if (state[SDL_SCANCODE_UP]) sphereScene.camera.transform.rotation.x += ROT_STEP; else if (state[SDL_SCANCODE_DOWN]) sphereScene.camera.transform.rotation.x -= ROT_STEP; else if (state[SDL_SCANCODE_LEFT]) sphereScene.camera.transform.rotation.y += ROT_STEP; else if (state[SDL_SCANCODE_RIGHT]) sphereScene.camera.transform.rotation.y -= ROT_STEP; else if (state[SDL_SCANCODE_K]) sphereScene.camera.transform.rotation.z += ROT_STEP; else if (state[SDL_SCANCODE_L]) sphereScene.camera.transform.rotation.z -= ROT_STEP; } if (state[SDL_SCANCODE_M]) { TPE_bodyWake(world.bodies); TPE_bodySpin(bodies,TPE_vec3(50,40,100)); TPE_bodyAccelerate(bodies,TPE_vec3( 500, 500, 30)); } #define S 10 if (state[SDL_SCANCODE_J]) TPE_bodyAccelerate(bodies,TPE_vec3(S,0,0)); else if (state[SDL_SCANCODE_G]) TPE_bodyAccelerate(bodies,TPE_vec3(-S,0,0)); if (state[SDL_SCANCODE_Y]) TPE_bodyAccelerate(bodies,TPE_vec3(0,0,S)); else if (state[SDL_SCANCODE_H]) TPE_bodyAccelerate(bodies,TPE_vec3(0,0,-S)); #undef S #define SHIFT_STEP 50 if (state[SDL_SCANCODE_P]) sphereScene.camera.transform.translation.y += SHIFT_STEP; else if (state[SDL_SCANCODE_O]) sphereScene.camera.transform.translation.y -= SHIFT_STEP; #undef SHIFT_STEP SDL_RenderClear(renderer); SDL_RenderCopy(renderer,textureSDL,NULL,NULL); SDL_RenderPresent(renderer); time = time + MSPF - SDL_GetTicks(); if (time > 1) usleep(time * 1000); frame++; } return 0; }