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="https://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 }