View Javadoc
1   package SteeringStuff;
2   
3   import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
4   import cz.cuni.amis.pogamut.base3d.worldview.object.Rotation;
5   import javax.vecmath.Tuple3d;
6   import javax.vecmath.Vector2d;
7   import javax.vecmath.Vector3d;
8   
9   /**
10   * This class provides usefull tool for steerings, especially common mathematical calculations.
11   * @author Marki
12   */
13  public class SteeringTools {
14  
15      public enum LineType {STRAIGHT_LINE, HALF_LINE, ABSCISSA};
16  
17      /**Computes the intersection of the lines A and B (line has the start point and its direction).
18       * If this intersection doesn't exist, we return null.
19       * The types set if they are straight lines, half lines or abscissas. If the intersection doesn't lie in the right section
20       * (i.e. in the abscissa), we return null. Otherwise we return the point of intersection.*/
21      public static Vector2d getIntersection(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB, LineType typeA, LineType typeB) {
22          Vector2d result = null;
23          double lengthA = dA.length();
24          double lengthB = dB.length();
25          dA.normalize();
26          dB.normalize();
27          if (!dA.equals(dB)) {
28              if (dA.x == 0) dA.x = 0.001;
29              if (dB.x == 0) dB.x = 0.001;
30              if (dA.y == 0) dA.y = 0.001;
31              if (dB.y == 0) dB.y = 0.001;
32              double tB = ( (sA.y - sB.y) / dB.y ) + ( ( dA.y * (sB.x - sA.x) ) / (dA.x * dB.y) );
33              tB = tB / (1 - ((dB.x * dA.y)/(dA.x * dB.y)) );
34              double tA = ( sB.x - sA.x + (tB*dB.x) );
35              tA = tA / dA.x;
36              double pointX = sA.x + tA * dA.x;
37              double pointY = sA.y + tA * dA.y;
38              
39              result = new Vector2d(pointX, pointY);            
40              switch (typeA) {
41                  case HALF_LINE: if (tA < 0) result = null;
42                      break;
43                  case ABSCISSA: if (tA < 0 || tA > lengthA) result = null;
44                      break;
45              }
46              switch (typeB) {
47                  case HALF_LINE: if (tB < 0) result = null;
48                      break;
49                  case ABSCISSA: if (tB < 0 || tB > lengthB) result = null;
50                      break;
51              }
52          }
53          return result;
54      }
55  
56      /**Gets the intersection of the half-lines A and B (line has the start point and its direction.
57       * If there isn't the intersection of the half-lines, or the direction is the same, we return null.*/
58      public static Vector2d getIntersectionOld(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB) {
59          Vector2d result = null;
60          dA.normalize();
61          dB.normalize();
62          if (!dA.equals(dB)) {
63              if (dA.x == 0) dA.x = 0.001;
64              if (dB.x == 0) dB.x = 0.001;
65              if (dA.y == 0) dA.y = 0.001;
66              if (dB.y == 0) dB.y = 0.001;
67              double tB = ( (sA.y - sB.y) / dB.y ) + ( ( dA.y * (sB.x - sA.x) ) / (dA.x * dB.y) );
68              tB = tB / (1 - ((dB.x * dA.y)/(dA.x * dB.y)) );
69              double tA = ( sB.x - sA.x + (tB*dB.x) );
70              tA = tA / dA.x;
71              double pointX = sA.x + tA * dA.x;
72              double pointY = sA.y + tA * dA.y;
73              if (tA >= 0 && tB >= 0) {    //The intersection of the lines lies also on the half-lines.
74                  result = new Vector2d(pointX, pointY);
75              }
76          }
77          return result;
78      }
79  
80  
81      /**Gets the intersection of the half-lines A and B (line has the start point and its direction.
82       * If there isn't the intersection of the half-lines, or the direction is the same, we return null.*/
83      public static boolean haveSameDirection(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB) {        
84          dA.normalize();
85          dB.normalize();
86          if (dA.equals(dB)) {
87              return true;
88          } else {
89              return false;
90          }
91      }
92  
93      /* Computes the nearest point of the line segment to the pointP.*/
94      public static Vector2d getNearestPoint(Vector2d start, Vector2d end, Vector2d pointP, boolean justAbscissa) {
95  
96          //Now we need an equation for the line on which the points start and end lie.
97          double a;
98          double b;
99          double c;
100         Vector2d abscissa = new Vector2d(end.x - start.x, end.y - start.y);
101         //Coefficients in the equation are normal vector of tmp.
102         a = abscissa.y;
103         b = -abscissa.x;
104         //start point lies on the line, therefore we can use it to get c.
105         c = -a * start.x - b * start.y;
106 
107         //Special cases solving.
108         if (a == 0) {
109             a = 0.001; //In case something messes up and a ends up being zero, we need to fix it, otherwise we'd divide by zero later.
110         }
111         if (a * a + b * b == 0) {
112             a = a + 0.001; //Similar for a^2+b^2==0
113         }
114 
115         //Coefficients of the equation for the line perpendicular to our line are -b, a, d; d will be counted. PointHeading lies on it, therefore we use its coordinates.
116         double d = b * pointP.x - a * pointP.y;
117 
118         //Now we have to get the intersection of linex ax+by+c=0 and -bx+ ay+d=0, the foot point.
119         //This is a general solution of system of equations consisting of ax+by+c=0 and -bx+ay+d=0.
120         //We could use some Gaussian solver as well, but since we don't need to solve general systems of equations, this should be faster.
121         double footXCor = (b * ((a * d + b * c) / (a * a + b * b)) - c) / a;
122         double footYCor = (-a * d - b * c) / (a * a + b * b);
123 
124         /** The nearest point on the line to the pointP.*/
125         Vector2d foot = new Vector2d(footXCor, footYCor);
126 
127         /*The point in the middle of the abscissa.*/
128         Vector2d middlePoint = new Vector2d((start.x + end.x) / 2,(start.y + end.y) / 2);
129 
130         Vector2d footToMiddlePoint = new Vector2d(foot.x - middlePoint.x, foot.y - middlePoint.y);
131 
132         /** The nearest point of the abscissa to the pointP.*/
133         Vector2d nearestPoint = new Vector2d(foot.x, foot.y);
134 
135         if (justAbscissa) {
136             /* The foot point doesn't lie between start and end. Therefore we will choose start or end point - that which is nearer to the pointP.*/
137             if (footToMiddlePoint.length() > abscissa.length()) {
138                 Vector2d startToPointP = new Vector2d(start.x - pointP.x,start.y - pointP.y);
139                 Vector2d endToPointP = new Vector2d(end.x - pointP.x,end.y - pointP.y);
140                 if (startToPointP.length() < endToPointP.length()) {
141                     nearestPoint = start;
142                 } else {
143                     nearestPoint = end;
144                 }
145             }
146         }
147         return nearestPoint;
148     }
149 
150     public static boolean pointIsLeftFromTheVector(Vector3d vector, Vector3d point) {
151         double a = vector.x;
152         double b = vector.y;
153         //if (SteeringManager.DEBUG) System.out.println("Rovnice "+b+"*"+point.x+" - "+a+"*"+point.y+" = "+(b*point.x - a*point.y));
154         return b*point.x - a*point.y  >= 0; //Equation of the half-plane is b*x - a*y <= 0. That means that the point is on the left side of the vector.
155     }
156 
157     /**Returns the rotation vector, that after combining with the actualVelocity the vector on the left or right side will be created.*/
158     public static Vector3d getTurningVector(Vector3d actualVelocity, boolean left) {
159         Vector3d turningVector;
160         if (left)
161             turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);   //Turns 45° left.
162         else
163             turningVector = new Vector3d(-actualVelocity.y, actualVelocity.x, 0);   //Turns 45° right.
164         turningVector.scale(1 / (Math.sqrt(2)));
165         Vector3d negativeVector = new Vector3d(-actualVelocity.x, -actualVelocity.y, 0);
166         negativeVector.scale(1 - 1 / Math.sqrt(2));
167         turningVector.add((Tuple3d) negativeVector);
168         return turningVector;
169     }
170 
171     /**Returns the rotation vector perpendicular to the actualVelocity.*/
172     public static Vector3d getTurningVector2(Vector3d actualVelocity, boolean left) {
173         Vector3d turningVector;
174         if (left)
175             turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);   //Turns 45° left.
176         else
177             turningVector = new Vector3d(-actualVelocity.y, actualVelocity.x, 0);   //Turns 45° right.
178         return turningVector;
179     }
180 
181     public static double radiansToDegrees(double rad) {
182         return ((180*rad / Math.PI) % 360);
183     }
184 
185     public static double degreesToRadians(double deg) {
186         return ( Math.PI*deg / 180);
187     }
188 
189     /**
190      * @return all points of intersection between circles
191      * @param P0 center of first circle
192      * @param r0 radius of first circle
193      * @param P1
194      * @param r1
195 
196      */
197     public static Location[] commonPoints(Location P0, double r0, Location P1, double r1) {
198         Location[] result = new Location[2];
199         result[0] = null;
200         result[1] = null;
201 
202         int d = (int) P1.getDistance2D(P0);
203         // no commonpoints
204         if (d > r0 + r1 || d < Math.abs(r0 - r1)) {
205             return result;
206         }
207 
208         double a = (double) (r0 * r0 - r1 * r1 + d * d) / (double) (2 * d);
209         Location P2 = P0.add((P1.sub(P0)).scale(a / d));
210 
211         double h = Math.sqrt(r0 * r0 - a * a);
212 
213         int x3 = (int) (P2.x - h * (P1.y - P0.y) / d);
214         int y3 = (int) (P2.y + h * (P1.x - P0.x) / d);
215 
216         int x32 = (int) (P2.x + h * (P1.y - P0.y) / d);
217         int y32 = (int) (P2.y - h * (P1.x - P0.x) / d);
218         result[0] = new Location(x3, y3);
219         result[1] = new Location(x32, y32);
220         return result;
221     }
222     @Deprecated
223     public static double getAngleOld(Location botLocation, Location focus, Location point)
224     {
225         
226         Vector2d foot = getNearestPoint(new Vector2d(botLocation.x,botLocation.y),new Vector2d(focus.x,focus.y),new Vector2d(point.x,point.y),false);
227         Location footL = new Location(foot.x,foot.y,botLocation.z);
228         Location pointZ = new Location(point.x,point.y,botLocation.z);
229         
230         
231         
232         
233         return Math.asin(pointZ.sub(footL).getLength() / pointZ.sub(botLocation).getLength());
234         
235     }
236     public static double getAngle(Location botLocation, Rotation botRotation, Location point)
237     {
238         Location moveVec = point.sub(botLocation).getNormalized();
239         
240         return Math.acos(botRotation.toLocation().getNormalized().dot2D(moveVec));
241         
242     }
243 
244 }