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
ViewVC logotype

View of /examples/steering/steering.sml

Parent Directory Parent Directory | Revision Log Revision Log


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