1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302

/*! Call all functions with a Vector as argument the vector should contain:
     - the end point as origin
     - the end angle as angle in degrees in clockwise direction (eg. 0° facing north, 90° facing east, ...)
     - the circle radius as magnitude

    Start Vector is in the origin facing in positive x-direction

    Every struct defined here is 2 dimensional and uses f64 */

/// Point
pub struct Point {
    pub x: f64,
    pub y: f64,
}

/// Vector with origin, angle and magnitude
pub struct Vector {
    pub origin: Point,
    pub angle: f64,
    pub magnitude: f64,
}

/// Circle
pub struct Circle {
    pub center: Point,
    pub radius: f64,
}

/// Circle route with a circle and a angle for how long to drive on this circle
pub struct CircleRoute {
    pub circle: Circle,
    pub angle: f64,
}

/// Route with a start Circle, a tangent straight and a end Circle (eg. rsl, rsr, lsr, lsl)
pub struct RouteCSC {
    pub start: CircleRoute,
    pub tangent: Vector,
    pub end: CircleRoute,
}

/// Route with 3 Circles (eg. rlr, lrl) (not yet implemented)
#[allow(unused)]
pub struct RouteCCC {
    pub start: Circle,
    pub middle: Circle,
    pub end: Circle,
}


/// right straight right route
pub fn rsr(end: Vector) -> Result <RouteCSC, ()> {
    let mut route_csc = RouteCSC {
        start: CircleRoute { circle: Circle { center: Point {x: end.magnitude, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
        tangent: Vector { origin: Point { x: 0.0, y: 0.0 }, angle: 0.0, magnitude: 0.0},
        end: CircleRoute { circle: Circle { center: Point {x: 0.0, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
    };

    // get the center point by adding the end vector to the end point
    // this works because the argument is the angle in positive y direction
    // not positive x direction so we dont have to rotate it here anymore
    // the angle has to be counter clockwise though (thats why 360 - end.angle)
    route_csc.end.circle.center = Point {
        x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(),
        y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin()
    };

    // get the tangent pitch which is the same as the pitch between the two
    // circle centers since our circles have the same radius
    route_csc.tangent.angle = ((route_csc.end.circle.center.y - route_csc.start.circle.center.y) /
                               (route_csc.end.circle.center.x - route_csc.start.circle.center.x)).atan().to_degrees();

    // if the end circle center x value is smaller than the
    // start circle center x value
    // the angle would be 180° rotated so to prevent that:
    if route_csc.end.circle.center.x < route_csc.start.circle.center.x { route_csc.tangent.angle += 180.0; }

    // get the tangent magnitude this, again, is the same as the distance
    // between the two circle centers since our circles have the same radius
    route_csc.tangent.magnitude = ( (route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) +
                                    (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0) ).sqrt();

    // get the angle of the start circle
    route_csc.start.angle = 90.0 - route_csc.tangent.angle;

    // make the angle pretty
    if route_csc.start.angle < 0.0 { route_csc.start.angle += 360.0; }
    if route_csc.start.angle >= 360.0 { route_csc.start.angle -= 360.0; }

    // get the tangent origin by moving the vector from the start circle center
    // 90° to it's own direction and the magnitude of the circle radius
    route_csc.tangent.origin = Point{
        x: route_csc.start.circle.center.x + route_csc.start.circle.radius * (180.0-route_csc.start.angle).to_radians().cos(),
        y: route_csc.start.circle.center.y + route_csc.start.circle.radius * (180.0-route_csc.start.angle).to_radians().sin()
    };

    // get the angle of the start circle
    // the angle where we start from the tangent equals the one we finish
    // so we can use that in here
    route_csc.end.angle = end.angle - route_csc.start.angle;

    // make the angle pretty
    if route_csc.end.angle < 0.0 { route_csc.end.angle += 360.0; }
    if route_csc.end.angle >= 360.0 { route_csc.end.angle -= 360.0; }

    Ok(route_csc)
}

/// left straight left route
pub fn lsl(end: Vector) -> Result <RouteCSC, ()> {
    let mut route_csc = RouteCSC {
        start: CircleRoute { circle: Circle { center: Point {x:-end.magnitude, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
        tangent: Vector { origin: Point { x: 0.0, y: 0.0 }, angle: 0.0, magnitude: 0.0},
        end: CircleRoute { circle: Circle { center: Point {x: 0.0, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
    };

    // get the center point by adding the end vector to the end point
    // we have to rotate the vector 180° (90° because the given angle is from the y axis
    // and 90 more to not get the tangent but the vector to the center point)
    // and again we have to use the counter clockwise direction
    route_csc.end.circle.center = Point {
        x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(),
        y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin()
    };

    // get the tangent pitch which is the same as the pitch between the two
    // circle centers since our circles have the same radius
    route_csc.tangent.angle = ((route_csc.end.circle.center.y - route_csc.start.circle.center.y) /
                               (route_csc.end.circle.center.x - route_csc.start.circle.center.x)).atan().to_degrees();

    // if the end circle center x value is smaller than the
    // start circle center x value
    // the angle would be 180° rotated so to prevent that:
    if route_csc.end.circle.center.x < route_csc.start.circle.center.x { route_csc.tangent.angle += 180.0; }

    // make the angle positive
    if route_csc.tangent.angle < 0.0 { route_csc.tangent.angle += 360.0; }

    // get the tangent magnitude this, again, is the same as the distance
    // between the two circle centers since our circles have the same radius
    route_csc.tangent.magnitude = ( (route_csc.end.circle.center.x - route_csc.start.circle.center.x).abs().powf(2.0) +
                                    (route_csc.end.circle.center.y - route_csc.start.circle.center.y).abs().powf(2.0) ).sqrt();

    // get the angle of the start circle
    route_csc.start.angle = route_csc.tangent.angle - 90.0;

    // make the angle pretty
    if route_csc.start.angle < 0.0 { route_csc.start.angle += 360.0; }
    if route_csc.start.angle >= 360.0 { route_csc.start.angle -= 360.0; }

    // get the tangent origin by moving the vector from the start circle center
    // 90° to it's own direction and the magnitude of the circle radius
    route_csc.tangent.origin = Point{
        x: route_csc.start.circle.center.x + route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(),
        y: route_csc.start.circle.center.y + route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin()
    };


    // get the angle of the start circle
    // the angle where we start from the tangent equals the one we finish
    // so we can use that in here
    route_csc.end.angle = end.angle - route_csc.start.angle;

    // make the angle pretty
    if route_csc.end.angle < 0.0 { route_csc.end.angle += 360.0; }
    if route_csc.end.angle >= 360.0 { route_csc.end.angle -= 360.0; }

    Ok(route_csc)
}

/// right straight left route
pub fn rsl(end: Vector) -> Result <RouteCSC, ()> {
    let mut route_csc = RouteCSC {
        start: CircleRoute { circle: Circle { center: Point {x: end.magnitude, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
        tangent: Vector { origin: Point { x: 0.0, y: 0.0 }, angle: 0.0, magnitude: 0.0},
        end: CircleRoute { circle: Circle { center: Point {x: 0.0, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
    };

    // get the center point by adding the end vector to the end point
    // we have to rotate the vector 180° (90° because the given angle is from the y axis
    // and 90 more to not get the tangent but the vector to the center point)
    // and again we have to use the counter clockwise direction
    route_csc.end.circle.center = Point {
        x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(),
        y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin()
    };

    // check if inside tangent can even be constructed
    if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0)).sqrt()
        < 2.0 * end.magnitude {
            return Err(());
    }

    // get the tangent length via some simple trigonometry
    route_csc.tangent.magnitude = (( route_csc.end.circle.center.x - route_csc.start.circle.center.x ).powf(2.0) +
                                   ( route_csc.end.circle.center.y - route_csc.start.circle.center.y ).powf(2.0) - ( 2.0 * end.magnitude ).powf(2.0)).sqrt();

    // tangent middle is the same as the middle of the straight from the center of the start
    let tangent_middle = Point {
        x: (route_csc.end.circle.center.x + route_csc.start.circle.center.x) / 2.0,
        y: (route_csc.end.circle.center.y + route_csc.start.circle.center.y) / 2.0
    };

    // get the tangent angle
    route_csc.tangent.angle = ((route_csc.end.circle.center.y - tangent_middle.y)/(route_csc.end.circle.center.x - tangent_middle.x)).atan().to_degrees() -
                               ( 2.0 * end.magnitude / route_csc.tangent.magnitude).atan().to_degrees();

    // if the end circle center x value is smaller than the
    // start circle center x value
    // the angle would be 180° rotated so to prevent that:
    if route_csc.end.circle.center.x < route_csc.start.circle.center.x { route_csc.tangent.angle += 180.0; }

    // get the angle of the start circle
    route_csc.start.angle = 90.0 - route_csc.tangent.angle;

    // make the angle pretty
    if route_csc.start.angle < 0.0 { route_csc.start.angle += 360.0; }
    if route_csc.start.angle >= 360.0 { route_csc.start.angle -= 360.0; }

    // get the tangent origin by moving the vector from the start circle center
    // along its right angle vector
    route_csc.tangent.origin = Point{
        x: route_csc.start.circle.center.x + route_csc.start.circle.radius * (180.0-route_csc.start.angle).to_radians().cos(),
        y: route_csc.start.circle.center.y + route_csc.start.circle.radius * (180.0-route_csc.start.angle).to_radians().sin()
    };

    // get the angle of the end circle
    route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle;

    // make the angle pretty
    if route_csc.end.angle < 0.0 { route_csc.end.angle += 360.0; }
    if route_csc.end.angle >= 360.0 { route_csc.end.angle -= 360.0; }

    Ok(route_csc)
}

/// left straight right route
pub fn lsr(end: Vector) -> Result <RouteCSC, ()> {
    let mut route_csc = RouteCSC {
        start: CircleRoute { circle: Circle { center: Point {x: -end.magnitude, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
        tangent: Vector { origin: Point { x: 0.0, y: 0.0 }, angle: 0.0, magnitude: 0.0},
        end: CircleRoute { circle: Circle { center: Point {x: 0.0, y: 0.0}, radius: end.magnitude}, angle: 0.0 },
    };

    // get the center point by adding the end vector to the end point
    // this works because the argument is the angle in positive y direction
    // not positive x direction so we dont have to rotate it here anymore
    // the angle has to be counter clockwise though (thats why 360 - end.angle)
    route_csc.end.circle.center = Point {
        x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(),
        y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin()
    };

    // check if inside tangent can even be constructed
    if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0)).sqrt()
        < 2.0 * end.magnitude {
            return Err(());
    }

    // get the tangent length via some simple trigonometry
    route_csc.tangent.magnitude = (( route_csc.end.circle.center.x - route_csc.start.circle.center.x ).powf(2.0) +
                                   ( route_csc.end.circle.center.y - route_csc.start.circle.center.y ).powf(2.0) - ( 2.0 * end.magnitude ).powf(2.0)).sqrt();

    // tangent middle is the same as the middle of the straight from the center of the start
    let tangent_middle = Point {
        x: (route_csc.end.circle.center.x + route_csc.start.circle.center.x) / 2.0,
        y: (route_csc.end.circle.center.y + route_csc.start.circle.center.y) / 2.0
    };

    // get the tangent angle
    route_csc.tangent.angle = ((route_csc.end.circle.center.y - tangent_middle.y)/(route_csc.end.circle.center.x - tangent_middle.x)).atan().to_degrees() +
                               ( 2.0 * end.magnitude / route_csc.tangent.magnitude).atan().to_degrees();

    // if the end circle center x value is smaller than the
    // start circle center x value
    // the angle would be 180° rotated so to prevent that:
    if route_csc.end.circle.center.x < route_csc.start.circle.center.x { route_csc.tangent.angle += 180.0; }

    // get the angle of the start circle
    route_csc.start.angle = route_csc.tangent.angle - 90.0;

    // make the angle pretty
    if route_csc.start.angle < 0.0 { route_csc.start.angle += 360.0; }
    if route_csc.start.angle >= 360.0 { route_csc.start.angle -= 360.0; }

    // get the tangent origin by moving the vector from the start circle center
    // 90° to it's own direction and the magnitude of the circle radius
    route_csc.tangent.origin = Point{
        x: route_csc.start.circle.center.x + route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(),
        y: route_csc.start.circle.center.y + route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin()
    };

    // get the angle of the end circle
    route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle;

    // make the angle pretty
    if route_csc.end.angle < 0.0 { route_csc.end.angle += 360.0; }
    if route_csc.end.angle >= 360.0 {route_csc.end.angle -= 360.0; }

    Ok(route_csc)
}