reworked collisions + added proper (and homogenous) force computation

main.ml

@@ -34,7 +34,7 @@ type ball = {
   xy : pt_2d ;
   v : pt_2d ;
   a : pt_2d ;
-  angv : pt_2d ;
+  fres : pt_2d ;
 } ;;
 
 let univ_dt = 0.05 ;;
@@ -42,6 +42,8 @@ let univ_friction = 0.8 ;;
 let univ_g = 300.0 ;;
 let pi = 3.14159265358979343 ;;
 
+let gforce = {x = 0. ; y = -. univ_g} ;;
+
 let score = ref 0 ;;
 
 (* ------------------------------------------------------------------------------------- *)
@@ -97,6 +99,16 @@ let vect_normal_2D (p1 : pt_2d) (p2 : pt_2d) =
     y = (p2.x -. p1.x) ;
   } ;;
 
+let return_proj_of_point (m : pt_2d) (spt : pt_2d) (ept : pt_2d) =
+  match (-. ((ept.x -. spt.x) *. (spt.x -. m.x) +. (ept.y -. spt.y) *. (spt.y -. m.y)) /. ((ept.x -. spt.x) *. (ept.x -. spt.x) +. (ept.y -. spt.y) *. (ept.y -. spt.y))) with
+    | k when k >= 0. && k <= 1. -> (vect_convexf spt ept k)
+    | k when k < 0. -> spt
+    | k -> ept ;;
+
+let return_proj_of_point_D (m : pt_2d) (spt : pt_2d) (ept : pt_2d) =
+  let theta = (-. ((ept.x -. spt.x) *. (spt.x -. m.x) +. (ept.y -. spt.y) *. (spt.y -. m.y)) /. ((ept.x -. spt.x) *. (ept.x -. spt.x) +. (ept.y -. spt.y) *. (ept.y -. spt.y))) in
+  (vect_convexf spt ept theta) ;;
+
 let vect_dot_product_2D (p1 : pt_2d) (p2 : pt_2d) =
   p1.x *. p2.x +. p1.y *. p2.y ;;
 
@@ -112,6 +124,11 @@ let vect_scale_2D (v1 : pt_2d) (v2 : pt_2d) =
 let vect_normalize_2D (v1 : pt_2d) =
   vect_mult_2D v1 (1.0 /. (vect_norm_2D v1)) ;;
 
+let vect_symmetry (m : pt_2d) (p1 : pt_2d) (p2 : pt_2d) =
+  let proj = return_proj_of_point_D m p1 p2 in
+  let ortho = vect_diff_2D proj m in
+  vect_sum_2D (vect_sum_2D ortho ortho) m ;;
+
 (* ------------------------------------------------------------------------------------- *)
 (* ------------------------------------------------------------------------------------- *)
 (* XXXXXX Physics functions *)
@@ -131,12 +148,6 @@ let distance_line_segment (m : pt_2d) (spt : pt_2d) (ept : pt_2d) =
     | k when k >= 0. && k <= 1. -> vect_dist_2D (vect_convexf spt ept k) m
     | k when k < 0. -> vect_dist_2D spt m
     | k -> vect_dist_2D ept m ;;
-
-let return_proj_of_point (m : pt_2d) (spt : pt_2d) (ept : pt_2d) =
-  match (-. ((ept.x -. spt.x) *. (spt.x -. m.x) +. (ept.y -. spt.y) *. (spt.y -. m.y)) /. ((ept.x -. spt.x) *. (ept.x -. spt.x) +. (ept.y -. spt.y) *. (ept.y -. spt.y))) with
-    | k when k >= 0. && k <= 1. -> (vect_convexf spt ept k)
-    | k when k < 0. -> spt
-    | k -> ept;;
   
 let is_collision (b : ball) (poly : polygon) (dt : float) =
   (* returns the 1st point of the line that the ball collides with *)
@@ -156,6 +167,9 @@ let is_collision (b : ball) (poly : polygon) (dt : float) =
 let update_ball_data (b : ball) (polys : polygon array) (dt : float) =
   let add = ref true in
 
+  b.fres.x <- 0. ;
+  b.fres.y <- 0. ;
+
   for p = 0 to (Array.length polys -1) do
     let hit = (is_collision b polys.(p) dt) in
     if !add && hit <> -1 then begin
@@ -163,30 +177,35 @@ let update_ball_data (b : ball) (polys : polygon array) (dt : float) =
       score := !score + polys.(p).score ;
 
       (* apply normal reaction force *)
-      b.v.x <- b.v.x -. b.a.x *. dt ;
-      b.v.y <- b.v.y -. b.a.y *. dt ;
-
       let hit2 = (hit +1) mod (Array.length polys.(p).vertexes) in
       let proj = return_proj_of_point b.xy polys.(p).vertexes.(hit) polys.(p).vertexes.(hit2) in
+      let proj_n = vect_normalize_2D (vect_diff_2D b.xy proj) in
+      let reaction_force_2 = vect_mult_2D proj_n (univ_g *. b.mass *. (vect_dot_product_2D (vect_normalize_2D gforce) proj_n)) in
 
-      let nv = vect_norm_2D b.v in
+      b.fres.x <- b.fres.x +. reaction_force_2.x *. polys.(p).restitution ;
-      
+      b.fres.y <- b.fres.y +. reaction_force_2.y *. polys.(p).restitution ;
-      let reaction_force = vect_mult_2D (vect_normalize_2D (vect_diff_2D b.xy proj)) nv in
-      
-      b.v.x <- ((reaction_force.x *. polys.(p).restitution))  ;
-      b.v.y <- ((reaction_force.y *. polys.(p).restitution))  ;
 
-      (* apply friction/rotational force *)
+      (* change velocity according to angle *)
+      let director = vect_diff_2D polys.(p).vertexes.(hit2) polys.(p).vertexes.(hit) in
+      let symmetric = vect_symmetry b.v {x = 0. ; y = 0.} director in
+
+      b.v.x <- symmetric.x ;
+      b.v.y <- symmetric.y ;
     end
   done ;
 
-  b.xy.x <- b.xy.x +. b.v.x *. dt ;
+  (* P = mg *)
-  b.xy.y <- b.xy.y +. b.v.y *. dt ;
+  b.fres.y <- b.fres.y -. univ_g *. b.mass ;
+
+  (* PFD : ma = sum(F) *)
+  b.a.x <- b.fres.x /. b.mass ;
+  b.a.y <- b.fres.y /. b.mass ;
 
   b.v.x <- b.v.x +. b.a.x *. dt ;
   b.v.y <- b.v.y +. b.a.y *. dt ;
 
-  b.a.y <- -. univ_g ;;
+  b.xy.x <- b.xy.x +. b.v.x *. dt ;
+  b.xy.y <- b.xy.y +. b.v.y *. dt ;;
 
 (* ------------------------------------------------------------------------------------- *)
 (* ------------------------------------------------------------------------------------- *)
@@ -252,7 +271,7 @@ let create_ball (r : float) (x0 : int) (y0 : int) (m : float) (red : int) (green
     xy = {x = float_of_int x0; y = float_of_int y0} ;
     v = {x = 0. ; y = 0.} ;
     a = {x = 0. ; y = 0.} ;
-    angv = {x = 0. ; y = 0.} ;
+    fres = {x = 0. ; y = 0.} ;
   } ;;
 
 let create_polygon (arr : (int * int) array) (rest : float) (pts : int) (red : int) (green : int) (blue : int) =
@@ -275,8 +294,9 @@ let simulate () =
   open_graph " 1200x800" ;
   set_window_title "WAH" ;
 
-  let pinball = create_ball 25.0 600 700 0.15 169 169 169 in
+  let pinball = create_ball 25.0 150 730 0.15 169 169 169 in
-  let triangle = create_polygon [|(100, 100); (1100, 800); (1100, 100)|] 0.75 0 128 255 128 in
+  let triangle = create_polygon [|(100, 100); (100, 700); (1100, 300); (1100, 100)|] 0.15 0 128 255 128 in
+  let triangle2 = create_polygon [|(1100, 100); (1200, 100); (1200, 750); (1100, 750)|] 1.0 0 255 128 128 in
 
   while true do
     let __start = Unix.gettimeofday() in
@@ -289,13 +309,14 @@ let simulate () =
 
     set_line_width 1 ;
     draw_polygon triangle ;
+    draw_polygon triangle2 ;
     draw_ball pinball ;
 
     auto_synchronize true ;
     Unix.sleepf 0.005 ;
 
     let __end = Unix.gettimeofday() in
-    update_ball_data pinball [|triangle|] (__end -. __start) ;
+    update_ball_data pinball [|triangle; triangle2|] (__end -. __start) ;
   done;
 
   close_graph () ;;