41 #ifndef SPATIAL_MATH_H_
42 #define SPATIAL_MATH_H_
49 const double PI = 3.141592;
51 void _rotate(
double* plane,
double angle);
53 template<
typename GPType>
54 std::vector<GPType> calculateDisplacement(
int dimCount,
int unitDimension,
double scale,
55 const std::vector<double>& anglesInRadians) {
57 std::vector<double> displacement(dimCount, 0);
58 displacement[unitDimension] = 1;
59 double tmp[] = { 0, 0 };
61 for (
int i = 0; i < dimCount; i++) {
62 if (i == unitDimension) {
64 }
else if (i > unitDimension) {
65 tmp[0] = displacement[unitDimension];
66 tmp[1] = displacement[i];
67 _rotate(tmp, anglesInRadians[c]);
68 displacement[unitDimension] = tmp[0];
69 displacement[i] = tmp[1];
70 }
else if (i < unitDimension) {
71 tmp[0] = displacement[i];
72 tmp[1] = displacement[unitDimension];
73 _rotate(tmp, anglesInRadians[c]);
74 displacement[unitDimension] = tmp[1];
75 displacement[i] = tmp[0];
79 for (
size_t i = 0; i < displacement.size(); ++i) {
80 displacement[i] = displacement[i] * scale;
86 std::vector<int> calculateDisplacement<int> (
int dimCount,
int unitDimension,
double scale,
const std::vector<double>& anglesInRadians);
95 double toRadians(
double angdeg);
104 double toDegrees(
double angrad);