From e9a3f829032684c8946a3a815aad47e86d20991e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erkki=20Sepp=C3=A4l=C3=A4?= Date: Sat, 18 Oct 2014 09:52:44 +0300 Subject: [PATCH] sopwith: added an example of Sopwith plane physics --- doc/sopwith.ml | 296 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 296 insertions(+) create mode 100644 doc/sopwith.ml diff --git a/doc/sopwith.ml b/doc/sopwith.ml new file mode 100644 index 0000000..3998b1a --- /dev/null +++ b/doc/sopwith.ml @@ -0,0 +1,296 @@ +module C = Chipmunk +module CO = Chipmunk.OO +module CL = Chipmunk.Low_level +open Physics + +open V.VecOps + +let pi = 4.0 *. atan 1.0 + +let negate = Gg.V2.neg + +let draw_cp_polygon = false +let draw_force_vectors = false +let plane_mass = 500.0 +let yaw_speed = 0.005 (* relative to forward speed *) +let accel_speed = 1000.0 +let decel_speed = 2000.0 +let initial_motor_speed = 0.0 +let initial_speed = 0.0 +let max_motor_speed = 4000.0 +let fwd_frict_coeff = 0.2 +let nor_frict_coeff = 2.0 +let tail_frict_coeff = 0.5 +let head_area = 1.0 +let plane_area = 10.0 +let tail_area = 0.2 +let turn_speed = 0.2 +let wing_lift = 0.1 + +(* +let initial_motor_speed = 0.0 +let frict_coeff = 0.0 +let head_area = 0.0 +let tail_area = 0.0 +*) + +type yaw = + | YawRight + | YawRightToOther + | YawOtherToRight + | YawOther + +let angle_of_yaw v = 180.0 *. (1.0 -. (cos (v *. pi) /. 2.0 +. 0.5)) + +let sign = function + | x when x >= 0.0 -> 1.0 + | _ -> -1.0 + +let polygon = [(-25.0, 5.0); (13.0,5.0); (13.0, -2.0); (8.0, -6.0); (2.0, -4.0);] + +class plane (space : CO.cp_space) (at0 : Gg.V2.t) = + (* let schema = Ase.normalize_model (List.concat (Ase.load "foo.ase")) in *) + let schema = (* Ase.normalize_model *) (List.concat (List.map snd (Ase.flip_on (fun (name, _) -> try ignore (Pcre.exec ~pat:"flip" name); true with Not_found -> false) (Ase.load "sopwith.ase")))) in + (* let schema = (* Ase.normalize_model *) (List.concat (Ase.load "sphere.ase")) in *) + (* let schema = (* Ase.normalize_model *) (List.concat (Ase.load "cube.ase")) in *) + let body = + new CO.cp_body plane_mass + (CO.moment_for_poly plane_mass + (Array.of_list (List.map cpvt polygon)) + (cpvt (5.0, 3.0))) in + let shapes = ref [] in + let add_shape shape = + space#add_shape shape; + shape#set_friction 0.0; + shapes := shape::!shapes + in + (*let motor_sound = MotorSound.create () in*) + let add_shapes flipped = + let mapv (x, y) = + if flipped then cpvt (x, -.y) + else cpvt (x, y) + in + let optrev xs = + if flipped then List.rev xs + else xs + in + let optrev_array xs = + if flipped then Array.of_list (List.rev (Array.to_list xs)) + else xs + in + let listmap f xs = optrev (List.map f xs) in + List.iter (fun shape -> space#remove_shape shape) !shapes; + List.iter (fun v -> v#free) !shapes; + shapes := []; + List.iter add_shape + [new CO.cp_shape body (CO.POLY_SHAPE ((Array.of_list (listmap mapv polygon)), mapv (0.0, 0.0))); + new CO.cp_shape body (CO.CIRCLE_SHAPE (4.0, mapv (-25.0, 0.0))); + new CO.cp_shape body (CO.POLY_SHAPE (optrev_array [|mapv (-25.0, 5.0); mapv (-10.0, 5.0); + mapv (-25.0, -3.0)|], mapv (0.0, 0.0))); + new CO.cp_shape body (CO.CIRCLE_SHAPE (4.0, mapv (8.0, -3.0)))] + in +object (self) + val mutable accelerating = false + val mutable decelerating = false + val mutable turning_cw = false + val mutable turning_ccw = false + val mutable yawing = YawRight + val mutable yaw = 0.0 + val mutable motor_speed = initial_motor_speed + val mutable forces = [] + + method where = uncpv body#get_pos + + method angle = body#get_angle + + method private reset_forces = + body#reset_forces; + forces <- [] + + method private add_force label force at = + body#apply_force (cpv force) (cpv at); + forces <- (label, force, at)::forces; + + method step (message : string -> unit) (delta : float) = + (* Speed of the plane *) + let vel = uncpv body#get_vel in + let abs_vel = V.abs2 vel in + + let base = V.base (V.vec_of_ang (~-.(body#get_angle))) in + + let to_base v = V.mul2 v base in + + let fwd = V.vec_of_ang body#get_angle in + let normal = V.vec_of_ang (body#get_angle +. pi /. 2.0) in + + (* What is the tail speed towards the normal of the plane speed vector *) + let tail_speed = body#get_a_vel *. pi *. 2.0 *. 20.0 in + let tail_vel = to_base (Gg.V2.v tail_speed 0.0) in + let abs_tail_vel = vel +.| tail_vel in + + (* How much is the speed towards plane head. can be negative. *) + let fwd_vel = Gg.V2.dot vel fwd in + let normal_vel = Gg.V2.dot vel normal in + + let head_angle = body#get_angle in + + let rel_force label force at = self#add_force label (to_base force) (to_base at) in + + let speed_angle = + let (vel_x, vel_y) = uncpvt body#get_vel in + if abs_vel < 1.0 then + head_angle + else + atan2 vel_y vel_x + in + + let air_wing_angle = + let v = head_angle -. speed_angle in + if v > pi then + v -. 2.0 *. pi + else + v + in + + let lift_coeff = Lift.lift air_wing_angle in + + (*Printf.printf "air wing angle: %f %f %f -> %f\n%!" + air_wing_angle + (fst (uncpv body#get_vel)) + (snd (uncpv body#get_vel)) + lift_coeff; + + Printf.printf "body angle: %f\n%!" body#get_angle;*) + + self#reset_forces; + + (* If accelerating, increase motor speed *) + if accelerating then motor_speed <- min max_motor_speed (motor_speed +. delta *. accel_speed); + + if decelerating then motor_speed <- max 0.0 (motor_speed -. delta *. decel_speed); + + (*MotorSound.set_freq motor_sound motor_speed;*) + + (* Motor speed applies forward force *) + self#add_force "motor" ((V.v2 (motor_speed *. 10.0) *.| V.vec_of_ang body#get_angle)) Gg.V2.zero; + + (* Air friction (and drag?) opposes movement towards plane velocity *) + (* self#add_force "fwddrag" *) + (* (Gg.V2.smul (fwd_frict_coeff *. fwd_vel ** 2.0 *. head_area) (V.unit (negate vel))) *) + (* (to_base (Gg.V2.v 0.0 0.0)); *) + + (* Air friction (and drag?) opposes movement towards plane velocity normal also *) + self#add_force "hddrag" + (Gg.V2.smul (nor_frict_coeff *. normal_vel ** 2.0 *. plane_area *. sign normal_vel) (negate (V.unit normal))) + (to_base (Gg.V2.v ~-.1.0 0.0)); + + (* self#add_force "tldrag" *) + (* (Gg.V2.smul (nor_frict_coeff *. tail_speed ** 2.0 *. tail_area *. sign tail_speed) (negate (V.unit normal))) *) + (* (to_base (-20.0, 0.0)); *) + + (* Upward lift *) + (* self#add_force (Gg.V2.smul (0.01 *. fwd_vel ** 2.0) (V.vec_of_ang (body#get_angle +. pi /. 2.0))) (0.0, 0.0); *) + self#add_force "lift" (Gg.V2.smul (wing_lift *. fwd_vel ** 2.0 *. lift_coeff) (V.vec_of_ang (body#get_angle +. pi /. 2.0))) Gg.V2.zero; + + (* (\* When turning cw, apply opposing (ccw) force to the tail *\) *) + if turning_cw then + rel_force "cw" (Gg.V2.smul (turn_speed *. fwd_vel ** 2.0 *. sign fwd_vel) (Gg.V2.v 0.0 1.0)) (Gg.V2.v ~-.20.0 0.0); + + (* (\* When turning ccw, apply opposing (cw) force to the tail *\) *) + if turning_ccw then + rel_force "ccw" (Gg.V2.smul (turn_speed *. fwd_vel ** 2.0 *. sign fwd_vel) (Gg.V2.v 0.0 ~-.1.0)) (Gg.V2.v ~-.20.0 0.0); + + (* (\* Such force (rotation) will also be opposed by air friction *\) *) + rel_force "tf" + (Gg.V2.v 0.0 (tail_frict_coeff *. tail_speed ** 2.0 *. sign tail_speed)) + (Gg.V2.v ~-.20.0 0.0); + + (* Printf.printf "%f %f\n%!" fwd_vel tail_speed; *) + (match yawing with + | YawRight | YawOther -> () + | YawRightToOther -> + yaw <- min 1.0 (yaw +. yaw_speed *. delta *. abs_float fwd_vel); + if yaw >= 1.0 then + (yawing <- YawOther; + add_shapes true; + ) + | YawOtherToRight -> + yaw <- min 2.0 (yaw +. yaw_speed *. delta *. abs_float fwd_vel); + if yaw >= 2.0 then + (yawing <- YawRight; + yaw <- 0.0; + add_shapes false) + ); + + let printf fmt = Printf.ksprintf message fmt in + printf "Location: %f, %f" (Gg.V2.x self#where) (Gg.V2.y self#where); + printf "Motor speed: %f" motor_speed; + printf "Velocity: %f" abs_vel; + + method begin_accel = accelerating <- true + method end_accel = accelerating <- false + + method begin_decel = decelerating <- true + method end_decel = decelerating <- false + + method begin_turn_cw = turning_cw <- true + method end_turn_cw = turning_cw <- false + + method begin_turn_ccw = turning_ccw <- true + method end_turn_ccw = turning_ccw <- false + + method begin_yaw_cw = () + method end_yaw_cw = () + + method begin_yaw_ccw = () + method end_yaw_ccw = () + + method swap_yaw = + match yawing with + | YawRightToOther | YawOtherToRight -> () + | YawRight -> yawing <- YawRightToOther + | YawOther -> yawing <- YawOtherToRight + + method render (surface : Sdlvideo.surface) = + let at = self#where in + let angle = self#angle in + let vel = uncpv body#get_vel in + let abs_vel = V.abs2 vel in + Render.render at 15.0 (angle_of_yaw yaw, 0.0, angle /. pi *. 180.0) schema; + GlMat.mode `modelview; + + Gl.disable `lighting; + Gl.disable `depth_test; + GlDraw.color (1.0, 1.0, 1.0); + + if draw_cp_polygon then + (GlDraw.begins `polygon; + List.iter (fun v -> GlDraw.vertex2 (Gg.V2.to_tuple (self#where +.| (V.mul2 (Gg.V2.of_tuple v) (V.base (V.vec_of_ang (~-.(body#get_angle)))))))) polygon; + GlDraw.ends ()); + + if draw_force_vectors then + List.iter + (fun (label, dir, at) -> + let at = self#where +.| at in + let dir = dir /.| V.v2 50.0 in + Render.vector ~label:(fun () -> (Lazy.force Display.font_render_annot) label) + (Gg.V3.v (Gg.V2.x at) (Gg.V2.y at) 0.0) + (Gg.V3.v (Gg.V2.x dir) (Gg.V2.y dir) 0.0) + ) + forces; + + Gl.enable `lighting; + Gl.enable `depth_test; + + method reset () = + body#set_a_vel 0.0; + body#set_angle 0.0; + body#set_pos (cpv at0); + body#set_vel (cpvt (initial_speed, 0.0)); + + initializer + self#reset (); + add_shapes false; + space#add_body body; + (*MotorSound.start motor_sound;*) +end