001         package com.croftsoft.apps.mars.ai;
002    
003         import com.croftsoft.core.math.geom.Point2DD;
004         import com.croftsoft.core.math.geom.PointXY;
005    
006         /*********************************************************************
007         * Used with the A* path-finding implementation.
008         *
009         * @version
010         *   2003-04-29
011         * @since
012         *   2003-04-29
013         * @author
014         *   <a href="https://www.croftsoft.com/">David Wallace Croft</a>
015         *********************************************************************/
016    
017         public final class  StateSpaceNode
018         //////////////////////////////////////////////////////////////////////
019         //////////////////////////////////////////////////////////////////////
020         {
021    
022         private final Point2DD  point2DD;
023    
024         //
025    
026         private double  heading;
027    
028         //////////////////////////////////////////////////////////////////////
029         //////////////////////////////////////////////////////////////////////
030    
031         public  StateSpaceNode (
032           PointXY  pointXY,
033           double   heading )
034         //////////////////////////////////////////////////////////////////////
035         {
036           point2DD = new Point2DD ( );
037    
038           setPointXY ( pointXY );
039    
040           setHeading ( heading );
041         }
042    
043         public  StateSpaceNode ( )
044         //////////////////////////////////////////////////////////////////////
045         {
046           point2DD = new Point2DD ( );
047         }
048    
049         //////////////////////////////////////////////////////////////////////
050         //////////////////////////////////////////////////////////////////////
051    
052         public double   getHeading ( ) { return heading;  }
053    
054         public PointXY  getPointXY ( ) { return point2DD; }
055    
056         //////////////////////////////////////////////////////////////////////
057         //////////////////////////////////////////////////////////////////////
058    
059         public void  set ( StateSpaceNode  stateSpaceNode )
060         //////////////////////////////////////////////////////////////////////
061         {
062           setHeading ( stateSpaceNode.getHeading ( ) );
063    
064           setPointXY ( stateSpaceNode.getPointXY ( ) );
065         }
066    
067         public void  setHeading ( double  heading )
068         //////////////////////////////////////////////////////////////////////
069         {
070           while ( heading < 0.0 )
071           {
072             heading += 2.0 * Math.PI;
073           }
074    
075           while ( heading > 2.0 * Math.PI )
076           {
077             heading -= 2.0 * Math.PI;
078           }
079    
080           this.heading = heading;         
081         }
082    
083         public void  setPointXY ( PointXY  pointXY )
084         //////////////////////////////////////////////////////////////////////
085         {
086           point2DD.setXY ( pointXY );
087         }
088    
089         //////////////////////////////////////////////////////////////////////
090         //////////////////////////////////////////////////////////////////////
091    
092         public double  distance ( StateSpaceNode  otherStateSpaceNode )
093         //////////////////////////////////////////////////////////////////////
094         {
095           return point2DD.distance ( otherStateSpaceNode.point2DD );
096         }
097    
098         public double  rotation ( StateSpaceNode  otherStateSpaceNode )
099         //////////////////////////////////////////////////////////////////////
100         {
101    // this needs to be fixed
102    
103           double  otherHeading = otherStateSpaceNode.heading;
104    
105           double  headingDelta = otherHeading - heading;
106    
107           if ( headingDelta < -Math.PI )
108           {
109             headingDelta = ( otherHeading + 2.0 * Math.PI ) - heading;
110           }
111           else if ( headingDelta > Math.PI )
112           {
113             headingDelta = ( otherHeading - 2.0 * Math.PI ) - heading;
114           }
115    
116           return headingDelta;
117         }
118    
119         public String  toString ( )
120         //////////////////////////////////////////////////////////////////////
121         {
122           return point2DD.toString ( );
123         }
124    
125         //////////////////////////////////////////////////////////////////////
126         //////////////////////////////////////////////////////////////////////
127         }