Home My Page Projects Code Snippets Project Openings 3D graphics for Standard ML
 Summary Activity SCM

# SCM Repository

[sml3d] View of /examples/steering/steering.sml
 [sml3d] / examples / steering / steering.sml

# View of /examples/steering/steering.sml

Revision 100 - (download) (annotate)
Thu Apr 10 18:45:48 2008 UTC (11 years, 3 months ago) by jhr
File size: 2859 byte(s)
```  Working on example
```
```(* steering.sml
*
* COPYRIGHT (c) 2008 John Reppy (http://cs.uchicago.edu/~jhr)
* All rights reserved.
*)

structure Steering =
struct

structure V = Vehicle
structure V3 = Vec3f

fun steerForSeek (v, target) = let
val desiredVelocity = V3.sub(target, V.position v)
in
V3.sub (desiredVelocity, V.velocity v)
end

fun steerForFlee (v, target) = let
val desiredVelocity = V3.sub(V.position v, target)
in
V3.sub (desiredVelocity, V.velocity v)
end

(* obstacles are represented as spheres *)
type obstacle = {c : SML3dTypes.vec3f, r : SML3dTypes.float}

fun steerToAvoid (v, minTimeToCollision, {c, r}) = let
(* minimum distance to obstacle before avoidance is required *)
val minDistanceToCollision = minTimeToCollision * V.speed v
val minDistanceToCenter = minDistanceToCollision + r
(* contact distance: sum of radii of obstacle and vehicle *)
val totalRadius = r + V.radius v
(* obstacle center relative to vehicle position *)
val localOffset = V3.sub(c, V.position v)
(* distance along vehicle's forward axis to obstacle's center *)
val forwardComponent = V3.dot (localOffset, V.forward v)
val forwardOffset = V3.scale(forwardComponent, V.forward v)
(* offset from forward axis to obstacle's center *)
val offForwardOffset = V3.sub(localOffset, forwardOffset)
(* test to see if sphere overlaps with obstacle-free corridor *)
val inCylinder = V3.length offForwardOffset < totalRadius;
val nearby = forwardComponent < minDistanceToCenter;
val inFront = forwardComponent > 0.0;
in
(* if all three conditions are met, steer away from sphere center *)
if (inCylinder andalso nearby andalso inFront)
then SOME(V3.neg offForwardOffset)
else NONE
end

(* ------------------------------------------------------------------------
// Obstacle Avoidance behavior
//
// Returns a steering force to avoid a given obstacle.  The purely
// lateral steering force will turn our vehicle towards a silhouette edge
// of the obstacle.  Avoidance is required when (1) the obstacle
// intersects the vehicle's current path, (2) it is in front of the
// vehicle, and (3) is within minTimeToCollision seconds of travel at the
// vehicle's current velocity.  Returns a zero vector value (Vec3::zero)
// when no avoidance is required.
*)
fun steerToAvoidObstacle (v, minTimeToCollision, obstacle) = (
case steerToAvoid (v, minTimeToCollision, obstacle)
of SOME vec => (
(* annotation for visual feedback *)
Annotate.avoidObstacle (v, minTimeToCollision * V.speed v);
SOME vec)
| NONE => NONE
(* end case *))

(* avoids all obstacles in a list of obstacles *)
(*
Vec3 steerToAvoidObstacles (const float minTimeToCollision,
const ObstacleGroup& obstacles);
*)

end
```

 root@smlnj-gforge.cs.uchicago.edu ViewVC Help Powered by ViewVC 1.0.0