sopwith: added an example of Sopwith plane physics
This commit is contained in:
Erkki Seppälä
parent
b377c9805d
commit
e9a3f82903
1 changed files with 296 additions and 0 deletions
296
doc/sopwith.ml
Normal file
296
doc/sopwith.ml
Normal file
|
|
@ -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
|
||||
Loading…
Reference in a new issue