Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 97c00ff7a3292278cb11474f3432f40d9f9a09fb
|
From: Pierre C. <Ba...@us...> - 2011-05-29 16:36:29
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 97c00ff7a3292278cb11474f3432f40d9f9a09fb (commit)
from 23679818e579d02c51823757eac4e73395bfacbd (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 97c00ff7a3292278cb11474f3432f40d9f9a09fb
Author: chambart <cha...@cr...>
Date: Sun May 29 18:31:00 2011 +0200
[info] change the pathfinding algorithm
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 337a6f5..f70581e 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -38,6 +38,8 @@ Library krobot
Krobot_bus,
Krobot_message,
Krobot_geom,
+ Krobot_solve,
+ Krobot_pathfinding,
Krobot_config
CSources:
krobot_message_stubs.c
@@ -153,7 +155,7 @@ Executable "krobot-planner"
Install: true
CompiledObject: best
MainIs: krobot_planner.ml
- BuildDepends: krobot, lwt.syntax, ocamlgraph
+ BuildDepends: krobot, lwt.syntax
Executable "krobot-objects"
Path: src/tools
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 14d98e6..b7b7b29 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 2b0a35e51cf2e902c1aae093db385268)
+# DO NOT EDIT (digest: b3a149aa36135c075418226bdf8c680d)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -44,11 +44,9 @@
<examples/dump_can.{native,byte}>: pkg_lwt.react
# Executable krobot-planner
<src/tools/krobot_planner.{native,byte}>: use_krobot
-<src/tools/krobot_planner.{native,byte}>: pkg_ocamlgraph
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: pkg_ocamlgraph
# Executable krobot-record
<src/tools/krobot_record.{native,byte}>: use_krobot
<src/tools/krobot_record.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index 1e10a3e..089c86e 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,9 +1,11 @@
# OASIS_START
-# DO NOT EDIT (digest: 802a60f0417652c8e0b1a85294fd57f0)
+# DO NOT EDIT (digest: a359da43dcf8c8c6335f80cc41192c30)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
src/lib/Krobot_geom
+src/lib/Krobot_solve
+src/lib/Krobot_pathfinding
src/lib/Krobot_config
src/can/Krobot_can_bus
# OASIS_STOP
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index 5fd4717..c4bd087 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,7 +1,7 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: d2e269dc4718223067ec6884e511c44b) *)
+(* DO NOT EDIT (digest: fd331bd6e5dd6cce06b8b38cd2cc1f54) *)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISGettext.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -24,7 +24,7 @@ module OASISGettext = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExpr.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISExpr.ml"
@@ -115,7 +115,7 @@ end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnvLight.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -212,7 +212,7 @@ end
module MyOCamlbuildFindlib = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
(** OCamlbuild extension, copied from
* http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
@@ -321,7 +321,7 @@ module MyOCamlbuildFindlib = struct
end
module MyOCamlbuildBase = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
(** Base functions for writing myocamlbuild.ml
@author Sylvain Le Gall
@@ -336,7 +336,7 @@ module MyOCamlbuildBase = struct
type name = string
type tag = string
-# 55 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 55 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
type t =
{
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index b4b7d73..c7a5e35 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,14 +1,14 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 710ef41bf3f100a1f1ecd818da6623e3) *)
+(* DO NOT EDIT (digest: 65ad0f8cf4ed7e4123fb286c493fdac9) *)
(*
Regenerated by OASIS v0.2.1~alpha1
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISGettext.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -31,7 +31,7 @@ module OASISGettext = struct
end
module OASISContext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISContext.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISContext.ml"
open OASISGettext
@@ -86,7 +86,7 @@ module OASISContext = struct
end
module OASISUtils = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISUtils.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISUtils.ml"
module MapString = Map.Make(String)
@@ -225,7 +225,7 @@ module OASISUtils = struct
end
module PropList = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/PropList.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/PropList.ml"
open OASISGettext
@@ -260,7 +260,7 @@ module PropList = struct
let clear t =
Hashtbl.clear t
-# 66 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/PropList.ml"
+# 66 "/home/chambart/bordel/oasis_0/src/oasis/PropList.ml"
end
module Schema =
@@ -501,7 +501,7 @@ module PropList = struct
end
module OASISMessage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISMessage.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISMessage.ml"
open OASISGettext
@@ -550,7 +550,7 @@ module OASISMessage = struct
end
module OASISVersion = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISVersion.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISVersion.ml"
open OASISGettext
@@ -738,7 +738,7 @@ module OASISVersion = struct
end
module OASISLicense = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISLicense.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISLicense.ml"
(** License for _oasis fields
@author Sylvain Le Gall
@@ -771,7 +771,7 @@ module OASISLicense = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExpr.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISExpr.ml"
@@ -861,7 +861,7 @@ module OASISExpr = struct
end
module OASISTypes = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTypes.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISTypes.ml"
@@ -938,7 +938,7 @@ module OASISTypes = struct
type plugin_data = (all_plugin * plugin_data_purpose * (unit -> unit)) list
-# 102 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTypes.ml"
+# 102 "/home/chambart/bordel/oasis_0/src/oasis/OASISTypes.ml"
type 'a conditional = 'a OASISExpr.choices
@@ -1095,7 +1095,7 @@ module OASISTypes = struct
end
module OASISUnixPath = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISUnixPath.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISUnixPath.ml"
type unix_filename = string
type unix_dirname = string
@@ -1174,7 +1174,7 @@ module OASISUnixPath = struct
end
module OASISSection = struct
-# 1 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISSection.ml"
+# 1 "/home/chambart/bordel/oasis_0/src/oasis/OASISSection.ml"
open OASISTypes
let section_kind_common =
@@ -1228,12 +1228,12 @@ module OASISSection = struct
end
module OASISBuildSection = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISBuildSection.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISBuildSection.ml"
end
module OASISExecutable = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExecutable.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISExecutable.ml"
open OASISTypes
@@ -1264,7 +1264,7 @@ module OASISExecutable = struct
end
module OASISLibrary = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISLibrary.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISLibrary.ml"
open OASISTypes
open OASISUtils
@@ -1555,33 +1555,33 @@ module OASISLibrary = struct
end
module OASISFlag = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISFlag.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISFlag.ml"
end
module OASISPackage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISPackage.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISPackage.ml"
end
module OASISSourceRepository = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISSourceRepository.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISSourceRepository.ml"
end
module OASISTest = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTest.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISTest.ml"
end
module OASISDocument = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISDocument.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/oasis/OASISDocument.ml"
end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnvLight.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -1678,7 +1678,7 @@ end
module BaseContext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseContext.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseContext.ml"
open OASISContext
@@ -1689,7 +1689,7 @@ module BaseContext = struct
end
module BaseMessage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseMessage.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseMessage.ml"
(** Message to user, overrid for Base
@author Sylvain Le Gall
@@ -1710,7 +1710,7 @@ module BaseMessage = struct
end
module BaseFilePath = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFilePath.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseFilePath.ml"
open Filename
@@ -1742,7 +1742,7 @@ module BaseFilePath = struct
end
module BaseEnv = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnv.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseEnv.ml"
open OASISGettext
open OASISUtils
@@ -2197,7 +2197,7 @@ module BaseEnv = struct
end
module BaseExec = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseExec.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseExec.ml"
open OASISGettext
open OASISUtils
@@ -2257,7 +2257,7 @@ module BaseExec = struct
end
module BaseFileUtil = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFileUtil.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseFileUtil.ml"
open OASISGettext
@@ -2435,7 +2435,7 @@ module BaseFileUtil = struct
end
module BaseArgExt = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseArgExt.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseArgExt.ml"
open OASISUtils
open OASISGettext
@@ -2463,7 +2463,7 @@ module BaseArgExt = struct
end
module BaseCheck = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseCheck.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseCheck.ml"
open BaseEnv
open BaseMessage
@@ -2589,7 +2589,7 @@ module BaseCheck = struct
end
module BaseOCamlcConfig = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseOCamlcConfig.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseOCamlcConfig.ml"
open BaseEnv
@@ -2699,7 +2699,7 @@ module BaseOCamlcConfig = struct
end
module BaseStandardVar = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseStandardVar.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseStandardVar.ml"
open OASISGettext
@@ -2958,7 +2958,7 @@ module BaseStandardVar = struct
end
module BaseFileAB = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFileAB.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseFileAB.ml"
open BaseEnv
open OASISGettext
@@ -3006,7 +3006,7 @@ module BaseFileAB = struct
end
module BaseLog = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseLog.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseLog.ml"
open OASISUtils
@@ -3125,7 +3125,7 @@ module BaseLog = struct
end
module BaseBuilt = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseBuilt.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseBuilt.ml"
open OASISTypes
open OASISGettext
@@ -3272,7 +3272,7 @@ module BaseBuilt = struct
end
module BaseCustom = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseCustom.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseCustom.ml"
open BaseEnv
open BaseMessage
@@ -3322,7 +3322,7 @@ module BaseCustom = struct
end
module BaseDynVar = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDynVar.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseDynVar.ml"
open OASISTypes
@@ -3366,7 +3366,7 @@ module BaseDynVar = struct
end
module BaseTest = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseTest.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseTest.ml"
open BaseEnv
open BaseMessage
@@ -3448,7 +3448,7 @@ module BaseTest = struct
end
module BaseDoc = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDoc.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseDoc.ml"
open BaseEnv
open BaseMessage
@@ -3478,7 +3478,7 @@ module BaseDoc = struct
end
module BaseSetup = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseSetup.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseSetup.ml"
open BaseEnv
open BaseMessage
@@ -3896,7 +3896,7 @@ module BaseSetup = struct
end
module BaseDev = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDev.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/base/BaseDev.ml"
@@ -3954,7 +3954,7 @@ end
module InternalConfigurePlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/internal/InternalConfigurePlugin.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/internal/InternalConfigurePlugin.ml"
(** Configure using internal scheme
@author Sylvain Le Gall
@@ -4170,7 +4170,7 @@ module InternalConfigurePlugin = struct
end
module InternalInstallPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/internal/InternalInstallPlugin.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/internal/InternalInstallPlugin.ml"
(** Install using internal scheme
@author Sylvain Le Gall
@@ -4574,7 +4574,7 @@ end
module OCamlbuildCommon = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
(** Functions common to OCamlbuild build and doc plugin
*)
@@ -4674,7 +4674,7 @@ module OCamlbuildCommon = struct
end
module OCamlbuildPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
(** Build using ocamlbuild
@author Sylvain Le Gall
@@ -4846,7 +4846,7 @@ module OCamlbuildPlugin = struct
end
module OCamlbuildDocPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
+# 21 "/home/chambart/bordel/oasis_0/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
(* Create documentation using ocamlbuild .odocl files
@author Sylvain Le Gall
@@ -5004,6 +5004,8 @@ let setup_t =
"Krobot_bus";
"Krobot_message";
"Krobot_geom";
+ "Krobot_solve";
+ "Krobot_pathfinding";
"Krobot_config"
];
lib_internal_modules = [];
@@ -5113,8 +5115,7 @@ let setup_t =
bs_build_depends =
[
InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None);
- FindlibPackage ("ocamlgraph", None)
+ FindlibPackage ("lwt.syntax", None)
];
bs_build_tools = [ExternalTool "ocamlbuild"];
bs_c_sources = [];
diff --git a/info/control2011/src/lib/krobot.mllib b/info/control2011/src/lib/krobot.mllib
index a445537..350b267 100644
--- a/info/control2011/src/lib/krobot.mllib
+++ b/info/control2011/src/lib/krobot.mllib
@@ -1,8 +1,10 @@
# OASIS_START
-# DO NOT EDIT (digest: cd69d2910ad86f88b392f7a3935ab5c4)
+# DO NOT EDIT (digest: b2880f314f76dff19566bda211d28310)
Krobot_can
Krobot_bus
Krobot_message
Krobot_geom
+Krobot_solve
+Krobot_pathfinding
Krobot_config
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_pathfinding.ml b/info/control2011/src/lib/krobot_pathfinding.ml
new file mode 100644
index 0000000..47f77d8
--- /dev/null
+++ b/info/control2011/src/lib/krobot_pathfinding.ml
@@ -0,0 +1,343 @@
+let rec map_filter f = function
+ | [] -> []
+ | t::q ->
+ match f t with
+ | None -> map_filter f q
+ | Some v -> v::(map_filter f q)
+
+let map_option f = function
+ | None -> None
+ | Some v -> Some (f v)
+
+let iter_option f = function
+ | None -> ()
+ | Some v -> f v
+
+module Vect = struct
+
+ type point = { px : float; py : float }
+ type vect = { vx : float; vy : float }
+ type circle = { c : point; r : float }
+ type segment = { p1 : point; p2 : point }
+ type line = { p : point; v : vect }
+
+ let epsilon = 0.0000000001
+
+ let vect p1 p2 = { vx = p2.px -. p1.px; vy = p2.py -. p1.py }
+
+ let ( +! ) p v = { px = v.vx +. p.px; py = v.vy +. p.py }
+ let ( -! ) p v = { px = p.px -. v.vx; py = p.py -. v.vy }
+
+ let ( +| ) v1 v2 = { vx = v1.vx +. v2.vx; vy = v1.vy +. v2.vy }
+ let ( -| ) v1 v2 = { vx = v1.vx -. v2.vx; vy = v1.vy -. v2.vy }
+ let ( *@ ) n v = { vx = n *. v.vx; vy = n *. v.vy }
+ let norm2 { vx; vy } = vx*.vx +. vy*.vy
+ let norm v = sqrt (norm2 v)
+ let unitaire v = ( 1. /. (norm v) ) *@ v
+ let turn_trigo { vx; vy } = { vx = -. vy; vy = vx }
+ let turn_antitrigo { vx; vy } = { vx = vy; vy = -. vx }
+ let line s = { p = s.p1; v = vect s.p1 s.p2 }
+ let scal v1 v2 = v1.vx *. v2.vx +. v1.vy *. v2.vy
+ let colineaire v1 v2 = abs_float (v1.vx *. v2.vy -. v1.vy *. v2.vx) < epsilon
+
+ let distance s p =
+ let middle =
+ ( scal
+ (vect s.p1 s.p2)
+ (vect s.p1 p) >= 0. )
+ && ( scal
+ (vect s.p2 s.p1)
+ (vect s.p2 p) >= 0. )
+ in
+ if middle
+ then
+ let l = line s in
+ let nv = unitaire l.v in
+ abs_float (scal (turn_trigo nv) (vect s.p1 p))
+ else
+ min (norm (vect s.p1 p)) (norm (vect s.p2 p))
+
+end
+
+module Tangentes = struct
+ open Vect
+
+ type 'a opt =
+ | One of 'a
+ | Two of ('a * 'a)
+
+ let map_opt f = function
+ | One v -> One (f v)
+ | Two (v1,v2) -> Two (f v1,f v2)
+
+ let iter_opt f = function
+ | One v -> f v
+ | Two (v1,v2) -> f v1; f v2
+
+ let p_iso c1 c2 =
+ if c1.r <> c2.r
+ then
+ let cm,cM = if c1.r < c2.r then c1,c2 else c2,c1 in
+ let ba = vect cM.c cm.c in
+ let p1 = cm.c +! (( cm.r /. ( cM.r -. cm.r ) ) *@ ba) in
+ let p2 = cM.c +! (( cM.r /. ( cM.r +. cm.r ) ) *@ ba) in
+ Two (p1,p2)
+ else
+ let ba = vect c1.c c2.c in
+ One (c1.c +! ( 0.5 *@ ba ))
+
+ let tangentes_point_circle p c =
+ let pc = vect p c.c in
+ let pc2 = norm2 pc in
+ let r2 = c.r *. c.r in
+ let y = sqrt ((r2 *. pc2) /. (pc2 -. r2)) in
+ let u = turn_trigo (unitaire pc) in
+ let v = y *@ u in
+ let s1,s2 = { p1 = p; p2 = c.c +! v }, { p1 = p; p2 = c.c -! v } in
+ let l1 = line s1
+ and l2 = line s2 in
+ let v1 = c.r *@ (unitaire (turn_trigo l1.v)) in
+ let v2 = c.r *@ (unitaire (turn_antitrigo l2.v)) in
+ { p1 = p; p2 = c.c +! v1 },
+ { p1 = p; p2 = c.c +! v2 }
+
+ let tangentes_same_size c1 c2 =
+ let ab = vect c1.c c2.c in
+ let v = c1.r *@ (turn_trigo (unitaire ab)) in
+ { p1 = c1.c +! v; p2 = c2.c +! v },
+ { p1 = c1.c -! v; p2 = c2.c -! v }
+
+ let tangentes c1 c2 =
+ let tan p c1 c2 =
+ let s1,s2 = tangentes_point_circle p c1 in
+ let s3,s4 = tangentes_point_circle p c2 in
+ { p1 = s1.p2; p2 = s3.p2 },
+ { p1 = s2.p2; p2 = s4.p2 }
+ in
+ let ((s1,s2),(s3,s4)) =
+ match (p_iso c1 c2) with
+ | One p -> tan p c1 c2, tangentes_same_size c1 c2
+ | Two (p1,p2) -> tan p1 c1 c2, tan p2 c1 c2
+ in
+ [s1;s2;s3;s4]
+
+end
+
+include Krobot_solve
+include Vect
+include Tangentes
+
+type board = { src : point;
+ dst : point;
+ obstacles : circle list; }
+
+type tangente =
+ { orig : point;
+ contact : point;
+ dir : vect;
+ distance : float; (* distance du point d'origine *)
+ circle : circle;
+ prec : tangente list; }
+
+type result =
+ { path : segment list;
+ length : float }
+
+let distance' s nv p =
+ let middle =
+ ( scal
+ (vect s.p1 s.p2)
+ (vect s.p1 p) >= 0. )
+ && ( scal
+ (vect s.p2 s.p1)
+ (vect s.p2 p) >= 0. )
+ in
+ if middle
+ then
+ abs_float (scal (turn_trigo nv) (vect s.p1 p))
+ else
+ sqrt ((min (norm2 (vect s.p1 p)) (norm2 (vect s.p2 p))))
+
+let check_segment b s =
+ let l = line s in
+ let nv = unitaire l.v in
+ let rec check = function
+ | [] -> true
+ | t::q ->
+ if distance' s nv t.c < t.r -. epsilon
+ then false
+ else check q
+ in
+ check b.obstacles
+
+let check_segment_1circle b c1 s =
+ let rec check = function
+ | [] -> true
+ | t::q ->
+ if t = c1
+ then check q
+ else
+ if distance s t.c < t.r
+ then false
+ else check q
+ in
+ check b.obstacles
+
+let check_segment_circle b (c1,c2) s =
+ let rec check = function
+ | [] -> true
+ | t::q ->
+ if t = c1 || t = c2
+ then check q
+ else
+ if distance s t.c < t.r
+ then false
+ else check q
+ in
+ check b.obstacles
+
+let start_list b =
+ let aux c {p1;p2} =
+ { orig = p1;
+ contact = p2;
+ dir = vect p1 p2;
+ distance = 0.;
+ circle = c;
+ prec = []; }
+ in
+ if check_segment b {p1 = b.src; p2 = b.dst}
+ then
+ (Some { length = norm (vect b.src b.dst);
+ path = [{p1 = b.dst; p2 = b.src}] },[])
+ else
+ (None,
+ List.flatten
+ (List.map
+ (fun c ->
+ let (s1,s2) = tangentes_point_circle b.src c in
+ List.map (aux c)
+ (List.filter (check_segment_1circle b c) [s1;s2])
+ ) b.obstacles))
+
+let intersect l1 l2 =
+ if colineaire l1.v l2.v
+ then None
+ else let v = [| l2.p.px -. l1.p.px; l2.p.py -. l1.p.py |] in
+ let m = [| [| l1.v.vx ; l2.v.vx |]; [| l1.v.vy ; l2.v.vy |] |] in
+ let k1,k2 =
+ match solve m v with
+ | [| k1 ; k2 |] -> k1,k2
+ | _ -> assert false in
+ Some (l1.p +! k1 *@ l1.v)
+
+let after {p1;p2} p =
+ scal (vect p1 p2) (vect p2 p) >= 0.
+
+let make_tangente b t c ({p1;p2} as s) =
+ if (scal (line s).v (vect t.contact t.circle.c) >= 0.)
+ && (check_segment_circle b (t.circle,c) s)
+ then
+ let l1 = { p = t.orig; v = t.dir } in
+ let l2 = line s in
+ match intersect l1 l2 with
+ | None -> None
+ | Some inter ->
+ if (after {p1 = t.orig;p2 = t.contact} inter)
+ && (check_segment_circle b (t.circle,c) {p1 = inter; p2 = t.contact})
+ then
+ Some { distance = norm (vect t.orig inter) +. t.distance;
+ orig = inter;
+ contact = p2;
+ circle = c;
+ dir = l2.v;
+ prec = t::t.prec}
+ else None
+ else None
+
+let make_tangente' b t c =
+ map_filter (make_tangente b t c) (tangentes t.circle c)
+
+let make_tangentes b t =
+ let aux1 t c = map_filter (make_tangente b t c) (tangentes t.circle c) in
+ List.flatten (List.map (aux1 t) b.obstacles)
+
+let make_result b inter t =
+ let points = b.dst::inter::(List.map (fun t -> t.orig) (t::t.prec)) in
+ let rec couples = function
+ | [] | [_] -> []
+ | p1::p2::l -> {p1;p2}::(couples (p2::l)) in
+ { length = norm (vect t.orig inter)
+ +. norm (vect inter b.dst)
+ +. t.distance;
+ path = couples points}
+
+let reach_tangente' b t =
+ let (s1,s2) = tangentes_point_circle b.dst t.circle in
+ let tan_line = { p = t.orig; v = t.dir } in
+ let check_seg s =
+ match intersect tan_line (line s) with
+ | None -> None
+ | Some inter ->
+ if (check_segment_1circle b t.circle { p1 = t.contact; p2 = inter })
+ && (check_segment_1circle b t.circle { p1 = b.dst; p2 = inter })
+ then Some (make_result b inter t)
+ else None
+ in
+ map_filter check_seg [s1;s2]
+
+let reach b tl =
+ let l = List.flatten (List.map (reach_tangente' b) tl) in
+ match l with
+ | [] -> None
+ | t::q ->
+ Some
+ (List.fold_left
+ (fun t1 t2 -> if t1.length < t2.length then t1 else t2) t q)
+
+let filter_tangents b min_reach tl =
+ List.filter (fun t -> t.distance +.
+ norm (vect t.orig t.contact) +.
+ norm (vect t.contact b.dst) < min_reach.length) tl
+
+let tangentes_step b tl =
+ let aux1 t c = map_filter (make_tangente b t c) (tangentes t.circle c) in
+ let aux2 t = List.map (aux1 t) b.obstacles in
+ List.flatten (List.flatten (List.map aux2 tl))
+
+let min_result r1 r2 = match r1,r2 with
+ | None, _ -> r2
+ | _, None -> r1
+ | Some v1, Some v2 when v1.length < v2.length -> r1
+ | _ -> r2
+
+let step b (r,tl) =
+ let tl = tangentes_step b tl in
+ let r = min_result (reach b tl) r in
+ let tl = match r with
+ | None -> tl
+ | Some r -> filter_tangents b r tl in
+ (r,tl)
+
+let stepn b n =
+ let rec stepn n (r,tl) =
+ if n = 0 then (r,tl)
+ else stepn (n-1) (step b (r,tl))
+ in
+ let (r,tl) = start_list b in
+ let r = min_result (reach b tl) r in
+ let tl = match r with
+ | None -> tl
+ | Some r -> filter_tangents b r tl in
+ stepn n (r,tl)
+
+let v_of_vertice { Krobot_geom.x; y } =
+ { px = x; py = y }
+
+let vertice_of_seg { p1 = {px;py} } =
+ { Krobot_geom.x = px; y = py }
+
+let find_path ~src ~dst objects =
+ let b = { src = v_of_vertice src;
+ dst = v_of_vertice dst;
+ obstacles = List.map (fun (v,r) -> { c = v_of_vertice v; r }) objects; } in
+ map_option (fun res -> List.rev_map vertice_of_seg res.path) (fst (stepn b 10))
diff --git a/info/control2011/src/lib/krobot_pathfinding.mli b/info/control2011/src/lib/krobot_pathfinding.mli
new file mode 100644
index 0000000..3719e4c
--- /dev/null
+++ b/info/control2011/src/lib/krobot_pathfinding.mli
@@ -0,0 +1,6 @@
+
+val find_path :
+ src:Krobot_geom.vertice ->
+ dst:Krobot_geom.vertice ->
+ ( Krobot_geom.vertice * float ) list ->
+ Krobot_geom.vertice list option
diff --git a/info/control2011/src/lib/krobot_solve.ml b/info/control2011/src/lib/krobot_solve.ml
new file mode 100644
index 0000000..f04d786
--- /dev/null
+++ b/info/control2011/src/lib/krobot_solve.ml
@@ -0,0 +1,93 @@
+let epsilon = 0.0000000001
+
+let add_l ~m ~v i (n,j) =
+ let a = Array.init (Array.length m.(i))
+ (fun k -> m.(i).(k) +. n *. m.(j).(k)) in
+ let m = Array.init (Array.length m)
+ (fun k -> if k <> i then m.(k) else a) in
+ let v = Array.init (Array.length v)
+ (fun k -> if k <> i then v.(k) else v.(i) +. n *. v.(j)) in
+ m,v
+
+let remove_l ~m ~v i =
+ let rec remove_l ~m ~v i j =
+ if j >= Array.length m
+ then m,v
+ else
+ let m,v = add_l ~m ~v j (-. (m.(j).(i) /. m.(i).(i)),i) in
+ remove_l ~m ~v i (j+1) in
+ remove_l ~m ~v i (i+1)
+
+let switch_a a i j =
+ Array.init (Array.length a)
+ (function
+ | k when k = i -> a.(j)
+ | k when k = j -> a.(i)
+ | k -> a.(k))
+
+let switch_l ~m ~v i j =
+ switch_a m i j,
+ switch_a v i j
+
+let reord_l ~m ~v i =
+ let rec reord_l ~m ~v j =
+ if j >= Array.length m
+ then m,v
+ else
+ if abs_float (m.(j).(i)) < epsilon
+ then reord_l ~m ~v (j+1)
+ else switch_l ~m ~v i j
+ in
+ reord_l ~m ~v i
+
+let triangle m v =
+ let rec triangle m v i =
+ if i >= Array.length m - 1
+ then m,v
+ else
+ let m,v = reord_l ~m ~v i in
+ if abs_float (m.(i).(i)) <= epsilon
+ then triangle m v (i+1)
+ else
+ let m,v = remove_l ~m ~v i in
+ triangle m v (i+1)
+ in
+ triangle m v 0
+
+let apply_res m v i =
+ let r = v.(i) /. m.(i).(i) in
+ let v = Array.init (Array.length v)
+ (fun k ->
+ if k >= i
+ then
+ if k = i
+ then v.(k) /. m.(i).(i)
+ else v.(k)
+ else v.(k) -. m.(k).(i) *. r) in
+ let m = Array.init (Array.length m)
+ (fun k ->
+ if k = i
+ then Array.init (Array.length m.(k))
+ (fun l -> if l = i then 1. else 0.)
+ else Array.init (Array.length m.(k))
+ (fun l ->
+ if l = i
+ then 0.
+ else m.(k).(l) )) in
+ m,v
+
+let solve m v =
+ let m,v = triangle m v in
+ let rec solve m v i =
+ if i < 0
+ then m,v
+ else
+ let m,v = apply_res m v i in
+ solve m v (i-1)
+ in
+ snd (solve m v (Array.length m - 1))
+
+let mult m v =
+ Array.init (Array.length m)
+ (fun k -> Array.fold_left (+.) 0.
+ (Array.mapi (fun i n -> v.(i) *. n) m.(k)))
diff --git a/info/control2011/src/lib/krobot_solve.mli b/info/control2011/src/lib/krobot_solve.mli
new file mode 100644
index 0000000..b594202
--- /dev/null
+++ b/info/control2011/src/lib/krobot_solve.mli
@@ -0,0 +1,6 @@
+
+val mult : float array array -> float array -> float array
+(** [mult m v] matrix multiplication *)
+
+val solve : float array array -> float array -> float array
+(** [solve m v] finds [x] such that [mult m x = v] *)
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index f68832d..312deea 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -62,40 +62,6 @@ type planner = {
| Motion planning |
+-----------------------------------------------------------------+ *)
-module Vertice_set = Set.Make(struct type t = vertice let compare = compare end)
-module Vertice_map = Map.Make(struct type t = vertice let compare = compare end)
-
-module Dijkstra =
- Graph.Path.Dijkstra
- (struct
- type t = Vertice_set.t Vertice_map.t
-
- module V = struct
- type t = vertice
- let compare = Pervasives.compare
- let hash = Hashtbl.hash
- let equal = (=)
- end
-
- module E = struct
- type t = vertice * vertice
- type label = float
- let label (a, b) = distance a b
- let dst (a, b) = b
- end
-
- let iter_succ_e f graph v =
- Vertice_set.iter (fun v' -> f (v, v')) (Vertice_map.find v graph)
- end)
- (struct
- type label = float
- type t = float
- let weight d = d
- let compare a b = compare a b
- let add a b = a +. b
- let zero = 0.
- end)
-
let sqr x = x *. x
(* Distance from borders to the robot. *)
@@ -104,84 +70,14 @@ let border_safety_distance = sqrt (sqr robot_size /. 2.) +. 0.05
(* Minimum distance from the center of objects to the center robot. *)
let object_safety_distance = object_radius +. robot_size /. 2.
-(* Test whether there is an intersection between the line (va, vb) and
- one of the objects. *)
-let rec intersection va vb objects =
- match objects with
- | [] ->
- false
- | vc :: rest ->
- (* Compute coefficients of the polynomial. *)
- let a = sqr (distance va vb)
- and b = -2. *. prod (vector va vc) (vector va vb)
- and c = sqr (distance va vc) -. sqr object_safety_distance in
- let delta = sqr b -. 4. *. a *. c in
- if delta < 0. then
- intersection va vb rest
- else
- let k1 = (-. b -. sqrt delta) /. (2. *. a)
- and k2 = (-. b +. sqrt delta) /. (2. *. a) in
- if (k1 >= 0. && k1 <= 1.) || (k2 >= 0. && k2 <= 1.) then
- true
- else
- intersection va vb rest
-
let find_path planner src dst =
- (* Remove the destination object from obstacles. *)
let objects = List.filter (fun obj -> distance dst obj >= object_radius) planner.objects in
- (* Build bounding boxes. *)
- let r1 = object_radius +. robot_size in
- let r2 = object_radius +. robot_size *. 3. /. 4. in
- let vertices =
- List.fold_left
- (fun set obj ->
- let add x y set =
- if (x >= border_safety_distance
- && x <= world_width -. border_safety_distance
- && y >= border_safety_distance
- && y <= world_height -. border_safety_distance) then
- Vertice_set.add { x; y } set
- else
- set
- in
- let set = add obj.x (obj.y -. r1) set in
- let set = add (obj.x +. r1) obj.y set in
- let set = add obj.x (obj.y +. r1) set in
- let set = add (obj.x -. r1) obj.y set in
- let set = add obj.x (obj.y -. r2) set in
- let set = add (obj.x +. r2) obj.y set in
- let set = add obj.x (obj.y +. r2) set in
- let set = add (obj.x -. r2) obj.y set in
- set)
- Vertice_set.empty objects
- in
- (* Add the source and the destination. *)
- let vertices = Vertice_set.add src (Vertice_set.add dst vertices) in
- (* Build the graph. *)
- let graph =
- Vertice_set.fold
- (fun va map ->
- let successors =
- Vertice_set.fold
- (fun vb set ->
- if va <> vb then
- if intersection va vb planner.objects then
- set
- else
- Vertice_set.add vb set
- else
- set)
- vertices Vertice_set.empty
- in
- Vertice_map.add va successors map)
- vertices Vertice_map.empty
- in
- try
- (* Compute the shortest path. *)
- let path, weight = Dijkstra.shortest_path graph src dst in
- List.map (fun (a, b) -> b) path
- with Not_found ->
- []
+ match
+ Krobot_pathfinding.find_path ~src ~dst
+ (List.map (fun v -> v,object_safety_distance) objects)
+ with
+ | None -> []
+ | Some l -> l
(* +-----------------------------------------------------------------+
| Primitives |
hooks/post-receive
--
krobot
|
