[from now] 2013/09/09 00:22:40

diff --git a/src/constraintSolver.ml b/src/constraintSolver.ml
index 6ba53e6..0f6a2de 100644
--- a/src/constraintSolver.ml
+++ b/src/constraintSolver.ml
@@ -81,10 +81,10 @@ let setup_constraint (bodyA, solverA) (bodyB, solverB) contact pair_type opt =
     let open Engine_option in
     let setup axis = let denom = V.dot (M3.mult_vec k axis) axis in
                      let rhs = -.(1.0 +. restriction) *. (V.dot relative_velocity axis) in
-                     Printf.printf "rhs : %f\n" (min 0.0 cp.ContactPoint.distance);
+                     Printf.printf "initial rhs : %f\n" (min 0.0 cp.ContactPoint.distance);
                      let rhs = rhs -.
                        (opt.contact_bias *. min 0.0 cp.ContactPoint.distance) /. opt.time_step in
-                     Printf.printf "rhs : %f\n" rhs;
+                     Printf.printf "calculated rhs : %f\n" rhs;
                      let rhs = rhs *. 1.0 /. denom in
                      {Constraint.axis; jac_diag_inv = 1.0 /. denom;
                       rhs; lower_limit = 0.0;
@@ -114,6 +114,9 @@ let solve (bodyA, solverA) (bodyB, solverB) contact opt =
     let scale = impulse *. solver.SolverBody.mass_inv in
     let inertia = M3.ratio solver.SolverBody.inertia_inv impulse in
     let cross = V.cross r ct.Constraint.axis in
+        Printf.printf  "scale : %f\n" scale;
+
+    Printf.printf  "axis : %s\n" (V.to_string ct.Constraint.axis) ;
     let calc_vec f =
       {solver with SolverBody.delta_linear_velocity =
           f solver.SolverBody.delta_linear_velocity
@@ -139,11 +142,15 @@ let solve (bodyA, solverA) (bodyB, solverB) contact opt =
     safe_hd cp.ContactPoint.constraints >>=
       (fun ctraint ->
         let delta_impulse = ctraint.Constraint.rhs in
+        Printf.printf "initial delta of impulse : %f\n" delta_impulse;
         let delta_velocity_a = V.add solverA.S.delta_linear_velocity (V.cross solverA.S.delta_angular_velocity rA)
         and delta_velocity_b = V.add solverB.S.delta_linear_velocity (V.cross solverB.S.delta_angular_velocity rB) in
         let delta_impulse = calc_delta delta_impulse ctraint delta_velocity_a delta_velocity_b in
+        Printf.printf "delta of impulse : %f\n" delta_impulse;
         let solverA = update_solver delta_impulse solverA ctraint rA `Add
         and solverB = update_solver delta_impulse solverB ctraint rB `Minus in
+        Printf.printf "solver A : %s\n" (V.to_string (solverA.S.delta_linear_velocity));
+        Printf.printf "solver B : %s\n" (V.to_string (solverB.S.delta_linear_velocity));
         return (solverA, solverB)) in

   let solve_for_contact solverA solverB =
diff --git a/src/engine.ml b/src/engine.ml
index ec7a7eb..eb20cbd 100644
--- a/src/engine.ml
+++ b/src/engine.ml
@@ -103,21 +103,6 @@ let broad_phase engine =
   engine.pair_swap <- swap;
   engine.pair_count.(swap) <- count;
   engine
-;;
-
-(* Get closest point from given voronoi regions with target point.  *)
-let get_closest_point_with_voronoi mesh vert plane = function
-  | Voronoi.Point(point) -> point
-  | Voronoi.Edge(edge_point1, edge_point2) ->
-    let edge = V.normalize |< V.sub edge_point2 edge_point1 in
-    V.scale ~v:edge ~scale:(V.dot vert (V.sub vert edge_point1))
-  | Voronoi.Shape ->
-    let facet = mesh.Mesh.facets |> flip Array.get plane in
-    let normal = facet.Mesh.Facet.normal in
-    V.sub vert (V.scale ~v:normal ~scale:(V.dot vert normal))
-
-let is_judge_plane plane normal = V.dot plane.Mesh.Facet.normal normal >= 0.0
-;;

 let simple_inverse m =
   let mat3 = M.upper3x3 m |> Candyvec.Matrix3.transpose
@@ -125,98 +110,6 @@ let simple_inverse m =
   let trans = M.translation vec
   and rotate = M.replace_upper3x3 m mat3 in
   M.multiply trans rotate
-;;
-
-(* TRANSLATE: body_bの座標系をbody_aの座標形に変換し、衝突寸前の状態に変換する
-   行列を返す。
-*)
-let get_coodinate_localization_matrix (axis, dist) body_a body_b =
-  let world_a = RI.get_world_transform body_a
-  and world_b = RI.get_world_transform body_b in
-  let trans_b = M.translation (V.invert |< V.scale ~v:axis ~scale:(dist *. 1.1)) in
-  let open Sugarpot.Std.Option.Open in
-  simple_inverse world_a |> (fun mat -> M.multiply trans_b (M.multiply mat world_b))
-;;
-
-(* TODO: 最近接点を探す際、どちらか一方のどれかの形状を座標系として
-   利用できるようにする。bodyが入れ替わっても、座標系は変わってはならない。
-   最終的に取得した点はワールド座標系における衝突点として、それぞれの
-   ローカル座標における衝突点を最終的に必要とする。ここでは、逆行列を必ず求める必要が
-   あるので、簡便のために、正当ではない逆行列を返す可能性があるような関数を4x4行列に
-   作っておく。
-*)
-
-let get_plane_closest_points (axis, dist) body_a body_b trans_mat =
-  let shapes = body_a.RI.collidable.Collidable.shapes in
-  let contacts : Candyvec.Vector.t list ref = ref [] in
-
-  (* TRANSLATE: それぞれのメッシュについて、offsetを反映させた上で実行させる。 *)
-  Array.iter (fun shape ->
-    let mesh = shape.Shape.mesh |> flip Mesh.transform_vertices trans_mat in
-    Array.iteri (fun index plane ->
-      if is_judge_plane plane axis then
-        let voronoi_region = Voronoi.voronoi_region mesh index in
-
-        (* TRANSLATE: Body Bの各shapeにおけるそれぞれの頂点について処理をする *)
-        let points = Array.fold_left (fun contacts shape ->
-          let mesh_b = shape.Shape.mesh |> flip Mesh.transform_vertices trans_mat in
-          Array.fold_left (fun contacts vert ->
-            let point = get_closest_point_with_voronoi mesh vert index |<
-                Voronoi.recent_of_region vert voronoi_region in
-
-            point :: contacts
-          ) [] mesh_b.Mesh.vertices
-        ) [] body_b.RI.collidable.Collidable.shapes in
-        contacts := points @ !contacts
-      else ()
-    ) mesh.Mesh.facets
-  ) shapes;
-  List.map (fun x ->
-    (V.normalize x, V.norm x)) !contacts
-;;
-
-(* TRANSLATE: エッジ同士における最近接点を取得する *)
-let get_edge_closest_points ((axis, dist) : V.t * float)
-    (body_a : RigidBodyInfo.t) (body_b : RigidBodyInfo.t) (trans_mat : M.t): (V.t * float) list =
-  let shapes = body_a.RI.collidable.Collidable.shapes in
-  let contacts : Candyvec.Vector.t list ref = ref [] in
-  let transformed_mesh mat = flip Mesh.transform_vertices mat in
-  let edge_to_segment vertices edge = let (a, b) = edge.Mesh.Edge.vertex_ids in
-                                      Candyvec.Segment.make vertices.(a) vertices.(b) in
-
-  (* TRANSLATE: それぞれのメッシュについて、offsetを反映させた上で、処理の果ていを行う *)
-  Array.iter (fun shape ->
-    let mesh = transformed_mesh trans_mat shape.Shape.mesh in
-    Array.iteri (fun index edge_a ->
-        (* TRANSLATE: Body Bの各shapeにおけるそれぞれの頂点について処理をする *)
-      Array.iter (fun shape_b ->
-        let mesh_b = transformed_mesh trans_mat shape_b.Shape.mesh in
-        let points = Array.fold_left (fun contacts edge_b ->
-          let edge_a = edge_to_segment shape.Shape.mesh.Mesh.vertices edge_a
-          and edge_b = edge_to_segment shape_b.Shape.mesh.Mesh.vertices edge_b in
-          let (e1, e2) = Candyvec.Segment.closest edge_a edge_b in
-          e1 :: contacts
-        ) [] mesh_b.Mesh.edges in
-
-        contacts := points @ !contacts
-      ) |< body_b.RI.collidable.Collidable.shapes
-    ) mesh.Mesh.edges
-  ) shapes;
-  List.map (fun x -> (V.normalize x, V.norm x)) !contacts
-;;
-
-(* TRANSLATE: 二つのbodyにおける最近接点を取得する。 *)
-let get_closest_point (axis, dist) body_a body_b trans_mat =
-  let plane_base_closests = get_plane_closest_points (axis, dist) body_a body_b trans_mat in
-  let edge_base_closests = get_edge_closest_points (axis, dist) body_a body_b trans_mat in
-  List.fold_left (fun (axis, dist) (newaxis, newdist) ->
-    if dist < newdist then (axis, dist)
-    else (newaxis, newdist)
-  ) (List.fold_left (fun (axis, dist) (newaxis, newdist) ->
-    if dist < newdist then (axis, dist)
-    else (newaxis, newdist)
-  ) (axis, dist) edge_base_closests) plane_base_closests
-;;

 let update_contact_points bodies ((axis, dist) : V.t * float) (pair : Pair.t) : Pair.t =
   (* TRANSLATE: 最近接点をpairに追加する。 *)
@@ -301,21 +194,22 @@ let narrow_phase engine =
     | (None, _) | (_, None) -> failwith "not found one or two of pair"
     | (Some body_a, Some body_b) ->
       match is_separate body_a body_b  with
-    (* TRANSLATE: APlaneの場合、body Aを基準として判定する。  *)
+      (* TRANSLATE: APlaneの場合、body Aを基準として判定する。  *)
       | Some (APlane, axis, dist) ->
-        let a_to_b = get_coodinate_localization_matrix (axis, dist) body_a body_b in
-        Some(get_closest_point (axis, dist) body_a body_b a_to_b)
-    (* TRANSLATE: BPlaneの場合、body Bを基準として判定する。  *)
+        print_string "APlane \n";
+        Some(ClosestPoint.get_closest_point (axis, dist) body_a body_b)
+      (* TRANSLATE: BPlaneの場合、body Bを基準として判定する。  *)
       | Some (BPlane, axis, dist) ->
         print_string "BPlane \n";
-        let a_to_b = get_coodinate_localization_matrix (axis, dist) body_a body_b in
-        Some(get_closest_point (axis, dist) body_b body_a a_to_b)
-    (* TRANSLATE: Edgeの場合、body Aを基準として判定する。  *)
+        Some(ClosestPoint.get_closest_point (axis, dist) body_b body_a)
+      (* TRANSLATE: Edgeの場合、body Aを基準として判定する。  *)
       | Some (Edge, axis, dist) ->
-        let a_to_b = get_coodinate_localization_matrix (axis, dist) body_a body_b in
-        Some(get_closest_point (axis, dist) body_a body_b a_to_b)
-    (* TRANSLATE: 分離平面が存在する場合には、このペアに対して何も行わない *)
-      | None -> print_string "None"; None in
+        let closest, dist = (ClosestPoint.get_closest_point (axis, dist) body_a body_b) in
+        Printf.printf "closest point : %s\n" (V.to_string closest);
+        Some(ClosestPoint.get_closest_point (axis, dist) body_a body_b)
+    (* wTRANSLATE: 分離平面が存在する場合には、このペアに対して何も行わない *)
+      | None -> print_string "separation None\n"; None
+  in

   let updated_pair = Array.mapi (fun index pair ->
     match pair.Pair.pair_type with
@@ -324,6 +218,7 @@ let narrow_phase engine =
       match solve_contact_point index pair with
       | None -> pair
       | Some (axis, dist) ->
+        Printf.printf "Reduction axis : %s %f" (V.to_string axis) dist;
         (* TRANSLATE: ここで取得した衝突点は、すべて剛体Aを基準にとったものとなるため、
            一度ワールド座標系に衝突点を変換する。
         *)
@@ -428,7 +323,7 @@ let update_bodies engine =
   let update_state_position ri =
     let state = ri.RI.state in
     let pos = state.State.pos in
-    let state = {state with State.pos = V.add pos state.State.linear_velocity} in
+    let state = {state with State.pos = V.add pos (V.scale ~scale:time_step ~v:state.State.linear_velocity)} in
     let delta = calc_delta_orientation state.State.angular_velocity time_step in
     let state = {state with State.orientation = Q.multiply state.State.orientation delta} in
     {ri with RI.state = state} in
diff --git a/src/force.ml b/src/force.ml
index ed84d50..94d7f18 100644
--- a/src/force.ml
+++ b/src/force.ml
@@ -6,6 +6,5 @@ let apply_force ~body ~state ~force ~torque ~time_step =
   let delta_velocity = V.scale ~v:delta_accel ~scale:time_step in
   (* TRANSLATE: オイラー陽解法を利用する *)
   let velocity = state.State.linear_velocity in
-  let delta_velocity =  V.scale ~v:delta_velocity ~scale:time_step in
   let new_velocity = V.add velocity delta_velocity in
   (body, {state with State.linear_velocity = new_velocity})
diff --git a/src/separatingAxis.ml b/src/separatingAxis.ml
index 4e24c85..8f90553 100644
--- a/src/separatingAxis.ml
+++ b/src/separatingAxis.ml
@@ -3,24 +3,24 @@ open Sugarpot.Std.Prelude
 open Candyvec.Std

 module R = RigidBodyInfo
-
-type depth = float
 module V = Vector

+type depth = float
 type separating_type = Edge             (* Edge to edge *)
                        | APlane         (* A plane of the body A *)
                        | BPlane         (* A plane of the body B *)
-
 type separating_axis = separating_type * V.t * depth

-(* TRANSLATE: ある軸上にメッシュを投影した場合の最大値と最小値を取得する *)
-let get_projection axis mesh =
-  let vertices = mesh.Mesh.vertices in
+type mesh_info = Mesh.t * Candyvec.Std.Matrix4.t

-  Array.fold_left (fun (pmax, pmin) elem ->
-    let projected = V.dot elem axis in
-    (max pmax projected, min pmin projected)
-  ) (min_float, max_float) vertices
+module Base = struct
+  (* TRANSLATE: ある軸上にメッシュを投影した場合の最大値と最小値を取得する *)
+  let get_maximum_range axis vertices =
+    Array.fold_left (fun (pmax, pmin) elem ->
+      let projected = V.dot elem axis in
+      (max pmax projected, min pmin projected)
+    ) (min_float, max_float) vertices
+end

 (* TRANSLATE: Noneを返した時点で終了するArray.fold_left *)
 let breakable_fold ary f init =
@@ -39,7 +39,7 @@ let is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_ax
   | None -> None
   | Some inversed ->
     (* TRANSLATE: Bのローカル座標系からAのローカル座標形への変換行列 *)
-    let trans_a2b = Matrix4.multiply world_a inversed in
+    let trans_a2b = Matrix4.multiply inversed world_a in

     let sep_axisb = Matrix4.mult_vec trans_a2b sep_axis |> V.normalize in
     let offset_vec = V.sub sep_axis sep_axisb in
@@ -48,8 +48,8 @@ let is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_ax
        換算することで、演算負荷を抑えることができる。
     *)
     let offset = Vector.dot offset_vec sep_axis in
-    let (amax, amin) = get_projection sep_axis mesh_a
-    and (bmax, bmin) = get_projection sep_axisb mesh_b in
+    let (amax, amin) = Base.get_maximum_range sep_axis mesh_a.Mesh.vertices
+    and (bmax, bmin) = Base.get_maximum_range sep_axisb mesh_b.Mesh.vertices in
     let (bmax, bmin) = (bmax +. offset, bmin +. offset) in
     let d1 = amin -. bmax
     and d2 = bmin -. amax in
@@ -57,72 +57,76 @@ let is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_ax
     if d1 >= 0.0 || d2 >= 0.0 then None
     else if d1 < d2 then Some (Vector.invert sep_axis, d2)
     else Some (sep_axis, d1)
-;;
+
+(* TRANSLATE: 各面法線を分離軸として判定する *)
+let face_intersect (shape_a, world_a) (shape_b, world_b) (styp, axis, dist) septype =
+  let faces = match septype with
+    | APlane -> shape_a.Shape.mesh.Mesh.facets
+    | BPlane -> shape_b.Shape.mesh.Mesh.facets
+    | _ -> failwith "face_intersect allowed only APlane or BPlane separation type"
+  in
+  let mesh_a = shape_a.Shape.mesh
+  and mesh_b = shape_b.Shape.mesh in
+  let per_face (styp, axis, dist) face =
+    let sep_axis = face.Mesh.Facet.normal in
+    match is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_axis with
+    | None -> None
+    | Some (newaxis, newdist) ->
+      if newdist < dist then Some (styp, axis, dist) else Some (septype, newaxis, newdist) in
+  breakable_fold faces per_face (septype, axis, dist)
+
+(* TRANSLATE: 各エッジ同士の外積を分離軸として判定する。 *)
+let edge_intersect (shape_a, world_a) (shape_b, world_b) (styp, axis, dist) =
+  let mesh_a = shape_a.Shape.mesh
+  and mesh_b = shape_b.Shape.mesh in
+  let vertices_a = mesh_a.Mesh.vertices
+  and vertices_b = mesh_b.Mesh.vertices
+  and edges_a = mesh_a.Mesh.edges
+  and edges_b = mesh_b.Mesh.edges in
+  let edge_to_vec edge vertices =
+    let first, second = edge.Mesh.Edge.vertex_ids in
+    Vector.sub vertices.(second) vertices.(first) in
+
+  let module V = Candyvec.Std.Vector in
+  breakable_fold edges_a (fun (styp, axis, dist) edge ->
+    let edge_a = edge_to_vec edge vertices_a in
+    breakable_fold edges_b (fun (styp, axis, dist) edge ->
+      let edge_b = edge_to_vec edge vertices_b in
+      let sep_axis = Vector.cross edge_a edge_b |> Vector.normalize in
+      match is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_axis with
+      | None -> None
+      | Some (newaxis, newdist) ->
+        (* TRANSLATE: 貫通深度が最も浅い部分を取得する *)
+        if newdist < dist then Some (styp, axis, dist) else Some (Edge, newaxis, newdist)
+    ) (styp, axis, dist)
+  ) (styp, axis, dist)
+
+let to_shape_info body =
+  let state = body.R.state in
+  let world = State.to_world_transform state in
+  (body.R.collidable.Collidable.shapes, world)

 let judge_intersect ~body_a ~body_b =
   let open Candyvec in
-  let state_a = body_a.R.state
-  and state_b = body_b.R.state in
-  let world_a = Util.world_transform state_a.State.orientation state_a.State.pos
-  and world_b = Util.world_transform state_b.State.orientation state_b.State.pos in

-  let shapes_a = body_a.R.collidable.Collidable.shapes
-  and shapes_b = body_b.R.collidable.Collidable.shapes in
+  let (shapes_a, world_a) = to_shape_info body_a
+  and (shapes_b, world_b) = to_shape_info body_b in
   let length_a = Array.length shapes_a
   and length_b = Array.length shapes_b in
-
-  (* TRANSLATE: 各面法線を分離軸として判定する *)
-  let face_intersect faces shape_a shape_b (styp, axis, dist) septype =
-    let mesh_a = shape_a.Shape.mesh
-    and mesh_b = shape_b.Shape.mesh in
-    let per_face (styp, axis, dist) face =
-      let sep_axis = face.Mesh.Facet.normal in
-      match is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_axis with
-      | None -> None
-      | Some (newaxis, newdist) ->
-        if newdist < dist then Some (styp, axis, dist) else Some (septype, newaxis, newdist) in
-    breakable_fold faces per_face (styp, axis, dist) in
-
-  (* TRANSLATE: 各エッジ同士の外積を分離軸として判定する。 *)
-  let edge_intersect shape_a shape_b (styp, axis, dist) =
-    let mesh_a = shape_a.Shape.mesh
-    and mesh_b = shape_b.Shape.mesh in
-    let vertices_a = mesh_a.Mesh.vertices
-    and vertices_b = mesh_b.Mesh.vertices
-    and edges_a = mesh_a.Mesh.edges
-    and edges_b = mesh_b.Mesh.edges in
-    let edge_to_vec edge vertices =
-      let first, second = edge.Mesh.Edge.vertex_ids in
-      Vector.sub vertices.(second) vertices.(first) in
-
-    let module V = Candyvec.Std.Vector in
-    breakable_fold edges_a (fun (styp, axis, dist) edge ->
-      let edge_a = edge_to_vec edge vertices_a in
-      breakable_fold edges_b (fun (styp, axis, dist) edge ->
-        let edge_b = edge_to_vec edge vertices_b in
-        let sep_axis = Vector.cross edge_a edge_b |> Vector.normalize in
-        match is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_axis with
-        | None -> None
-        | Some (newaxis, newdist) ->
-          (* TRANSLATE: 貫通深度が最も浅い部分を取得する *)
-          if newdist < dist then Some (styp, axis, dist) else Some (Edge, newaxis, newdist)
-      ) (styp, axis, dist)
-    ) (styp, axis, dist) in
-
+  let sep_init = (APlane, Vector.zero, -. max_float) in
   let open Sugarpot.Std.Option.Open in
   let rec intersect_loop ind_a ind_b =
     if ind_a >= length_a then None
     else if ind_b >= length_b then intersect_loop (succ ind_a) 0
     else
-      let shape_a = shapes_a.(ind_a)
-      and shape_b = shapes_b.(ind_b) in
+      let shape_a = (shapes_a.(ind_a), world_a)
+      and shape_b = (shapes_b.(ind_b), world_b) in

       (* TRANSLATE: Aの各面法線ベクトル、Bの各面法線ベクトル、各Edgeの外積を
          分離軸として判定する。どこかで分離平面が見付かれば、その時点で判定は終了する。
       *)
-      (face_intersect shape_a.Shape.mesh.Mesh.facets shape_a shape_b
-         (APlane, Vector.zero, -. max_float) APlane) >>=
+      (face_intersect shape_a shape_b sep_init APlane) >>=
         (fun sep_axis ->
-          face_intersect shape_b.Shape.mesh.Mesh.facets shape_a shape_b sep_axis BPlane) >>=
+          face_intersect shape_a shape_b sep_axis BPlane) >>=
         (fun sep_axis -> edge_intersect shape_a shape_b sep_axis) in
   intersect_loop 0 0
diff --git a/src/separatingAxis.mli b/src/separatingAxis.mli
index 93f747f..f34ff3e 100644
--- a/src/separatingAxis.mli
+++ b/src/separatingAxis.mli
@@ -15,6 +15,29 @@ type separating_type = Edge             (* Edge to edge *)

 type separating_axis = separating_type * Candyvec.Vector.t * depth

+type mesh_info = Mesh.t * Candyvec.Std.Matrix4.t
+
+(** A Module what is provided base functions for SeparationAxis module. *)
+module Base : sig
+  val get_maximum_range : Candyvec.Std.Vector.t -> Candyvec.Std.Vector.t array -> float * float
+  (** Get minimum and maximum result by dotted vector.
+      @return (maximum, minimum)
+  *)
+end
+
+
+val is_separate_axis : info_a:mesh_info -> info_b:mesh_info -> sep_axis:Candyvec.Std.Vector.t ->
+  (Candyvec.Std.Vector.t * float) option
+(** What checking separated to between two meshes are based on separate axis.
+
+    Notice, sep_axis is located on the coodinate on the info_a local coodinate.
+
+    @param info_a a mesh and world transform matrix to check separating with info_b.
+    @param info_b a mesh and world transform matrix to check separating with info_a.
+    @param sep_axis a separate axis to check separating two meshes.
+*)
+
+val judge_intersect: body_a:RigidBodyInfo.t -> body_b:RigidBodyInfo.t -> separating_axis option
 (** Judge each meshes separeted or not. This funciton is based on
     separating axis theorem.
     Judging separation on the separating axis theorem targeted convex mesh is
@@ -24,4 +47,3 @@ type separating_axis = separating_type * Candyvec.Vector.t * depth
     @param mesh_b a mesh to check separation each meshes.
     @return separating axis and penetrate depth if meshes are not separation, or return None.
 *)
-val judge_intersect: body_a:RigidBodyInfo.t -> body_b:RigidBodyInfo.t -> separating_axis option
diff --git a/src/voronoi.ml b/src/voronoi.ml
index 8df9736..d552fb4 100644
--- a/src/voronoi.ml
+++ b/src/voronoi.ml
@@ -19,7 +19,7 @@ type t = region_type list
 *)
 type recent_type = Point of V.t
                    | Edge of V.t * V.t
-                   | Shape
+                   | Shape of V.t

 module Base = struct
@@ -30,7 +30,6 @@ module Base = struct
   let make_edge_region (v1, v2) normal =
     let edge_normal = V.cross (V.sub v2 v1) normal in
     REdge ((v1, v2), edge_normal, normal)
-  ;;

   (* TRANSLATE: 指定された点を含む二本のエッジを取得する *)
   let point_of_contained_edge v (e1, e2, e3) =
@@ -43,7 +42,6 @@ module Base = struct
     | (_, true, true) -> (e2, e3)
     | (true, _, true) -> (e3, e1)
     | _ -> failwith "matching point of edge not found"
-  ;;

   (* TRANSLATE: 点のボロノイ領域を求める *)
   let make_point_region (e1, e2, e3) normal point =
@@ -52,7 +50,6 @@ module Base = struct
     | (REdge (_, normal1, _), REdge (_, normal2, _)) ->
       RPoint (point, normal1, normal2, normal)
     | _ -> failwith "error"
-  ;;

   let make_region (v1, v2, v3) =
     let edges = [(v1, v2); (v2, v3); (v3, v1)] in
@@ -64,19 +61,14 @@ module Base = struct
     let edges = ((v1, v2), (v2, v3), (v3, v1)) in
     let point_regions = List.map (make_point_region edges normal) [v1;v2;v3] in
     List.concat [edge_regions; point_regions]
-  ;;

   (* TRANSLATE: ある点を、法線ベクトルの面上に投影する *)
   let projection_point ~base ~normal ~point =
-    let subbed = V.sub point base in
-    let crossed = V.normalize (V.cross normal subbed) in
-    let crossed_with_crossed = V.normalize (V.cross normal crossed) in
-    let dotted = V.dot subbed crossed_with_crossed in
-    V.scale ~scale:dotted ~v:(V.normalize crossed_with_crossed)
-  ;;
+    let subbed = V.dot normal (V.sub point base) in
+    let multiplied = V.scale ~scale:subbed ~v:normal in
+    V.sub point multiplied

   let is_contain edge point = V.dot point edge >= 0.0
-  ;;

   (* translate: エッジのボロノイ領域に含まれるかどうかを返却する *)
   let contain_region_for_edge edge point =
@@ -90,7 +82,6 @@ module Base = struct
       List.for_all id
         [is_contain edge1 base1; is_contain edge2 base2; is_contain enormal base1]
     | _ -> failwith "contain_region_for_edge can only apply to REdge"
-  ;;

   let contain_region_for_point p point =
     match p with
@@ -99,7 +90,6 @@ module Base = struct
       let is_contain edge = is_contain edge base in
       is_contain e1 && is_contain e2
     | _ -> failwith "contain_region_for_point can only apply to RPoint"
-  ;;

   let recent_region ~region ~point =
     let is_contain_shape =
@@ -108,9 +98,17 @@ module Base = struct
           let projected = projection_point ~base:e1 ~normal:snormal ~point in
           (V.dot projected enormal) < 0.0
       | _ -> false
-      ) region in
+      ) region
+    in
+    let get_point_in_shape =
+      List.hd |< List.map (function
+      | REdge ((e1, e2), enormal, snormal) ->
+          projection_point ~base:e1 ~normal:snormal ~point
+      | _ -> failwith "can not calculate projection point on the plane"
+      ) (List.filter (function | REdge _ -> true | _ -> false) region)
+    in

-    if is_contain_shape then Shape
+    if is_contain_shape then Shape get_point_in_shape
     else
       let calculated = List.filter (fun current ->
         match current with
@@ -124,7 +122,6 @@ module Base = struct
         | RPoint (p, _, _, _) -> Point p
         end
       | _ -> failwith "recent region not found..."
-  ;;
 end

 let voronoi_region mesh facet =
@@ -135,7 +132,6 @@ let voronoi_region mesh facet =

   (* TRANSLATE: 三角形のボロノイ領域になる6個のボロノイ領域を返却する *)
   Base.make_region (vertices.(v1), vertices.(v2), vertices.(v3))
-;;

 (* TRANSLATE: 対象が、あるボロノイ領域における最近接領域の種類を取得する *)
 let recent_of_region target voronois =
diff --git a/src/voronoi.mli b/src/voronoi.mli
index dac34ac..d3ce055 100644
--- a/src/voronoi.mli
+++ b/src/voronoi.mli
@@ -10,7 +10,7 @@
 type t
 type recent_type = Point of Candyvec.Vector.t
                    | Edge of Candyvec.Vector.t * Candyvec.Vector.t
-                   | Shape
+                   | Shape of Candyvec.Vector.t

 module Base : sig
   type t
diff --git a/test/realcamel_tests.ml b/test/realcamel_tests.ml
index 0b23512..a7f43b2 100644
--- a/test/realcamel_tests.ml
+++ b/test/realcamel_tests.ml
@@ -3,6 +3,7 @@ open OUnit
 let suite = "realcamel tests" >:::  [
   Mesh_test.suite;
   Voronoi_test.suite;
+  SeparateAxis_test.suite;
 ]

 let _ =
[from now] 2013/09/11 23:42:26

diff --git a/src/separatingAxis.ml b/src/separatingAxis.ml
index 8f90553..d672432 100644
--- a/src/separatingAxis.ml
+++ b/src/separatingAxis.ml
@@ -20,6 +20,15 @@ module Base = struct
       let projected = V.dot elem axis in
       (max pmax projected, min pmin projected)
     ) (min_float, max_float) vertices
+
+  let detect_separation sep_axis (amax, amin) (bmax, bmin) =
+    let d1 = amin -. bmax
+    and d2 = bmin -. amax in
+
+    if d1 >= 0.0 || d2 >= 0.0 then None
+    else if d1 < d2 then Some (Vector.invert sep_axis, d2)
+    else Some (sep_axis, d1)
+
 end

 (* TRANSLATE: Noneを返した時点で終了するArray.fold_left *)
@@ -51,12 +60,7 @@ let is_separate_axis ~info_a:(mesh_a, world_a) ~info_b:(mesh_b, world_b) ~sep_ax
     let (amax, amin) = Base.get_maximum_range sep_axis mesh_a.Mesh.vertices
     and (bmax, bmin) = Base.get_maximum_range sep_axisb mesh_b.Mesh.vertices in
     let (bmax, bmin) = (bmax +. offset, bmin +. offset) in
-    let d1 = amin -. bmax
-    and d2 = bmin -. amax in
-
-    if d1 >= 0.0 || d2 >= 0.0 then None
-    else if d1 < d2 then Some (Vector.invert sep_axis, d2)
-    else Some (sep_axis, d1)
+    Base.detect_separation sep_axis (amax, amin) (bmax, bmin)

 (* TRANSLATE: 各面法線を分離軸として判定する *)
 let face_intersect (shape_a, world_a) (shape_b, world_b) (styp, axis, dist) septype =
diff --git a/src/separatingAxis.mli b/src/separatingAxis.mli
index f34ff3e..c2fdb78 100644
--- a/src/separatingAxis.mli
+++ b/src/separatingAxis.mli
@@ -23,6 +23,11 @@ module Base : sig
   (** Get minimum and maximum result by dotted vector.
       @return (maximum, minimum)
   *)
+
+
+  val detect_separation : Candyvec.Std.Vector.t -> float * float -> float * float ->
+    (Candyvec.Std.Vector.t * float) option
+  (** Detect separating axis between objects. *)
 end
Merge branch 'feature/separating-axis' into develop
Add function of the Collidable module that is to build Collidable with shapes.
Add collidable tests.
Add test to ClosestPoint
Apply modifications of Candyvec
Merge branch 'feature/closest-point' into develop