|
@@ -81,8 +81,8 @@ void vector_3::apply_rotation(matrix_3x3 matrix) {
|
81
|
81
|
z = resultZ;
|
82
|
82
|
}
|
83
|
83
|
|
84
|
|
-void vector_3::debug(const char title[]) {
|
85
|
|
- SERIAL_PROTOCOL(title);
|
|
84
|
+void vector_3::debug(const char * const title) {
|
|
85
|
+ serialprintPGM(title);
|
86
|
86
|
SERIAL_PROTOCOLPGM(" x: ");
|
87
|
87
|
SERIAL_PROTOCOL_F(x, 6);
|
88
|
88
|
SERIAL_PROTOCOLPGM(" y: ");
|
|
@@ -101,14 +101,14 @@ void apply_rotation_xyz(matrix_3x3 matrix, float &x, float &y, float &z) {
|
101
|
101
|
}
|
102
|
102
|
|
103
|
103
|
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
|
104
|
|
- //row_0.debug("row_0");
|
105
|
|
- //row_1.debug("row_1");
|
106
|
|
- //row_2.debug("row_2");
|
|
104
|
+ //row_0.debug(PSTR("row_0"));
|
|
105
|
+ //row_1.debug(PSTR("row_1"));
|
|
106
|
+ //row_2.debug(PSTR("row_2"));
|
107
|
107
|
matrix_3x3 new_matrix;
|
108
|
108
|
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
|
109
|
109
|
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
|
110
|
110
|
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
|
111
|
|
- //new_matrix.debug("new_matrix");
|
|
111
|
+ //new_matrix.debug(PSTR("new_matrix"));
|
112
|
112
|
return new_matrix;
|
113
|
113
|
}
|
114
|
114
|
|
|
@@ -123,14 +123,14 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
|
123
|
123
|
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
|
124
|
124
|
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
|
125
|
125
|
|
126
|
|
- // x_row.debug("x_row");
|
127
|
|
- // y_row.debug("y_row");
|
128
|
|
- // z_row.debug("z_row");
|
|
126
|
+ // x_row.debug(PSTR("x_row"));
|
|
127
|
+ // y_row.debug(PSTR("y_row"));
|
|
128
|
+ // z_row.debug(PSTR("z_row"));
|
129
|
129
|
|
130
|
130
|
// create the matrix already correctly transposed
|
131
|
131
|
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
|
132
|
132
|
|
133
|
|
- // rot.debug("rot");
|
|
133
|
+ // rot.debug(PSTR("rot"));
|
134
|
134
|
return rot;
|
135
|
135
|
}
|
136
|
136
|
|
|
@@ -142,8 +142,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
|
142
|
142
|
return new_matrix;
|
143
|
143
|
}
|
144
|
144
|
|
145
|
|
-void matrix_3x3::debug(const char title[]) {
|
146
|
|
- SERIAL_PROTOCOLLN(title);
|
|
145
|
+void matrix_3x3::debug(const char * const title) {
|
|
146
|
+ serialprintPGM(title);
|
147
|
147
|
uint8_t count = 0;
|
148
|
148
|
for (uint8_t i = 0; i < 3; i++) {
|
149
|
149
|
for (uint8_t j = 0; j < 3; j++) {
|