1 package SteeringStuff;
2
3 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
4 import javax.vecmath.Tuple3d;
5 import javax.vecmath.Vector2d;
6 import javax.vecmath.Vector3d;
7
8
9
10
11
12 public class SteeringTools {
13
14 public enum LineType {STRAIGHT_LINE, HALF_LINE, ABSCISSA};
15
16
17
18
19
20 public static Vector2d getIntersection(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB, LineType typeA, LineType typeB) {
21 Vector2d result = null;
22 double lengthA = dA.length();
23 double lengthB = dB.length();
24 dA.normalize();
25 dB.normalize();
26 if (!dA.equals(dB)) {
27 if (dA.x == 0) dA.x = 0.001;
28 if (dB.x == 0) dB.x = 0.001;
29 if (dA.y == 0) dA.y = 0.001;
30 if (dB.y == 0) dB.y = 0.001;
31 double tB = ( (sA.y - sB.y) / dB.y ) + ( ( dA.y * (sB.x - sA.x) ) / (dA.x * dB.y) );
32 tB = tB / (1 - ((dB.x * dA.y)/(dA.x * dB.y)) );
33 double tA = ( sB.x - sA.x + (tB*dB.x) );
34 tA = tA / dA.x;
35 double pointX = sA.x + tA * dA.x;
36 double pointY = sA.y + tA * dA.y;
37
38 result = new Vector2d(pointX, pointY);
39 switch (typeA) {
40 case HALF_LINE: if (tA < 0) result = null;
41 break;
42 case ABSCISSA: if (tA < 0 || tA > lengthA) result = null;
43 break;
44 }
45 switch (typeB) {
46 case HALF_LINE: if (tB < 0) result = null;
47 break;
48 case ABSCISSA: if (tB < 0 || tB > lengthB) result = null;
49 break;
50 }
51 }
52 return result;
53 }
54
55
56
57 public static Vector2d getIntersectionOld(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB) {
58 Vector2d result = null;
59 dA.normalize();
60 dB.normalize();
61 if (!dA.equals(dB)) {
62 if (dA.x == 0) dA.x = 0.001;
63 if (dB.x == 0) dB.x = 0.001;
64 if (dA.y == 0) dA.y = 0.001;
65 if (dB.y == 0) dB.y = 0.001;
66 double tB = ( (sA.y - sB.y) / dB.y ) + ( ( dA.y * (sB.x - sA.x) ) / (dA.x * dB.y) );
67 tB = tB / (1 - ((dB.x * dA.y)/(dA.x * dB.y)) );
68 double tA = ( sB.x - sA.x + (tB*dB.x) );
69 tA = tA / dA.x;
70 double pointX = sA.x + tA * dA.x;
71 double pointY = sA.y + tA * dA.y;
72 if (tA >= 0 && tB >= 0) {
73 result = new Vector2d(pointX, pointY);
74 }
75 }
76 return result;
77 }
78
79
80
81
82 public static boolean haveSameDirection(Vector2d sA, Vector2d dA, Vector2d sB, Vector2d dB) {
83 dA.normalize();
84 dB.normalize();
85 if (dA.equals(dB)) {
86 return true;
87 } else {
88 return false;
89 }
90 }
91
92
93 public static Vector2d getNearestPoint(Vector2d start, Vector2d end, Vector2d pointP, boolean justAbscissa) {
94
95
96 double a;
97 double b;
98 double c;
99 Vector2d abscissa = new Vector2d(end.x - start.x, end.y - start.y);
100
101 a = abscissa.y;
102 b = -abscissa.x;
103
104 c = -a * start.x - b * start.y;
105
106
107 if (a == 0) {
108 a = 0.001;
109 }
110 if (a * a + b * b == 0) {
111 a = a + 0.001;
112 }
113
114
115 double d = b * pointP.x - a * pointP.y;
116
117
118
119
120 double footXCor = (b * ((a * d + b * c) / (a * a + b * b)) - c) / a;
121 double footYCor = (-a * d - b * c) / (a * a + b * b);
122
123
124 Vector2d foot = new Vector2d(footXCor, footYCor);
125
126
127 Vector2d middlePoint = new Vector2d((start.x + end.x) / 2,(start.y + end.y) / 2);
128
129 Vector2d footToMiddlePoint = new Vector2d(foot.x - middlePoint.x, foot.y - middlePoint.y);
130
131
132 Vector2d nearestPoint = new Vector2d(foot.x, foot.y);
133
134 if (justAbscissa) {
135
136 if (footToMiddlePoint.length() > abscissa.length()) {
137 Vector2d startToPointP = new Vector2d(start.x - pointP.x,start.y - pointP.y);
138 Vector2d endToPointP = new Vector2d(end.x - pointP.x,end.y - pointP.y);
139 if (startToPointP.length() < endToPointP.length()) {
140 nearestPoint = start;
141 } else {
142 nearestPoint = end;
143 }
144 }
145 }
146 return nearestPoint;
147 }
148
149 public static boolean pointIsLeftFromTheVector(Vector3d vector, Vector3d point) {
150 double a = vector.x;
151 double b = vector.y;
152
153 return b*point.x - a*point.y >= 0;
154 }
155
156
157 public static Vector3d getTurningVector(Vector3d actualVelocity, boolean left) {
158 Vector3d turningVector;
159 if (left)
160 turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);
161 else
162 turningVector = new Vector3d(-actualVelocity.y, actualVelocity.x, 0);
163 turningVector.scale(1 / (Math.sqrt(2)));
164 Vector3d negativeVector = new Vector3d(-actualVelocity.x, -actualVelocity.y, 0);
165 negativeVector.scale(1 - 1 / Math.sqrt(2));
166 turningVector.add((Tuple3d) negativeVector);
167 return turningVector;
168 }
169
170
171 public static Vector3d getTurningVector2(Vector3d actualVelocity, boolean left) {
172 Vector3d turningVector;
173 if (left)
174 turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);
175 else
176 turningVector = new Vector3d(-actualVelocity.y, actualVelocity.x, 0);
177 return turningVector;
178 }
179
180 public static double radiansToDegrees(double rad) {
181 return ((180*rad / Math.PI) % 360);
182 }
183
184 public static double degreesToRadians(double deg) {
185 return ( Math.PI*deg / 180);
186 }
187
188
189
190
191
192
193
194
195
196 public static Location[] commonPoints(Location P0, double r0, Location P1, double r1) {
197 Location[] result = new Location[2];
198 result[0] = null;
199 result[1] = null;
200
201 int d = (int) P1.getDistance2D(P0);
202
203 if (d > r0 + r1 || d < Math.abs(r0 - r1)) {
204 return result;
205 }
206
207 double a = (double) (r0 * r0 - r1 * r1 + d * d) / (double) (2 * d);
208 Location P2 = P0.add((P1.sub(P0)).scale(a / d));
209
210 double h = Math.sqrt(r0 * r0 - a * a);
211
212 int x3 = (int) (P2.x - h * (P1.y - P0.y) / d);
213 int y3 = (int) (P2.y + h * (P1.x - P0.x) / d);
214
215 int x32 = (int) (P2.x + h * (P1.y - P0.y) / d);
216 int y32 = (int) (P2.y - h * (P1.x - P0.x) / d);
217 result[0] = new Location(x3, y3);
218 result[1] = new Location(x32, y32);
219 return result;
220 }
221 }