1 /*
2  * Kuklomenos
3  * Copyright (C) 2008-2009 Martin Bays <mbays@sdf.lonestar.org>
4  *
5  * This program is free software: you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation, either version 3 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program.  If not, see http://www.gnu.org/licenses/.
17  */
18 
19 #include <cmath>
20 
21 #include "coords.h"
22 
RelPolarCoord(const RelCartCoord & c)23 RelPolarCoord::RelPolarCoord(const RelCartCoord& c)
24 {
25     dist = sqrt(c.dx*c.dx + c.dy*c.dy);
26     angle = atan2(-c.dx, c.dy)/(PI/2);
27 }
28 
RelCartCoord(const RelPolarCoord & rp)29 RelCartCoord::RelCartCoord(const RelPolarCoord &rp)
30 {
31     dx = -rp.dist*sin(rp.angle * (PI/2));
32     dy = rp.dist*cos(rp.angle * (PI/2));
33 }
34 
rotated(float angle) const35 RelCartCoord RelCartCoord::rotated(float angle) const
36 {
37     if (angle == 0)
38 	return *this;
39 
40     RelPolarCoord p(*this);
41     p.angle += angle;
42     return p;
43 }
44 
dist(RelPolarCoord c)45 float dist(RelPolarCoord c)
46 { return c.dist; }
angle(RelPolarCoord c)47 Angle angle(RelPolarCoord c)
48 { return c.angle; }
49 
50 // angleDiff: returns signed difference between the angles, i.e. angle of
51 // shortest rotation to turn from 'a1' to 'a2'. Return value is in (-2,2].
angleDiff(Angle a1,Angle a2)52 float angleDiff(Angle a1, Angle a2)
53 {
54     Angle copy = a2;
55     a2 -= a1;
56     float a = a2;
57 
58     if (a > 2)
59 	return a-4;
60     else
61 	return a;
62 }
63