8 double hypot(
double x,
double y) {
10 Eigen::Vector2d vec(x, y);
14 bool polarFromXY(
double &r,
double &theta,
double x,
double y) {
18 if (r < std::numeric_limits<double>::epsilon()) {
28 void xyFromPolar(
double &x,
double &y,
double r,
double theta) {
35 double const c =
cosd(rotAngle);
36 double const mc = 1.0 - c;
37 double const s =
sind(rotAngle);
39 double axisMag = axis.norm();
40 double const ux = axis(0) / axisMag;
41 double const uy = axis(1) / axisMag;
42 double const uz = axis(2) / axisMag;
45 (ux*ux + (1.0 - ux*ux)*c), (ux*uy*mc - uz*s), (ux*uz*mc + uy*s),
46 (ux*uy*mc + uz*s), (uy*uy + (1.0 - uy*uy)*c), (uy*uz*mc - ux*s),
47 (uz*ux*mc - uy*s), (uy*uz*mc + ux*s), (uz*uz + (1.0 - uz*uz)*c);
void xyFromPolar(double &x, double &y, double r, double theta)
double hypot(double x, double y)
double cosd(double ang)
cosine of angle in degrees
bool polarFromXY(double &r, double &theta, double x, double y)
double sind(double ang)
sine of angle in degrees
double atan2d(double x, double y)
arctangent2 in degrees
void computeRotationMatrix(Eigen::Matrix3d &rotMat, Eigen::Vector3d const &axis, double rotAngle)