27 lines
832 B
C++
27 lines
832 B
C++
#include "math.h"
|
|
#include "turret.h"
|
|
|
|
double degToRad(double deg) { return deg * M_PI / 180.0; }
|
|
|
|
double radToDeg(double rad) { return rad * 180.0 / M_PI; }
|
|
|
|
void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle,
|
|
vec2<long> &step) {
|
|
step.x = angle.x * stepRatio.x;
|
|
step.y = angle.y * stepRatio.y;
|
|
}
|
|
|
|
void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
|
|
vec2<long> zero, vec3<double> position,
|
|
vec2<long> &step) {
|
|
// x = -x; // natural axis direction
|
|
position.y = -position.y; // natural axis direction
|
|
double dX = position.x + offset.x;
|
|
double dY = position.y + offset.y;
|
|
double dZ = position.z + offset.z;
|
|
|
|
vec2<double> angle(atan2(dZ, dY) - M_PI / 2, atan2(dZ, dX) - M_PI / 2);
|
|
|
|
angleToStep(stepRatio, angle, step);
|
|
}
|