001 package com.croftsoft.core.ai.astar;
002
003 import java.util.*;
004
005 import com.croftsoft.core.lang.NullArgumentException;
006 import com.croftsoft.core.math.geom.Point2DD;
007 import com.croftsoft.core.math.geom.PointXY;
008
009 /*********************************************************************
010 * Gradient grid cartographer for continuous space.
011 *
012 * The adjacent nodes are spaced farther apart as you move away from
013 * the starting point.
014 *
015 * @version
016 * 2003-05-10
017 * @since
018 * 2003-04-21
019 * @author
020 * <a href="http://www.CroftSoft.com/">David Wallace Croft</a>
021 *********************************************************************/
022
023 public final class GradientCartographer
024 implements Cartographer
025 //////////////////////////////////////////////////////////////////////
026 //////////////////////////////////////////////////////////////////////
027 {
028
029 public static final double DEFAULT_INIT_STEP_SIZE = 1.0;
030
031 public static final int DEFAULT_DIRECTIONS = 8;
032
033 //
034
035 private final List adjacentList;
036
037 private final double initStepSize;
038
039 private final SpaceTester spaceTester;
040
041 private final int directions;
042
043 //
044
045 private PointXY startPointXY;
046
047 private PointXY goalPointXY;
048
049 //////////////////////////////////////////////////////////////////////
050 //////////////////////////////////////////////////////////////////////
051
052 public GradientCartographer (
053 SpaceTester spaceTester,
054 double initStepSize,
055 int directions )
056 //////////////////////////////////////////////////////////////////////
057 {
058 NullArgumentException.check ( this.spaceTester = spaceTester );
059
060 this.initStepSize = initStepSize;
061
062 this.directions = directions;
063
064 adjacentList = new ArrayList ( );
065 }
066
067 public GradientCartographer ( SpaceTester spaceTester )
068 //////////////////////////////////////////////////////////////////////
069 {
070 this (
071 spaceTester,
072 DEFAULT_INIT_STEP_SIZE,
073 DEFAULT_DIRECTIONS );
074 }
075
076 //////////////////////////////////////////////////////////////////////
077 //////////////////////////////////////////////////////////////////////
078
079 public void setStartPointXY ( PointXY startPointXY )
080 //////////////////////////////////////////////////////////////////////
081 {
082 NullArgumentException.check ( this.startPointXY = startPointXY );
083 }
084
085 public void setGoalPointXY ( PointXY goalPointXY )
086 //////////////////////////////////////////////////////////////////////
087 {
088 NullArgumentException.check ( this.goalPointXY = goalPointXY );
089 }
090
091 //////////////////////////////////////////////////////////////////////
092 // interface Cartographer methods
093 //////////////////////////////////////////////////////////////////////
094
095 public double estimateCostToGoal ( Object node )
096 //////////////////////////////////////////////////////////////////////
097 {
098 return ( ( PointXY ) node ).distanceXY ( goalPointXY );
099 }
100
101 public Iterator getAdjacentNodes ( Object node )
102 //////////////////////////////////////////////////////////////////////
103 {
104 PointXY pointXY = ( PointXY ) node;
105
106 adjacentList.clear ( );
107
108 double distanceToGoal = pointXY.distanceXY ( goalPointXY );
109
110 double distanceFromStart = pointXY.distanceXY ( startPointXY );
111
112 double stepSize
113 = ( ( int ) ( distanceFromStart / initStepSize ) ) * initStepSize;
114
115 stepSize = Math.max ( stepSize, initStepSize );
116
117 if ( distanceToGoal <= stepSize )
118 {
119 adjacentList.add ( goalPointXY );
120
121 return adjacentList.iterator ( );
122 }
123
124 double x = pointXY.getX ( );
125
126 double y = pointXY.getY ( );
127
128 double headingToGoal = Math.atan2 (
129 goalPointXY.getY ( ) - y,
130 goalPointXY.getX ( ) - x );
131
132 for ( int i = 0; i < directions; i++ )
133 {
134 double heading = headingToGoal + i * 2.0 * Math.PI / directions;
135
136 PointXY step = new Point2DD (
137 x + stepSize * Math.cos ( heading ),
138 y + stepSize * Math.sin ( heading ) );
139
140 if ( spaceTester.isSpaceAvailable ( step ) )
141 {
142 adjacentList.add ( step );
143 }
144 }
145
146 return adjacentList.iterator ( );
147 }
148
149 public double getCostToAdjacentNode (
150 Object fromNode,
151 Object toNode )
152 //////////////////////////////////////////////////////////////////////
153 {
154 return ( ( PointXY ) fromNode ).distanceXY ( ( PointXY ) toNode );
155 }
156
157 public boolean isGoalNode ( Object node )
158 //////////////////////////////////////////////////////////////////////
159 {
160 return goalPointXY.distanceXY ( ( PointXY ) node ) == 0.0;
161 }
162
163 //////////////////////////////////////////////////////////////////////
164 //////////////////////////////////////////////////////////////////////
165 }