47 lines
1.4 KiB
C++
47 lines
1.4 KiB
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 radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
|
|
vec2<long> &step) {
|
|
step.x = angle.x * stepRatio.x;
|
|
step.y = angle.y * stepRatio.y;
|
|
}
|
|
|
|
void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
|
|
vec2<double> &angle) {
|
|
angle.x = step.x / stepRatio.x;
|
|
angle.y = step.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);
|
|
|
|
radToStep(stepRatio, angle, step);
|
|
}
|
|
|
|
void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
|
|
vec3<long> current, vec2<long> zero, vec2<long> step,
|
|
vec3<double> &position) {
|
|
|
|
vec2<double> angle;
|
|
stepToRad(stepRatio, step, angle);
|
|
|
|
vec2<double> dXYZ(tan(angle.x) * current.z, tan(angle.y) * current.z);
|
|
|
|
position.x = dXYZ.x - offset.x;
|
|
position.y = -(dXYZ.y - offset.y); // natural axis direction
|
|
position.z = current.z - offset.z;
|
|
}
|