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)23RelPolarCoord::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)29RelCartCoord::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) const35RelCartCoord 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)45float dist(RelPolarCoord c) 46 { return c.dist; } angle(RelPolarCoord c)47Angle 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)52float 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