├── Makefile ├── .gitignore ├── contact.ml ├── src ├── META ├── ode_version.sh ├── ode_version.ml ├── ask_version.ml ├── _style.css └── Makefile ├── examples ├── katamari.sh ├── drawstuff.make ├── demo_opt.make ├── Makefile ├── demo_exec.sh ├── simple.ml ├── demo_chain2.ml ├── demo_friction.ml ├── demo_cylvssphere.ml ├── demo_plane2d.ml ├── demo_feedback.ml ├── demo_buggy.ml ├── demo_I.ml ├── demo_boxstack.ml ├── demo_basket.ml ├── drawstuff.ml └── katamari.ml ├── LICENSE_ZLIB.txt ├── CHANGES.txt ├── opam ├── LICENSE_BSD.txt ├── README.txt └── LICENSE_LGPL.txt /Makefile: -------------------------------------------------------------------------------- 1 | all: all 2 | %:: 3 | $(MAKE) -C src $@ 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.[oa] 2 | *.cm[ioxa] 3 | *.cmx[as] 4 | *.so 5 | *.swp 6 | *~ 7 | *.opt 8 | ode.mli 9 | ode_version.h 10 | -------------------------------------------------------------------------------- /contact.ml: -------------------------------------------------------------------------------- 1 | print_endline begin 2 | String.map 3 | (fun c -> char_of_int (int_of_char c - 1)) 4 | "npoojfs/gmpsfouAhnbjm/dpn" 5 | end 6 | -------------------------------------------------------------------------------- /src/META: -------------------------------------------------------------------------------- 1 | name="ocamlode" 2 | version="0.7" 3 | description="OCaml bindings for the Open Dynamics Engine (ODE)." 4 | requires="" 5 | archive(byte) = "ode.cma" 6 | archive(native) = "ode.cmxa" 7 | linkopts = "-cclib \"`ode-config --libs`\"" 8 | -------------------------------------------------------------------------------- /examples/katamari.sh: -------------------------------------------------------------------------------- 1 | opam install extlib 2 | opam install ocamlsdl 3 | opam install lablgl 4 | 5 | # -I $(ocamlfind query ode) 6 | 7 | ocamlopt.opt \ 8 | bigarray.cmxa unix.cmxa \ 9 | -I $(ocamlfind query sdl) sdl.cmxa \ 10 | -I $(ocamlfind query extlib) extLib.cmxa \ 11 | -I $(ocamlfind query lablgl) lablgl.cmxa lablglut.cmxa \ 12 | -I ../src ode.cmxa \ 13 | katamari.ml \ 14 | -o katamari.opt 15 | -------------------------------------------------------------------------------- /examples/drawstuff.make: -------------------------------------------------------------------------------- 1 | #GL_PATH="+glMLite" 2 | GL_PATH="$(ocamlfind query glMLite)" 3 | 4 | .PHONY: all opt 5 | all: drawstuff.cma 6 | opt: drawstuff.cmxa 7 | 8 | drawstuff.cma: drawstuff.ml 9 | ocamlc -a -o $@ \ 10 | -I $(GL_PATH) GL.cma Glu.cma Glut.cma \ 11 | -I ../src ode.cma \ 12 | $< 13 | 14 | drawstuff.cmxa: drawstuff.ml 15 | ocamlopt -a -o $@ \ 16 | -I $(GL_PATH) GL.cmx Glu.cmx Glut.cmx \ 17 | -I ../src ode.cmx \ 18 | $< 19 | 20 | # vim: filetype=make 21 | -------------------------------------------------------------------------------- /src/ode_version.sh: -------------------------------------------------------------------------------- 1 | # this file is not used, but kept in case it can be usefull 2 | 3 | ODE_VER=`ode-config --version`.0.0.0 4 | REGEX="\([0-9]*\)[.]\([0-9]*\)[.]\([0-9]*\).*" 5 | ODE_MAJOR=`echo $ODE_VER | sed "s/$REGEX/\1/"` 6 | ODE_MINOR=`echo $ODE_VER | sed "s/$REGEX/\2/"` 7 | ODE_MICRO=`echo $ODE_VER | sed "s/$REGEX/\3/"` 8 | 9 | MAJOR_VERSION="ODE_VERSION_MAJOR=$ODE_MAJOR" 10 | MINOR_VERSION="ODE_VERSION_MINOR=$ODE_MINOR" 11 | MICRO_VERSION="ODE_VERSION_MICRO=$ODE_MICRO" 12 | 13 | echo "-D$MAJOR_VERSION -D$MINOR_VERSION -D$MICRO_VERSION" 14 | 15 | -------------------------------------------------------------------------------- /examples/demo_opt.make: -------------------------------------------------------------------------------- 1 | GL_PATH="+glMLite" 2 | DEMO=demo_buggy.ml 3 | 4 | .PHONY: demo all 5 | all: demo 6 | 7 | ode.cmxa: ode.ml ode_c.c 8 | make ode.cmxa -f Makefile 9 | 10 | drawstuff.cmxa: drawstuff.ml 11 | make \ 12 | drawstuff.cmxa \ 13 | -f drawstuff.make \ 14 | -e GL_PATH=$(GL_PATH) 15 | 16 | demo: ode.cmxa drawstuff.cmxa 17 | ocamlopt -ccopt -g \ 18 | -I $(GL_PATH) GL.cmxa Glu.cmxa Glut.cmxa \ 19 | -I . -cclib -l_mlode_stubs ode.cmxa drawstuff.cmxa \ 20 | $(DEMO) -o `basename $(DEMO) .ml`.opt 21 | 22 | # vim: filetype=make 23 | -------------------------------------------------------------------------------- /src/ode_version.ml: -------------------------------------------------------------------------------- 1 | #load "unix.cma" 2 | #load "str.cma" 3 | 4 | let () = 5 | let ic, oc = Unix.open_process "ode-config --version" in 6 | let r = input_line ic in 7 | let rs = r ^ ".0.0.0" in 8 | let pat = Str.regexp "\\([0-9]+\\)[.]\\([0-9]+\\)[.]\\([0-9]+\\).*" in 9 | if not(Str.string_match pat rs 0) 10 | then failwith "error while matching ode version" 11 | else begin 12 | let ode_major = Str.matched_group 1 rs 13 | and ode_minor = Str.matched_group 2 rs 14 | and ode_micro = Str.matched_group 3 rs 15 | in 16 | let argv = Array.to_list Sys.argv in 17 | if List.mem "-major" argv then print_string ode_major else 18 | if List.mem "-minor" argv then print_string ode_minor else 19 | if List.mem "-micro" argv then print_string ode_micro 20 | end 21 | ;; 22 | 23 | (* vim: sw=2 sts=2 ts=2 et fdm=marker 24 | *) 25 | -------------------------------------------------------------------------------- /examples/Makefile: -------------------------------------------------------------------------------- 1 | # This file is a build script for the ocaml-ode bindings. 2 | # Copyright (C) 2008 Florent Monnier 3 | # 4 | # This Makefile builds the ocaml-ode bindings. 5 | # 6 | # This Makefile is provided "AS-IS", without any express or implied warranty. 7 | # In no event will the authors be held liable for any damages arising from 8 | # the use of this software. 9 | # 10 | # Permission is granted to anyone to use this software for any purpose, 11 | # including commercial applications, and to alter it and redistribute it 12 | # freely. 13 | 14 | OCAMLC := ocamlc.opt 15 | OCAMLOPT := ocamlopt.opt 16 | OCAMLDOC := ocamldoc.opt 17 | 18 | all: 19 | 20 | .PHONY: all demo clean 21 | 22 | DEMO = katamari 23 | 24 | demo: $(DEMO) 25 | $(DEMO): $(DEMO).ml ode.cmxa 26 | $(OCAMLOPT) -I ../src ode.cmxa $< -o $@ 27 | 28 | clean: 29 | $(RM) *.[oa] *.so *.cm[ixoa] *.cmxa *.opt *~ 30 | 31 | #EOF 32 | -------------------------------------------------------------------------------- /LICENSE_ZLIB.txt: -------------------------------------------------------------------------------- 1 | This software is provided "AS-IS", without any express or implied warranty. 2 | In no event will the authors be held liable for any damages arising from 3 | the use of this software. 4 | 5 | Permission is granted to anyone to use this software for any purpose, 6 | including commercial applications, and to alter it and redistribute it 7 | freely, subject to the following restrictions: 8 | 9 | 1. The origin of this software must not be misrepresented; you must not 10 | claim that you wrote the original software. If you use this software 11 | in a product, an acknowledgment in the product documentation would be 12 | appreciated but is not required. 13 | 14 | 2. Altered source versions must be plainly marked as such, and must not be 15 | misrepresented as being the original software. 16 | 17 | 3. This notice may not be removed or altered from any source distribution. 18 | 19 | -------------------------------------------------------------------------------- /CHANGES.txt: -------------------------------------------------------------------------------- 1 | ocamlode-0.5-r2 2 | - Original package from Richard Jones. 3 | 4 | ocamlode-0.5-r4 5 | - 2 bugs corrected 6 | - possible to wrap an ODE compiled with dSINGLE or dDOUBLE 7 | - added missing functions and up-dated to the last version of ODE 8 | - a new makefile that does not require extra dependencies 9 | (the original one requires extlib, findlib, lablgl, sdl) 10 | - type constraint for each kind of geometry 11 | - added some demos from the ODE sources converted from C to OCaml 12 | - generating ode_version.h for versioning 13 | 14 | ocamlode-0.6 15 | - 1 bugs corrected in ode_c.c in the function ocamlode_dPlaneSpace() 16 | - 1 correction in the demo file demo_I.ml 17 | 18 | ocamlode-0.6.2 19 | - switched the core bindings from LGPL to Zlib license 20 | - organise the files into a directory hierarchy src/ examples/ 21 | 22 | ocamlode-0.7 23 | - remove functions deprecated in ODE version 0.16 24 | 25 | -------------------------------------------------------------------------------- /src/ask_version.ml: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env ocaml 2 | 3 | while true do try 4 | Printf.printf "Please enter the ODE version number you're using\n\ 5 | Example: 0.10.0\n%!"; 6 | let major, minor, micro = 7 | Scanf.sscanf (input_line stdin) "%d.%d.%d" (fun a b c -> (a,b,c)) 8 | in 9 | let out_file = (try Sys.argv.(1) with _ -> "ode_version.h") in 10 | 11 | Printf.printf "Understood ODE version:\n"; 12 | Printf.printf " major version number: %d\n" major; 13 | Printf.printf " minor version number: %d\n" minor; 14 | Printf.printf " micro version number: %d\n" micro; 15 | Printf.printf "writting according header: %s\n" out_file; 16 | Printf.printf "%!"; 17 | 18 | let oc = open_out out_file in 19 | Printf.fprintf oc "/* generated by ask_version.ml */\n"; 20 | Printf.fprintf oc "#define ODE_VERSION_MAJOR %d\n" major; 21 | Printf.fprintf oc "#define ODE_VERSION_MINOR %d\n" minor; 22 | Printf.fprintf oc "#define ODE_VERSION_MICRO %d\n" micro; 23 | close_out oc; 24 | exit 0 25 | with Scanf.Scan_failure _ -> () done 26 | ;; 27 | 28 | -------------------------------------------------------------------------------- /examples/demo_exec.sh: -------------------------------------------------------------------------------- 1 | # allows to run this script from any directory 2 | cd `dirname $0` 3 | 4 | 5 | opam install glMLite 6 | 7 | GL_PATH="$(ocamlfind query glMLite)" 8 | 9 | ODE_PATH="../src" 10 | 11 | 12 | # build the libraries 13 | make ode.cma 14 | make drawstuff.cma -f drawstuff.make -e GL_PATH="$GL_PATH" 15 | 16 | 17 | # usage of the demos 18 | echo " 19 | - use the mouse to rotate the camera 20 | - use arrows to move around (ex: up arrow to go forward) 21 | - use page up/down to go higher / lower 22 | - escape or 'q' key to quit 23 | " 24 | 25 | if [ $# == 0 ] 26 | then # without any argument this script executes all the demos 27 | DEMOS="demo_chain2.ml demo_plane2d.ml demo_buggy.ml \ 28 | demo_basket.ml demo_friction.ml demo_feedback.ml \ 29 | demo_I.ml demo_boxstack.ml" 30 | else 31 | # otherwise executes only the requested demos 32 | DEMOS=$* 33 | fi 34 | 35 | for demo in $DEMOS 36 | do # executes the demos in interpreted mode 37 | echo "# running '$demo'" 38 | ocaml -w -6 \ 39 | -I "$GL_PATH" \ 40 | -I "$ODE_PATH" \ 41 | drawstuff.cma ode.cma \ 42 | $demo 43 | done 44 | 45 | -------------------------------------------------------------------------------- /opam: -------------------------------------------------------------------------------- 1 | opam-version: "2.0" 2 | name: "ode" 3 | license: "Zlib" 4 | authors: [ 5 | "Richard W.M. Jones" 6 | "Florent Monnier" 7 | ] 8 | maintainer: "https://github.com/fccm/" 9 | homepage: "https://github.com/fccm/OCamlODE" 10 | bug-reports: "https://github.com/fccm/OCamlODE/issues" 11 | dev-repo: "git+https://github.com/fccm/OCamlODE.git" 12 | doc: "http://decapode314.free.fr/ocaml/ode/doc/" 13 | 14 | tags: [ "bindings" "physics" "dynamics" "engine" "3D" "cross-platform" ] 15 | synopsis: "Bindings to the ODE library (Open Dynamics Engine)" 16 | description: """ 17 | An OCaml interface to the ODE Physics SDK. 18 | 19 | ODE homepage: https://www.ode.org/ 20 | 21 | ODE is an open source, high performance library for simulating rigid body dynamics. 22 | It is fully featured, stable, mature and platform independent. 23 | It has advanced joint types and integrated collision detection with friction. 24 | ODE is useful for simulating vehicles, objects in virtual reality environments and 25 | virtual creatures. 26 | It is currently used in many computer games, 3D authoring tools and simulation tools. 27 | 28 | This version of the bindings is known to work with ODE version 0.16 29 | """ 30 | depends: [ 31 | "ocaml" 32 | "ocamlfind" {build} 33 | "conf-ode" 34 | ] 35 | build: [ 36 | [make "-C" "src" "all"] 37 | ] 38 | install: [ 39 | [make "-C" "src" "install_findlib"] 40 | ] 41 | -------------------------------------------------------------------------------- /LICENSE_BSD.txt: -------------------------------------------------------------------------------- 1 | 2 | This is the BSD-style license for the Open Dynamics Engine 3 | ---------------------------------------------------------- 4 | 5 | Open Dynamics Engine 6 | Copyright (c) 2001-2007, Russell L. Smith. 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions 11 | are met: 12 | 13 | Redistributions of source code must retain the above copyright notice, 14 | this list of conditions and the following disclaimer. 15 | 16 | Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | Neither the names of ODE's copyright owner nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 28 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 29 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 30 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 31 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 32 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 33 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 34 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | -------------------------------------------------------------------------------- /src/_style.css: -------------------------------------------------------------------------------- 1 | body { background-color: #AAAAAA; padding-bottom: 2em; } 2 | tr { background-color: #AAAAAA } 3 | td.typefieldcomment { background-color: #AAAAAA;} 4 | pre { margin-bottom: 4px } 5 | div.sig_block {margin-left: 2em} 6 | 7 | 8 | a:link {color: #25F; text-decoration: none;} 9 | a:hover {color: #D00; text-decoration: none; background-color: #5F8;} 10 | a:active {color: Red; text-decoration: underline; } 11 | a:visited {color: #47F; text-decoration: none; } 12 | 13 | pre a:link { color: #026; } 14 | pre a:hover { color: #026; background-color: #888;} 15 | pre a:active { color: #026; } 16 | pre a:visited { color: #026; } 17 | 18 | code a:visited {color: #47F; text-decoration: none; } 19 | code a:link {color: #25F; text-decoration: none;} 20 | code a:hover {color: #D00; text-decoration: none; background-color: #5F8;} 21 | code a:active {color: Red; text-decoration: underline; } 22 | 23 | .keyword { font-weight: bold; color: Red; } 24 | .keywordsign { color: #C04600 } 25 | .superscript { font-size: 4 } 26 | .subscript { font-size: 4 } 27 | .comment { color: Green } 28 | .constructor { color: Blue } 29 | .type { color: #536 } 30 | .string { color: Maroon } 31 | .warning { color: Red; font-weight: bold } 32 | .info { margin-left: 3em; margin-right: 3em } 33 | .code { color: #157; } 34 | h1{font-size:20pt;text-align: center; } 35 | 36 | h2, h3, h4, h5, h6, 37 | div.h7, div.h8, div.h9 38 | {font-size:20pt; border:1px solid #000; margin-top:2em; margin-bottom:2px; text-align:center; padding:2px;} 39 | 40 | h2{background:#90BDFF;} 41 | h3{background:#90DDFF;} 42 | h4{background:#90EDFF;} 43 | h5{background:#90FDFF;} 44 | h6{background:#90BDFF;} 45 | div.h7{background:#9DF;} 46 | div.h8{background:#EFF;} 47 | div.h9{background:#FFF;} 48 | 49 | .typetable { border-style: hidden } 50 | .indextable { border-style: hidden } 51 | .paramstable { border-style: hidden; padding: 5pt 5pt} 52 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | OCaml bindings for the Open Dynamics Engine (ODE). 2 | Copyright (C) 2005 Richard W.M. Jones 3 | Copyright (C) 2008 Florent Monnier 4 | Current Maintainer: Florent Monnier 5 | For contact informations run: `ocaml contact.ml` 6 | 7 | This is a set of bindings in OCaml for the Open Dynamics Engine (ODE; 8 | http://www.ode.org/). 9 | 10 | License 11 | ------- 12 | 13 | This library is distributed under the Zlib License. 14 | http://opensource.org/licenses/Zlib 15 | 16 | Most of the demos that come from the ODE sources can be used either 17 | along the terms of the GNU LGPL or the BSD license. 18 | 19 | Impatients 20 | ---------- 21 | 22 | For impatients, just run the script demo_exec.sh, it should compile 23 | everything and execute the demos. 24 | 25 | Notes on the style of bindings 26 | ------------------------------ 27 | 28 | The bindings are currently quite literal. Most ODE functions are 29 | mapped literally into OCaml. There is no attempt to use special 30 | features of OCaml, particularly garbage collection, so you must 31 | destroy ODE objects by hand. It is intended that someone would write 32 | a pleasant modular / object-oriented wrapper around these basic 33 | bindings which would use finalisers to support garbage collection. 34 | 35 | The bindings can adapt itself to an ODE library compiled with dDOUBLE 36 | or dSINGLE. But if you compile with dDOUBLE, there is opportunity to 37 | use OCaml structures which are binary-compatible with ODE structures, 38 | which speeds datas exchange. 39 | 40 | Debugging 41 | --------- 42 | 43 | If it crashes (and it may well do so), try turning on type checking 44 | ('#define TYPE_CHECKING 1' at the top of ode_c.c). 45 | 46 | Make sure that the '-g' flag is being passed in $(CFLAGS), and for 47 | additional safety, make sure optimisation ('-O...') is turned off. 48 | 49 | Run the program under gdb and get a stack trace. 50 | 51 | Speed 52 | ----- 53 | 54 | Build the profile target ('make profile') and try running the example 55 | game ('katamari_p.opt'). 56 | 57 | To view the profile, do 'gprof katamari_p.opt'. 58 | -------------------------------------------------------------------------------- /examples/simple.ml: -------------------------------------------------------------------------------- 1 | (* 2 | As explained in the manual of ODE a typical simulation will proceed like this: 3 | 4 | - Create a dynamics world. 5 | - Create bodies in the dynamics world. 6 | - Set the state (position etc) of all bodies. 7 | - Create joints in the dynamics world. 8 | - Attach the joints to the bodies. 9 | - Set the parameters of all joints. 10 | - Create a collision world and collision geometry objects, as necessary. 11 | - Create a joint group to hold the contact joints. 12 | - Loop: 13 | - Apply forces to the bodies as necessary. 14 | - Adjust the joint parameters as necessary. 15 | - Call collision detection. 16 | - Create a contact joint for every collision point, 17 | and put it in the contact joint group. 18 | - Take a simulation step. 19 | - Remove all joints in the contact joint group. 20 | - Destroy the dynamics and collision worlds. 21 | 22 | Here is how it looks like in OCaml with the most simple possible example: 23 | *) 24 | 25 | open Ode.LowLevel 26 | 27 | let () = 28 | dInitODE (); 29 | let wrl = dWorldCreate () in 30 | dWorldSetGravity wrl 0. 0. (-0.9); 31 | let space = dHashSpaceCreate None in 32 | let plane = dCreatePlane (Some space) 0. 0. 1. 0. in 33 | let cgrp = dJointGroupCreate() in 34 | 35 | let (lx,ly,lz) = (1.,1.,1.) in 36 | let b = dBodyCreate wrl in 37 | dBodySetPosition b 0. 0. 1.; 38 | let m = dMassCreate () in 39 | dMassSetBox m 2.4 lx ly lz; 40 | dMassAdjust m 1.0; 41 | dBodySetMass b m; 42 | let g = dCreateBox (Some space) lx ly lz in 43 | dGeomSetBody g (Some b); 44 | 45 | let near ga gb = 46 | let surf_params = { surf_param_zero with 47 | sp_mode = [`dContactBounce]; 48 | sp_mu = dInfinity; 49 | sp_bounce = 0.7; 50 | sp_bounce_vel = 0.1; 51 | } in 52 | let cnt_arr = dCollide ga gb 5 in 53 | ArrayLabels.iter cnt_arr ~f:(fun cnt_geom -> 54 | let cnt = { 55 | c_surface = surf_params; 56 | c_geom = cnt_geom; 57 | c_fdir1 = { x=0.; y=0.; z=0.; w=0. } 58 | } in 59 | let j = dJointCreateContact wrl (Some cgrp) cnt in 60 | dJointAttach j (dGeomGetBody ga) 61 | (dGeomGetBody gb); 62 | ); 63 | in 64 | 65 | Sys.catch_break true; 66 | try while true do 67 | dSpaceCollide space near; 68 | let p = dGeomGetPosition g in 69 | Printf.printf " (%6.3f %6.3f %6.3f)\n%!" p.x p.y p.z; 70 | dWorldStep wrl 0.1; 71 | dJointGroupEmpty cgrp; 72 | Unix.sleep 1; 73 | done 74 | with Sys.Break -> 75 | dBodyDestroy b; 76 | dGeomDestroy g; 77 | dGeomDestroy plane; 78 | dSpaceDestroy space; 79 | dWorldDestroy wrl; 80 | dCloseODE (); 81 | ;; 82 | 83 | -------------------------------------------------------------------------------- /src/Makefile: -------------------------------------------------------------------------------- 1 | # This file is a build script for the ocaml-ode bindings. 2 | # Copyright (C) 2008 Florent Monnier 3 | # 4 | # This Makefile builds the ocaml-ode bindings. 5 | # 6 | # This Makefile is provided "AS-IS", without any express or implied warranty. 7 | # In no event will the authors be held liable for any damages arising from 8 | # the use of this software. 9 | # 10 | # Permission is granted to anyone to use this software for any purpose, 11 | # including commercial applications, and to alter it and redistribute it 12 | # freely. 13 | 14 | OCAMLC := ocamlc.opt 15 | OCAMLOPT := ocamlopt.opt 16 | OCAMLDOC := ocamldoc.opt 17 | 18 | all: ode.cma ode.cmxa 19 | all: ode.cma 20 | 21 | .PHONY: all dist doc uninstall clean clean-doc 22 | 23 | #ode_version.exe: ode_version.c 24 | # # .exe for cygwin and mingw users 25 | # gcc $< -o $@ 26 | #ode_version.h: ode_version.exe 27 | ## if the version can't be parsed automaticaly, ask to the user 28 | # ./$< > $@ || ocaml ask_version.ml $@ 29 | 30 | # another way to get ODE's version (maybe this one is more portable) 31 | ODE_MAJOR := $(shell ocaml ode_version.ml -major) 32 | ODE_MINOR := $(shell ocaml ode_version.ml -minor) 33 | ODE_MICRO := $(shell ocaml ode_version.ml -micro) 34 | 35 | MAJOR_VERSION := ODE_VERSION_MAJOR=$(ODE_MAJOR) 36 | MINOR_VERSION := ODE_VERSION_MINOR=$(ODE_MINOR) 37 | MICRO_VERSION := ODE_VERSION_MICRO=$(ODE_MICRO) 38 | 39 | ode_c.o: ode_c.c 40 | # ode_version.h 41 | $(OCAMLC) -c -pp 'cpp -D$(MAJOR_VERSION) -D$(MINOR_VERSION) -D$(MICRO_VERSION)' $< 42 | 43 | # still another way to get the proper version macros 44 | # $(OCAMLC) -c -pp 'cpp $(shell sh ode_version.sh)' $< 45 | 46 | dll_mlode_stubs.so: ode_c.o 47 | ocamlmklib -o _mlode_stubs $< \ 48 | `ode-config --libs` 49 | 50 | ode.mli: ode.ml 51 | $(OCAMLC) -i $< > $@ 52 | 53 | ode.cmi: ode.mli 54 | $(OCAMLC) -c $< 55 | 56 | ode.cmo: ode.ml ode.cmi 57 | $(OCAMLC) -c $< 58 | 59 | ode.cma: ode.cmo dll_mlode_stubs.so 60 | $(OCAMLC) -a -custom -o $@ $< \ 61 | -dllib dll_mlode_stubs.so 62 | 63 | ode.cmx: ode.ml ode.cmi 64 | $(OCAMLOPT) -c $< 65 | 66 | ode.cmxa ode.a: ode.cmx dll_mlode_stubs.so 67 | $(OCAMLOPT) -a -o $@ $< \ 68 | -cclib -l_mlode_stubs \ 69 | -cclib "`ode-config --libs`" 70 | 71 | doc: ode.ml ode.cmi 72 | if [ ! -d doc ]; then mkdir doc ; fi 73 | $(OCAMLDOC) -html -colorize-code -css-style _style.css -d doc $< 74 | cp _style.css doc/ 75 | 76 | 77 | clean: 78 | rm -f *.[oa] *.so *.cm[ixoa] *.cmxa *.opt *~ 79 | 80 | clean-doc: 81 | rm -f doc/*.{html,css} 82 | rmdir doc/ 83 | 84 | # install 85 | 86 | PREFIX = "`$(OCAMLC) -where`/ode" 87 | 88 | DIST_FILES=\ 89 | ode.cmi \ 90 | ode.cma \ 91 | ode.cmxa \ 92 | ode.cmx \ 93 | ode.a \ 94 | lib_mlode_stubs.a 95 | 96 | SO_DIST_FILES=\ 97 | dll_mlode_stubs.so 98 | 99 | 100 | install: $(DIST_FILES) $(SO_DIST_FILES) 101 | if [ ! -d $(PREFIX) ]; then install -d $(PREFIX) ; fi 102 | 103 | install -m 0755 \ 104 | $(SO_DIST_FILES) \ 105 | $(PREFIX)/ 106 | 107 | install -m 0644 \ 108 | $(DIST_FILES) \ 109 | META \ 110 | $(PREFIX)/ 111 | 112 | uninstall: 113 | rm -i $(PREFIX)/* 114 | rmdir $(PREFIX)/ 115 | 116 | 117 | # findlib install 118 | 119 | install_findlib: $(DIST_FILES) $(SO_DIST_FILES) META 120 | ocamlfind install ode $^ 121 | 122 | uninstall_findlib: $(DIST_FILES) $(SO_DIST_FILES) META 123 | ocamlfind remove ode 124 | 125 | #EOF 126 | -------------------------------------------------------------------------------- /examples/demo_chain2.ml: -------------------------------------------------------------------------------- 1 | (* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 2 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 3 | * 4 | * This demo is free software; you can redistribute it and/or modify it 5 | * under the terms of EITHER: 6 | * (1) The GNU Lesser General Public License as published by the Free 7 | * Software Foundation; either version 2.1 of the License, or (at 8 | * your option) any later version. The text of the GNU Lesser 9 | * General Public License is included with this library in the 10 | * file LICENSE_LGPL.txt. 11 | * (2) The BSD-style license that is included with this library in 12 | * the file LICENSE_BSD.txt. 13 | * 14 | * This library is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 17 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 18 | *) 19 | 20 | (* Converted from C to OCaml by Florent Monnier *) 21 | 22 | open Ode 23 | open LowLevel 24 | open Drawstuff 25 | 26 | (* some constants *) 27 | 28 | let num = 10 (* number of boxes *) 29 | let side = (0.2) (* side length of a box *) 30 | let mass = (1.0) (* mass of a box *) 31 | 32 | 33 | (* this is called by space.collide when two objects in space are 34 | potentially colliding. *) 35 | 36 | let nearCallback world contactgroup = fun o1 o2 -> 37 | let b1 = dGeomGetBody o1 38 | and b2 = dGeomGetBody o2 in 39 | let surf_params = {surf_param_zero with 40 | sp_mu = dInfinity; 41 | } in 42 | let create_contact () = 43 | let cnt_arr = dCollide o1 o2 5 in 44 | let mk_contact cnt_geom = 45 | let cnt = { 46 | c_surface = surf_params; 47 | c_geom = cnt_geom; 48 | c_fdir1 = { x=0.; y=0.; z=0.; w=0. } 49 | } in 50 | let j = dJointCreateContact world (Some contactgroup) cnt in 51 | dJointAttach j b1 b2; 52 | in 53 | Array.iter mk_contact cnt_arr; 54 | in 55 | match b1, b2 with 56 | | Some _b1, Some _b2 57 | (* exit without doing anything if the two bodies are connected by a joint *) 58 | when not(dAreConnected _b1 _b2) -> create_contact() 59 | | None, Some _ 60 | | Some _, None -> create_contact() 61 | | _ -> () 62 | ;; 63 | 64 | 65 | 66 | let ( += ) a b = (a := !a +. b) ;; 67 | 68 | (* drawing the scene *) 69 | let sim_draw body = fun () -> 70 | let sides = (side,side,side) 71 | and color = (1.9, 0.7, 0.0) 72 | in 73 | for i=0 to pred num do 74 | dsDrawBox (dBodyGetPosition body.(i)) 75 | (dBodyGetRotation body.(i)) sides color; 76 | done; 77 | dsDrawPlane (0.,0.,0.) (1.0, 0.0, 0.0); 78 | ;; 79 | 80 | 81 | (* simulation step *) 82 | let sim_step world space body contactgroup = 83 | let angle = ref 0.0 in 84 | (function pause -> 85 | if not(pause) then begin 86 | angle += 0.05; 87 | dBodyAddForce body.(num-1) 0. 0. (1.5 *. ((sin !angle) +. 1.0)); 88 | 89 | dSpaceCollide space (nearCallback world contactgroup); 90 | dWorldStep world 0.05; 91 | 92 | (* remove all contact joints *) 93 | dJointGroupEmpty contactgroup; 94 | end; 95 | ) 96 | ;; 97 | 98 | 99 | (* main *) 100 | let () = 101 | (* dynamics and collision objects *) 102 | let world = dWorldCreate() 103 | and space = dSimpleSpaceCreate None in 104 | let body = Array.init num (function _ -> dBodyCreate world) in 105 | let contactgroup = dJointGroupCreate() in 106 | 107 | let free_stuff() = 108 | dJointGroupDestroy contactgroup; 109 | Array.iter dBodyDestroy body; 110 | dSpaceDestroy space; 111 | dWorldDestroy world; 112 | in 113 | 114 | (* create world *) 115 | dInitODE(); 116 | 117 | dWorldSetGravity world 0. 0. (-0.5); 118 | dWorldSetCFM world (1e-5); 119 | let plane = dCreatePlane (Some space) 0. 0. 1. 0. in 120 | 121 | let init_body i b = 122 | let k = (float i) *. side in 123 | dBodySetPosition b k k (k +. 0.4); 124 | let m = dMassCreate() in 125 | dMassSetBox m 1. side side side; 126 | dMassAdjust m mass; 127 | dBodySetMass body.(i) m; 128 | dBodySetData body.(i) i; 129 | in 130 | Array.iteri init_body body; 131 | 132 | let box = 133 | Array.init num (function i -> 134 | let b = dCreateBox (Some space) side side side in 135 | dGeomSetBody b (Some body.(i)); 136 | (b)) 137 | in 138 | let joint = 139 | Array.init (num-1) (function i -> 140 | let j = dJointCreateBall world None in 141 | dJointAttach j (Some body.(i)) (Some body.(i+1)); 142 | let k = (float i +. 0.5) *. side in 143 | dJointSetBallAnchor j k k (k +. 0.4); 144 | (j)) 145 | in 146 | 147 | let free_stuff() = 148 | Array.iter dJointDestroy joint; 149 | Array.iter dGeomDestroy box; 150 | dGeomDestroy plane; 151 | free_stuff(); 152 | dCloseODE(); 153 | in 154 | 155 | begin 156 | (* set initial viewpoint *) 157 | let pos = (3.7, -2.8, -1.4) 158 | and angles = (98.4, 304.8) in 159 | 160 | (* call sim_step every N milliseconds *) 161 | let timer_msecs = 20 in 162 | 163 | let dsd = 164 | ( (pos, angles, timer_msecs, world), 165 | (sim_draw body), 166 | (sim_step world space body contactgroup), 167 | (fun _ -> ()), 168 | (free_stuff) 169 | ) 170 | in 171 | 172 | (* run simulation *) 173 | dsSimulationLoop 480 360 dsd; 174 | end; 175 | ;; 176 | 177 | -------------------------------------------------------------------------------- /examples/demo_friction.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* 22 | test the Coulomb friction approximation. 23 | 24 | a 10x10 array of boxes is made, each of which rests on the ground. 25 | a horizantal force is applied to each box to try and get it to slide. 26 | box[i][j] has a mass (i+1)*mass and a force (j+1)*force. by the Coloumb 27 | friction model, the box should only slide if the force is greater than mu 28 | times the contact normal force, i.e. 29 | 30 | f > mu * body_mass * gravity 31 | (j+1)*force > mu * (i+1)*mass * gravity 32 | (j+1) > (i+1) * (mu*mass*gravity/force) 33 | (j+1) > (i+1) * k 34 | 35 | this should be independent of the number of contact points, as N contact 36 | points will each have 1/N'th the normal force but the pushing force will 37 | have to overcome N contacts. the constants are chosen so that k=1. 38 | thus you should see a triangle made of half the bodies in the array start 39 | to slide. 40 | *) 41 | 42 | (* Converted from C to OCaml by Florent Monnier *) 43 | 44 | open Ode.LowLevel 45 | open Drawstuff 46 | 47 | (* some constants *) 48 | 49 | let length = 0.2 (* box length & width *) 50 | let height = 0.05 (* box height *) 51 | let mass = 0.2 (* mass of box[i][j] = (i+1) * mass *) 52 | let force = 0.05 (* force applied to box[i][j] = (j+1) * force *) 53 | let mu = 0.5 (* the global mu to use *) 54 | let gravity = 0.5 (* the global gravity to use *) 55 | let n1 = 10 (* number of different forces to try *) 56 | let n2 = 10 (* number of different masses to try *) 57 | 58 | 59 | 60 | (* this is called by dSpaceCollide when two objects in space are *) 61 | (* potentially colliding. *) 62 | 63 | let nearCallback world ground contactgroup = fun o1 o2 -> 64 | (* only collide things with the ground *) 65 | if not(o1 <> ground && o2 <> ground) then 66 | begin 67 | let b1 = dGeomGetBody o1 68 | and b2 = dGeomGetBody o2 in 69 | 70 | (* up to 3 contacts per box *) 71 | let surf_param = {surf_param_zero with 72 | sp_mode = [`dContactSoftCFM; `dContactApprox1]; 73 | sp_mu = mu; 74 | sp_soft_cfm = 0.01; 75 | } in 76 | let contact_geom_arr = dCollide o1 o2 3 in 77 | ArrayLabels.iter contact_geom_arr ~f:(fun contact_geom -> 78 | let contact = { 79 | c_surface = surf_param; 80 | c_geom = contact_geom; 81 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 82 | } in 83 | let c = dJointCreateContact world (Some contactgroup) contact in 84 | dJointAttach c b1 b2; 85 | ); 86 | end; 87 | ;; 88 | 89 | 90 | (* simulation step *) 91 | let sim_step world space ground body contactgroup = 92 | function true -> () (* pause *) 93 | | false -> 94 | (* apply forces to all bodies *) 95 | for i=0 to pred n1 do 96 | let body_i = body.(i) in 97 | for j=0 to pred n2 do 98 | dBodyAddForce body_i.(j) (force *. float(i+1)) 0. 0.; 99 | done; 100 | done; 101 | 102 | dSpaceCollide space (nearCallback world ground contactgroup); 103 | dWorldStep world 0.05; 104 | 105 | (* remove all contact joints *) 106 | dJointGroupEmpty contactgroup; 107 | ;; 108 | 109 | 110 | (* display simulation scene *) 111 | let sim_draw box = fun () -> 112 | let color = (1.,0.,1.) 113 | and sides = (length, length, height) in 114 | for i=0 to pred n1 do 115 | let box_i = box.(i) in 116 | for j=0 to pred n2 do 117 | let box_ij = box_i.(j) in 118 | dsDrawBox (dGeomGetPosition box_ij) 119 | (dGeomGetRotation box_ij) sides color; 120 | done; 121 | done; 122 | ;; 123 | 124 | 125 | (* main *) 126 | let () = 127 | let m = dMassCreate() in 128 | 129 | (* create world *) 130 | dInitODE(); 131 | let world = dWorldCreate() 132 | and space = dHashSpaceCreate None 133 | and contactgroup = dJointGroupCreate() in 134 | dWorldSetGravity world 0. 0. (-. gravity); 135 | let ground = dCreatePlane (Some space) 0. 0. 1. 0. in 136 | 137 | let split_array arr = 138 | let a = Array.map fst arr 139 | and b = Array.map snd arr in 140 | (a, b) 141 | in 142 | 143 | (* dynamics and collision objects *) 144 | let body, box = 145 | split_array(Array.init n1 (fun i -> 146 | split_array(Array.init n2 (fun j -> 147 | let _i = float i and _j = float j in 148 | 149 | let body = dBodyCreate world in 150 | dMassSetBox m 1. length length height; 151 | dMassAdjust m (mass *. (_j +. 1.)); 152 | dBodySetMass body m; 153 | dBodySetPosition body (_i *. 2. *. length) 154 | (_j *. 2. *. length) (height *. 0.5); 155 | 156 | let box = dCreateBox (Some space) length length height in 157 | dGeomSetBody box (Some body); 158 | (body, box) 159 | )) 160 | )) 161 | in 162 | 163 | let destroy_all() = 164 | dJointGroupDestroy contactgroup; 165 | dSpaceDestroy space; 166 | dWorldDestroy world; 167 | dCloseODE(); 168 | in 169 | 170 | begin 171 | (* set initial viewpoint *) 172 | let pos = (3.2, 1.5, -4.0) 173 | and angles = (116.4, 245.0) in 174 | 175 | (* call sim_step every N milliseconds *) 176 | let timer_msecs = 10 in 177 | 178 | (* simulation params (for the drawstuff lib) *) 179 | let dsd = 180 | ( (pos, angles, timer_msecs, world), 181 | (sim_draw box), 182 | (sim_step world space ground body contactgroup), 183 | (fun _ -> ()), 184 | (destroy_all) 185 | ) 186 | in 187 | (* run simulation *) 188 | dsSimulationLoop 480 360 dsd; 189 | end; 190 | ;; 191 | 192 | -------------------------------------------------------------------------------- /examples/demo_cylvssphere.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* Test for cylinder vs sphere, by Bram Stolk *) 22 | 23 | (* Converted from C to OCaml by Florent Monnier *) 24 | 25 | open Ode.LowLevel 26 | open Drawstuff 27 | 28 | let show_contacts = true 29 | 30 | let cyl_radius = 0.6 31 | let cyl_length = 2.0 32 | let sphere_radius = 0.5 33 | 34 | 35 | let draw_contacts = ref (fun () -> ()) ;; 36 | 37 | (* this is called by dSpaceCollide when two objects in space are *) 38 | (* potentially colliding. *) 39 | 40 | let rec nearCallback world contactgroup = fun o1 o2 -> 41 | if (dGeomIsSpace o1) || (dGeomIsSpace o2) then 42 | begin 43 | Printf.eprintf "colliding space\n%!"; 44 | (* colliding a space with something *) 45 | dSpaceCollide2 o1 o2 (nearCallback world contactgroup); 46 | (* Note we do not want to test intersections within a space, *) 47 | (* only between spaces. *) 48 | () 49 | end else 50 | begin 51 | let contact_geom_arr = dCollide o1 o2 32 in 52 | ArrayLabels.iter contact_geom_arr ~f:(function contact_geom -> 53 | let contact = { 54 | c_surface = { surf_param_zero with sp_mu = 50.0 }; 55 | c_geom = contact_geom; 56 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 57 | } in 58 | let c = dJointCreateContact world (Some contactgroup) contact in 59 | dJointAttach c (dGeomGetBody contact_geom.cg_g1) (dGeomGetBody contact_geom.cg_g2); 60 | draw_contacts := (function () -> 61 | if (show_contacts) then 62 | begin 63 | let ri = dRGetIdentity() in 64 | let ss = (0.12,0.12,0.12) in 65 | let color = (0.6, 0.8, 0.9) 66 | in 67 | let pos = contact_geom.cg_pos 68 | and depth = contact_geom.cg_depth 69 | and norm = contact_geom.cg_normal 70 | in 71 | dsDrawBox pos ri ss color; 72 | let endp = 73 | { x = pos.x +. depth *. norm.x; 74 | y = pos.y +. depth *. norm.y; 75 | z = pos.z +. depth *. norm.z; 76 | w = 0. } 77 | in 78 | let color = (0.7, 0.9, 1.0) in 79 | dsDrawLine pos endp color; 80 | end; 81 | ); 82 | ); 83 | end; 84 | ;; 85 | 86 | 87 | (* called when a key pressed *) 88 | 89 | let command = function 90 | | ' ' -> () 91 | | _ -> () 92 | ;; 93 | 94 | 95 | (* simulation step *) 96 | 97 | let sim_step world space contactgroup = fun pause -> 98 | dSpaceCollide space (nearCallback world contactgroup); 99 | if not(pause) then 100 | dWorldQuickStep world 0.01; (* 100 Hz *) 101 | dJointGroupEmpty contactgroup; 102 | ;; 103 | 104 | 105 | (* draw the scene *) 106 | 107 | let sim_draw cylbody sphbody = fun () -> 108 | let color = (1.0, 0.8, 0.0) in 109 | (* 110 | dsDrawCylinder (dBodyGetPosition cylbody) 111 | (dBodyGetRotation cylbody) cyl_length cyl_radius color; 112 | *) 113 | dsDrawWireCylinder (dBodyGetPosition cylbody) 114 | (dBodyGetRotation cylbody) cyl_length cyl_radius color; 115 | 116 | (* 117 | dsDrawSphere (dBodyGetPosition sphbody) 118 | (dBodyGetRotation sphbody) sphere_radius color; 119 | *) 120 | dsDrawWireSphere (dBodyGetPosition sphbody) 121 | (dBodyGetRotation sphbody) sphere_radius color; 122 | 123 | (!draw_contacts)(); 124 | 125 | dsDrawPlane (0.0, 0.0, 0.0) (1.0, 0.0, 0.0); 126 | ;; 127 | 128 | 129 | let pi = 3.1415926535_8979323846 130 | 131 | (* main *) 132 | let () = 133 | let m = dMassCreate() in 134 | 135 | (* create world *) 136 | dInitODE(); 137 | let world = dWorldCreate() 138 | and space = dHashSpaceCreate None 139 | and contactgroup = dJointGroupCreate() in 140 | dWorldSetGravity world 0. 0. (-9.8); 141 | dWorldSetQuickStepNumIterations world 32; 142 | 143 | (* dynamics and collision objects *) 144 | 145 | let _ = dCreatePlane (Some space) 0. 0. 1. 0.0 in 146 | 147 | let cylbody = dBodyCreate world in 148 | (* 149 | let q = dQFromAxisAndAngle 1. 0. 0. (pi *. 0.5) in 150 | let q = dQFromAxisAndAngle 1. 0. 0. (pi *. 1.0) in 151 | *) 152 | let q = dQFromAxisAndAngle 1. 0. 0. (pi *. (-0.77)) in 153 | 154 | dBodySetQuaternion cylbody q; 155 | dMassSetCylinder m 1.0 Dir_z cyl_radius cyl_length; 156 | dBodySetMass cylbody m; 157 | let cylgeom = dCreateCylinder None cyl_radius cyl_length in 158 | dGeomSetBody cylgeom (Some cylbody); 159 | dBodySetPosition cylbody 0. 0. 3.; 160 | dSpaceAdd space cylgeom; 161 | 162 | let sphbody = dBodyCreate world in 163 | dMassSetSphere m 1. sphere_radius; 164 | dBodySetMass sphbody m; 165 | let sphgeom = dCreateSphere None sphere_radius in 166 | dGeomSetBody sphgeom (Some sphbody); 167 | dBodySetPosition sphbody 0. 0. 5.5; 168 | dSpaceAdd space sphgeom; 169 | 170 | let free_env() = 171 | dJointGroupEmpty (contactgroup); 172 | dJointGroupDestroy (contactgroup); 173 | 174 | dGeomDestroy(sphgeom); 175 | dGeomDestroy (cylgeom); 176 | 177 | dSpaceDestroy (space); 178 | dWorldDestroy (world); 179 | dCloseODE(); 180 | in 181 | 182 | begin 183 | (* set initial viewpoint *) 184 | let pos = (5.9, 5.7, -5.0) 185 | and angles = (112.2, 211.6) in 186 | 187 | (* call sim_step every N milliseconds *) 188 | let timer_msecs = 20 in 189 | 190 | (* simulation params (for the drawstuff lib) *) 191 | let dsd = 192 | ( (pos, angles, timer_msecs, world), 193 | (sim_draw cylbody sphbody), 194 | (sim_step world space contactgroup), 195 | (command), 196 | (free_env) 197 | ) 198 | in 199 | (* run simulation *) 200 | dsSimulationLoop 480 360 dsd; 201 | end; 202 | ;; 203 | 204 | -------------------------------------------------------------------------------- /LICENSE_LGPL.txt: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /examples/demo_plane2d.ml: -------------------------------------------------------------------------------- 1 | (* Test the Plane2D constraint. *) 2 | (* Converted from C to OCaml by Florent Monnier *) 3 | 4 | open Ode.LowLevel 5 | open Drawstuff 6 | 7 | let drand48() = Random.float 1.0 8 | 9 | let n_bodies = 40 10 | let stage_size = 8.0 (* in m *) 11 | 12 | let time_step = 0.01 13 | let k_spring = 10.0 14 | let k_damp = 10.0 15 | 16 | 17 | let first (v,_,_) = v ;; 18 | let second (_,v,_) = v ;; 19 | let third (_,_,v) = v ;; 20 | 21 | let split_array arr = 22 | let a = Array.map first arr 23 | and b = Array.map second arr 24 | and c = Array.map third arr in 25 | (a, b, c) 26 | ;; 27 | 28 | let ( += ) a b = (a := !a +. b) ;; 29 | let ( %. ) = mod_float ;; 30 | 31 | 32 | (* collision callback *) 33 | let cb_near_collision dyn_world coll_contacts o1 o2 = 34 | let b1 = dGeomGetBody o1 35 | and b2 = dGeomGetBody o2 in 36 | 37 | match b1, b2 with 38 | (* exit without doing anything if the two bodies are static *) 39 | | None, None -> () 40 | (* exit without doing anything if the two bodies are connected by a joint *) 41 | | Some _b1, Some _b2 42 | when (dAreConnected _b1 _b2) -> () 43 | | _ -> 44 | 45 | try 46 | let cnt_geom_arr = dCollide o1 o2 1 in 47 | if cnt_geom_arr <> [| |] then 48 | let contact ={ 49 | c_surface = surface_param ~mu:0.0 []; 50 | c_geom = cnt_geom_arr.(0); 51 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 52 | } in 53 | let c = dJointCreateContact dyn_world (Some coll_contacts) contact in 54 | dJointAttach c b1 b2; 55 | with _ -> () 56 | ;; 57 | 58 | 59 | 60 | let track_to_pos body joint_id target_x target_y = 61 | let curr_x = (dBodyGetPosition body).x 62 | and curr_y = (dBodyGetPosition body).y in 63 | 64 | dJointSetPlane2DXParam joint_id DParamVel (1. *. (target_x -. curr_x)); 65 | dJointSetPlane2DYParam joint_id DParamVel (1. *. (target_y -. curr_y)); 66 | ;; 67 | 68 | 69 | (* simulation step *) 70 | let cb_sim_step dyn_world coll_space dyn_bodies plane2d_joints coll_contacts = 71 | let angle = ref 0. in 72 | (function pause -> 73 | if not(pause) then 74 | begin 75 | angle += 0.01; 76 | 77 | track_to_pos 78 | dyn_bodies.(0) 79 | plane2d_joints.(0) 80 | ((stage_size /. 2.) +. (stage_size /. 2.0) *. (cos !angle)) 81 | ((stage_size /. 2.) +. (stage_size /. 2.0) *. (sin !angle)); 82 | 83 | if false then begin 84 | let f0 = 0.001 in 85 | for b=0 to pred n_bodies do 86 | let p = float(1 + b) /. (float n_bodies) 87 | and q = float(2 - b) /. (float n_bodies) 88 | in 89 | dBodyAddForce dyn_bodies.(b) 90 | (f0 *. cos(p *. !angle)) 91 | (f0 *. sin(q *. !angle)) 0.; 92 | done; 93 | dBodyAddTorque dyn_bodies.(0) 0. 0. 0.1; 94 | end; 95 | 96 | let n = 10 in 97 | for i=0 to pred n do 98 | dSpaceCollide coll_space (cb_near_collision dyn_world coll_contacts); 99 | dWorldStep dyn_world (time_step /. float n); 100 | dJointGroupEmpty coll_contacts; 101 | done; 102 | end; 103 | 104 | if true then 105 | begin 106 | (* XXX hack Plane2D constraint error reduction here: *) 107 | for b=0 to pred n_bodies do 108 | let rot = dBodyGetAngularVel dyn_bodies.(b) in 109 | 110 | let quat_ptr = dBodyGetQuaternion dyn_bodies.(b) in 111 | let quat = { 112 | q1 = quat_ptr.q1; 113 | q2 = 0.; 114 | q3 = 0.; 115 | q4 = quat_ptr.q4; 116 | } in 117 | let quat_len = sqrt (quat.q1 *. quat.q1 +. quat.q4 *. quat.q4) in 118 | let quat = {quat with 119 | q1 = quat.q1 /. quat_len; 120 | q4 = quat.q4 /. quat_len; 121 | } in 122 | dBodySetQuaternion dyn_bodies.(b) quat; 123 | dBodySetAngularVel dyn_bodies.(b) 0. 0. rot.z; 124 | done; 125 | end; 126 | 127 | if false then 128 | begin 129 | (* XXX friction *) 130 | for b=0 to pred n_bodies do 131 | let vel = dBodyGetLinearVel dyn_bodies.(b) 132 | and rot = dBodyGetAngularVel dyn_bodies.(b) 133 | and s = 1.00 134 | and t = 0.99 in 135 | dBodySetLinearVel dyn_bodies.(b) (s *. vel.x) (s *. vel.y) (s *. vel.z); 136 | dBodySetAngularVel dyn_bodies.(b) (t *. rot.x) (t *. rot.y) (t *. rot.z); 137 | done; 138 | end; 139 | ) 140 | ;; 141 | 142 | 143 | 144 | (* drawing the scene *) 145 | let cb_sim_draw dyn_bodies bodies_sides = fun () -> 146 | (* ode drawstuff *) 147 | for b=0 to pred n_bodies do 148 | let color = 149 | if b = 0 150 | then (1.0, 0.6, 0.0) 151 | else (0.0, 0.5, 1.0) 152 | in 153 | dsDrawBox (dBodyGetPosition dyn_bodies.(b)) 154 | (dBodyGetRotation dyn_bodies.(b)) 155 | bodies_sides.(b) 156 | color; 157 | done; 158 | dsDrawPlane (4.3, 4.3, -0.6) ~scale:(2.1) (1.0, 0.0, 0.0); 159 | ;; 160 | 161 | 162 | 163 | (* main *) 164 | let () = 165 | Random.self_init(); 166 | dInitODE(); 167 | 168 | (* dynamic world *) 169 | 170 | let cf_mixing = 0.001 in (* = 1. /. time_step *. k_spring +. k_damp *) 171 | let err_reduct = 0.5 in (* = time_step *. k_spring *. cf_mixing *) 172 | 173 | let dyn_world = dWorldCreate() in 174 | 175 | dWorldSetERP dyn_world err_reduct; 176 | dWorldSetCFM dyn_world cf_mixing; 177 | dWorldSetGravity dyn_world 0.0 0.0 (-1.0); 178 | 179 | let coll_space = dSimpleSpaceCreate None in 180 | 181 | let free_env() = 182 | dSpaceDestroy coll_space; 183 | dWorldDestroy dyn_world; 184 | in 185 | 186 | (* dynamic bodies *) 187 | let dyn_bodies, bodies_sides, plane2d_joints = 188 | split_array( 189 | Array.init n_bodies (fun b -> 190 | let l = 1. +. (sqrt (float n_bodies)) in 191 | let bf = float b in 192 | 193 | let x = ((0.5 +. (bf /. l)) /. l *. stage_size) 194 | and y = ((0.5 +. (bf %. l)) /. l *. stage_size) 195 | and z = 1.0 +. 0.1 *. (drand48()) 196 | in 197 | 198 | let _x = (5. *. (0.2 +. 0.7 *. drand48()) /. (sqrt (float n_bodies))) 199 | and _y = (5. *. (0.2 +. 0.7 *. drand48()) /. (sqrt (float n_bodies))) 200 | and _z = z 201 | in 202 | let body_sides = (_x, _y, _z) in 203 | 204 | let body = dBodyCreate dyn_world in 205 | dBodySetPosition body x y (z /. 2.); 206 | dBodySetData body b; 207 | dBodySetLinearVel body (3. *. (drand48() -. 0.5)) 208 | (3. *. (drand48() -. 0.5)) 0.; 209 | 210 | let m = dMassCreate() in 211 | dMassSetBox m 1. _x _y _z; 212 | dMassAdjust m (0.1 *. _x *. _y); 213 | dBodySetMass body m; 214 | 215 | let joint = dJointCreatePlane2D dyn_world None in 216 | dJointAttach joint (Some body) None; 217 | 218 | (body, body_sides, joint))) 219 | in 220 | 221 | dJointSetPlane2DXParam plane2d_joints.(0) DParamFMax 10.; 222 | dJointSetPlane2DYParam plane2d_joints.(0) DParamFMax 10.; 223 | 224 | 225 | (* collision geoms and joints *) 226 | let _ = dCreatePlane (Some coll_space) ( 1.) ( 0.) (0.) (0.); 227 | and _ = dCreatePlane (Some coll_space) (-1.) ( 0.) (0.) (-. stage_size); 228 | and _ = dCreatePlane (Some coll_space) ( 0.) ( 1.) (0.) (0.); 229 | and _ = dCreatePlane (Some coll_space) ( 0.) (-1.) (0.) (-. stage_size); 230 | in 231 | 232 | for b=0 to pred n_bodies do 233 | let coll_box_id = 234 | let (lx, ly, lz) = bodies_sides.(b) in 235 | dCreateBox (Some coll_space) ~lx ~ly ~lz 236 | in 237 | dGeomSetBody coll_box_id (Some dyn_bodies.(b)); 238 | done; 239 | 240 | let coll_contacts = dJointGroupCreate() in 241 | 242 | let free_env() = 243 | Array.iter dBodyDestroy dyn_bodies; 244 | Array.iter dJointDestroy plane2d_joints; 245 | dJointGroupDestroy coll_contacts; 246 | free_env(); 247 | dCloseODE(); 248 | in 249 | 250 | begin 251 | (* set initial viewpoint *) 252 | let pos = (3.2, 1.6, -7.2) 253 | and angles = (134.2, 230.8) in 254 | 255 | (* call sim_step every N milliseconds *) 256 | let timer_msecs = 10 in 257 | 258 | (* simulation params (for the drawstuff lib) *) 259 | let dsd = 260 | ( (pos, angles, timer_msecs, dyn_world), 261 | (cb_sim_draw dyn_bodies bodies_sides), 262 | (cb_sim_step dyn_world coll_space dyn_bodies plane2d_joints coll_contacts), 263 | (fun _ -> ()), 264 | (free_env) 265 | ) 266 | in 267 | dsSimulationLoop 480 360 dsd; 268 | end; 269 | ;; 270 | 271 | -------------------------------------------------------------------------------- /examples/demo_feedback.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* Test for breaking joints, by Bram Stolk *) 22 | 23 | (* Converted from C to OCaml by Florent Monnier *) 24 | 25 | open Ode.LowLevel 26 | open Drawstuff 27 | 28 | let stackcnt = 10 (* nr of weights on bridge *) 29 | let segmcnt = 16 (* nr of segments in bridge *) 30 | let segmdim = {x=0.9; y=4.; z=0.1; w=0.} 31 | 32 | 33 | (* this is called by dSpaceCollide when two objects in space are *) 34 | (* potentially colliding. *) 35 | 36 | let rec nearCallback world contactgroup = fun o1 o2 -> 37 | if (dGeomIsSpace o1) || (dGeomIsSpace o2) then 38 | begin 39 | (* colliding a space with something *) 40 | dSpaceCollide2 o1 o2 (nearCallback world contactgroup); 41 | (* Note we do not want to test intersections within a space, *) 42 | (* only between spaces. *) 43 | () 44 | end else 45 | let contact_geom_arr = dCollide o1 o2 32 in 46 | ArrayLabels.iter contact_geom_arr ~f:(fun contact_geom -> 47 | let surf_param = {surf_param_zero with 48 | sp_mode = [`dContactSoftERP; `dContactSoftCFM; `dContactApprox1]; 49 | sp_mu = 100.0; 50 | sp_soft_erp = 0.96; 51 | sp_soft_cfm = 0.02; 52 | } in 53 | let contact = { 54 | c_surface = surf_param; 55 | c_geom = contact_geom; 56 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 57 | } in 58 | let c = dJointCreateContact world (Some contactgroup) contact in 59 | dJointAttach c (dGeomGetBody contact_geom.cg_g1) 60 | (dGeomGetBody contact_geom.cg_g2); 61 | ); 62 | ;; 63 | 64 | 65 | let inspect_joints hinges stress colors jfeedbacks = 66 | let forcelimit = 2000.0 in 67 | for i=0 to pred(segmcnt-1) do 68 | try 69 | let _ = dJointGetBody hinges.(i) (0) in 70 | begin 71 | (* This joint has not snapped already... inspect it. *) 72 | let l0 = dLENGTH((dJointFeedback_of_buffer jfeedbacks.(i)).f1) 73 | and l1 = dLENGTH((dJointFeedback_of_buffer jfeedbacks.(i)).f2) 74 | in 75 | colors.(i+0) <- 0.95 *. colors.(i+0) +. 0.05 *. l0 /. forcelimit; 76 | colors.(i+1) <- 0.95 *. colors.(i+1) +. 0.05 *. l1 /. forcelimit; 77 | 78 | if (l0 > forcelimit) || (l1 > forcelimit) 79 | then stress.(i) <- stress.(i) + 1 80 | else (* stress.(i) <- 0; *) (); 81 | 82 | (* 83 | if stress.(i) > 4 then 84 | *) 85 | if stress.(i) >= 1 then 86 | begin 87 | (* Low-pass filter the noisy feedback data. *) 88 | (* Only after 4 consecutive timesteps with excessive load, snap. *) 89 | Printf.eprintf "SNAP! (that was the sound of joint %d breaking)\n%!" i; 90 | dJointAttach hinges.(i) None None; 91 | end; 92 | end; 93 | with 94 | Failure _ -> () 95 | done; 96 | ;; 97 | 98 | 99 | 100 | (* simulation loop *) 101 | 102 | let sim_step world space contactgroup hinges stress colors jfeedbacks = 103 | function true -> () (* pause *) 104 | | false -> 105 | (* 106 | let simstep = 0.010 in (* 10ms simulation steps *) 107 | *) 108 | let simstep = 0.005 in (* 5ms simulation steps *) 109 | let dt = dsElapsedTime() in 110 | 111 | let nrofsteps = truncate(ceil(dt /. simstep)) in 112 | for i=0 to pred nrofsteps do 113 | dSpaceCollide space (nearCallback world contactgroup); 114 | dWorldQuickStep world simstep; 115 | dJointGroupEmpty contactgroup; 116 | inspect_joints hinges stress colors jfeedbacks; 117 | done; 118 | ;; 119 | 120 | 121 | 122 | let draw_geom g color = 123 | let pos = dGeomGetPosition g 124 | and rot = dGeomGetRotation g in 125 | 126 | let kind = geom_kind g in 127 | match kind with 128 | | Box_geom g -> 129 | let sides = dGeomBoxGetLengths g in 130 | let dims = (sides.x, sides.y, sides.z) in 131 | dsDrawBox pos rot dims color; 132 | | Cylinder_geom g -> 133 | let (r, l) = dGeomCylinderGetParams g in 134 | dsDrawCylinder pos rot l r color; 135 | | _ -> () 136 | ;; 137 | 138 | (* draw scene *) 139 | 140 | let sim_draw seggeoms stackgeoms colors = fun () -> 141 | for i=0 to pred segmcnt do 142 | let b=0.2 in 143 | let v = colors.(i) in 144 | 145 | let v = if (v > 1.0) then 1.0 else v in 146 | let r, g = 147 | if (v < 0.5) 148 | then (2. *. v), (1.0) 149 | else (1.0), (2. *. (1.0 -. v)) 150 | in 151 | let color = (r,g,b) in 152 | draw_geom seggeoms.(i) color; 153 | done; 154 | 155 | let color = (0.3, 0.6, 1.0) in 156 | ArrayLabels.iter stackgeoms ~f:(fun stackgeom -> 157 | draw_geom stackgeom color); 158 | 159 | dsDrawPlane (0.,0.,0.) ~scale:3.6 (1.0, 0.0, 0.0); 160 | ;; 161 | 162 | 163 | let split_array arr = 164 | let a = Array.map fst arr 165 | and b = Array.map snd arr in 166 | (a, b) 167 | ;; 168 | 169 | 170 | (* main *) 171 | let () = 172 | let m = dMassCreate() in 173 | 174 | (* create world *) 175 | dInitODE(); 176 | let world = dWorldCreate() 177 | and space = dHashSpaceCreate None 178 | and contactgroup = dJointGroupCreate () in 179 | dWorldSetGravity world 0. 0. (-9.8); 180 | dWorldSetQuickStepNumIterations world 20; 181 | 182 | (* dynamics and collision objects *) 183 | 184 | let (segbodies, seggeoms) = 185 | split_array(Array.init segmcnt (fun i -> 186 | let segbody = dBodyCreate world in 187 | let x = (float i) -. ((float segmcnt) /. 2.0) in 188 | dBodySetPosition segbody x 0. 5.; 189 | dMassSetBox m 1. segmdim.x segmdim.y segmdim.z; 190 | dBodySetMass segbody m; 191 | let seggeom = dCreateBox None segmdim.x segmdim.y segmdim.z in 192 | dGeomSetBody seggeom (Some segbody); 193 | dSpaceAdd space seggeom; 194 | (segbody, seggeom) 195 | )) 196 | in 197 | 198 | let (hinges, stress) = 199 | split_array(Array.init (segmcnt-1) (fun i -> 200 | let hinge = dJointCreateHinge world None in 201 | dJointAttach hinge (Some segbodies.(i)) (Some segbodies.(i+1)); 202 | dJointSetHingeAnchor hinge ((float i) +. 0.5 -. (float segmcnt) /. 2.0) 0. 5.; 203 | dJointSetHingeAxis hinge 0. 1. 0.; 204 | dJointSetHingeParam hinge DParamFMax 8000.0; 205 | let stress = 0 in 206 | (hinge, stress) 207 | )) 208 | in 209 | 210 | (* NOTE: *) 211 | (* Here we tell ODE where to put the feedback on the forces for this hinge *) 212 | let jfeedbacks = Array.map (fun hinge -> dJointSetFeedback hinge) hinges in 213 | 214 | let (stackbodies, stackgeoms) = 215 | split_array(Array.init stackcnt (fun i -> 216 | 217 | let stackbody = dBodyCreate world in 218 | dMassSetBox m 2.0 2. 2. 0.6; 219 | dBodySetMass stackbody m; 220 | 221 | let stackgeom = dCreateBox None 2. 2. 0.6 in 222 | dGeomSetBody stackgeom (Some stackbody); 223 | dBodySetPosition stackbody 0. 0. (float(8+2*i)); 224 | dSpaceAdd space stackgeom; 225 | 226 | (stackbody, stackgeom) 227 | )) 228 | in 229 | 230 | let slider_0 = dJointCreateSlider world None in 231 | dJointAttach slider_0 (Some segbodies.(0)) None; 232 | dJointSetSliderAxis slider_0 1. 0. 0.; 233 | dJointSetSliderParam slider_0 DParamFMax 4000.0; 234 | dJointSetSliderParam slider_0 DParamLoStop 0.0; 235 | dJointSetSliderParam slider_0 DParamHiStop 0.2; 236 | 237 | let slider_1 = dJointCreateSlider world None in 238 | dJointAttach slider_1 (Some segbodies.(segmcnt-1)) None; 239 | dJointSetSliderAxis slider_1 1. 0. 0.; 240 | dJointSetSliderParam slider_1 DParamFMax 4000.0; 241 | dJointSetSliderParam slider_1 DParamLoStop 0.0; 242 | dJointSetSliderParam slider_1 DParamHiStop (-0.2); 243 | 244 | let sliders = [| slider_0; slider_1 |] in 245 | 246 | let groundgeom = dCreatePlane (Some space) 0. 0. 1. 0. in 247 | 248 | let colors = Array.make segmcnt 0.0 in 249 | 250 | let destroy_all() = 251 | dJointGroupEmpty contactgroup; 252 | dJointGroupDestroy contactgroup; 253 | 254 | (* First destroy seggeoms, then space, then the world. *) 255 | Array.iter dGeomDestroy seggeoms; 256 | Array.iter dGeomDestroy stackgeoms; 257 | dGeomDestroy groundgeom; 258 | 259 | Array.iter dJointDestroy sliders; 260 | 261 | (* Make sure that this function is called to free the memory buffer! *) 262 | Array.iter dJointFeedbackBufferDestroy jfeedbacks; 263 | 264 | dSpaceDestroy space; 265 | dWorldDestroy world; 266 | dCloseODE(); 267 | in 268 | 269 | begin 270 | (* set initial viewpoint *) 271 | let pos = (13.4, -12.6, -7.8) 272 | and angles = (100.8, 318.2) in 273 | 274 | (* call sim_step every N milliseconds *) 275 | let timer_msecs = 20 in 276 | 277 | let dsd = 278 | ( (pos, angles, timer_msecs, world), 279 | (sim_draw seggeoms stackgeoms colors), 280 | (sim_step world space contactgroup hinges stress colors jfeedbacks), 281 | (fun _ -> ()), 282 | (destroy_all) 283 | ) 284 | in 285 | 286 | (* run simulation *) 287 | dsSimulationLoop 480 360 dsd; 288 | end; 289 | ;; 290 | 291 | -------------------------------------------------------------------------------- /examples/demo_buggy.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* Converted from C to OCaml by Florent Monnier *) 22 | 23 | (* 24 | buggy with suspension. 25 | this also shows you how to use geom groups. 26 | *) 27 | 28 | open Ode.LowLevel 29 | open Drawstuff 30 | 31 | 32 | (* some constants *) 33 | 34 | let length = 0.7 (* chassis length *) 35 | let width = 0.5 (* chassis width *) 36 | let height = 0.2 (* chassis height *) 37 | let radius = 0.18 (* wheel radius *) 38 | let startz = 0.5 (* starting height of chassis *) 39 | let cmass = 1.0 (* chassis mass *) 40 | let wmass = 0.2 (* wheel mass *) 41 | 42 | 43 | 44 | (* things that the user controls *) 45 | 46 | let speed = ref 0. 47 | let steer = ref 0. (* user commands *) 48 | 49 | 50 | (* usage message *) 51 | let usage() = 52 | print_endline 53 | "Press:\n \ 54 | 'a' to increase speed.\n \ 55 | 'z' to decrease speed.\n \ 56 | '1' to steer left.\n \ 57 | '2' to steer right.\n \ 58 | ' ' to reset speed and steering.\n"; 59 | ;; 60 | 61 | 62 | let ( += ) a b = (a := !a +. b) 63 | let ( -= ) a b = (a := !a -. b) 64 | 65 | (* called when a key pressed *) 66 | let command = function 67 | | 'a' | 'A' -> speed += 0.3; 68 | | 'z' | 'Z' -> speed -= 0.3; 69 | | '1' -> steer -= 0.5; 70 | | '2' -> steer += 0.5; 71 | | ' ' -> 72 | speed := 0.; 73 | steer := 0.; 74 | | _ -> () 75 | ;; 76 | 77 | 78 | let b_xor a b = 79 | match a, b with 80 | | false, false | true, true -> false 81 | | _ -> true 82 | ;; 83 | 84 | (* this is called by dSpaceCollide when two objects in space are *) 85 | (* potentially colliding. *) 86 | 87 | let near_callback world ground ground_box contactgroup = fun o1 o2 -> 88 | let _o1 = geom_kind o1 89 | and _o2 = geom_kind o2 90 | in 91 | (* only collide things with the ground *) 92 | let g1 = (_o1 = (Plane_geom ground) || _o1 = (Box_geom ground_box)) 93 | and g2 = (_o2 = (Plane_geom ground) || _o2 = (Box_geom ground_box)) 94 | in 95 | if not(b_xor g1 g2) then () 96 | else begin 97 | let contact_geom_ar = dCollide o1 o2 10 in 98 | ArrayLabels.iter contact_geom_ar ~f:(fun contact_geom -> 99 | let surface = { surf_param_zero with 100 | sp_mode = [`dContactSlip1; `dContactSoftERP; 101 | `dContactSlip2; `dContactSoftCFM; `dContactApprox1]; 102 | sp_mu = dInfinity; 103 | sp_slip1 = 0.1; 104 | sp_slip2 = 0.1; 105 | sp_soft_erp = 0.5; 106 | sp_soft_cfm = 0.3; 107 | } in 108 | let contact = { 109 | c_surface = surface; 110 | c_geom = contact_geom; 111 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 112 | } in 113 | let c = dJointCreateContact world (Some contactgroup) contact in 114 | dJointAttach c (dGeomGetBody contact_geom.cg_g1) 115 | (dGeomGetBody contact_geom.cg_g2); 116 | ); 117 | end; 118 | ;; 119 | 120 | 121 | (* simulation loop *) 122 | 123 | let sim_loop world space ground ground_box joint contactgroup = fun pause -> 124 | if not(pause) then 125 | begin 126 | (* motor *) 127 | dJointSetHinge2Param joint.(0) DParamVel2 (-. !speed); 128 | dJointSetHinge2Param joint.(0) DParamFMax2 0.1; 129 | 130 | (* steering *) 131 | let v = !steer -. (dJointGetHinge2Angle1 joint.(0)) in 132 | let v = if v > 0.1 then 0.1 else v in 133 | let v = if v < -0.1 then -0.1 else v in 134 | let v = v *. 10.0 in 135 | dJointSetHinge2Param joint.(0) DParamVel v; 136 | dJointSetHinge2Param joint.(0) DParamFMax 0.2; 137 | dJointSetHinge2Param joint.(0) DParamLoStop (-0.75); 138 | dJointSetHinge2Param joint.(0) DParamHiStop ( 0.75); 139 | dJointSetHinge2Param joint.(0) DParamFudgeFactor 0.1; 140 | 141 | dSpaceCollide space (near_callback world ground ground_box contactgroup); 142 | dWorldStep world 0.05; 143 | 144 | (* remove all contact joints *) 145 | dJointGroupEmpty contactgroup; 146 | end; 147 | ;; 148 | 149 | 150 | (* display the scene *) 151 | 152 | let sim_draw body ground_box joint = fun () -> 153 | let color = (0., 0.2, 0.8) 154 | and sides = (length, width, height) in 155 | dsDrawBox (dBodyGetPosition body.(0)) 156 | (dBodyGetRotation body.(0)) sides color; 157 | 158 | let color = (0., 0.6, 1.) in 159 | for i=1 to 3 do 160 | dsDrawCylinder (dBodyGetPosition body.(i)) 161 | (dBodyGetRotation body.(i)) 0.02 radius color; 162 | (* 163 | dsDrawSphere (dBodyGetPosition body.(i)) 164 | (dBodyGetRotation body.(i)) radius color; 165 | *) 166 | done; 167 | 168 | let ss = dGeomBoxGetLengths ground_box in 169 | dsDrawBox (dGeomGetPosition ground_box) 170 | (dGeomGetRotation ground_box) (ss.x, ss.y, ss.z) color; 171 | 172 | dsDrawPlane (3.0, 0.0, 0.0) ~scale:(2.0) (1.0, 0.0, 0.0); 173 | 174 | (* 175 | Printf.printf "%.10f %.10f %.10f %.10f\n" 176 | (dJointGetHinge2Angle1 joint.(1)) 177 | (dJointGetHinge2Angle1 joint.(2)) 178 | (dJointGetHinge2Angle1Rate joint.(1)) 179 | (dJointGetHinge2Angle1Rate joint.(2)); 180 | *) 181 | ;; 182 | 183 | 184 | let pi = 3.1415926535_8979323846 185 | 186 | (* main *) 187 | let () = 188 | usage(); 189 | let m = dMassCreate() in 190 | 191 | (* create world *) 192 | dInitODE(); 193 | let world = dWorldCreate() 194 | and space = dHashSpaceCreate None 195 | and contactgroup = dJointGroupCreate() 196 | in 197 | dWorldSetGravity world 0. 0. (-0.5); 198 | let ground = dCreatePlane (Some space) 0. 0. 1. 0. in 199 | 200 | (* dynamics and collision objects (chassis, 3 wheels, environment) *) 201 | 202 | let body = Array.init 4 (fun _ -> dBodyCreate world) in 203 | 204 | (* chassis body *) 205 | dBodySetPosition body.(0) 0. 0. startz; 206 | dMassSetBox m 1. length width height; 207 | dMassAdjust m cmass; 208 | dBodySetMass body.(0) m; 209 | let box = [| 210 | dCreateBox None length width height; 211 | |] in 212 | dGeomSetBody box.(0) (Some body.(0)); 213 | 214 | (* wheel bodies *) 215 | let sphere = 216 | Array.init 3 (fun i -> 217 | let q = dQFromAxisAndAngle 1. 0. 0. (pi *. 0.5) in 218 | dBodySetQuaternion body.(i+1) q; 219 | dMassSetSphere m 1. radius; 220 | dMassAdjust m wmass; 221 | dBodySetMass body.(i+1) m; 222 | let sphere = dCreateSphere None radius in 223 | dGeomSetBody sphere (Some body.(i+1)); 224 | (sphere)) 225 | in 226 | dBodySetPosition body.(1) ( 0.5 *. length) ( 0.0 ) (startz -. height *. 0.5); 227 | dBodySetPosition body.(2) (-0.5 *. length) ( width *. 0.5) (startz -. height *. 0.5); 228 | dBodySetPosition body.(3) (-0.5 *. length) (-. width *. 0.5) (startz -. height *. 0.5); 229 | 230 | (* front and back wheel hinges *) 231 | let joint = 232 | (* joint.(0) is the front wheel *) 233 | Array.init 3 (fun i -> 234 | let joint = dJointCreateHinge2 world None in 235 | dJointAttach joint (Some body.(0)) (Some body.(i+1)); 236 | let a = dBodyGetPosition body.(i+1) in 237 | dJointSetHinge2Anchor joint a.x a.y a.z; 238 | (* 239 | dJointSetHinge2Axis1 joint 0. 0. 1.; 240 | dJointSetHinge2Axis2 joint 0. 1. 0.; 241 | *) 242 | (joint) 243 | ) 244 | in 245 | 246 | (* set joint suspension *) 247 | for i=0 to pred 3 do 248 | dJointSetHinge2Param joint.(i) DParamSuspensionERP 0.4; 249 | dJointSetHinge2Param joint.(i) DParamSuspensionCFM 0.8; 250 | done; 251 | 252 | (* lock back wheels along the steering axis *) 253 | for i=1 to pred 3 do 254 | (* set stops to make sure wheels always stay in alignment *) 255 | dJointSetHinge2Param joint.(i) DParamLoStop 0.; 256 | dJointSetHinge2Param joint.(i) DParamHiStop 0.; 257 | (* 258 | (* the following alternative method is no good as the wheels may get out 259 | of alignment: *) 260 | dJointSetHinge2Param joint.(i) DParamVel 0.; 261 | dJointSetHinge2Param joint.(i) DParamFMax dInfinity; 262 | *) 263 | done; 264 | 265 | (* create car space and add it to the top level space *) 266 | let car_space = dSimpleSpaceCreate (Some space) in 267 | dSpaceSetCleanup car_space false; 268 | dSpaceAdd car_space box.(0); 269 | dSpaceAdd car_space sphere.(0); 270 | dSpaceAdd car_space sphere.(1); 271 | dSpaceAdd car_space sphere.(2); 272 | 273 | (* environment *) 274 | let ground_box = dCreateBox (Some space) 2. 1.5 1. in 275 | let r = dRFromAxisAndAngle 0. 1. 0. (-0.15) in 276 | dGeomSetPosition ground_box 2. 0. (-0.34); 277 | dGeomSetRotation ground_box r; 278 | 279 | let destroy_all() = 280 | dGeomDestroy box.(0); 281 | Array.iter dGeomDestroy sphere; 282 | Array.iter dBodyDestroy body; 283 | dJointGroupDestroy contactgroup; 284 | dSpaceDestroy space; 285 | dWorldDestroy world; 286 | dCloseODE(); 287 | in 288 | 289 | begin 290 | (* set initial viewpoint *) 291 | let pos = (3.0, 4.9, -2.6) 292 | and angles = (102.6, 235.2) in 293 | 294 | (* call sim_step every N milliseconds *) 295 | let timer_msecs = 20 in 296 | 297 | (* simulation params (for the drawstuff lib) *) 298 | let dsd = 299 | ( (pos, angles, timer_msecs, world), 300 | (sim_draw body ground_box joint), 301 | (sim_loop world space ground ground_box joint contactgroup), 302 | (command), 303 | (destroy_all) 304 | ) 305 | in 306 | (* run simulation *) 307 | dsSimulationLoop 480 360 dsd; 308 | end; 309 | ;; 310 | 311 | -------------------------------------------------------------------------------- /examples/demo_I.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* 22 | Test that the rotational physics is correct. 23 | 24 | An "anchor body" has a number of other randomly positioned bodies 25 | ("particles") attached to it by ball-and-socket joints, giving it some 26 | random effective inertia tensor. the effective inertia matrix is calculated, 27 | and then this inertia is assigned to another "test" body. a random torque is 28 | applied to both bodies and the difference in angular velocity and orientation 29 | is observed after a number of iterations. 30 | 31 | typical errors for each test cycle are about 1e-5 ... 1e-4. 32 | *) 33 | 34 | (* Converted roughly from C to OCaml by Florent Monnier *) 35 | 36 | open Ode.LowLevel 37 | open Drawstuff 38 | 39 | (* some constants *) 40 | 41 | let num = 10 (* number of particles *) 42 | let side = 0.1 (* visual size of the particles *) 43 | 44 | (* emulate C variables *) 45 | 46 | exception Null_pointer 47 | let ( !< ) v = match !v with Some v -> v | None -> raise Null_pointer ;; 48 | let ( !> ) v = match !v with Some _ -> true | None -> false ;; 49 | let ( =: ) a b = (a := Some b) ;; 50 | 51 | (* dynamics objects an globals *) 52 | 53 | let world = ref None 54 | let anchor_body = ref None 55 | let particle = ref None 56 | let test_body = ref None 57 | let torque = Array.make 3 0. 58 | let iteration = ref 0 59 | 60 | (* utils *) 61 | 62 | let () = Random.self_init() ;; 63 | let dRandReal() = Random.float 1.0 ;; 64 | 65 | let ( += ) a b = (a := !a +. b) 66 | let ( -= ) a b = (a := !a -. b) 67 | let ( /= ) a b = (a := !a /. b) 68 | 69 | 70 | (* compute the mass parameters of a particle set. 71 | q = particle positions, 72 | pm = particle masses 73 | *) 74 | let computeMassParams m q pm = 75 | dMassSetZero (m); 76 | let m_mass = ref(dMass_mass m) 77 | and m_I = dMass_I m 78 | and m_c = dMass_c m 79 | in 80 | let r00 = ref m_I.r11 81 | and r01 = ref m_I.r12 82 | and r02 = ref m_I.r13 83 | and r03 = ref m_I.r14 84 | and r04 = ref m_I.r21 85 | and r05 = ref m_I.r22 86 | and r06 = ref m_I.r23 87 | and r07 = ref m_I.r24 88 | and r08 = ref m_I.r31 89 | and r09 = ref m_I.r32 90 | and r10 = ref m_I.r33 91 | and r11 = ref m_I.r34 92 | in 93 | let m_c0 = ref m_c.x 94 | and m_c1 = ref m_c.y 95 | and m_c2 = ref m_c.z 96 | and m_c3 = ref m_c.w 97 | in 98 | for i=0 to pred num do 99 | m_mass += pm.(i); 100 | 101 | m_c0 += pm.(i) *. q.(i).(0); 102 | m_c1 += pm.(i) *. q.(i).(1); 103 | m_c2 += pm.(i) *. q.(i).(2); 104 | 105 | r00 += pm.(i) *. (q.(i).(1) *. q.(i).(1) +. q.(i).(2) *. q.(i).(2)); 106 | r05 += pm.(i) *. (q.(i).(0) *. q.(i).(0) +. q.(i).(2) *. q.(i).(2)); 107 | r10 += pm.(i) *. (q.(i).(0) *. q.(i).(0) +. q.(i).(1) *. q.(i).(1)); 108 | r01 -= pm.(i) *. (q.(i).(0) *. q.(i).(1)); 109 | r02 -= pm.(i) *. (q.(i).(0) *. q.(i).(2)); 110 | r06 -= pm.(i) *. (q.(i).(1) *. q.(i).(2)); 111 | done; 112 | r04 := !r01; 113 | r08 := !r02; 114 | r09 := !r06; 115 | 116 | let m_c = { 117 | x = !m_c0 /. !m_mass; 118 | y = !m_c1 /. !m_mass; 119 | z = !m_c2 /. !m_mass; 120 | w = !m_c3; 121 | } 122 | and m_I = { 123 | r11 = !r00; 124 | r12 = !r01; 125 | r13 = !r02; 126 | r14 = !r03; 127 | r21 = !r04; 128 | r22 = !r05; 129 | r23 = !r06; 130 | r24 = !r07; 131 | r31 = !r08; 132 | r32 = !r09; 133 | r33 = !r10; 134 | r34 = !r11; 135 | } in 136 | dMass_set_I m m_I; 137 | dMass_set_mass m !m_mass; 138 | dMass_set_c m m_c; 139 | ;; 140 | 141 | 142 | let reset_test() = 143 | let m = dMassCreate() 144 | and anchor_m = dMassCreate() 145 | in 146 | let pos1 = [| 1.; 0.; 1.; |] (* point of reference (POR) *) 147 | and pos2 = [| -1.; 0.; 1. |] (* point of reference (POR) *) 148 | in 149 | 150 | (* particles with random positions (relative to POR) and masses *) 151 | let pm = Array.init num (fun _ -> dRandReal() +. 0.1) in 152 | let q = Array.init num (fun _ -> 153 | [| dRandReal() -. 0.5; 154 | dRandReal() -. 0.5; 155 | dRandReal() -. 0.5; |] 156 | ) in 157 | 158 | (* adjust particle positions so centor of mass = POR *) 159 | computeMassParams m q pm; 160 | let m_c = dMass_c m in 161 | for i=0 to pred num do 162 | q.(i).(0) <- q.(i).(0) -. m_c.x; 163 | q.(i).(1) <- q.(i).(1) -. m_c.y; 164 | q.(i).(2) <- q.(i).(2) -. m_c.z; 165 | done; 166 | 167 | if !> world then dWorldDestroy ! 177 | let particle = dBodyCreate ! () (* pause *) | false -> 257 | dBodyAddTorque != 100) then begin 263 | (* measure the difference between the anchor and test bodies *) 264 | let w1 = dBodyGetAngularVel ! 284 | let sides = (side,side,side) 285 | and sides2 = (6.*.side, 6.*.side, 6.*.side) 286 | and sides3 = (3.*.side, 3.*.side, 3.*.side) 287 | in 288 | let color = (1.,1.,1.) in 289 | dsDrawBox (dBodyGetPosition ! ()), 328 | (free_env) 329 | ) 330 | in 331 | (* run simulation *) 332 | dsSimulationLoop 480 360 dsd; 333 | end; 334 | ;; 335 | 336 | (* vim: sw=2 sts=2 ts=2 et 337 | *) 338 | -------------------------------------------------------------------------------- /examples/demo_boxstack.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* WIP OCaml version by Florent Monnier *) 22 | 23 | open Ode.LowLevel 24 | open Drawstuff 25 | 26 | 27 | (*<---- Convex Object *) 28 | let planes = [| 29 | (* planes for a cube, these should coincide with the face array *) 30 | 1.0; 0.0; 0.0; 0.25; 31 | 0.0; 1.0; 0.0; 0.25; 32 | 0.0; 0.0; 1.0; 0.25; 33 | -1.0; 0.0; 0.0; 0.25; 34 | 0.0; -1.0; 0.0; 0.25; 35 | 0.0; 0.0; -1.0; 0.25; 36 | |] 37 | 38 | let points = [| 39 | (* points for a cube *) 40 | 0.25; 0.25; 0.25; (* point 0 *) 41 | -0.25; 0.25; 0.25; (* point 1 *) 42 | 43 | 0.25;-0.25; 0.25; (* point 2 *) 44 | -0.25;-0.25; 0.25; (* point 3 *) 45 | 46 | 0.25; 0.25;-0.25; (* point 4 *) 47 | -0.25; 0.25;-0.25; (* point 5 *) 48 | 49 | 0.25;-0.25;-0.25; (* point 6 *) 50 | -0.25;-0.25;-0.25; (* point 7 *) 51 | |] 52 | 53 | let polygons = [| 54 | (* Polygons for a cube (6 squares) *) 55 | 4;0;2;6;4; (* positive X *) 56 | 4;1;0;4;5; (* positive Y *) 57 | 4;0;1;3;2; (* positive Z *) 58 | 4;3;1;5;7; (* negative X *) 59 | 4;2;3;7;6; (* negative Y *) 60 | 4;5;4;6;7; (* negative Z *) 61 | |] 62 | (*----> Convex Object *) 63 | 64 | 65 | (* some constants *) 66 | 67 | let _NUM = 100 (* max number of objects *) 68 | let density = (5.0) (* density of all objects *) 69 | let gpb = 3 (* maximum number of geometries per body *) 70 | let max_contacts = 8 (* maximum number of contact points per body *) 71 | let use_geom_offset = true 72 | 73 | 74 | (* some variables *) 75 | 76 | type 'a my_object = { 77 | body : dBodyID; (* the body *) 78 | geom : 'a dGeomID array; (* geometries representing this body *) 79 | } 80 | 81 | let num = ref 0 (* number of objects in simulation *) 82 | let nextobj = ref 0 (* next object to recycle if num==_NUM *) 83 | 84 | let selected = ref(-1) (* selected object *) 85 | let show_aabb = ref false (* show geom AABBs? *) 86 | let show_contacts = ref false (* show contact points? *) 87 | let random_pos = ref true (* drop objects from random position? *) 88 | let show_body = true 89 | 90 | (* utils *) 91 | 92 | let () = Random.self_init() 93 | let dRandReal() = Random.float 1.0 94 | 95 | let opt = function Some v -> v | None -> raise Not_found 96 | 97 | let draw_contacts = ref (fun () -> ()) 98 | 99 | 100 | (* this is called by dSpaceCollide when two objects in space are 101 | potentially colliding. *) 102 | 103 | let nearCallback world contactgroup = fun o1 o2 -> 104 | 105 | let b1 = dGeomGetBody o1 106 | and b2 = dGeomGetBody o2 in 107 | match b1, b2 with 108 | (* exit without doing anything if the two bodies are connected by a joint *) 109 | | Some _b1, Some _b2 110 | when (dAreConnectedExcluding _b1 _b2 JointTypeContact) -> () 111 | | _ -> 112 | 113 | (* up to max_contacts contacts per geom-geom *) 114 | let surf_param = { surf_param_zero with 115 | sp_mode = [`dContactBounce; `dContactSoftCFM]; 116 | sp_mu = dInfinity; 117 | sp_mu2 = 0.0; 118 | sp_bounce = 0.1; 119 | sp_bounce_vel = 0.1; 120 | sp_soft_cfm = 0.01; 121 | } in 122 | ArrayLabels.iter (dCollide o1 o2 max_contacts) ~f:(fun contact_geom -> 123 | let ri = dRGetIdentity() 124 | and ss = (0.02, 0.02, 0.02) 125 | and contact = { 126 | c_surface = surf_param; 127 | c_geom = contact_geom; 128 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.} 129 | } in 130 | let c = dJointCreateContact world (Some contactgroup) contact in 131 | dJointAttach c b1 b2; 132 | draw_contacts := (fun () -> 133 | let color = (1.0, 0.8, 0.2) in 134 | if !show_contacts then dsDrawWireBox (contact_geom.cg_pos) ri ss color; 135 | ); 136 | ); 137 | ;; 138 | 139 | 140 | (* usage message *) 141 | 142 | let usage() = 143 | print_endline " 144 | To drop another object, press: 145 | 'b' for box, 146 | 's' for sphere, 147 | 'c' for capsule, 148 | 'y' for cylinder, 149 | 'n' for a convex object, 150 | 'x' for a composite object. 151 | Press \"space\" to select an object. 152 | Press 'd' to disable the selected object. 153 | Press 'e' to enable the selected object. 154 | Press 'a' to toggle showing the geom AABBs. 155 | Press 't' to toggle showing the contact points. 156 | Press 'r' to toggle dropping from random position/orientation.\n"; 157 | ;; 158 | 159 | 160 | (* called when a key pressed *) 161 | 162 | let command world space obj convex_data = fun cmd -> 163 | 164 | let m = dMassCreate() in 165 | 166 | match Char.lowercase_ascii cmd with 167 | | ' ' -> 168 | incr selected; 169 | if (!selected >= !num) then selected := 0; 170 | if (!selected < 0) then selected := 0; 171 | 172 | | 'd' when (!selected >= 0 && !selected < !num) -> 173 | dBodyDisable ((opt obj.(!selected)).body); 174 | 175 | | 'e' when (!selected >= 0 && !selected < !num) -> 176 | dBodyEnable ((opt obj.(!selected)).body); 177 | 178 | | 'a' -> 179 | show_aabb := not !show_aabb; 180 | 181 | | 't' -> 182 | show_contacts := not(!show_contacts); 183 | 184 | | 'r' -> 185 | random_pos := not(!random_pos); 186 | 187 | | 'b' | 's' | 'c' (* | 'x' *) | 'y' | 'n' -> 188 | begin 189 | let setBody = ref false in 190 | let i = 191 | if (!num < _NUM) then begin 192 | let i = !num in 193 | incr num; 194 | (i) 195 | end 196 | else begin 197 | let i = !nextobj in 198 | incr nextobj; 199 | if (!nextobj >= !num) then nextobj := 0; 200 | 201 | (* destroy the body and geoms for slot i *) 202 | dBodyDestroy (opt obj.(i)).body; 203 | Array.iter dGeomDestroy (opt obj.(i)).geom; 204 | obj.(i) <- None; 205 | (i) 206 | end 207 | in 208 | 209 | let obj_i_body = dBodyCreate world in 210 | 211 | let sides = { 212 | x = dRandReal() *. 0.5 +. 0.1; 213 | y = dRandReal() *. 0.5 +. 0.1; 214 | z = dRandReal() *. 0.5 +. 0.1; 215 | w = 0.; 216 | } in 217 | 218 | let rot = 219 | if (!random_pos) then 220 | begin 221 | dBodySetPosition obj_i_body 222 | (dRandReal() *. 2. -. 1.) 223 | (dRandReal() *. 2. -. 1.) 224 | (dRandReal() +. 2.); 225 | dRFromAxisAndAngle (dRandReal() *. 2.0 -. 1.0) 226 | (dRandReal() *. 2.0 -. 1.0) 227 | (dRandReal() *. 2.0 -. 1.0) 228 | (dRandReal() *. 10.0 -. 5.0); 229 | end 230 | else 231 | begin 232 | let maxheight = ref 0. in 233 | for k=0 to pred !num do 234 | let _body = 235 | if k = i 236 | then obj_i_body 237 | else (opt obj.(k)).body 238 | in 239 | let pos = dBodyGetPosition _body in 240 | if (pos.z > !maxheight) then maxheight := pos.z; 241 | done; 242 | dBodySetPosition obj_i_body 0. 0. (!maxheight +. 1.); 243 | dRGetIdentity() 244 | (* dRFromAxisAndAngle 0. 0. 1. ( (*dRandReal() *. 10.0 -. 5.0*) 0.); *) 245 | end 246 | in 247 | 248 | dBodySetRotation obj_i_body rot; 249 | dBodySetData obj_i_body i; 250 | 251 | (* create dynamics and collision objects *) 252 | let obj_i_geom = 253 | begin match cmd with 254 | | 'b' -> 255 | dMassSetBox m density sides.x sides.y sides.z; 256 | let geom0 = dCreateBox (Some space) sides.x sides.y sides.z in 257 | [| (Obj.magic geom0 : 'a dGeomID) |] 258 | 259 | | 'c' -> 260 | let radius = sides.x *. 0.5 261 | and length = sides.y in 262 | dMassSetCapsule m density Dir_z radius length; 263 | let geom0 = dCreateCapsule (Some space) sides.x sides.y in 264 | [| (Obj.magic geom0 : 'a dGeomID) |] 265 | 266 | (*<---- Convex Object *) 267 | | 'n' -> 268 | dMassSetBox m density 0.25 0.25 0.25; 269 | let geom0 = dCreateConvex (Some space) convex_data in 270 | [| (Obj.magic geom0 : 'a dGeomID) |] 271 | 272 | (*----> Convex Object *) 273 | 274 | | 'y' -> 275 | dMassSetCylinder m density Dir_z sides.x sides.y; 276 | let geom0 = dCreateCylinder (Some space) sides.x sides.y in 277 | [| (Obj.magic geom0 : 'a dGeomID) |] 278 | 279 | | 's' -> 280 | let radius = sides.x *. 0.5 in 281 | dMassSetSphere m density radius; 282 | let geom0 = dCreateSphere (Some space) radius in 283 | [| (Obj.magic geom0 : 'a dGeomID) |] 284 | 285 | | 'x' when use_geom_offset -> 286 | [| |] 287 | (* TODO 288 | setBody := true; 289 | (* start accumulating masses for the encapsulated geometries *) 290 | dMass m2; 291 | dMassSetZero (&m); 292 | 293 | dReal dpos[gpb][3]; (* delta-positions for encapsulated geometries *) 294 | dMatrix3 drot[gpb]; 295 | 296 | (* set random delta positions *) 297 | for (j=0; j 342 | [| |] 343 | (* TODO 344 | dGeomID g2[gpb]; (* encapsulated geometries *) 345 | dReal dpos[gpb][3]; (* delta-positions for encapsulated geometries *) 346 | 347 | (* start accumulating masses for the encapsulated geometries *) 348 | dMass m2; 349 | dMassSetZero (&m); 350 | 351 | (* set random delta positions *) 352 | for (j=0; j 405 | invalid_arg "bug command" (* this point should never been reached *) 406 | end 407 | in 408 | 409 | if not(!setBody) then 410 | ArrayLabels.iter obj_i_geom ~f:(fun geom -> 411 | dGeomSetBody geom (Some obj_i_body); 412 | ); 413 | 414 | dBodySetMass obj_i_body m; 415 | 416 | obj.(i) <- Some { 417 | body = obj_i_body; 418 | geom = obj_i_geom; 419 | }; 420 | 421 | end 422 | | _ -> () 423 | ;; 424 | 425 | 426 | (* draw a geom *) 427 | 428 | let rec drawGeom g pos rot show_aabb color = 429 | 430 | let pos = match pos with Some pos -> pos | None -> dGeomGetPosition g 431 | and rot = match rot with Some rot -> rot | None -> dGeomGetRotation g in 432 | 433 | begin match geom_kind g with 434 | | Box_geom g -> 435 | let l = dGeomBoxGetLengths g in 436 | let sides = (l.x, l.y, l.z) in 437 | dsDrawBox pos rot sides color; 438 | 439 | | Sphere_geom g -> 440 | dsDrawSphere pos rot (dGeomSphereGetRadius g) color; 441 | 442 | | Capsule_geom g -> 443 | let radius, length = dGeomCapsuleGetParams g in 444 | dsDrawCapsule pos rot length radius color; 445 | 446 | (*<---- Convex Object *) 447 | | Convex_geom g -> 448 | dsDrawConvex pos rot planes 449 | points 450 | polygons color; 451 | (*----> Convex Object *) 452 | 453 | | Cylinder_geom g -> 454 | let radius, length = dGeomCylinderGetParams g in 455 | dsDrawCylinder pos rot length radius color; 456 | 457 | | GeomTransform_geom g -> 458 | let g2 = None (* dGeomTransformGetGeom g *) in 459 | begin match g2 with 460 | | None -> prerr_endline "Error: Empty GeomTransform" 461 | | Some g2 -> 462 | let pos2 = dGeomGetPosition g2 463 | and rot2 = dGeomGetRotation g2 in 464 | let actual_pos = dMultiply0_331 rot pos2 in 465 | let actual_pos = { 466 | x = actual_pos.x +. pos.x; 467 | y = actual_pos.y +. pos.y; 468 | z = actual_pos.z +. pos.z; 469 | w = 0.; 470 | } in 471 | let actual_R = dMultiply0_333 rot rot2 in 472 | drawGeom g2 (Some actual_pos) (Some actual_R) false color; 473 | end 474 | | _ -> () 475 | end; 476 | 477 | if show_body then begin 478 | let body = dGeomGetBody g in 479 | match body with 480 | | None -> () 481 | | Some body -> 482 | let bodypos = dBodyGetPosition body 483 | and bodyr = dBodyGetRotation body in 484 | let bodySides = (0.1, 0.1, 0.1) 485 | and color = (0.,1.,0.) in 486 | (* 487 | dsDrawWireBox bodypos bodyr bodySides color; 488 | *) 489 | dsDrawAbovePoint bodypos color; ignore(bodyr,bodySides); 490 | end; 491 | if show_aabb then begin 492 | (* draw the bounding box for this geom *) 493 | let aabb = dGeomGetAABB g in 494 | let bbpos = { 495 | x = 0.5 *. (aabb.(0) +. aabb.(1)); 496 | y = 0.5 *. (aabb.(2) +. aabb.(3)); 497 | z = 0.5 *. (aabb.(4) +. aabb.(5)); 498 | w = 0.; 499 | } 500 | and bbsides = ( 501 | aabb.(1) -. aabb.(0), 502 | aabb.(3) -. aabb.(2), 503 | aabb.(5) -. aabb.(4) 504 | ) 505 | and ri = dRGetIdentity() 506 | and color = (0.,0.8,1.) in 507 | dsDrawWireBox bbpos ri bbsides color; 508 | end; 509 | ;; 510 | 511 | 512 | (* simulation loop *) 513 | 514 | let sim_step world space obj contactgroup = fun pause -> 515 | 516 | dSpaceCollide space (nearCallback world contactgroup); 517 | if not(pause) then dWorldQuickStep world 0.02; 518 | 519 | (* remove all contact joints *) 520 | dJointGroupEmpty (contactgroup); 521 | ;; 522 | 523 | 524 | (* draw the scene *) 525 | 526 | let sim_draw obj = fun () -> 527 | 528 | (!draw_contacts)(); 529 | 530 | dsDrawPlane (0.0, 0.0, 0.0) ~scale:(1.4) (1.0, 0.0, 0.0); 531 | 532 | ArrayLabels.iteri obj ~f:(fun i obj_i -> 533 | match obj_i with 534 | | None -> () 535 | | Some obj_i -> 536 | ArrayLabels.iter obj_i.geom ~f:(fun obj_i_geom_j -> 537 | let color = 538 | if i = !selected then 539 | (0., 0.7, 1.) 540 | else if (dBodyIsEnabled obj_i.body) then 541 | (1.0, 0.9, 0.) 542 | else 543 | (0.9, 0.7, 0.) 544 | in 545 | drawGeom obj_i_geom_j None None !show_aabb color; 546 | ); 547 | ); 548 | ;; 549 | 550 | 551 | (* main *) 552 | 553 | let () = 554 | usage(); 555 | 556 | (* create world *) 557 | dInitODE(); 558 | let world = dWorldCreate() 559 | and space = dHashSpaceCreate None 560 | and contactgroup = dJointGroupCreate() in 561 | dWorldSetGravity world 0. 0. (-0.5); 562 | dWorldSetCFM world (1e-5); 563 | dWorldSetAutoDisableFlag world true; 564 | 565 | if true then 566 | dWorldSetAutoDisableAverageSamplesCount world 10; 567 | 568 | dWorldSetContactMaxCorrectingVel world 0.1; 569 | dWorldSetContactSurfaceLayer world 0.001; 570 | let _ = dCreatePlane (Some space) 0. 0. 1. 0. in 571 | let obj = Array.make _NUM None in 572 | 573 | let convex_data = dConvexDataBuild planes points polygons in 574 | 575 | let free_env() = 576 | dConvexDataDestroy convex_data; 577 | dJointGroupDestroy contactgroup; 578 | dSpaceDestroy space; 579 | dWorldDestroy world; 580 | dCloseODE(); 581 | in 582 | 583 | begin 584 | (* set initial viewpoint *) 585 | let pos = (2.5, 6.1, -3.6) 586 | and angles = (111.2, 202.2) in 587 | 588 | (* call sim_step every N milliseconds *) 589 | let timer_msecs = 10 in 590 | 591 | (* simulation params (for the drawstuff lib) *) 592 | let dsd = 593 | ( (pos, angles, timer_msecs, world), 594 | (sim_draw obj), 595 | (sim_step world space obj contactgroup), 596 | (command world space obj convex_data), 597 | (free_env) 598 | ) 599 | in 600 | (* run simulation *) 601 | dsSimulationLoop 480 360 dsd; 602 | end; 603 | ;; 604 | 605 | (* vim: sw=2 sts=2 ts=2 et 606 | *) 607 | -------------------------------------------------------------------------------- /examples/demo_basket.ml: -------------------------------------------------------------------------------- 1 | (* 2 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. 3 | * All rights reserved. Email: russ@q12.org Web: www.q12.org 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of EITHER: 7 | * (1) The GNU Lesser General Public License as published by the Free 8 | * Software Foundation; either version 2.1 of the License, or (at 9 | * your option) any later version. The text of the GNU Lesser 10 | * General Public License is included with this library in the 11 | * file LICENSE_LGPL.txt. 12 | * (2) The BSD-style license that is included with this library in 13 | * the file LICENSE_BSD.txt. 14 | * 15 | * This library is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 18 | * LICENSE_LGPL.txt and LICENSE_BSD.txt for more details. 19 | *) 20 | 21 | (* Basket ball demo. 22 | Serves as a test for the sphere vs trimesh collider 23 | By Bram Stolk. *) 24 | 25 | (* Converted from C to OCaml by Florent Monnier *) 26 | 27 | let usage() = 28 | print_endline " Press the spacebar to reset the position of the ball.\n"; 29 | ;; 30 | 31 | open Ode.LowLevel 32 | open Drawstuff 33 | 34 | (* this is our world meshA *) 35 | 36 | 37 | (* some constants *) 38 | 39 | let radius = 0.14 40 | 41 | 42 | let is_nan f = (Stdlib.compare nan f) = 0 ;; 43 | 44 | (* this is called by dSpaceCollide when two objects in space are 45 | potentially colliding. *) 46 | 47 | let rec nearCallback world contactgroup = fun o1 o2 -> 48 | 49 | if (dGeomIsSpace o1) || (dGeomIsSpace o2) then 50 | begin 51 | (* colliding a space with something *) 52 | dSpaceCollide2 o1 o2 (nearCallback world contactgroup); 53 | (* Note we do not want to test intersections within a space, *) 54 | (* only between spaces. *) 55 | () 56 | end else 57 | begin 58 | let contact_geom_arr = dCollide o1 o2 32 in 59 | ArrayLabels.iter contact_geom_arr ~f:(fun contact_geom -> 60 | (* 61 | (* checks for valid datas *) 62 | assert(dVALIDVEC3(contact_geom.cg_pos)); 63 | assert(dVALIDVEC3(contact_geom.cg_normal)); 64 | assert(not(is_nan(contact_geom.cg_depth))); 65 | *) 66 | let surf_param = {surf_param_zero with 67 | sp_mode = [`dContactSlip1; `dContactSoftERP; 68 | `dContactSlip2; `dContactSoftCFM; `dContactApprox1;]; 69 | sp_mu = 50.0; (* was: dInfinity *) 70 | sp_slip1 = 0.7; 71 | sp_slip2 = 0.7; 72 | sp_soft_erp = 0.96; 73 | sp_soft_cfm = 0.04; 74 | } in 75 | let contact = { 76 | c_surface = surf_param; 77 | c_geom = contact_geom; 78 | c_fdir1 = {x=0.; y=0.; z=0.; w=0.}; 79 | } in 80 | let c = dJointCreateContact world (Some contactgroup) contact in 81 | dJointAttach c (dGeomGetBody contact_geom.cg_g1) 82 | (dGeomGetBody contact_geom.cg_g2); 83 | ); 84 | end; 85 | ;; 86 | 87 | 88 | 89 | (* puts the ball at its initial position *) 90 | let reset_ball sphbody = 91 | let sx=0.0 and sy=3.40 and sz=7.05 in 92 | let q = dQGetIdentity() in 93 | dBodySetPosition sphbody sx sy sz; 94 | dBodySetQuaternion sphbody q; 95 | dBodySetLinearVel sphbody 0. 0. 0.; 96 | dBodySetAngularVel sphbody 0. 0. 0.; 97 | ;; 98 | 99 | 100 | (* called when a key pressed *) 101 | let command sphbody = function 102 | | ' ' -> reset_ball sphbody; 103 | | _ -> () 104 | ;; 105 | 106 | 107 | (* simulation loop *) 108 | 109 | let sim_step world space contactgroup = fun pause -> 110 | let simstep = 0.001 in (* 1ms simulation steps *) 111 | (* 112 | let dt = dsElapsedTime() in 113 | *) 114 | let dt = 0.01666 in 115 | 116 | let nrofsteps = truncate(ceil (dt /. simstep)) in 117 | (* Printf.eprintf "dt=%f, nr of steps = %d\n" dt nrofsteps; *) 118 | 119 | if not(pause) then 120 | for i=0 to pred nrofsteps do 121 | dSpaceCollide space (nearCallback world contactgroup); 122 | dWorldQuickStep world simstep; 123 | dJointGroupEmpty contactgroup; 124 | done; 125 | ;; 126 | 127 | 128 | (* draw simulation scene *) 129 | 130 | let sim_draw sphbody world_mesh 131 | world_indices world_vertices world_normals = fun () -> 132 | let color = (1.,0.2,0.) in 133 | let spos = dBodyGetPosition sphbody 134 | and srot = dBodyGetRotation sphbody in 135 | dsDrawSphere spos srot radius color; 136 | 137 | (* draw world trimesh *) 138 | let color = (0.4,0.8,0.2) in 139 | let pos = dGeomGetPosition world_mesh 140 | and rot = dGeomGetRotation world_mesh 141 | in 142 | dsDrawTriangles pos rot world_vertices world_indices 143 | (Some world_normals) color; 144 | ;; 145 | 146 | 147 | (* main *) 148 | let () = 149 | usage(); 150 | (* {{{ datas *) 151 | 152 | let world_normals = [| 153 | 0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;1.;0.;0.;1.;0.;0.; 154 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;-0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;0.;0.;-1.; 155 | 0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;-0.;1.;0.;0.;1.;0.;0.;1.; 156 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;1.;0.;-0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 157 | 0.;1.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.; 158 | -1.;0.;0.;-1.;0.;0.;0.;-0.948064;0.318080;0.;-0.989482;0.144655;0.;-0.983494;0.180939; 159 | 0.;-0.983494;0.180939;0.;-0.908999;0.416798;0.;-0.948064;0.318080;0.;0.;1.;0.;0.;1.;0.; 160 | 0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.; 161 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 162 | 1.;0.;0.;1.;0.;0.;1.;0.;-1.;0.;-0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;1.; 163 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;-0.132460; 164 | 0.991188;0.;0.264920;0.964270;0.;0.132460;0.991188;0.;0.132460;0.991188;0.;-0.264920; 165 | 0.964270;0.;-0.132460;0.991188;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.; 166 | -1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 167 | 0.;1.;0.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;-1.;0.;0.;-1.;0.; 168 | 0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 169 | 1.;0.;0.;1.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;-0.687592;-0.726097; 170 | -0.;-0.881727;-0.471761;0.;-0.687592;-0.726097;-0.;-0.881727;-0.471761;0.;-0.881727; 171 | -0.471761;0.;-0.687592;-0.726097;-0.;0.687592;-0.726097;0.;0.928375;-0.371644;0.; 172 | 0.824321;-0.566123;0.;0.687592;-0.726097;0.;0.824321;-0.566123;0.;0.687592;-0.726097; 173 | 0.;-0.881727;-0.471761;0.;-0.985594;-0.169128;0.;-0.985594;-0.169128;0.;-0.985594; 174 | -0.169128;0.;-0.881727;-0.471761;0.;-0.881727;-0.471761;0.;0.928375;-0.371644;0.; 175 | 0.985594;-0.169128;0.;0.985594;-0.169128;0.;0.928375;-0.371644;0.;0.985594;-0.169128; 176 | 0.;0.824321;-0.566123;0.;-0.870167;0.492758;0.;-0.870167;0.492758;0.;-0.870167;0.492758; 177 | 0.;-0.870167;0.492758;0.;-0.870167;0.492758;0.;-0.870167;0.492758;0.;0.870167;0.492758; 178 | 0.;0.870167;0.492758;0.;0.870167;0.492758;0.;0.870167;0.492758;0.;0.870167;0.492758;0.; 179 | 0.870167;0.492758;-0.;-0.390313;0.920682;0.;-0.132460;0.991188;0.;-0.264920;0.964270;0.; 180 | -0.264920;0.964270;0.;-0.390313;0.920682;0.;-0.390313;0.920682;0.;0.390313;0.920682;0.; 181 | 0.132460;0.991188;0.;0.264920;0.964270;0.;0.390313;0.920682;0.;0.264920;0.964270;0.; 182 | 0.390313;0.920682;-0.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.; 183 | 0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.; 184 | 0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.; 185 | -1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.; 186 | 0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.; 187 | 0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.; 188 | -1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.;0.;-1.;0.; 189 | 0.;-1.;0.;0.;-1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.; 190 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 191 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 192 | 0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.; 193 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 194 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 195 | 0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.985594;0.169128;0.;0.824321;0.566123;0.;0.928375; 196 | 0.371644;0.;0.928375;0.371644;0.;0.985594;0.169128;0.;0.985594;0.169128;0.;0.824321; 197 | 0.566123;0.;0.687592;0.726097;0.;0.687592;0.726097;0.;0.687592;0.726097;0.;0.928375; 198 | 0.371644;0.;0.824321;0.566123;0.;0.;1.;0.;-0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.; 199 | 0.;-0.687592;0.726097;0.;-0.687592;0.726097;0.;-0.881727;0.471761;0.;-0.881727; 200 | 0.471761;0.;-0.881727;0.471761;0.;-0.687592;0.726097;0.;-0.881727;0.471761;0.; 201 | -0.985594;0.169128;0.;-0.985594;0.169128;0.;-0.985594;0.169128;0.;-0.881727; 202 | 0.471761;0.;-0.881727;0.471761;0.;-0.870166;-0.492758;0.;-0.870166;-0.492758;0.; 203 | -0.870166;-0.492758;0.;-0.870166;-0.492758;0.;-0.870166;-0.492758;0.;-0.870166; 204 | -0.492758;0.;-0.390314;-0.920682;0.;-0.132460;-0.991188;0.;-0.264921;-0.964270;0.; 205 | -0.264921;-0.964270;0.;-0.390314;-0.920682;0.;-0.390314;-0.920682;0.;-0.132460; 206 | -0.991188;0.;0.264921;-0.964270;0.;0.132460;-0.991188;0.;0.132460;-0.991188;0.; 207 | -0.264921;-0.964270;0.;-0.132460;-0.991188;0.;0.264921;-0.964270;0.;0.390314; 208 | -0.920682;0.;0.390314;-0.920682;0.;0.390314;-0.920682;0.;0.132460;-0.991188;0.; 209 | 0.264921;-0.964270;0.;0.870166;-0.492758;0.;0.870166;-0.492758;0.;0.870166;-0.492758; 210 | 0.;0.870166;-0.492758;0.;0.870166;-0.492758;0.;0.870166;-0.492758;0.;0.;0.;1.;0.;0.; 211 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.; 212 | 0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 213 | 0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;-0.527606;0.849489;0.;-0.793893;0.608057;0.; 214 | -0.715135;0.698986;0.;-0.715135;0.698986;0.;-0.418249;0.908332;0.;-0.527606;0.849489; 215 | 0.;-0.075284;0.997162;0.;-0.253577;0.967315;0.;-0.202069;0.979371;0.;-0.202069;0.979371; 216 | 0.;-0.075284;0.997162;0.;-0.075284;0.997162;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 217 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.; 218 | 0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.160137;0.987095;0.;0.049305;0.998784;0.;0.049305; 219 | 0.998784;0.;0.049305;0.998784;0.;0.221401;0.975183;0.;0.160137;0.987095;0.;0.696124; 220 | 0.717921;0.;0.696124;0.717921;0.;0.433340;0.901230;0.;0.433340;0.901230;0.;0.433340; 221 | 0.901230;0.;0.696124;0.717921;0.;0.696124;0.717921;0.;0.696124;0.717921;0.;0.838308; 222 | 0.545197;0.;0.696124;0.717921;0.;0.872167;0.489208;0.;0.838308;0.545197;0.;-0.994126; 223 | 0.108225;0.;-0.983494;0.180939;0.;-0.989482;0.144655;0.;-0.994126;0.108225;0.;-0.989482; 224 | 0.144655;0.;-0.994126;0.108225;0.;-0.948064;0.318080;0.;-0.908999;0.416798;0.;-0.793893; 225 | 0.608057;0.;-0.908999;0.416798;0.;-0.715135;0.698986;0.;-0.793893;0.608057;0.;-0.527606; 226 | 0.849489;0.;-0.418249;0.908332;0.;-0.253577;0.967315;0.;-0.418249;0.908332;0.;-0.202069; 227 | 0.979371;0.;-0.253577;0.967315;0.;-0.075284;0.997162;0.;-0.075284;0.997162;0.;0.;1.;0.; 228 | -0.075284;0.997162;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.; 229 | 1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.;1.;0.;0.049305; 230 | 0.998784;0.;0.;1.;0.;0.049305;0.998784;0.;0.049305;0.998784;0.;0.160137;0.987095;0.; 231 | 0.221401;0.975183;0.;0.433340;0.901230;0.;0.221401;0.975183;0.;0.433340;0.901230;0.; 232 | 0.433340;0.901230;0.;0.902172;0.431376;0.;0.838308;0.545197;0.;0.872167;0.489208;0.; 233 | 0.872167;0.489208;0.;0.902172;0.431376;0.;0.902172;0.431376; 234 | |] 235 | in 236 | 237 | let world_vertices = [| 238 | -4.;-4.;-0.1; 4.;-4.;-0.1; 4.;-4.;0.1; -4.;-4.;-0.1; 4.;-4.;0.1; 239 | -4.;-4.;0.1; 4.;0.;0.1; 4.;-4.;-0.1; 4.;4.;-0.1; 4.;0.;0.1; 240 | 4.;4.;-0.1; 4.;4.;0.1; 4.;0.;0.1; 4.;-4.;0.1; 4.;-4.;-0.1; 241 | -4.;-4.;-0.1; -4.;4.;-0.1; 4.;4.;-0.1; -4.;-4.;-0.1; 4.;4.;-0.1; 242 | 4.;-4.;-0.1;0.066;-2.06;2.;0.066;-1.94;2.;-0.066;-2.06;2.;0.066;-1.94; 243 | 2.;-0.066;-1.94;2.;-0.066;-2.06;2.;-4.;4.;0.1;4.;4.;0.1;4.;4.;-0.1;4.; 244 | 4.;-0.1;-4.;4.;-0.1;-4.;4.;0.1;-4.;-4.;0.1;-4.;0.;0.1;-4.;-4.;-0.1;-4.; 245 | 0.;0.1;-4.;4.;0.1;-4.;4.;-0.1;-4.;0.;0.1;-4.;4.;-0.1;-4.;-4.;-0.1;0.36; 246 | 3.244444;1.466974;0.36;3.422222;2.266974;-0.36;3.422222;2.266974;-0.36; 247 | 3.422222;2.266974;-0.36;3.244444;1.466974;0.36;3.244444;1.466974;4.;-4.; 248 | 0.1;0.066;-2.06;0.1;-0.066;-2.06;0.1;-0.066;-2.06;0.1;-4.;-4.;0.1;4.;-4.; 249 | 0.1;4.;0.;0.1;0.066;-1.94;0.1;4.;-4.;0.1;0.066;-1.94;0.1;0.066;-2.06;0.1; 250 | 4.;-4.;0.1;-0.066;-1.94;0.1;0.066;-1.94;0.1;4.;0.;0.1;4.;0.;0.1;-4.;0.; 251 | 0.1;-0.066;-1.94;0.1;-0.066;-2.06;0.1;-0.066;-1.94;0.1;-4.;0.;0.1;-4.;0.; 252 | 0.1;-4.;-4.;0.1;-0.066;-2.06;0.1;0.066;-2.06;2.;-0.066;-2.06;2.;-0.066; 253 | -2.06;0.1;-0.066;-2.06;0.1;0.066;-2.06;0.1;0.066;-2.06;2.;0.066;-1.94; 254 | 1.95;0.066;-1.94;2.;0.066;-2.06;2.;0.066;-2.06;2.;0.066;-2.06;0.1;0.066; 255 | -1.94;1.95;0.066;-2.06;0.1;0.066;-1.94;0.1;0.066;-1.94;1.95;-0.052853; 256 | -1.50639;2.;0.052853;-1.50639;2.;0.052853;-1.50639;1.95;0.052853;-1.50639; 257 | 1.95;-0.052853;-1.50639;1.95;-0.052853;-1.50639;2.;-0.066;-2.06;0.1; 258 | -0.066;-2.06;2.;-0.066;-1.94;1.95;-0.066;-2.06;0.1;-0.066;-1.94;1.95; 259 | -0.066;-1.94;0.1;-0.066;-2.06;2.;-0.066;-1.94;2.;-0.066;-1.94;1.95;-0.066; 260 | -1.94;0.1;-0.066;-1.94;1.95;0.066;-1.94;1.95;-0.066;-1.94;0.1;0.066;-1.94; 261 | 1.95;0.066;-1.94;0.1;-0.066;-1.94;1.95;-0.066;-1.84;1.95;0.066;-1.94;1.95; 262 | -0.066;-1.84;1.95;0.066;-1.84;1.95;0.066;-1.94;1.95;-0.066;-1.94;2.; 263 | -0.066;-1.84;2.;-0.066;-1.84;1.95;-0.066;-1.84;1.95;-0.066;-1.94;1.95; 264 | -0.066;-1.94;2.;0.066;-1.94;2.;0.066;-1.84;2.;-0.066;-1.94;2.;0.066; 265 | -1.84;2.;-0.066;-1.84;2.;-0.066;-1.94;2.;0.066;-1.94;1.95;0.066;-1.84; 266 | 1.95;0.066;-1.84;2.;0.066;-1.94;1.95;0.066;-1.84;2.;0.066;-1.94;2.; 267 | -0.066;-1.84;2.;-0.1716;-1.74;2.;-0.066;-1.84;1.95;-0.1716;-1.74;2.; 268 | -0.1716;-1.74;1.95;-0.066;-1.84;1.95;0.066;-1.84;1.95;0.1716;-1.74;1.95; 269 | 0.1716;-1.74;2.;0.066;-1.84;1.95;0.1716;-1.74;2.;0.066;-1.84;2.;-0.1716; 270 | -1.74;2.;-0.18876;-1.64;2.;-0.18876;-1.64;1.95;-0.18876;-1.64;1.95; 271 | -0.1716;-1.74;1.95;-0.1716;-1.74;2.;0.1716;-1.74;1.95;0.18876;-1.64; 272 | 1.95;0.18876;-1.64;2.;0.1716;-1.74;1.95;0.18876;-1.64;2.;0.1716;-1.74; 273 | 2.;-0.18876;-1.64;2.;-0.132132;-1.54;2.;-0.132132;-1.54;1.95;-0.132132; 274 | -1.54;1.95;-0.18876;-1.64;1.95;-0.18876;-1.64;2.;0.18876;-1.64;1.95; 275 | 0.132132;-1.54;1.95;0.132132;-1.54;2.;0.18876;-1.64;1.95;0.132132;-1.54; 276 | 2.;0.18876;-1.64;2.;-0.132132;-1.54;2.;-0.052853;-1.50639;2.;-0.052853; 277 | -1.50639;1.95;-0.052853;-1.50639;1.95;-0.132132;-1.54;1.95;-0.132132; 278 | -1.54;2.;0.132132;-1.54;1.95;0.052853;-1.50639;1.95;0.052853;-1.50639; 279 | 2.;0.132132;-1.54;1.95;0.052853;-1.50639;2.;0.132132;-1.54;2.;0.18876; 280 | -1.64;1.95;0.173397;-1.642679;1.95;0.121808;-1.551577;1.95;0.121808; 281 | -1.551577;1.95;0.132132;-1.54;1.95;0.18876;-1.64;1.95;0.1716;-1.74;1.95; 282 | 0.15795;-1.732697;1.95;0.173397;-1.642679;1.95;0.1716;-1.74;1.95; 283 | 0.173397;-1.642679;1.95;0.18876;-1.64;1.95;0.1716;-1.74;1.95;0.066; 284 | -1.84;1.95;0.060149;-1.825311;1.95;0.1716;-1.74;1.95;0.060149;-1.825311; 285 | 1.95;0.15795;-1.732697;1.95;-0.066;-1.84;1.95;-0.060149;-1.825311;1.95; 286 | 0.066;-1.84;1.95;-0.060149;-1.825311;1.95;0.060149;-1.825311;1.95;0.066; 287 | -1.84;1.95;-0.1716;-1.74;1.95;-0.15795;-1.732697;1.95;-0.060149; 288 | -1.825311;1.95;-0.1716;-1.74;1.95;-0.060149;-1.825311;1.95;-0.066;-1.84; 289 | 1.95;-0.173397;-1.642679;1.95;-0.15795;-1.732697;1.95;-0.1716;-1.74; 290 | 1.95;-0.1716;-1.74;1.95;-0.18876;-1.64;1.95;-0.173397;-1.642679;1.95; 291 | -0.121808;-1.551577;1.95;-0.173397;-1.642679;1.95;-0.18876;-1.64;1.95; 292 | -0.18876;-1.64;1.95;-0.132132;-1.54;1.95;-0.121808;-1.551577;1.95; 293 | -0.052853;-1.50639;1.95;-0.049868;-1.521079;1.95;-0.121808;-1.551577; 294 | 1.95;-0.052853;-1.50639;1.95;-0.121808;-1.551577;1.95;-0.132132;-1.54; 295 | 1.95;0.049868;-1.521079;1.95;-0.049868;-1.521079;1.95;-0.052853; 296 | -1.50639;1.95;-0.052853;-1.50639;1.95;0.052853;-1.50639;1.95;0.049868; 297 | -1.521079;1.95;0.052853;-1.50639;1.95;0.132132;-1.54;1.95;0.121808; 298 | -1.551577;1.95;0.052853;-1.50639;1.95;0.121808;-1.551577;1.95;0.049868; 299 | -1.521079;1.95;-0.18876;-1.64;2.;-0.173397;-1.642679;2.;-0.121808; 300 | -1.551577;2.;-0.121808;-1.551577;2.;-0.132132;-1.54;2.;-0.18876;-1.64; 301 | 2.;-0.1716;-1.74;2.;-0.15795;-1.732697;2.;-0.173397;-1.642679;2.; 302 | -0.173397;-1.642679;2.;-0.18876;-1.64;2.;-0.1716;-1.74;2.;-0.066;-1.84; 303 | 2.;-0.060149;-1.825311;2.;-0.1716;-1.74;2.;-0.060149;-1.825311;2.; 304 | -0.15795;-1.732697;2.;-0.1716;-1.74;2.;0.066;-1.84;2.;0.060149; 305 | -1.825311;2.;-0.066;-1.84;2.;0.060149;-1.825311;2.;-0.060149;-1.825311; 306 | 2.;-0.066;-1.84;2.;0.1716;-1.74;2.;0.15795;-1.732697;2.;0.060149; 307 | -1.825311;2.;0.1716;-1.74;2.;0.060149;-1.825311;2.;0.066;-1.84;2.; 308 | 0.173397;-1.642679;2.;0.15795;-1.732697;2.;0.1716;-1.74;2.;0.1716;-1.74; 309 | 2.;0.18876;-1.64;2.;0.173397;-1.642679;2.;0.121808;-1.551577;2.; 310 | 0.173397;-1.642679;2.;0.18876;-1.64;2.;0.18876;-1.64;2.;0.132132;-1.54; 311 | 2.;0.121808;-1.551577;2.;0.052853;-1.50639;2.;0.049868;-1.521079;2.; 312 | 0.121808;-1.551577;2.;0.052853;-1.50639;2.;0.121808;-1.551577;2.; 313 | 0.132132;-1.54;2.;-0.049868;-1.521079;2.;0.049868;-1.521079;2.;0.052853; 314 | -1.50639;2.;0.052853;-1.50639;2.;-0.052853;-1.50639;2.;-0.049868; 315 | -1.521079;2.;-0.121808;-1.551577;2.;-0.049868;-1.521079;2.;-0.052853; 316 | -1.50639;2.;-0.052853;-1.50639;2.;-0.132132;-1.54;2.;-0.121808; 317 | -1.551577;2.;-0.173397;-1.642679;2.;-0.15795;-1.732697;2.;-0.15795; 318 | -1.732697;1.95;-0.15795;-1.732697;1.95;-0.173397;-1.642679;1.95; 319 | -0.173397;-1.642679;2.;-0.15795;-1.732697;2.;-0.060149;-1.825311;2.; 320 | -0.060149;-1.825311;1.95;-0.060149;-1.825311;1.95;-0.15795;-1.732697; 321 | 1.95;-0.15795;-1.732697;2.;-0.060149;-1.825311;2.;0.060149;-1.825311;2.; 322 | 0.060149;-1.825311;1.95;0.060149;-1.825311;1.95;-0.060149;-1.825311; 323 | 1.95;-0.060149;-1.825311;2.;0.060149;-1.825311;1.95;0.060149;-1.825311; 324 | 2.;0.15795;-1.732697;2.;0.15795;-1.732697;2.;0.15795;-1.732697;1.95; 325 | 0.060149;-1.825311;1.95;0.15795;-1.732697;2.;0.173397;-1.642679;2.; 326 | 0.173397;-1.642679;1.95;0.173397;-1.642679;1.95;0.15795;-1.732697;1.95; 327 | 0.15795;-1.732697;2.;0.173397;-1.642679;2.;0.121808;-1.551577;2.; 328 | 0.121808;-1.551577;1.95;0.121808;-1.551577;1.95;0.173397;-1.642679;1.95; 329 | 0.173397;-1.642679;2.;0.121808;-1.551577;2.;0.049868;-1.521079;2.; 330 | 0.049868;-1.521079;1.95;0.049868;-1.521079;1.95;0.121808;-1.551577;1.95; 331 | 0.121808;-1.551577;2.;0.049868;-1.521079;2.;-0.049868;-1.521079;2.; 332 | -0.049868;-1.521079;1.95;-0.049868;-1.521079;1.95;0.049868;-1.521079; 333 | 1.95;0.049868;-1.521079;2.;-0.049868;-1.521079;2.;-0.121808;-1.551577; 334 | 2.;-0.121808;-1.551577;1.95;-0.121808;-1.551577;1.95;-0.049868;-1.521079;1.95; 335 | -0.049868;-1.521079;2.;-0.121808;-1.551577;2.;-0.173397;-1.642679;2.;-0.173397; 336 | -1.642679;1.95;-0.173397;-1.642679;1.95;-0.121808;-1.551577;1.95;-0.121808; 337 | -1.551577;2.;-0.36;3.6;0.1;0.36;3.6;0.1;4.;4.;0.1;4.;4.;0.1;-4.;4.;0.1;-0.36; 338 | 3.6;0.1;-0.36;0.4;0.1;-0.36;3.6;0.1;-4.;4.;0.1;-4.;4.;0.1;-4.;0.;0.1;-0.36;0.4; 339 | 0.1;4.;0.;0.1;0.36;0.4;0.1;-0.36;0.4;0.1;-0.36;0.4;0.1;-4.;0.;0.1;4.;0.;0.1;4.; 340 | 4.;0.1;0.36;3.6;0.1;4.;0.;0.1;0.36;3.6;0.1;0.36;0.4;0.1;4.;0.;0.1;0.36;2.888889; 341 | 1.023752;0.36;3.066667;1.166974;-0.36;3.066667;1.166974;-0.36;3.066667;1.166974; 342 | -0.36;2.888889;1.023752;0.36;2.888889;1.023752;0.36;2.533333;0.939976;0.36; 343 | 2.711111;0.966974;-0.36;2.711111;0.966974;-0.36;2.711111;0.966974;-0.36;2.533333; 344 | 0.939976;0.36;2.533333;0.939976;-0.36;2.177778;0.939976;0.36;2.177778;0.939976; 345 | 0.36;2.355556;0.939976;0.36;2.355556;0.939976;-0.36;2.355556;0.939976;-0.36; 346 | 2.177778;0.939976;-0.36;1.822222;0.939976;0.36;1.822222;0.939976;0.36;2.;0.939976; 347 | 0.36;2.;0.939976;-0.36;2.;0.939976;-0.36;1.822222;0.939976;-0.36;1.466667;0.939976; 348 | 0.36;1.466667;0.939976;0.36;1.644444;0.939976;0.36;1.644444;0.939976;-0.36; 349 | 1.644444;0.939976;-0.36;1.466667;0.939976;0.36;1.111111;0.957571;0.36;1.288889; 350 | 0.939976;-0.36;1.288889;0.939976;-0.36;1.288889;0.939976;-0.36;1.111111;0.957571; 351 | 0.36;1.111111;0.957571;-0.36;0.755556;1.134246;0.36;0.755556;1.134246;0.36; 352 | 0.933333;1.009739;0.36;0.933333;1.009739;-0.36;0.933333;1.009739;-0.36;0.755556; 353 | 1.134246;0.36;0.755556;1.134246;-0.36;0.755556;1.134246;0.36;0.577778;1.37213; 354 | -0.36;0.755556;1.134246;-0.36;0.577778;1.37213;0.36;0.577778;1.37213;-0.36;3.6; 355 | 3.9;-0.36;3.422222;2.266974;0.36;3.422222;2.266974;-0.36;3.6;3.9;0.36;3.422222; 356 | 2.266974;0.36;3.6;3.9;0.36;3.244444;1.466974;-0.36;3.244444;1.466974;0.36;3.066667; 357 | 1.166974;-0.36;3.244444;1.466974;-0.36;3.066667;1.166974;0.36;3.066667;1.166974; 358 | 0.36;2.888889;1.023752;-0.36;2.888889;1.023752;0.36;2.711111;0.966974;-0.36;2.888889; 359 | 1.023752;-0.36;2.711111;0.966974;0.36;2.711111;0.966974;0.36;2.533333;0.939976; 360 | -0.36;2.533333;0.939976;-0.36;2.355556;0.939976;0.36;2.533333;0.939976;-0.36; 361 | 2.355556;0.939976;0.36;2.355556;0.939976;0.36;2.177778;0.939976;-0.36;2.177778; 362 | 0.939976;-0.36;2.;0.939976;0.36;2.177778;0.939976;-0.36;2.;0.939976;0.36;2.; 363 | 0.939976;0.36;1.822222;0.939976;-0.36;1.822222;0.939976;-0.36;1.644444;0.939976; 364 | 0.36;1.822222;0.939976;-0.36;1.644444;0.939976;0.36;1.644444;0.939976;0.36; 365 | 1.466667;0.939976;-0.36;1.466667;0.939976;-0.36;1.288889;0.939976;0.36;1.466667; 366 | 0.939976;-0.36;1.288889;0.939976;0.36;1.288889;0.939976;0.36;1.111111;0.957571; 367 | -0.36;1.111111;0.957571;0.36;0.933333;1.009739;-0.36;1.111111;0.957571;-0.36; 368 | 0.933333;1.009739;0.36;0.933333;1.009739;0.36;0.4;1.743932;0.36;0.577778;1.37213; 369 | -0.36;0.577778;1.37213;-0.36;0.577778;1.37213;-0.36;0.4;1.743932;0.36;0.4;1.743932; 370 | |] 371 | in 372 | 373 | let world_indices = [| 374 | 0;1;2;3;4;5;6;7;8;9;10;11;12;13;14;15;16;17;18;19;20;21;22;23;24;25;26; 375 | 27;28;29;30;31;32;33;34;35;36;37;38;39;40;41;42;43;44;45;46;47;48;49;50; 376 | 51;52;53;54;55;56;57;58;59;60;61;62;63;64;65;66;67;68;69;70;71;72;73;74; 377 | 75;76;77;78;79;80;81;82;83;84;85;86;87;88;89;90;91;92;93;94;95;96;97;98; 378 | 99;100;101;102;103;104;105;106;107;108;109;110;111;112;113;114;115;116; 379 | 117;118;119;120;121;122;123;124;125;126;127;128;129;130;131;132;133;134; 380 | 135;136;137;138;139;140;141;142;143;144;145;146;147;148;149;150;151;152; 381 | 153;154;155;156;157;158;159;160;161;162;163;164;165;166;167;168;169;170; 382 | 171;172;173;174;175;176;177;178;179;180;181;182;183;184;185;186;187;188; 383 | 189;190;191;192;193;194;195;196;197;198;199;200;201;202;203;204;205;206; 384 | 207;208;209;210;211;212;213;214;215;216;217;218;219;220;221;222;223;224; 385 | 225;226;227;228;229;230;231;232;233;234;235;236;237;238;239;240;241;242; 386 | 243;244;245;246;247;248;249;250;251;252;253;254;255;256;257;258;259;260; 387 | 261;262;263;264;265;266;267;268;269;270;271;272;273;274;275;276;277;278; 388 | 279;280;281;282;283;284;285;286;287;288;289;290;291;292;293;294;295;296; 389 | 297;298;299;300;301;302;303;304;305;306;307;308;309;310;311;312;313;314; 390 | 315;316;317;318;319;320;321;322;323;324;325;326;327;328;329;330;331;332; 391 | 333;334;335;336;337;338;339;340;341;342;343;344;345;346;347;348;349;350; 392 | 351;352;353;354;355;356;357;358;359;360;361;362;363;364;365;366;367;368; 393 | 369;370;371;372;373;374;375;376;377;378;379;380;381;382;383;384;385;386; 394 | 387;388;389;390;391;392;393;394;395;396;397;398;399;400;401;402;403;404; 395 | 405;406;407;408;409;410;411;412;413;414;415;416;417;418;419;420;421;422; 396 | 423;424;425;426;427;428;429;430;431;432;433;434;435;436;437;438;439;440; 397 | 441;442;443;444;445;446;447;448;449;450;451;452;453;454;455;456;457;458; 398 | 459;460;461;462;463;464;465;466;467;468;469;470;471;472;473;474;475;476; 399 | 477;478;479;480;481;482;483;484;485; 400 | |] 401 | in 402 | 403 | (* }}} *) 404 | 405 | (* 406 | Printf.printf "world_normals(%d) world_vertices(%d) world_indices(%d)\n%!" 407 | (Array.length world_normals) 408 | (Array.length world_vertices) 409 | (Array.length world_indices); 410 | *) 411 | 412 | (* create world *) 413 | dInitODE(); 414 | let world = dWorldCreate() 415 | and space = dHashSpaceCreate None 416 | and contactgroup = dJointGroupCreate() in 417 | 418 | dWorldSetGravity world 0. 0. (-9.8); 419 | dWorldSetQuickStepNumIterations world 64; 420 | 421 | (* dynamics and collision objects *) 422 | 423 | (* Create a static world using a triangle mesh that we can collide with. *) 424 | let data = dGeomTriMeshDataCreate() in 425 | dGeomTriMeshDataBuild data world_vertices world_indices; 426 | let world_mesh = dCreateTriMesh (Some space) data () in 427 | 428 | dGeomTriMeshEnableTC world_mesh SphereClass false; 429 | dGeomTriMeshEnableTC world_mesh BoxClass false; 430 | dGeomSetPosition world_mesh 0. 0. 0.5; 431 | 432 | let r = dRGetIdentity() in 433 | dGeomSetRotation world_mesh r; 434 | 435 | let m = dMassCreate() in 436 | 437 | (* create the ball *) 438 | let sphbody = dBodyCreate world in 439 | dMassSetSphere m 1. radius; 440 | dBodySetMass sphbody m; 441 | let sphgeom = dCreateSphere None radius in 442 | dGeomSetBody sphgeom (Some sphbody); 443 | reset_ball sphbody; 444 | dSpaceAdd space sphgeom; 445 | 446 | (* destroy all the objects when leaving the simulation *) 447 | let exit_func() = 448 | dBodyDestroy sphbody; 449 | 450 | dGeomDestroy sphgeom; 451 | dGeomDestroy world_mesh; 452 | 453 | dGeomTriMeshDataDestroy data; 454 | 455 | dJointGroupEmpty contactgroup; 456 | dJointGroupDestroy contactgroup; 457 | 458 | dSpaceDestroy space; 459 | dWorldDestroy world; 460 | 461 | dCloseODE(); 462 | in 463 | 464 | (* run simulation *) 465 | begin 466 | (* set initial viewpoint *) 467 | let pos = (7.6, 5.5, -4.4) 468 | and angles = (98.6, 233.4) in 469 | 470 | (* call sim_step every N milliseconds *) 471 | let timer_msecs = 10 in 472 | 473 | (* simulation params (for the drawstuff lib) *) 474 | let dsd = 475 | ( (pos, angles, timer_msecs, world), 476 | (sim_draw sphbody world_mesh world_indices world_vertices world_normals), 477 | (sim_step world space contactgroup), 478 | (command sphbody), 479 | (exit_func) 480 | ) 481 | in 482 | dsSimulationLoop 480 360 dsd; 483 | end; 484 | ;; 485 | 486 | (* vim: sw=2 sts=2 ts=2 et fdm=marker 487 | *) 488 | -------------------------------------------------------------------------------- /examples/drawstuff.ml: -------------------------------------------------------------------------------- 1 | (* Small Drawing Library 2 | * Copyright (C) 2008 Florent Monnier 3 | * Some part are borrowed from the ODE's Drawstuff lib. 4 | * 5 | * This program is free software: you can redistribute it and/or 6 | * modify it under the terms of the GNU General Public License 7 | * as published by the Free Software Foundation, either version 3 8 | * of the License, or (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see 17 | *) 18 | open GL 19 | open Glu 20 | open Glut 21 | open Ode.LowLevel 22 | 23 | let dsError msg = Printf.eprintf "Error: %s\n%!" msg; exit 1 ;; 24 | 25 | let reshape ~width ~height = 26 | glMatrixMode GL_PROJECTION; 27 | glLoadIdentity(); 28 | gluPerspective 45. (float width /. float height) 0.5 80.; 29 | glViewport 0 0 width height; 30 | glMatrixMode GL_MODELVIEW; 31 | glutPostRedisplay(); 32 | ;; 33 | 34 | type vec3d = { mutable _x:float; mutable _y:float; mutable _z:float } 35 | let pos = { _x=0.; _y=0.; _z=0. } 36 | 37 | let angle_y = ref 0. 38 | let angle_x = ref 0. 39 | let xold = ref 0 40 | let yold = ref 0 41 | 42 | let motion ~x ~y = 43 | begin 44 | angle_y := !angle_y +. 0.2 *. float(!yold - y); 45 | angle_x := !angle_x +. 0.2 *. float(!xold - x); 46 | glutPostRedisplay(); 47 | end; 48 | xold := x; 49 | yold := y; 50 | ;; 51 | 52 | let mouse ~button ~state ~x ~y = 53 | xold := x; 54 | yold := y; 55 | ;; 56 | 57 | let sim_pause = ref false ;; 58 | 59 | let keyboard world command exit_func sim_step = fun ~key ~x ~y -> 60 | begin match key with 61 | | '\027' | 'q' -> 62 | exit_func(); 63 | (* 64 | raise Exit 65 | *) 66 | exit(0) 67 | | 'p' -> sim_pause := not(!sim_pause) 68 | | '.' -> sim_step false; Glut.glutPostRedisplay(); 69 | | 'v' -> 70 | Printf.printf "let pos = (%.1f, %.1f, %.1f)\n" pos._x pos._y pos._z; 71 | Printf.printf "and angles = (%.1f, %.1f) in\n" !angle_y !angle_x; 72 | Printf.printf "%!"; 73 | (* 74 | | 'D' -> 75 | dWorldExportDIF world (Sys.argv.(0) ^ ".dump") ""; 76 | *) 77 | | _ -> () 78 | end; 79 | command key; 80 | glutPostRedisplay(); 81 | ;; 82 | 83 | let deg_to_rad = 3.14159265358979312 /. 180. ;; 84 | let cosd a = cos(a *. deg_to_rad) ;; 85 | let sind a = sin(a *. deg_to_rad) ;; 86 | 87 | let moving = Array.make 6 false ;; 88 | 89 | let special ~key ~x ~y = 90 | match key with 91 | | GLUT_KEY_PAGE_UP -> moving.(0) <- true 92 | | GLUT_KEY_PAGE_DOWN -> moving.(1) <- true 93 | | GLUT_KEY_DOWN -> moving.(2) <- true 94 | | GLUT_KEY_UP -> moving.(3) <- true 95 | | GLUT_KEY_LEFT -> moving.(4) <- true 96 | | GLUT_KEY_RIGHT -> moving.(5) <- true 97 | | _ -> () 98 | ;; 99 | 100 | let special_up ~key ~x ~y = 101 | match key with 102 | | GLUT_KEY_PAGE_UP -> moving.(0) <- false 103 | | GLUT_KEY_PAGE_DOWN -> moving.(1) <- false 104 | | GLUT_KEY_DOWN -> moving.(2) <- false 105 | | GLUT_KEY_UP -> moving.(3) <- false 106 | | GLUT_KEY_LEFT -> moving.(4) <- false 107 | | GLUT_KEY_RIGHT -> moving.(5) <- false 108 | | _ -> () 109 | ;; 110 | 111 | let move_around() = 112 | if moving.(0) then (pos._z <- pos._z -. 0.06); (* up *) 113 | if moving.(1) then (pos._z <- pos._z +. 0.06); (* down *) 114 | if moving.(2) then begin (* backward *) 115 | let x = sind !angle_x 116 | and y = cosd !angle_x in 117 | pos._x <- pos._x -. (0.08 *. x); 118 | pos._y <- pos._y -. (0.08 *. y); 119 | end; 120 | if moving.(3) then begin (* forward *) 121 | let x = sind !angle_x 122 | and y = cosd !angle_x in 123 | pos._x <- pos._x +. (0.08 *. x); 124 | pos._y <- pos._y +. (0.08 *. y); 125 | end; 126 | if moving.(4) then begin (* left *) 127 | let x = (cosd !angle_x) 128 | and y = -. (sind !angle_x) in 129 | pos._x <- pos._x -. (0.04 *. x); 130 | pos._y <- pos._y -. (0.04 *. y); 131 | end; 132 | if moving.(5) then begin (* right *) 133 | let x = (cosd !angle_x) 134 | and y = -. (sind !angle_x) in 135 | pos._x <- pos._x +. (0.04 *. x); 136 | pos._y <- pos._y +. (0.04 *. y); 137 | end; 138 | ;; 139 | 140 | let display draw_scene = fun () -> 141 | glClear ~mask:[GL_COLOR_BUFFER_BIT; GL_DEPTH_BUFFER_BIT]; 142 | glLoadIdentity(); 143 | 144 | glRotate ~x:1.0 ~y:0.0 ~z:0.0 ~angle:( !angle_y); 145 | glRotate ~x:0.0 ~y:0.0 ~z:1.0 ~angle:(-. !angle_x); 146 | glRotate ~x:0.0 ~y:1.0 ~z:0.0 ~angle:(-. 180.); 147 | 148 | glTranslate pos._x pos._y pos._z; 149 | 150 | draw_scene(); 151 | glFlush(); 152 | glutSwapBuffers(); 153 | ;; 154 | 155 | 156 | let gl_init () = 157 | let light_ambient = (0.0, 0.0, 0.0, 1.0) 158 | and light_diffuse = (1.0, 1.0, 1.0, 1.0) 159 | and light_specular = (1.0, 1.0, 1.0, 1.0) 160 | and light_position = (1.0, 1.0, 1.0, 0.0) 161 | 162 | and global_ambient = (0.3, 0.3, 0.8, 1.0) in 163 | 164 | glPointSize ~size:2.0; 165 | glClearColor ~r:0.2 ~g:0.3 ~b:0.5 ~a:0.0; 166 | 167 | glEnable GL_DEPTH_TEST; 168 | 169 | glLight (GL_LIGHT 0) (Light.GL_AMBIENT light_ambient); 170 | glLight (GL_LIGHT 0) (Light.GL_DIFFUSE light_diffuse); 171 | glLight (GL_LIGHT 0) (Light.GL_SPECULAR light_specular); 172 | glLight (GL_LIGHT 0) (Light.GL_POSITION light_position); 173 | glLightModel (GL_LIGHT_MODEL_AMBIENT global_ambient); 174 | ;; 175 | 176 | 177 | let gl_backend width height (params,draw_scene,sim_step,command,exit_func) = 178 | ignore(glutInit Sys.argv); 179 | glutInitDisplayMode[GLUT_RGBA; GLUT_DOUBLE; GLUT_DEPTH]; 180 | glutInitWindowSize ~width ~height; 181 | glutInitWindowPosition 160 140; 182 | ignore(glutCreateWindow ~title:Sys.argv.(0)); 183 | 184 | (* user provided initialisation function *) 185 | let (px,py,pz), (ax, ay), (msecs), (world) = params in 186 | angle_y := ax; 187 | angle_x := ay; 188 | pos._x <- px; 189 | pos._y <- py; 190 | pos._z <- pz; 191 | 192 | (* callback functions *) 193 | glutDisplayFunc ~display:(display draw_scene); 194 | glutReshapeFunc ~reshape; 195 | glutKeyboardFunc ~keyboard:(keyboard world command exit_func sim_step); 196 | glutMouseFunc ~mouse; 197 | glutMotionFunc ~motion; 198 | glutSpecialFunc ~special; 199 | glutSpecialUpFunc ~special_up; 200 | 201 | let rec timer ~value = 202 | sim_step !sim_pause; 203 | if not !sim_pause then 204 | glutPostRedisplay(); 205 | glutTimerFunc ~msecs ~timer ~value:(); 206 | in 207 | glutTimerFunc ~msecs ~timer ~value:(); 208 | 209 | let rec move_timer ~value = 210 | move_around(); 211 | glutTimerFunc ~msecs:10 ~timer:move_timer ~value:(); 212 | in 213 | glutTimerFunc ~msecs:10 ~timer:move_timer ~value:(); 214 | 215 | (* init openGL *) 216 | gl_init(); 217 | 218 | (* enter the main loop *) 219 | glutMainLoop(); 220 | ;; 221 | 222 | 223 | let current_state = ref false ;; 224 | 225 | let dsSimulationLoop width height dsd = 226 | if (!current_state) then 227 | dsError "dsSimulationLoop() called more than once"; 228 | current_state := true; 229 | 230 | (* look for flags that apply to us *) 231 | let argc = Array.length Sys.argv in 232 | for i=1 to pred argc do 233 | if Sys.argv.(i) = "-pause" then sim_pause := true; 234 | done; 235 | 236 | gl_backend width height dsd; 237 | 238 | current_state := false; 239 | ;; 240 | 241 | 242 | (* ======================================================= *) 243 | 244 | let dsSetColor (r,g,b) = glColor3 r g b ;; 245 | 246 | let setTransform pos r = 247 | let matrix = [| 248 | [| r.r11; r.r21; r.r31; 0.; |]; 249 | [| r.r12; r.r22; r.r32; 0.; |]; 250 | [| r.r13; r.r23; r.r33; 0.; |]; 251 | [| pos.x; pos.y; pos.z; 1.; |]; |] 252 | in 253 | glMultMatrix matrix; 254 | (* 255 | (* data exchange is optimum with a flat array *) 256 | let matrix = [| 257 | r.r11; r.r21; r.r31; 0.; 258 | r.r12; r.r22; r.r32; 0.; 259 | r.r13; r.r23; r.r33; 0.; 260 | pos.x; pos.y; pos.z; 1.; |] 261 | in 262 | glMultMatrixFlat matrix; 263 | *) 264 | ;; 265 | 266 | (* {{{ dsDrawBox *) 267 | 268 | let drawBox (sx,sy,sz) (r,g,b) = 269 | let lx = sx *. 0.5 270 | and ly = sy *. 0.5 271 | and lz = sz *. 0.5 in 272 | 273 | glColor3 r g b; 274 | 275 | (* sides *) 276 | glBegin GL_TRIANGLE_STRIP; 277 | glNormal3 (-1.) (0.) (0.); 278 | glVertex3 (-.lx) (-.ly) (-.lz); 279 | glVertex3 (-.lx) (-.ly) (lz); 280 | glVertex3 (-.lx) (ly) (-.lz); 281 | glVertex3 (-.lx) (ly) (lz); 282 | glNormal3 (0.) (1.) (0.); 283 | glVertex3 (lx) (ly) (-.lz); 284 | glVertex3 (lx) (ly) (lz); 285 | glNormal3 (1.) (0.) (0.); 286 | glVertex3 (lx) (-.ly) (-.lz); 287 | glVertex3 (lx) (-.ly) (lz); 288 | glNormal3 (0.) (-1.) (0.); 289 | glVertex3 (-.lx) (-.ly) (-.lz); 290 | glVertex3 (-.lx) (-.ly) (lz); 291 | glEnd(); 292 | 293 | let f = 0.8 in 294 | glColor3 (r *. f) (g *. f) (b *. f); 295 | 296 | (* top face *) 297 | glBegin GL_TRIANGLE_FAN; 298 | glNormal3 (0.) (0.) (1.); 299 | glVertex3 (-.lx) (-.ly) (lz); 300 | glVertex3 (lx) (-.ly) (lz); 301 | glVertex3 (lx) (ly) (lz); 302 | glVertex3 (-.lx) (ly) (lz); 303 | glEnd(); 304 | 305 | (* bottom face *) 306 | glBegin GL_TRIANGLE_FAN; 307 | glNormal3 (0.) (0.) (-1.); 308 | glVertex3 (-.lx) (-.ly) (-.lz); 309 | glVertex3 (-.lx) (ly) (-.lz); 310 | glVertex3 (lx) (ly) (-.lz); 311 | glVertex3 (lx) (-.ly) (-.lz); 312 | glEnd(); 313 | ;; 314 | 315 | let drawWireBox (sx,sy,sz) (r,g,b) = 316 | let lx = sx *. 0.5 317 | and ly = sy *. 0.5 318 | and lz = sz *. 0.5 in 319 | 320 | glColor3 r g b; 321 | 322 | (* top face *) 323 | glBegin GL_LINE_LOOP; 324 | glNormal3 (0.) (0.) (1.); 325 | glVertex3 (-.lx) (-.ly) (lz); 326 | glVertex3 (lx) (-.ly) (lz); 327 | glVertex3 (lx) (ly) (lz); 328 | glVertex3 (-.lx) (ly) (lz); 329 | glEnd(); 330 | 331 | (* bottom face *) 332 | glBegin GL_LINE_LOOP; 333 | glNormal3 (0.) (0.) (-1.); 334 | glVertex3 (-.lx) (-.ly) (-.lz); 335 | glVertex3 (-.lx) (ly) (-.lz); 336 | glVertex3 (lx) (ly) (-.lz); 337 | glVertex3 (lx) (-.ly) (-.lz); 338 | glEnd(); 339 | 340 | (* bottom face *) 341 | glBegin GL_LINES; 342 | glNormal3 (0.) (0.) (-1.); 343 | glVertex3 (-.lx) (-.ly) ( lz); 344 | glVertex3 (-.lx) (-.ly) (-.lz); 345 | 346 | glVertex3 (lx) (ly) ( lz); 347 | glVertex3 (lx) (ly) (-.lz); 348 | 349 | glVertex3 (-.lx) (ly) (lz); 350 | glVertex3 (-.lx) (ly) (-.lz); 351 | 352 | glVertex3 (lx) (-.ly) (lz); 353 | glVertex3 (lx) (-.ly) (-.lz); 354 | glEnd(); 355 | ;; 356 | 357 | 358 | let dsDrawBox pos r sides color = 359 | glShadeModel GL_FLAT; 360 | glPushMatrix(); 361 | setTransform pos r; 362 | drawBox sides color; 363 | glPopMatrix(); 364 | ;; 365 | 366 | let dsDrawWireBox pos r sides color = 367 | glShadeModel GL_FLAT; 368 | glPushMatrix(); 369 | setTransform pos r; 370 | drawWireBox sides color; 371 | glPopMatrix(); 372 | ;; 373 | 374 | (* }}} *) 375 | (* {{{ dsDraw Plane, Line, Edge, Point *) 376 | 377 | let dsDrawPlane (x,y,z) ?(scale=1.0) (r,g,b) = 378 | glColor3 r g b; 379 | glPushMatrix(); 380 | glTranslate x y z; 381 | glScale scale scale scale; 382 | glBegin GL_LINES; 383 | for i = -2 to 2 do 384 | glVertex3 ( 2.0) (float i) 0.0; 385 | glVertex3 (-2.0) (float i) 0.0; 386 | done; 387 | for j = -2 to 2 do 388 | glVertex3 (float j) ( 2.0) 0.0; 389 | glVertex3 (float j) (-2.0) 0.0; 390 | done; 391 | glEnd(); 392 | glPopMatrix(); 393 | ;; 394 | 395 | 396 | let dsDrawLine p1 p2 (r,g,b) = 397 | glColor3 r g b; 398 | glBegin GL_LINES; 399 | glVertex3 p1.x p1.y p1.z; 400 | glVertex3 p2.x p2.y p2.z; 401 | glEnd(); 402 | ;; 403 | 404 | let dsDrawEdge pos rot p1 p2 (r,g,b) = 405 | glColor3 r g b; 406 | glPushMatrix(); 407 | setTransform pos rot; 408 | glBegin GL_LINES; 409 | glVertex3 p1.x p1.y p1.z; 410 | glVertex3 p2.x p2.y p2.z; 411 | glEnd(); 412 | glPopMatrix(); 413 | ;; 414 | 415 | let dsDrawPoint p (r,g,b) = 416 | glColor3 r g b; 417 | glBegin GL_POINTS; 418 | glVertex3 p.x p.y p.z; 419 | glEnd(); 420 | ;; 421 | 422 | let dsDrawAbovePoint p (r,g,b) = 423 | glColor3 r g b; 424 | glDisable GL_DEPTH_TEST; 425 | glBegin GL_POINTS; 426 | glVertex3 p.x p.y p.z; 427 | glEnd(); 428 | glEnable GL_DEPTH_TEST; 429 | ;; 430 | 431 | (* }}} *) 432 | (* {{{ dsDrawCylinder *) 433 | 434 | let pi = 3.1415926535_8979323846 435 | 436 | (* draw a cylinder of length l and radius r, aligned along the z axis *) 437 | let drawCylinder l rad zoffset n (r,g,b) = 438 | 439 | let l = l *. 0.5 440 | and a = (pi *. 2.0) /. (float n) in 441 | let sa = sin a 442 | and ca = cos a in 443 | 444 | glColor3 r g b; 445 | 446 | (* draw cylinder body *) 447 | let ny= ref 1. and nz= ref 0. in (* normal vector = (0,ny,nz) *) 448 | glBegin GL_TRIANGLE_STRIP; 449 | for i=0 to n do 450 | glNormal3 !ny !nz 0.; 451 | glVertex3 (!ny *. rad) (!nz *. rad) (zoffset +. l); 452 | glVertex3 (!ny *. rad) (!nz *. rad) (zoffset -. l); 453 | 454 | (* rotate ny,nz *) 455 | let tmp = (ca *. !ny) -. (sa *. !nz) in 456 | nz := (sa *. !ny) +. (ca *. !nz); 457 | ny := tmp; 458 | done; 459 | glEnd(); 460 | 461 | (* draw top cap *) 462 | glShadeModel GL_FLAT; 463 | let ny= ref 1. and nz= ref 0. in (* normal vector = (0,ny,nz) *) 464 | glBegin GL_TRIANGLE_FAN; 465 | glNormal3 0. 0. 1.; 466 | glVertex3 0. 0. (l +. zoffset); 467 | for i=0 to n do 468 | if (i=1 || i=n/2+1) then 469 | glColor3 r g b; 470 | glNormal3 0. 0. 1.; 471 | glVertex3 (!ny *. rad) (!nz *. rad) (l +. zoffset); 472 | if (i=1 || i=n/2+1) then 473 | glColor3 (r *. 0.75) (g *. 0.75) (b *. 0.75); 474 | 475 | (* rotate ny,nz *) 476 | let tmp = (ca *. !ny) -. (sa *. !nz) in 477 | nz := (sa *. !ny) +. (ca *. !nz); 478 | ny := tmp; 479 | done; 480 | glEnd(); 481 | 482 | (* draw bottom cap *) 483 | let ny= ref 1. and nz= ref 0. in 484 | (* normal vector = (0,ny,nz) *) 485 | glBegin GL_TRIANGLE_FAN; 486 | glNormal3 0. 0. (-1.); 487 | glVertex3 0. 0. (-. l +. zoffset); 488 | for i=0 to n do 489 | if (i=1 || i=n/2+1) then 490 | glColor3 (r *. 0.75) (g *. 0.75) (b *. 0.75); 491 | glNormal3 0. 0. (-1.); 492 | glVertex3 (!ny *. rad) (!nz *. rad) (-. l +. zoffset); 493 | if (i=1 || i=n/2+1) then 494 | glColor3 r g b; 495 | 496 | (* rotate ny,nz *) 497 | let tmp = (ca *. !ny) +. (sa *. !nz) in 498 | nz := (-. sa *. !ny) +. (ca *. !nz); 499 | ny := tmp; 500 | done; 501 | glEnd(); 502 | ;; 503 | 504 | 505 | let dsDrawCylinder pos r length radius color = 506 | if not(!current_state) then 507 | dsError "drawing function called outside simulation loop"; 508 | 509 | glShadeModel GL_SMOOTH; 510 | glPushMatrix(); 511 | setTransform pos r; 512 | let n = 16 in (* number of sides to the cylinder (divisible by 4) *) 513 | drawCylinder length radius 0.0 n color; 514 | glPopMatrix(); 515 | ;; 516 | 517 | let dsDrawWireCylinder pos r length radius color = 518 | if not(!current_state) then 519 | dsError "drawing function called outside simulation loop"; 520 | 521 | glPolygonMode GL_FRONT_AND_BACK GL_LINE; 522 | glPushMatrix(); 523 | setTransform pos r; 524 | let n = 12 in (* number of sides to the cylinder (divisible by 4) *) 525 | drawCylinder length radius 0.0 n color; 526 | glPopMatrix(); 527 | glPolygonMode GL_FRONT_AND_BACK GL_FILL; 528 | ;; 529 | 530 | (* }}} *) 531 | (* {{{ dsDrawSphere *) 532 | 533 | let dsDrawSphere pos rot radius (r,g,b) = 534 | glShadeModel GL_FLAT; 535 | glPushMatrix(); 536 | setTransform pos rot; 537 | glColor3 r g b; 538 | glutSolidSphere ~radius ~slices:8 ~stacks:8; 539 | glPopMatrix(); 540 | ;; 541 | 542 | let dsDrawWireSphere pos rot radius (r,g,b) = 543 | glShadeModel GL_FLAT; 544 | glPushMatrix(); 545 | setTransform pos rot; 546 | glColor3 r g b; 547 | glutWireSphere ~radius ~slices:8 ~stacks:8; 548 | glPopMatrix(); 549 | ;; 550 | 551 | (* }}} *) 552 | (* {{{ dsDrawCapsule *) 553 | 554 | let drawCapsule l r (_r,g,b) = 555 | (* number of sides to the cylinder (divisible by 4): *) 556 | let n = 3 * 4 in 557 | 558 | let l = l *. 0.5 in 559 | let a = pi *. 2.0 /. (float n) in 560 | let sa = sin a 561 | and ca = cos a in 562 | 563 | glColor3 _r g b; 564 | 565 | (* draw cylinder body *) 566 | let ny = ref 1. and nz = ref 0. in (* normal vector = (0,ny,nz) *) 567 | glBegin GL_TRIANGLE_STRIP; 568 | for i=0 to n do 569 | glNormal3 !ny !nz 0.; 570 | glVertex3 (!ny *. r) (!nz *. r) (l); 571 | glNormal3 !ny !nz 0.; 572 | glVertex3 (!ny *. r) (!nz *. r) (-. l); 573 | (* rotate ny,nz *) 574 | let tmp = ca *. !ny -. sa *. !nz in 575 | nz := sa *. !ny +. ca *. !nz; 576 | ny := tmp; 577 | done; 578 | glEnd(); 579 | 580 | glColor3 (_r *. 0.75) (g *. 0.75) (b *. 0.75); 581 | 582 | (* draw first cylinder cap *) 583 | let start_nx = ref 0. 584 | and start_ny = ref 1. in 585 | for j=0 to pred (n/4) do 586 | (* get start_n2 = rotated start_n *) 587 | let start_nx2 = ( ca) *. !start_nx +. sa *. !start_ny 588 | and start_ny2 = (-. sa) *. !start_nx +. ca *. !start_ny in 589 | (* get n=start_n and n2=start_n2 *) 590 | let nx = !start_nx and ny = ref !start_ny and nz = ref 0. in 591 | let nx2 = start_nx2 and ny2 = ref start_ny2 and nz2 = ref 0. in 592 | glBegin GL_TRIANGLE_STRIP; 593 | for i=0 to n do 594 | glNormal3 !ny2 !nz2 nx2; 595 | glVertex3 (!ny2 *. r) (!nz2 *. r) (l +. nx2 *. r); 596 | glNormal3 !ny !nz nx; 597 | glVertex3 (!ny *. r) (!nz *. r) (l +. nx *. r); 598 | (* rotate n,n2 *) 599 | let tmp = ca *. !ny -. sa *. !nz in 600 | nz := sa *. !ny +. ca *. !nz; 601 | ny := tmp; 602 | let tmp = ca *. !ny2 -. sa *. !nz2 in 603 | nz2 := sa *. !ny2 +. ca *. !nz2; 604 | ny2 := tmp; 605 | done; 606 | glEnd(); 607 | start_nx := start_nx2; 608 | start_ny := start_ny2; 609 | done; 610 | 611 | (* draw second cylinder cap *) 612 | let start_nx = ref 0. 613 | and start_ny = ref 1. in 614 | for j=0 to pred (n/4) do 615 | (* get start_n2 = rotated start_n *) 616 | let start_nx2 = ca *. !start_nx -. sa *. !start_ny 617 | and start_ny2 = sa *. !start_nx +. ca *. !start_ny in 618 | (* get n=start_n and n2=start_n2 *) 619 | let nx = !start_nx and ny = ref !start_ny and nz = ref 0. in 620 | let nx2 = start_nx2 and ny2 = ref start_ny2 and nz2 = ref 0. in 621 | glBegin GL_TRIANGLE_STRIP; 622 | for i=0 to n do 623 | glNormal3 !ny !nz nx; 624 | glVertex3 (!ny *. r) (!nz *. r) (-. l +. nx *. r); 625 | glNormal3 !ny2 !nz2 nx2; 626 | glVertex3 (!ny2 *. r) (!nz2 *. r) (-. l +. nx2 *. r); 627 | (* rotate n,n2 *) 628 | let tmp = ca *. !ny -. sa *. !nz in 629 | nz := sa *. !ny +. ca *. !nz; 630 | ny := tmp; 631 | let tmp = ca *. !ny2 -. sa *. !nz2 in 632 | nz2 := sa *. !ny2 +. ca *. !nz2; 633 | ny2 := tmp; 634 | done; 635 | glEnd(); 636 | start_nx := start_nx2; 637 | start_ny := start_ny2; 638 | done; 639 | ;; 640 | 641 | let dsDrawCapsule pos rot len rad color = 642 | glShadeModel GL_FLAT; 643 | glPushMatrix(); 644 | setTransform pos rot; 645 | drawCapsule len rad color; 646 | glPopMatrix(); 647 | ;; 648 | 649 | (* }}} *) 650 | (* {{{ dsDrawTriangles *) 651 | 652 | let dsDrawTriangles pos rot world_vertices world_indices world_normals (r,g,b) = 653 | glPolygonMode ~face:GL_FRONT ~mode:GL_FILL; 654 | glPolygonMode ~face:GL_BACK ~mode:GL_LINE; 655 | glShadeModel GL_FLAT; 656 | glShadeModel GL_SMOOTH; 657 | glEnable GL_LIGHTING; 658 | glEnable GL_LIGHT0; 659 | glColor3 r g b; 660 | glPushMatrix(); 661 | setTransform pos rot; 662 | glBegin GL_TRIANGLES; 663 | let numi = Array.length world_indices in 664 | let last = pred(numi/3) in 665 | for i=0 to last do 666 | (* coordinates indices *) 667 | let i0 = world_indices.(i*3+0) 668 | and i1 = world_indices.(i*3+1) 669 | and i2 = world_indices.(i*3+2) in 670 | 671 | (* vertices coordinates *) 672 | let x0 = world_vertices.(i0*3) 673 | and x1 = world_vertices.(i1*3) 674 | and x2 = world_vertices.(i2*3) 675 | 676 | and y0 = world_vertices.(i0*3+1) 677 | and y1 = world_vertices.(i1*3+1) 678 | and y2 = world_vertices.(i2*3+1) 679 | 680 | and z0 = world_vertices.(i0*3+2) 681 | and z1 = world_vertices.(i1*3+2) 682 | and z2 = world_vertices.(i2*3+2) 683 | in 684 | 685 | match world_normals with 686 | | Some world_normals -> 687 | (* normals coordinates *) 688 | let nx0 = world_normals.(i0*3+0) 689 | and ny0 = world_normals.(i0*3+1) 690 | and nz0 = world_normals.(i0*3+2) 691 | 692 | and nx1 = world_normals.(i0*3+0) 693 | and ny1 = world_normals.(i0*3+1) 694 | and nz1 = world_normals.(i0*3+2) 695 | 696 | and nx2 = world_normals.(i0*3+0) 697 | and ny2 = world_normals.(i0*3+1) 698 | and nz2 = world_normals.(i0*3+2) 699 | in 700 | 701 | glNormal3 nx0 ny0 nz0; glVertex3 x0 y0 z0; 702 | glNormal3 nx1 ny1 nz1; glVertex3 x1 y1 z1; 703 | glNormal3 nx2 ny2 nz2; glVertex3 x2 y2 z2; 704 | 705 | | None -> 706 | glVertex3 x0 y0 z0; 707 | glVertex3 x1 y1 z1; 708 | glVertex3 x2 y2 z2; 709 | 710 | done; 711 | glEnd(); 712 | glPopMatrix(); 713 | glDisable GL_LIGHTING; 714 | glDisable GL_LIGHT0; 715 | ;; 716 | 717 | 718 | let dsDrawWireTriangles pos rot world_vertices world_indices world_normals color = 719 | glPolygonMode GL_FRONT_AND_BACK GL_LINE; 720 | dsDrawTriangles pos rot world_vertices world_indices world_normals color; 721 | glPolygonMode GL_FRONT_AND_BACK GL_FILL; 722 | ;; 723 | 724 | (* }}} *) 725 | (* {{{ dsDrawConvex *) 726 | 727 | let drawConvex _planes _points _polygons = 728 | let polyindex = ref 0 in 729 | let _planecount = (Array.length _planes) / 4 in 730 | for i=0 to pred _planecount do 731 | let pointcount = _polygons.(!polyindex) in 732 | incr polyindex; 733 | glBegin GL_POLYGON; 734 | glNormal3 (_planes.((i*4)+0)) 735 | (_planes.((i*4)+1)) 736 | (_planes.((i*4)+2)); 737 | 738 | for j=0 to pred pointcount do 739 | glVertex3 (_points.((_polygons.(!polyindex)*3)+0)) 740 | (_points.((_polygons.(!polyindex)*3)+1)) 741 | (_points.((_polygons.(!polyindex)*3)+2)); 742 | incr polyindex; 743 | done; 744 | glEnd(); 745 | done; 746 | ;; 747 | 748 | let polyfactor = 1.0 749 | let polyunits = 1.0 750 | let fill = true 751 | 752 | let _drawConvex planes points polygons = 753 | if (fill) then begin 754 | glEnable GL_LIGHTING; 755 | glEnable GL_LIGHT0; 756 | glEnable GL_POLYGON_OFFSET_FILL; 757 | glPolygonOffset polyfactor polyunits; 758 | drawConvex planes points polygons; 759 | glDisable GL_POLYGON_OFFSET_FILL; 760 | glDisable GL_LIGHTING; 761 | glDisable GL_LIGHT0; 762 | end; 763 | 764 | glColor3 0.0 0.2 1.0; 765 | glPolygonMode GL_FRONT_AND_BACK GL_LINE; 766 | glPolygonOffset (-. polyfactor) (-. polyunits); 767 | if not(fill) then glEnable GL_POLYGON_OFFSET_LINE; 768 | drawConvex planes points polygons; 769 | glDisable GL_POLYGON_OFFSET_LINE; 770 | glPolygonMode GL_FRONT_AND_BACK GL_FILL; 771 | 772 | if not(fill) then begin 773 | glEnable GL_LIGHTING; 774 | glEnable GL_LIGHT0; 775 | drawConvex planes points polygons; 776 | glDisable GL_LIGHTING; 777 | glDisable GL_LIGHT0; 778 | end; 779 | ;; 780 | 781 | let dsDrawConvex pos rot planes points polygons (r,g,b) = 782 | if not(!current_state) then 783 | dsError "drawing function called outside simulation loop"; 784 | 785 | glColor3 r g b; 786 | glPushMatrix(); 787 | setTransform pos rot; 788 | _drawConvex planes points polygons; 789 | glPopMatrix(); 790 | ;; 791 | 792 | (* }}} *) 793 | (* {{{ dsDrawWireCapsule *) 794 | 795 | let drawWireCapsule l r (_r,g,b) = 796 | (* number of sides to the cylinder (divisible by 4): *) 797 | let n = 3 * 4 in 798 | 799 | let l = l *. 0.5 in 800 | let a = pi *. 2.0 /. (float n) in 801 | let sa = sin a 802 | and ca = cos a in 803 | 804 | glColor3 _r g b; 805 | 806 | (* draw cylinder body *) 807 | let ny = ref 1. and nz = ref 0. in (* normal vector = (0,ny,nz) *) 808 | let li = ref [] in 809 | for i=0 to n do 810 | let a = (!ny, !nz, 0., (!ny *. r), (!nz *. r), (l)) 811 | and b = (!ny, !nz, 0., (!ny *. r), (!nz *. r), (-. l)) 812 | in 813 | li := (a,b) :: !li; 814 | (* rotate ny,nz *) 815 | let tmp = ca *. !ny -. sa *. !nz in 816 | nz := sa *. !ny +. ca *. !nz; 817 | ny := tmp; 818 | done; 819 | let along = function 820 | (nx1,ny1,nz1, x1,y1,z1), 821 | (nx2,ny2,nz2, x2,y2,z2) -> 822 | glNormal3 nx1 ny1 nz1; glVertex3 x1 y1 z1; 823 | glNormal3 nx2 ny2 nz2; glVertex3 x2 y2 z2; 824 | in 825 | glBegin GL_LINES; 826 | List.iter along !li; 827 | glEnd(); 828 | 829 | let along = function 830 | (nx,ny,nz, x,y,z), _ -> 831 | glNormal3 nx ny nz; glVertex3 x y z; 832 | in 833 | glBegin GL_LINE_LOOP; 834 | List.iter along !li; 835 | glEnd(); 836 | 837 | let along = function 838 | _, (nx,ny,nz, x,y,z) -> 839 | glNormal3 nx ny nz; glVertex3 x y z; 840 | in 841 | glBegin GL_LINE_LOOP; 842 | List.iter along !li; 843 | glEnd(); 844 | 845 | (* draw first cylinder cap *) 846 | let start_nx = ref 0. 847 | and start_ny = ref 1. in 848 | for j=0 to pred (n/4) do 849 | (* get start_n2 = rotated start_n *) 850 | let start_nx2 = ( ca) *. !start_nx +. sa *. !start_ny 851 | and start_ny2 = (-. sa) *. !start_nx +. ca *. !start_ny in 852 | (* get n=start_n and n2=start_n2 *) 853 | let nx = !start_nx and ny = ref !start_ny and nz = ref 0. in 854 | let nx2 = start_nx2 and ny2 = ref start_ny2 and nz2 = ref 0. in 855 | glBegin GL_LINES; 856 | for i=0 to n do 857 | glNormal3 !ny2 !nz2 nx2; 858 | glVertex3 (!ny2 *. r) (!nz2 *. r) (l +. nx2 *. r); 859 | glNormal3 !ny !nz nx; 860 | glVertex3 (!ny *. r) (!nz *. r) (l +. nx *. r); 861 | (* rotate n,n2 *) 862 | let tmp = ca *. !ny -. sa *. !nz in 863 | nz := sa *. !ny +. ca *. !nz; 864 | ny := tmp; 865 | let tmp = ca *. !ny2 -. sa *. !nz2 in 866 | nz2 := sa *. !ny2 +. ca *. !nz2; 867 | ny2 := tmp; 868 | done; 869 | glEnd(); 870 | start_nx := start_nx2; 871 | start_ny := start_ny2; 872 | done; 873 | 874 | (* draw second cylinder cap *) 875 | let start_nx = ref 0. 876 | and start_ny = ref 1. in 877 | for j=0 to pred (n/4) do 878 | (* get start_n2 = rotated start_n *) 879 | let start_nx2 = ca *. !start_nx -. sa *. !start_ny 880 | and start_ny2 = sa *. !start_nx +. ca *. !start_ny in 881 | (* get n=start_n and n2=start_n2 *) 882 | let nx = !start_nx and ny = ref !start_ny and nz = ref 0. in 883 | let nx2 = start_nx2 and ny2 = ref start_ny2 and nz2 = ref 0. in 884 | glBegin GL_LINES; 885 | for i=0 to n do 886 | glNormal3 !ny !nz nx; 887 | glVertex3 (!ny *. r) (!nz *. r) (-. l +. nx *. r); 888 | glNormal3 !ny2 !nz2 nx2; 889 | glVertex3 (!ny2 *. r) (!nz2 *. r) (-. l +. nx2 *. r); 890 | (* rotate n,n2 *) 891 | let tmp = ca *. !ny -. sa *. !nz in 892 | nz := sa *. !ny +. ca *. !nz; 893 | ny := tmp; 894 | let tmp = ca *. !ny2 -. sa *. !nz2 in 895 | nz2 := sa *. !ny2 +. ca *. !nz2; 896 | ny2 := tmp; 897 | done; 898 | glEnd(); 899 | start_nx := start_nx2; 900 | start_ny := start_ny2; 901 | done; 902 | ;; 903 | 904 | let dsDrawWireCapsule pos rot len rad color = 905 | glShadeModel GL_FLAT; 906 | glPushMatrix(); 907 | setTransform pos rot; 908 | drawWireCapsule len rad color; 909 | glPopMatrix(); 910 | ;; 911 | 912 | (* }}} *) 913 | 914 | (* ======================================================= *) 915 | 916 | let dsElapsedTime = 917 | let prev_time = ref 0.0 in 918 | function () -> 919 | let curr = float (glutGet GLUT_ELAPSED_TIME) /. 1000.0 in 920 | 921 | if !sim_pause then (prev_time := curr; 0.01666) else 922 | begin 923 | if (!prev_time = 0.0) then prev_time := curr; 924 | 925 | let retval = curr -. !prev_time in 926 | prev_time := curr; 927 | 928 | let retval = if (retval>1.0) then 1.0 else retval in 929 | let retval = if (retval raise (Arg.Bad arg)) usage 38 | 39 | let width = !width 40 | let height = !height 41 | let seed = !seed 42 | 43 | let bg_r, bg_g, bg_b = 44 | Scanf.sscanf !bgcolor "rgb(%d,%d,%d)" 45 | (fun r g b -> 46 | let r = (float r) /. 255. 47 | and g = (float g) /. 255. 48 | and b = (float b) /. 255. in 49 | (r,g,b)) 50 | 51 | let pi = 3.14159265358979323846 52 | 53 | let stepsize = 0.01 (* Stepsize for physics (in secs). *) 54 | 55 | let initial = 0., 0., 1. (* Initial position for katamari. *) 56 | let initial_radius = 0.2 (* Initial radius. *) 57 | let initial_density = 5. (* Density of katamari. *) 58 | 59 | let camera = ref (3.*.pi/.2.) (* Current camera angle, radians. *) 60 | let camera_dist = 2. (* Camera distance from katamari. *) 61 | 62 | let pick_up_factor = 20. (* Can pick up boxes up to 1/factor 63 | * mass of total katamari. *) 64 | 65 | let ignore_factor = 5. (* Length of time before ignoring a 66 | * box which has been picked up. *) 67 | 68 | (* Current key state. *) 69 | let key_forward = ref false 70 | let key_backward = ref false 71 | let key_left = ref false 72 | let key_right = ref false 73 | 74 | let grown_factor = ref 100. 75 | 76 | (* "Physics time", counts in seconds starting at 0. *) 77 | let physics_time = ref 0. 78 | 79 | let vecneg { x = x; y = y; z = z; w = w } = 80 | { x = -.x; y = -.y; z = -.z; w = -.w } 81 | 82 | let vecnormalize { x = x; y = y; z = z } = 83 | let w = sqrt (x *. x +. y *. y +. z *. z) in 84 | { x = x/.w; y = y/.w; z = z/.w; w = 0. } 85 | 86 | let vecsub { x = x; y = y; z = z } { x = x'; y = y'; z = z' } = 87 | { x = x-.x'; y = y-.y'; z = z-.z'; w = 0. } 88 | 89 | let vecscalarmul m { x = x; y = y; z = z } = 90 | { x = m *. x; y = m *. y; z = m *. z; w = 0. } 91 | 92 | let veccross { x = b0; y = b1; z = b2 } { x = c0; y = c1; z = c2 } = 93 | { x = b1 *. c2 -. b2 *. c1; 94 | y = b2 *. c0 -. b0 *. c2; 95 | z = b0 *. c1 -. b1 *. c0; 96 | w = 0. } 97 | 98 | (* Timing helper functions. *) 99 | let timer_start, timer_stop = 100 | let timer = ref None in 101 | let timer_start label = timer := Some (Unix.gettimeofday (), label) in 102 | let timer_stop () = 103 | match !timer with 104 | | None -> failwith "timer_stop called without timer_start" 105 | | Some (start, label) -> 106 | let now = Unix.gettimeofday () in 107 | printf "%s: %g seconds\n" label (now -. start); 108 | timer := None 109 | in 110 | (timer_start, timer_stop) 111 | ;; 112 | 113 | let create_katamari ode space = 114 | let body = dBodyCreate ode in 115 | dBodySetAutoDisableFlag body false; 116 | let mass = dMassCreate () in 117 | dMassSetZero mass; 118 | dMassSetSphere mass ~density:initial_density ~radius:initial_radius; 119 | dBodySetMass body mass; 120 | let kata = dCreateSphere (Some space) ~radius:initial_radius in 121 | dGeomSetBody kata (Some body); 122 | let x, y, z = initial in 123 | dGeomSetPosition kata ~x ~y ~z; 124 | (kata, body) 125 | ;; 126 | 127 | type enabled_box = { 128 | b_box : box; 129 | b_body : dBodyID; (* Body. *) 130 | } 131 | and box = { 132 | b_size : float * float * float; (* Length of each side. *) 133 | b_mass : float; (* Mass (when embodied). *) 134 | b_geom : box_geom dGeomID; (* Geom. *) 135 | (* For picked boxes, this points to the transform geom. In other 136 | * boxes, it is the same as b_geom. 137 | *) 138 | b_tgeom : geom_type; 139 | (* For picked boxes, this is the time until we ignore the box, which 140 | * is calculated depending on the relative mass of the box. In other 141 | * boxes, it is undefined. 142 | *) 143 | b_time : float; 144 | } 145 | 146 | module BodyBoxMap = 147 | Map.Make 148 | (struct 149 | type t = dBodyID 150 | let compare = compare 151 | end) 152 | module GeomBoxMap = 153 | Map.Make 154 | (struct 155 | type t = geom_type 156 | let compare = compare 157 | end) 158 | 159 | let rec create_boxes ode space = 160 | let boxes = ref GeomBoxMap.empty in 161 | 162 | let create_box scale base_y base_x = 163 | let lx, ly, lz = 164 | Random.float scale +. scale, Random.float scale +. scale, 165 | Random.float scale +. scale in 166 | let geom = dCreateBox (Some space) ~lx ~ly ~lz in 167 | let x, y = 168 | Random.float (scale *. 10.) +. base_x, 169 | Random.float (scale *. 10.) +. base_y in 170 | let z = lz /. 2. in 171 | dGeomSetPosition geom ~x ~y ~z; 172 | let density = Random.float 1.9 +. 0.1 in 173 | let mass = lx *. ly *. lz *. density in 174 | 175 | let box = { b_size = (lx, ly, lz); b_mass = mass; 176 | b_geom = geom; b_tgeom = Box_geom geom; b_time = 0. } in 177 | boxes := GeomBoxMap.add (Box_geom geom) box !boxes 178 | in 179 | List.iter ( 180 | fun scale -> 181 | for i = -10 to 10; do 182 | let y = scale *. 10. in 183 | let x = float i *. y in 184 | create_box scale y x 185 | done 186 | ) [ 0.05; 0.1; 0.5; 1.0; 2.0; 5.0 ]; 187 | 188 | !boxes 189 | 190 | (* Enable boxes smaller than a certain mass. *) 191 | and enable_boxes ode large_boxes enabled_boxes max_mass = 192 | (* Get a list of boxes which are smaller than the max_mass, and thus 193 | * candidates to be enabled. 194 | *) 195 | let boxes_to_enable = 196 | GeomBoxMap.fold (fun _ box boxes -> 197 | if box.b_mass <= max_mass then box :: boxes else boxes) 198 | large_boxes [] in 199 | 200 | (* enable_box function below will move boxes from the large_boxes map 201 | * to the enabled_boxes map. 202 | *) 203 | let large_boxes = ref large_boxes in 204 | let enabled_boxes = ref enabled_boxes in 205 | 206 | (* Function to do the enabling of a single box. *) 207 | let enable_box box = 208 | let body = dBodyCreate ode in 209 | let mass = dMassCreate () in 210 | dMassSetZero mass; 211 | let lx, ly, lz = box.b_size in 212 | dMassSetBoxTotal mass ~total_mass:box.b_mass ~lx ~ly ~lz; 213 | let b_mass = dMass_mass mass in 214 | ignore(b_mass); 215 | dBodySetMass body mass; 216 | 217 | let { x = x; y = y; z = z } = dGeomGetPosition box.b_geom in 218 | dGeomSetBody box.b_geom (Some body); 219 | dGeomSetPosition box.b_geom ~x ~y ~z; 220 | 221 | let enabled_box = { b_box = box; b_body = body } in 222 | large_boxes := GeomBoxMap.remove (Box_geom box.b_geom) !large_boxes; 223 | enabled_boxes := BodyBoxMap.add body enabled_box !enabled_boxes 224 | in 225 | List.iter enable_box boxes_to_enable; 226 | 227 | !large_boxes, !enabled_boxes 228 | 229 | let init_gl () = 230 | GlDraw.viewport ~x:0 ~y:0 ~w:width ~h:height; 231 | GlClear.color (bg_r, bg_g, bg_b); 232 | GlClear.depth 1.0; 233 | Gl.enable `depth_test; 234 | GlFunc.depth_func `less; 235 | GlDraw.shade_model `smooth; 236 | 237 | (* 238 | (* Lighting. *) 239 | Gl.enable `lighting; 240 | Gl.enable `light0; 241 | GlLight.light ~num:0 (`position (0., 0., 10., 0.)); 242 | *) 243 | 244 | (* 245 | (* Fog *) 246 | Gl.enable `fog; 247 | GlMisc.hint `fog `nicest; 248 | GlLight.fog (`mode `exp); 249 | GlLight.fog (`color (bg_r, bg_g, bg_b, 1.)); 250 | GlLight.fog (`density 0.5); 251 | *) 252 | 253 | (* Projection matrix. *) 254 | GlMat.mode `projection; 255 | GlMat.load_identity (); 256 | GluMat.perspective ~fovy:60. ~aspect:(float width /. float height) 257 | ~z:(0.1, 100.); 258 | GlMat.mode `modelview 259 | 260 | (* The game state, used both for drawing and for physics. *) 261 | type state = { 262 | ode : dWorldID; (* The rigid body world. *) 263 | space : dSpaceID; (* Collision-detection world. *) 264 | plane : plane_geom dGeomID; (* Ground plane. *) 265 | 266 | kata_geom : sphere_geom dGeomID; (* Katamari geom. *) 267 | kata_body : dBodyID; (* Katamari body. *) 268 | 269 | (* Boxes (ie. the stuff you're supposed to pick up) exist in four 270 | * different states, which they transition between. large -> enabled 271 | * -> picked -> ignored. They start off in the 'large' state, where 272 | * they are just geoms (no bodies, so immovable). When the katamari 273 | * reaches a certain mass, large boxes which could potentially be 274 | * picked up are moved to the 'enabled' state. In this state they get 275 | * bodies and can now potentially be bumped around or picked up. When 276 | * they are picked up, they are moved into the 'picked' state. In the 277 | * picked up state, they are attached as geoms to the katamari and 278 | * their separate body is deleted (they become part of kata_body). 279 | * After a while, to make the game more playable, we ignore the boxes, 280 | * and they are moved onto the ignored list. 281 | * 282 | * Each box must be in exactly one state, and this is reflected by 283 | * which of the following structures they are contained in. 284 | *) 285 | large_boxes : box GeomBoxMap.t; (* dGeomID -> box *) 286 | enabled_boxes : enabled_box BodyBoxMap.t; (* dBody ID -> enabled box *) 287 | picked_boxes : box GeomBoxMap.t; (* dGeomID -> box *) 288 | ignored_boxes : box GeomBoxMap.t; (* dGeomID -> box *) 289 | } 290 | 291 | let draw_scene st = 292 | GlClear.clear [ `color; `depth ]; 293 | 294 | (* Initialise modelview matrix. *) 295 | GlMat.load_identity (); 296 | 297 | (* Get the position and velocity of the katamari. It affects the camera. *) 298 | let pos = dBodyGetPosition st.kata_body in 299 | let vel = dBodyGetLinearVel st.kata_body in 300 | let radius = dGeomSphereGetRadius st.kata_geom in 301 | ignore(vel); 302 | 303 | (* Camera. *) 304 | let () = 305 | let angle = !camera in 306 | let eye = 307 | { x = pos.x +. camera_dist *. cos angle; 308 | y = pos.y +. camera_dist *. sin angle; 309 | z = pos.z +. radius +. 0.03; 310 | w = 0. } in 311 | let up = 312 | let up = { x = 0.; y = 0.; z = 1.; w = 0. } in 313 | let cam = 314 | { x = eye.x -. pos.x; y = eye.y -. pos.y; z = eye.z -. pos.z; 315 | w = 0. } in 316 | (* Vector b will be perpendicular to the up vector and the vector 317 | * shooting out from the camera. 318 | *) 319 | let b = veccross cam up in 320 | let up = vecneg (veccross cam b) in 321 | up in 322 | let eye = eye.x, eye.y, eye.z in 323 | let center = pos.x, pos.y, pos.z in 324 | let up = up.x, up.y, up.z in 325 | GluMat.look_at ~eye ~center ~up in 326 | 327 | (* Katamari. *) 328 | GlMat.push (); 329 | let r = dGeomGetRotation st.kata_geom in 330 | let matrix = [| 331 | [| r.r11; r.r21; r.r31; 0. |]; 332 | [| r.r12; r.r22; r.r32; 0. |]; 333 | [| r.r13; r.r23; r.r33; 0. |]; 334 | [| pos.x; pos.y; pos.z; 1. |] 335 | |] in 336 | GlMat.mult (GlMat.of_array matrix); 337 | GlDraw.color (1., 1., 1.); 338 | Glut.wireSphere ~radius ~slices:10 ~stacks:10; 339 | GlMat.pop (); 340 | 341 | (* Boxes. Draw them in different colours so it's obvious what state 342 | * boxes are in. 343 | *) 344 | let draw_box col box = 345 | GlMat.push (); 346 | 347 | let geom = box.b_geom in 348 | let r = dGeomGetRotation geom in 349 | let pos = dGeomGetPosition geom in 350 | let matrix = [| 351 | [| r.r11; r.r21; r.r31; 0. |]; 352 | [| r.r12; r.r22; r.r32; 0. |]; 353 | [| r.r13; r.r23; r.r33; 0. |]; 354 | [| pos.x; pos.y; pos.z; 1. |] 355 | |] in 356 | GlMat.mult (GlMat.of_array matrix); 357 | 358 | let x, y, z = box.b_size in 359 | GlMat.scale ~x ~y ~z (); 360 | 361 | GlDraw.color col; 362 | Glut.wireCube ~size:1.; 363 | GlMat.pop (); 364 | in 365 | let col = ( 0., 0., 0. ) in 366 | GeomBoxMap.iter (fun _ box -> draw_box col box) st.large_boxes; 367 | let col = ( 1., 1., 0. ) in 368 | BodyBoxMap.iter (fun _ box -> draw_box col box.b_box) st.enabled_boxes; 369 | 370 | (* Picked up and ignored boxes are drawn slightly differently. *) 371 | let draw_box col box = 372 | GlMat.push (); 373 | 374 | let geom = box.b_geom in 375 | (* let r = dGeomGetRotation st.kata_geom in -- same as above *) 376 | let { x = x; y = y; z = z } = 377 | dGeomGetPosition geom in (* Relative to katamari centre. *) 378 | let { x = x; y = y; z = z } = 379 | dBodyGetRelPointPos st.kata_body ~px:x ~py:y ~pz:z in 380 | let matrix = [| 381 | [| r.r11; r.r21; r.r31; 0. |]; 382 | [| r.r12; r.r22; r.r32; 0. |]; 383 | [| r.r13; r.r23; r.r33; 0. |]; 384 | [| x; y; z; 1. |] 385 | |] in 386 | GlMat.mult (GlMat.of_array matrix); 387 | 388 | let x, y, z = box.b_size in 389 | GlMat.scale ~x ~y ~z (); 390 | 391 | GlDraw.color col; 392 | Glut.wireCube ~size:1.; 393 | GlMat.pop (); 394 | in 395 | let col = ( 1., 1., 1. ) in 396 | GeomBoxMap.iter (fun _ box -> 397 | draw_box col box) st.picked_boxes; 398 | let col = ( 0., 1., 1. ) in 399 | GeomBoxMap.iter (fun _ box -> 400 | draw_box col box) st.ignored_boxes 401 | 402 | (* Surface parameters used for all contact points. *) 403 | let surface_params = { 404 | sp_mode = [`dContactBounce]; 405 | sp_mu = dInfinity; sp_mu2 = 0.; 406 | sp_bounce = 0.7; sp_bounce_vel = 0.1; 407 | sp_soft_erp = 0.; sp_soft_cfm = 0.; 408 | sp_motion1 = 0.; sp_motion2 = 0.; 409 | sp_slip1 = 0.; sp_slip2 = 0.; 410 | } 411 | 412 | (* A group to hold the contact joints. *) 413 | let contact_joint_group = dJointGroupCreate () 414 | 415 | (* See function classify below. *) 416 | type class_t = 417 | | IsKatamari 418 | | IsIgnoredBox of box 419 | | IsPickedBox of box 420 | | IsLargeBox of box 421 | | IsEnabledBox of enabled_box 422 | | IsGround 423 | | IsScenery 424 | 425 | let generic_geom (geom : 'a dGeomID) = 426 | (Obj.magic geom : 'b dGeomID) 427 | 428 | (* Classify each geom/body. *) 429 | let classify st geom body = 430 | (* Most collisions are with the ground, so test this first. *) 431 | if (generic_geom geom) = st.plane then 432 | IsGround 433 | else if (generic_geom geom) = st.kata_geom then 434 | IsKatamari 435 | else 436 | try 437 | (match body with 438 | | None -> raise Not_found 439 | | Some body -> 440 | let box = BodyBoxMap.find body st.enabled_boxes in 441 | IsEnabledBox box 442 | ) 443 | with 444 | Not_found -> 445 | try 446 | let box = GeomBoxMap.find (Box_geom geom) st.large_boxes in 447 | IsLargeBox box 448 | with 449 | Not_found -> 450 | try 451 | let box = GeomBoxMap.find (Box_geom geom) st.picked_boxes in 452 | IsPickedBox box 453 | with 454 | Not_found -> 455 | try 456 | let box = GeomBoxMap.find (Box_geom geom) st.ignored_boxes in 457 | IsIgnoredBox box 458 | with 459 | Not_found -> IsScenery 460 | 461 | (* Pick up a box. This updates the state. *) 462 | let pick_up_box st box = 463 | let orig_geom = box.b_box.b_geom in 464 | let orig_body = box.b_body in 465 | 466 | (* Attach the box to the katamari. To do this we create a transform 467 | * geom and another box geom inside it, positioned at the correct relative 468 | * position. The original geom is destroyed because there is no way to 469 | * detach geoms from spaces (geoms inside transform geoms must not be 470 | * in any space). 471 | *) 472 | let lx, ly, lz = box.b_box.b_size in 473 | let geom = dCreateBox None ~lx ~ly ~lz in 474 | let tgeom = dCreateGeomTransform (Some st.space) in 475 | dGeomTransformSetGeom tgeom (Some geom); 476 | dGeomSetBody tgeom (Some st.kata_body); 477 | let { x = px; y = py; z = pz } = dGeomGetPosition orig_geom in 478 | let { x = x; y = y; z = z } = dBodyGetPosRelPoint st.kata_body ~px ~py ~pz in 479 | dGeomSetPosition geom ~x ~y ~z; 480 | let r = dGeomGetRotation orig_geom in 481 | dGeomSetRotation geom r; 482 | dGeomDestroy orig_geom; 483 | 484 | (* Adjust the mass of the katamari. *) 485 | let mass = dBodyGetMass st.kata_body in (* Current mass of katamari. *) 486 | let mass' = dBodyGetMass orig_body in (* Current mass of box. *) 487 | (* 488 | dMassRotate mass' r; (* Mass of box rotated. *) 489 | dMassTranslate mass' ~x ~y ~z; (* Mass of box translated. *) 490 | *) 491 | dMassAdd mass mass'; (* Mass of katamari + box. *) 492 | dBodySetMass st.kata_body mass; 493 | grown_factor := (dMass_mass mass) *. 790.; 494 | Printf.printf "mass: %f\n%!" (dMass_mass mass); 495 | 496 | (* Calculate a time before this box gets ignored. *) 497 | (* XXX Should be longer for heavier or awkwardly shaped boxes. *) 498 | let time = !physics_time +. ignore_factor in 499 | 500 | (* New box structure. *) 501 | let box = { box.b_box with 502 | b_geom = geom; b_tgeom = GeomTransform_geom tgeom; b_time = time } in 503 | 504 | (* Move the box to the picked list. *) 505 | let enabled_boxes = BodyBoxMap.remove orig_body st.enabled_boxes in 506 | let picked_boxes = GeomBoxMap.add (GeomTransform_geom tgeom) box st.picked_boxes in 507 | 508 | (* This box is no longer an independent rigid body. *) 509 | dBodyDestroy orig_body; 510 | 511 | (* Return updated state. *) 512 | { st with 513 | enabled_boxes = enabled_boxes; 514 | picked_boxes = picked_boxes } 515 | 516 | (* Run the physics loop to a particular time. *) 517 | let physics st to_time = 518 | let step st = 519 | (* Get katamari's current mass. *) 520 | let kata_mass = 521 | let mass = dBodyGetMass st.kata_body in 522 | dMass_mass mass in 523 | 524 | (* Accumulate forces on the katamari according to the current 525 | * key states. 526 | *) 527 | let () = 528 | (* The forwards force will be applied at this angle along the ground. *) 529 | let angle = pi +. !camera in 530 | let x = cos angle in 531 | let y = sin angle in 532 | let fx = x *. stepsize *. !grown_factor in 533 | let fy = y *. stepsize *. !grown_factor in 534 | if !key_forward then 535 | dBodyAddForce st.kata_body ~fx ~fy ~fz:0.; 536 | if !key_backward then 537 | dBodyAddForce st.kata_body ~fx:(-.fx/.10.) ~fy:(-.fy/.10.) ~fz:0.; 538 | if !key_left then 539 | camera := !camera +. stepsize *. 1.2; 540 | if !key_right then 541 | camera := !camera -. stepsize *. 1.2; 542 | 543 | (* When forwards/backwards NOT pressed, simulate a little natural 544 | * friction. 545 | *) 546 | if not !key_forward && not !key_backward then ( 547 | let vel = dBodyGetLinearVel st.kata_body in 548 | let vel = vecscalarmul (-. 100. *. stepsize) vel in 549 | dBodyAddForce st.kata_body ~fx:vel.x ~fy:vel.y ~fz:0. 550 | ) in 551 | 552 | (*----- Collision detection. -----*) 553 | let nr_contacts = ref 0 in 554 | let contacts = ref [] in 555 | 556 | let near geom1 geom2 = 557 | (* geom1 and geom2 are close. Test if they collide. *) 558 | let cs = dCollide geom1 geom2 ~max:4 in 559 | if Array.length cs > 0 then 560 | contacts := (geom1, geom2, cs) :: !contacts 561 | in 562 | dSpaceCollide st.space near; 563 | 564 | (* This gives us a list of geom-geom contacts. There is a 565 | * potential problem when iterating over this list: we might pick up 566 | * a geom, but references to that geom could exist later in the list. 567 | * The function 'loop' below loops avoid the contacts, avoiding this 568 | * case. 569 | *) 570 | let contacts = !contacts in 571 | 572 | let rec loop st = function 573 | | [] -> st 574 | | (geom1, geom2, contacts) :: rest -> 575 | (* Get the bodies (these might be None if colliding with large 576 | * boxes or other scenery). 577 | *) 578 | let body1 = dGeomGetBody geom1 in 579 | let body2 = dGeomGetBody geom2 in 580 | 581 | (* Classify each geom/body. *) 582 | let class1 = classify st geom1 body1 in 583 | let class2 = classify st geom2 body2 in 584 | 585 | (* Possible to pick something up? *) 586 | let st, rest = 587 | match class1, class2 with 588 | | IsIgnoredBox _, _ 589 | | _, IsIgnoredBox _ 590 | | IsKatamari, IsPickedBox _ 591 | | IsPickedBox _, IsKatamari 592 | | IsPickedBox _, IsPickedBox _ 593 | | IsGround, IsGround -> 594 | (* These sorts of collisions are uninteresting. *) 595 | st, rest 596 | 597 | | IsKatamari, IsEnabledBox box 598 | | IsEnabledBox box, IsKatamari 599 | | IsPickedBox _, IsEnabledBox box 600 | | IsEnabledBox box, IsPickedBox _ 601 | when box.b_box.b_mass *. pick_up_factor < kata_mass -> 602 | (* Pick it up - this updates the state because it 603 | * moves the box from the enabled list to the picked 604 | * list. 605 | *) 606 | let st = pick_up_box st box in 607 | 608 | (* Remove the picked geom if it occurs later on in 609 | * the contact list. 610 | *) 611 | let geom_to_remove = box.b_box.b_geom in 612 | let rest = List.filter ( 613 | fun (geom1, geom2, _) -> 614 | geom1 <> geom_to_remove && 615 | geom2 <> geom_to_remove 616 | ) rest in 617 | 618 | st, rest 619 | 620 | | _ -> 621 | (* Just an ordinary collision. *) 622 | (* For each collision, create a contact joint. *) 623 | Array.iter ( 624 | fun contact_geom -> 625 | incr nr_contacts; 626 | 627 | (* Create the contact joint. *) 628 | let contact = { 629 | c_surface = surface_params; 630 | c_geom = contact_geom; 631 | c_fdir1 = { x = 0.; y = 0.; z = 0.; w = 0. } 632 | } in 633 | let joint = 634 | dJointCreateContact st.ode 635 | (Some contact_joint_group) contact in 636 | 637 | (* Attach that joint to the two bodies. The 638 | * bodies may be 'None' indicating a collision with 639 | * the static world, but that's OK. 640 | *) 641 | dJointAttach joint body1 body2 642 | ) contacts; 643 | 644 | st, rest in 645 | 646 | loop st rest 647 | in 648 | let st = loop st contacts in 649 | 650 | (*----- Take a simulation step. -----*) 651 | dWorldQuickStep st.ode stepsize; 652 | physics_time := !physics_time +. stepsize; 653 | 654 | (* Remove and destroy the contact joints. *) 655 | dJointGroupEmpty contact_joint_group; 656 | 657 | (* Consider all the picked boxes and move any to the ignored list if 658 | * they have been picked for a certain time. 659 | *) 660 | let st = 661 | GeomBoxMap.fold ( 662 | fun geom box st -> 663 | if box.b_time <= !physics_time then 664 | (* Move to ignored list. *) 665 | { st with 666 | picked_boxes = GeomBoxMap.remove geom st.picked_boxes; 667 | ignored_boxes = GeomBoxMap.add geom box st.ignored_boxes } 668 | else st 669 | ) st.picked_boxes st in 670 | 671 | st 672 | in 673 | 674 | let rec loop st = 675 | if !physics_time < to_time then ( 676 | let st = step st in 677 | loop st 678 | ) else 679 | st in 680 | loop st 681 | 682 | let read_events () = 683 | let quit = ref false in 684 | let rec read_events () = 685 | match Sdlevent.poll () with 686 | | None -> () 687 | | Some event -> do_event event; read_events () 688 | and do_event = function 689 | | Sdlevent.QUIT -> quit := true (* window closed: quit *) 690 | | Sdlevent.KEYDOWN ke -> 691 | (match ke.Sdlevent.keysym with 692 | | Sdlkey.KEY_ESCAPE -> quit := true (* escape key: quit *) 693 | | Sdlkey.KEY_UP -> key_forward := true 694 | | Sdlkey.KEY_DOWN -> key_backward := true 695 | | Sdlkey.KEY_LEFT -> key_left := true 696 | | Sdlkey.KEY_RIGHT -> key_right := true 697 | | _ -> () (* ignore this key *) 698 | ) 699 | | Sdlevent.KEYUP ke -> 700 | (match ke.Sdlevent.keysym with 701 | | Sdlkey.KEY_UP -> key_forward := false 702 | | Sdlkey.KEY_DOWN -> key_backward := false 703 | | Sdlkey.KEY_LEFT -> key_left := false 704 | | Sdlkey.KEY_RIGHT -> key_right := false 705 | | _ -> () (* ignore this key *) 706 | ) 707 | | _ -> () (* ignore this event *) 708 | in 709 | read_events (); 710 | !quit 711 | 712 | let main () = 713 | if seed >= 0 then 714 | Random.init seed 715 | else 716 | Random.self_init (); 717 | 718 | (* Initialise SDL. *) 719 | Sdl.init [`VIDEO]; 720 | Sdlgl.set_attr []; 721 | let surface = Sdlvideo.set_video_mode ~w:width ~h:height ~bpp:32 [`OPENGL] in 722 | ignore(surface); 723 | 724 | dInitODE(); 725 | 726 | (* Create the ODE world. *) 727 | let ode = dWorldCreate () in 728 | dWorldSetGravity ode ~x:0. ~y:0. ~z:(-9.81); 729 | 730 | (* Create the objects in the world. *) 731 | let space = dHashSpaceCreate None in 732 | dHashSpaceSetLevels space (-4) 4; (* 1/16 .. 16 units. *) 733 | 734 | (* The ground plane goes through the world origin, with the normal 735 | * facing upwards towards +z. 736 | *) 737 | let plane = dCreatePlane (Some space) ~a:0. ~b:0. ~c:1. ~d:0. in 738 | 739 | (* Create the katamari. *) 740 | let kata_geom, kata_body = create_katamari ode space in 741 | 742 | (* Scatter boxes around. *) 743 | let large_boxes = create_boxes ode space in 744 | 745 | (* Enable boxes which are up to 10 times larger than could be picked up. *) 746 | let kata_mass = 747 | let mass = dBodyGetMass kata_body in 748 | dMass_mass mass in 749 | let max_mass = kata_mass *. 10. /. pick_up_factor in 750 | let large_boxes, enabled_boxes = 751 | enable_boxes ode large_boxes BodyBoxMap.empty max_mass in 752 | 753 | let st = { ode = ode; 754 | space = space; 755 | plane = plane; 756 | kata_geom = kata_geom; 757 | kata_body = kata_body; 758 | large_boxes = large_boxes; 759 | enabled_boxes = enabled_boxes; 760 | picked_boxes = GeomBoxMap.empty; 761 | ignored_boxes = GeomBoxMap.empty; } in 762 | 763 | (* Initialise GL state. *) 764 | init_gl (); 765 | 766 | (* Start the clocks counting. *) 767 | let current_time = 768 | let base = Unix.gettimeofday () in 769 | fun () -> Unix.gettimeofday () -. base 770 | in 771 | 772 | let rec main_loop st = 773 | draw_scene st; 774 | let quit = read_events () in 775 | let st = physics st (current_time ()) in 776 | Sdlgl.swap_buffers (); 777 | if not quit then 778 | main_loop st 779 | in 780 | main_loop st; 781 | 782 | (* Clean up the world. *) 783 | dGeomDestroy plane; 784 | dSpaceDestroy space; 785 | 786 | (* Destroy the ODE world and clean up. *) 787 | dWorldDestroy ode; 788 | dCloseODE (); 789 | 790 | (* Quit SDL. *) 791 | Sdl.quit (); 792 | 793 | (* Find any memory allocation bugs. *) 794 | Gc.compact () 795 | 796 | let () = 797 | let _ = Glut.init [| |] in 798 | main () 799 | --------------------------------------------------------------------------------