fix pendulum

game.c

@@ -18,6 +18,8 @@ void default_update_collision_poly(Body *body) {
     }
 }
 
+
+
 // collision poly size must be 2x shape_size + 1
 void cast_update_collision_poly(Body *body) {
     for (int i=0; i < body->collision_shape_size; i++) {
@@ -26,7 +28,6 @@ void cast_update_collision_poly(Body *body) {
 
         body->collision_poly[i].x = x;  
         body->collision_poly[i].y = y;  
-        printf("xxx %d\n", i);
     }
     for (int i=body->collision_shape_size - 1; i >= 0; i--) {
         double x = body->collision_shape[i].x + body->next_position.x;
@@ -35,12 +36,8 @@ void cast_update_collision_poly(Body *body) {
 
         body->collision_poly[k].x = x;  
         body->collision_poly[k].y = y;  
-        printf("xxx %d\n", k);
     }
 
-    for (int i = 0; i < body->collision_poly_size; i++) {
-        printf("colpoly: %f %f\n", body->collision_poly[i].x, body->collision_poly[i].y);
-    }
     /*int i=body->collision_shape_size - 1;*/
     /*double x = body->collision_shape[i].x + body->next_position.x;*/
     /*double y = body->collision_shape[i].y + body->next_position.y;*/
@@ -205,12 +202,12 @@ player_st get_player(int x, int y) {
     player.physics->num_strings = 1;
     player.physics->max_strings = 1;
     player.physics->strings = get_fixed_strings(1);
-    player.physics->strings->max_length = 890;
+    player.physics->strings->max_length = 210;
 
     // testing
     Vect end_pt;
-    end_pt.x = -100;
-    end_pt.y = 0;
+    end_pt.x = 2000; 
+    end_pt.y = 3000;
     player.physics->strings[0].set_end_point(player.physics->strings, &end_pt);
     player.physics->strings[0].attached = true;
 
@@ -293,7 +290,6 @@ double *project_col_poly(Body *shape, Vect V) {
     res[0] = min;
     res[1] = max;
 
-    printf("POLY PROJ: %f, %f\n", min, max);
     free(proj);
     return res;
 }
@@ -413,16 +409,16 @@ bool sat_collision_check(Body *one, Body *two, Vect *translation) {
     two_position.x /= two->collision_poly_size;
     two_position.y /= two->collision_poly_size;
 
-    printf("ONE POS: %f %f", one_position.x, one_position.y);
-    printf("TWO POS: %f %f", two_position.x, one_position.y);
+    /*printf("ONE POS: %f %f", one_position.x, one_position.y);*/
+    /*printf("TWO POS: %f %f", two_position.x, one_position.y);*/
 
     direction.x =  one_position.x - two_position.x;
     direction.y =  one_position.y - two_position.y;
 
     double dot = vect_scalar_projection(trans, direction);
 
-    printf("DIRECTION: %f %f\n\n", direction.x, direction.y);
-    printf("DOT: %f\n\n", dot);
+    /*printf("DIRECTION: %f %f\n\n", direction.x, direction.y);*/
+    /*printf("DOT: %f\n\n", dot);*/
 
     if (dot > 0) {
         trans = vect_scalar(trans, -1);
@@ -708,54 +704,35 @@ void advance_thing(Body * thing) {
 
         double st_len = vect_distance(thing->next_position, thing->strings[i].end_point);
         double max_len = thing->strings[i].max_length;
+        printf("STRING len: %f, %f\n", st_len, max_len);
         if (st_len > max_len) {
-
-            printf("STRING: %f, %f\n", st_len, max_len);
-
-            Vect end = thing->strings[i].end_point;
-            Vect position = thing->next_position;
+            Vect string_end = thing->strings[i].end_point;
+            Vect thing_position = thing->next_position;
 
             Vect string;
-            string.x = end.x - position.x;
-            string.y = end.y - position.y;
-
-            double disp = st_len - max_len;
+            string.x = string_end.x - thing_position.x;
+            string.y = string_end.y - thing_position.y;
+            double mag = vect_mag(string);
+            string.x /= mag;
+            string.y /= mag;
             double angle = atan2(string.y, string.x);
-            double scale_factor =  max_len / vect_mag(string);
+            double disp = mag - max_len;
+
+            thing->next_position.x += cos(angle) * disp;
+            thing->next_position.y += sin(angle) * disp;
+
 
             Vect corrected_string;
             corrected_string.x = cos(angle) * max_len;
             corrected_string.y = sin(angle) * max_len;
-            printf("corrected string : %f %f\n", corrected_string.x, corrected_string.y);
-            thing->next_position = vect_scalar(thing->next_position, 
-                    scale_factor);
-
-            /*thing->next_position.x += cos(angle) * disp;*/
-            /*thing->next_position.y += sin(angle) * disp;*/
-
-            // move the thing back to the right position
-            /*Vect adj_pos = vect_add(vect_scalar(corrected_string, -1),*/
-                    /*string);*/
-            /*adj_pos = vect_scalar(adj_pos, -1);*/
-            /*thing->position = vect_add(thing->position, adj_pos);*/
 
             // set velocity to 0 in direction of string
-            Vect vel_correction = project_vect(thing->vel, string);
-            double dir;
-            double corr = vect_scalar_projection(thing->vel, string);
-            if (corr < 0) {
-                corr *= -1;
-            } 
-            Vect v_corr;
-            v_corr.y = corr * sin(angle);
-            v_corr.x = corr * cos(angle);
-
-            thing->vel = vect_add(v_corr, thing->vel);
-
-        }
-
+            double corr_mag = vect_scalar_projection(thing->vel, string);
 
+            thing->vel.x -= corr_mag * cos(angle);
+            thing->vel.y -= corr_mag * sin(angle);
 
+        }
     }
 
     uint32_t now = SDL_GetTicks();
@@ -860,7 +837,36 @@ void advance_thing(Body * thing) {
 
 }
 
+
+/*void destroy_projectile(Projectile** proj) {*/
+    /*for (int i = 0; i < things_in_world; i ++) {*/
+        /*if (world[i].projectile == *proj) {*/
+
+        /*}*/
+    /*}*/
+/*}*/
+
+void *default_projectile_step(struct Projectile *self) {
+    advance_thing(self->physics);
+    return NULL;
+}
+
+void *default_projectile_on_collision(struct Projectile *self) {
+    self->physics->dynamics = false;
+    return NULL;
+}
+
+void get_projectile(Projectile** proj) {
+    *proj = calloc(1, sizeof(Projectile));
+    get_new_physics(&(*proj)->physics);
+    (*proj)->physics->dynamics = true;
+    (*proj)->on_step = default_projectile_step;
+    (*proj)->on_collision = default_projectile_on_collision;
+}
+
 void advance_things(void) {
+    int numcols;
+    Vect translation;
     for (int i = 0; i < things_in_world; i++) {
         switch (world[i].kind) {
             case PLAYER_W :
@@ -875,10 +881,18 @@ void advance_things(void) {
                 for (int k = 0; k < world[i].floor->numPolys; k++) {
                     advance_thing(world[i].floor->polys[k].physics);
                 }
+                break;
             case CEILING:
                 for (int k = 0; k < world[i].floor->numPolys; k++) {
                     advance_thing(world[i].floor->polys[k].physics);
                 }
+                break;
+            case PROJECTILE:
+                if ((numcols = process_collisions(world[i].projectile->physics,
+                                &translation))) {
+                    world[i].projectile->on_collision(world[i].projectile);
+                }
+                world[i].projectile->on_step(world[i].projectile);
         }
     }
 }
@@ -897,6 +911,23 @@ void add_to_world(world_thing thing) {
     //printf("Added to world: %d\n", thing.kind);
 }
 
+/* Send a projectile from body in direction dir (in degrees) with the force of 
+ * sten newtons */
+void fire_projectile(Body *from, int stren, int dir) {
+    Projectile *proj;
+    get_projectile(&proj);
+
+    double radians = (double)dir * (M_PI / 180);
+    set_motor_newtons(proj->physics, 0, stren * cos(radians), 
+            stren * sin(radians)); 
+    set_motor_timeout(proj->physics, 0, 2);
+
+    world_thing proj_world;
+    proj_world.projectile = proj;
+    proj_world.kind = PROJECTILE;
+    add_to_world(proj_world);
+}
+
 void get_room(void) {
     int floorsize = 100;
     FloorPoly *polys = generate_floor_simple(floorsize, true, 1300);
@@ -928,7 +959,7 @@ void startgame(SDL_Renderer * ren) {
     memset(world, 0, sizeof(world_thing) * 100);
     SDL_GetRendererOutputSize(ren, &width, &height);
 
-    player = get_player(100,1000);
+    player = get_player(2000,3000);
 
     world_thing player_world;
 
@@ -987,5 +1018,4 @@ void step(int interval) {
     }
 
     advance_things();
-    logwrite(DEBUG, "Updated Physics \n");
 }