Skip to content

Commit

Permalink
tetra2pos: fix relative mode and extend help
Browse files Browse the repository at this point in the history 
closes #5
  • Loading branch information
@ben-wes
ben-wes committed Jan 2, 2025
1 parent 8891592 commit d9c6f16
Show file tree
Hide file tree
Showing 2 changed files with 204 additions and 87 deletions.
189 changes: 140 additions & 49 deletions tetra2pos-help.pd
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
#N canvas 936 167 858 675 10;
#X obj 78 553 tetra2pos;
#X msg 354 508 5838.2 6506.4 6191.4 5890.7;
#X text 535 507 distances for point at (2 \, 5 \, 3) in meters;
#X text 559 570 1 Front: [0.0 \, 408.2 \, 0.0] \; 2 Left: [-500.0 \, -204.1 \, 0.0] \; 3 Right: [500.0 \, -204.1 \, 0.0] \; 4 Top: [0.0 \, 0 \, 612.4];
#X listbox 78 603 39 0 0 0 - - - 0;
#N canvas 194 225 1120 823 10;
#X text 75 729 1 Front: [0.0 \, 408.2 \, 0.0] \; 2 Left: [-500.0 \, -204.1 \, 0.0] \; 3 Right: [500.0 \, -204.1 \, 0.0] \; 4 Top: [0.0 \, 0 \, 612.4];
#X listbox 78 683 39 0 0 0 - - - 0;
#X obj 78 52 vsl 16 136 -10000 10000 0 0 empty empty empty 0 -9 0 10 #fcfcfc #000000 #000000 0 1;
#X text 48 41 10m;
#X text 43 183 -10m;
Expand All @@ -16,50 +13,144 @@
#X floatatom 78 196 7 0 0 0 - - - 0;
#X floatatom 128 196 7 0 0 0 - - - 0;
#X floatatom 178 196 7 0 0 0 - - - 0;
#X obj 78 431 unpack f f f;
#X obj 78 436 unpack f f f;
#X obj 78 452 expr sqrt(pow($f1 \, 2) + pow($f2-408.2 \, 2) + pow($f3 \, 2));
#X obj 147 305 unpack f f f;
#X obj 78 487 pack f f f f;
#X text 142 554 <-- edge length via arg (default is 1000mm);
#X obj 147 310 unpack f f f;
#X obj 78 517 pack f f f f;
#X text 142 632 <-- edge length via arg (default is 1000mm);
#X obj 147 326 expr sqrt(pow($f1 \, 2) + pow($f2 \, 2) + pow($f3-612.4 \, 2));
#X obj 124 347 unpack f f f;
#X obj 124 352 unpack f f f;
#X obj 124 368 expr sqrt(pow($f1-500 \, 2) + pow($f2+204.1 \, 2) + pow($f3 \, 2));
#X obj 101 389 unpack f f f;
#X obj 101 394 unpack f f f;
#X obj 101 410 expr sqrt(pow($f1+500 \, 2) + pow($f2+204.1 \, 2) + pow($f3 \, 2));
#X listbox 78 508 39 0 0 0 - - - 0;
#X text 560 553 mic coordinates for edge length 1000mm;
#X connect 0 0 4 0;
#X connect 1 0 0 0;
#X connect 5 0 14 0;
#X connect 8 0 15 0;
#X connect 9 0 16 0;
#X connect 10 0 12 0;
#X connect 10 1 12 1;
#X connect 11 0 10 0;
#X connect 11 1 12 2;
#X connect 12 0 13 0;
#X connect 13 0 17 0;
#X connect 13 1 25 0;
#X connect 13 2 23 0;
#X connect 13 3 19 0;
#X connect 14 0 12 0;
#X connect 15 0 10 0;
#X connect 16 0 11 0;
#X connect 17 0 18 0;
#X connect 17 1 18 1;
#X connect 17 2 18 2;
#X connect 18 0 20 0;
#X connect 19 0 22 0;
#X connect 19 1 22 1;
#X connect 19 2 22 2;
#X connect 20 0 27 0;
#X connect 22 0 20 3;
#X connect 23 0 24 0;
#X connect 23 1 24 1;
#X connect 23 2 24 2;
#X connect 24 0 20 2;
#X connect 25 0 26 0;
#X connect 25 1 26 1;
#X connect 25 2 26 2;
#X connect 26 0 20 1;
#X connect 27 0 0 0;
#X listbox 78 559 39 0 0 0 - - - 0;
#X text 76 712 mic coordinates for edge length 1000mm;
#X listbox 654 683 39 0 0 0 - - - 0;
#X obj 654 52 vsl 16 136 -10000 10000 0 0 empty empty empty 0 -9 0 10 #fcfcfc #000000 #000000 0 1;
#X text 624 41 10m;
#X text 619 183 -10m;
#X obj 704 52 vsl 16 136 -10000 10000 0 0 empty empty empty 0 -9 0 10 #fcfcfc #000000 #000000 0 1;
#X obj 754 52 vsl 16 136 -10000 10000 0 0 empty empty empty 0 -9 0 10 #fcfcfc #000000 #000000 0 1;
#X obj 704 238 t b f;
#X obj 754 216 t b f;
#X obj 654 262 pack f f f;
#X obj 654 284 t a a a a, f 12;
#X floatatom 654 196 7 0 0 0 - - - 0;
#X floatatom 704 196 7 0 0 0 - - - 0;
#X floatatom 754 196 7 0 0 0 - - - 0;
#X obj 654 436 unpack f f f;
#X obj 654 452 expr sqrt(pow($f1 \, 2) + pow($f2-408.2 \, 2) + pow($f3 \, 2));
#X obj 723 310 unpack f f f;
#X obj 654 538 pack f f f f;
#X obj 723 326 expr sqrt(pow($f1 \, 2) + pow($f2 \, 2) + pow($f3-612.4 \, 2));
#X obj 700 352 unpack f f f;
#X obj 700 368 expr sqrt(pow($f1-500 \, 2) + pow($f2+204.1 \, 2) + pow($f3 \, 2));
#X obj 677 394 unpack f f f;
#X obj 677 410 expr sqrt(pow($f1+500 \, 2) + pow($f2+204.1 \, 2) + pow($f3 \, 2));
#X listbox 654 559 39 0 0 0 - - - 0;
#X obj 654 579 list prepend relative;
#X obj 654 600 list trim;
#X text 838 53 relative distances;
#X obj 661 654 print TDOA result;
#X obj 85 654 print TOA result;
#X obj 654 633 tetra2pos;
#X obj 78 633 tetra2pos;
#X obj 654 517 +;
#X obj 677 517 +;
#X obj 700 517 +;
#X obj 723 517 +;
#X obj 757 481 hsl 136 16 -1000 1000 0 0 empty empty empty -2 -8 0 10 #fcfcfc #000000 #000000 0 1;
#X floatatom 754 502 5 0 0 0 - - - 0;
#X msg 344 222 5838.2 6506.4 6191.4 5890.7;
#X text 344 237 distances for point at (2 \, 5 \, 3) in meters;
#X obj 344 264 unpack f f f f;
#X connect 2 0 11 0;
#X connect 5 0 12 0;
#X connect 6 0 13 0;
#X connect 7 0 9 0;
#X connect 7 1 9 1;
#X connect 8 0 7 0;
#X connect 8 1 9 2;
#X connect 9 0 10 0;
#X connect 10 0 14 0;
#X connect 10 1 22 0;
#X connect 10 2 20 0;
#X connect 10 3 16 0;
#X connect 11 0 9 0;
#X connect 12 0 7 0;
#X connect 13 0 8 0;
#X connect 14 0 15 0;
#X connect 14 1 15 1;
#X connect 14 2 15 2;
#X connect 15 0 17 0;
#X connect 16 0 19 0;
#X connect 16 1 19 1;
#X connect 16 2 19 2;
#X connect 17 0 24 0;
#X connect 19 0 17 3;
#X connect 20 0 21 0;
#X connect 20 1 21 1;
#X connect 20 2 21 2;
#X connect 21 0 17 2;
#X connect 22 0 23 0;
#X connect 22 1 23 1;
#X connect 22 2 23 2;
#X connect 23 0 17 1;
#X connect 24 0 55 0;
#X connect 27 0 36 0;
#X connect 30 0 37 0;
#X connect 31 0 38 0;
#X connect 32 0 34 0;
#X connect 32 1 34 1;
#X connect 33 0 32 0;
#X connect 33 1 34 2;
#X connect 34 0 35 0;
#X connect 35 0 39 0;
#X connect 35 1 46 0;
#X connect 35 2 44 0;
#X connect 35 3 41 0;
#X connect 36 0 34 0;
#X connect 37 0 32 0;
#X connect 38 0 33 0;
#X connect 39 0 40 0;
#X connect 39 1 40 1;
#X connect 39 2 40 2;
#X connect 40 0 56 0;
#X connect 41 0 43 0;
#X connect 41 1 43 1;
#X connect 41 2 43 2;
#X connect 42 0 48 0;
#X connect 43 0 59 0;
#X connect 44 0 45 0;
#X connect 44 1 45 1;
#X connect 44 2 45 2;
#X connect 45 0 58 0;
#X connect 46 0 47 0;
#X connect 46 1 47 1;
#X connect 46 2 47 2;
#X connect 47 0 57 0;
#X connect 48 0 49 0;
#X connect 49 0 50 0;
#X connect 50 0 54 0;
#X connect 54 0 26 0;
#X connect 54 0 52 0;
#X connect 55 0 1 0;
#X connect 55 0 53 0;
#X connect 56 0 42 0;
#X connect 57 0 42 1;
#X connect 58 0 42 2;
#X connect 59 0 42 3;
#X connect 60 0 61 0;
#X connect 61 0 59 1;
#X connect 61 0 58 1;
#X connect 61 0 57 1;
#X connect 61 0 56 1;
#X connect 62 0 64 0;
#X connect 64 0 17 0;
#X connect 64 0 56 0;
#X connect 64 1 17 1;
#X connect 64 1 57 0;
#X connect 64 2 17 2;
#X connect 64 2 58 0;
#X connect 64 3 17 3;
#X connect 64 3 59 0;
102 changes: 64 additions & 38 deletions tetra2pos.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,49 +111,68 @@ static void solve_position_toa(t_float distances[4], t_float positions[4][3], t_
}

static void solve_position_tdoa(t_float distances[4], t_float positions[4][3], t_float result[3]) {
// Step 1: Get the measured differences (these are correct)
t_float tdoa[3];
for (int i = 0; i < 3; i++) {
tdoa[i] = distances[i + 1] - distances[0];
}

// Step 2: Find smallest possible r1 (distance to first mic) that makes
// the geometry possible with our measured differences.
// We know: r2 = r1 + tdoa[0]
// r3 = r1 + tdoa[1]
// r4 = r1 + tdoa[2]
// All these distances must satisfy the tetrahedron geometry

// Start with rough guess - half the edge length
t_float r1 = 0;
t_float real_distances[4];

// Try increasing r1 until we find distances that work
// We'll know they work when TOA gives good result
for (r1 = 100; r1 < 10000; r1 += 10) {
real_distances[0] = r1;
real_distances[1] = r1 + tdoa[0];
real_distances[2] = r1 + tdoa[1];
real_distances[3] = r1 + tdoa[2];

// Try these distances with TOA solver
solve_position_toa(real_distances, positions, result);

// Check if this solution makes sense by verifying distances
t_float error = 0;
for (int i = 0; i < 4; i++) {
t_float dx = result[0] - positions[i][0];
t_float dy = result[1] - positions[i][1];
t_float dz = result[2] - positions[i][2];
t_float calc_dist = sqrt(dx*dx + dy*dy + dz*dz);
error += fabs(calc_dist - real_distances[i]);
t_float min_error = 1e10;
t_float best_result[3] = {0, 0, 0};
t_float calc_distances[4] = {0};

t_float r1_start = 1000; // Start with a reasonable minimum distance
t_float r1_end = 10000; // Some reasonable maximum distance
t_float r1_step = 100; // Initial coarse step

// Two-phase search: first coarse, then fine
for (int phase = 0; phase < 2; phase++) {
for (t_float r1 = r1_start; r1 < r1_end; r1 += r1_step) {
t_float test_distances[4];
test_distances[0] = r1;
test_distances[1] = r1 + tdoa[0];
test_distances[2] = r1 + tdoa[1];
test_distances[3] = r1 + tdoa[2];

t_float test_result[3];
solve_position_toa(test_distances, positions, test_result);

t_float error = 0;
for (int i = 0; i < 4; i++) {
t_float dx = test_result[0] - positions[i][0];
t_float dy = test_result[1] - positions[i][1];
t_float dz = test_result[2] - positions[i][2];
calc_distances[i] = sqrt(dx*dx + dy*dy + dz*dz);
}

for (int i = 0; i < 3; i++) {
t_float calc_tdoa = calc_distances[i+1] - calc_distances[0];
error += fabs(calc_tdoa - tdoa[i]);
}

if (error < min_error) {
min_error = error;
best_result[0] = test_result[0];
best_result[1] = test_result[1];
best_result[2] = test_result[2];

if (phase == 1 && error < 0.1) {
goto search_complete;
}
}
}

// If error is small enough, this is our solution
if (error < 1.0) {
break;
if (phase == 0) {
t_float best_r1 = calc_distances[0];
r1_start = best_r1 - r1_step;
r1_end = best_r1 + r1_step;
r1_step = 1;
}
}

search_complete:
result[0] = best_result[0];
result[1] = best_result[1];
result[2] = best_result[2];
}

static void tetra2pos_relative(t_tetra2pos *x, t_symbol *s, int argc, t_atom *argv) {
Expand All @@ -172,10 +191,17 @@ static void tetra2pos_relative(t_tetra2pos *x, t_symbol *s, int argc, t_atom *ar
t_float position[3];
solve_position_tdoa(distances, x->positions, position);

// Force copy the values to ensure no optimization issues
t_float x_pos = position[0];
t_float y_pos = position[1];
t_float z_pos = position[2];

// Create and output the list
t_atom position_list[3];
SETFLOAT(&position_list[0], position[0]);
SETFLOAT(&position_list[1], position[1]);
SETFLOAT(&position_list[2], position[2]);
SETFLOAT(&position_list[0], x_pos);
SETFLOAT(&position_list[1], y_pos);
SETFLOAT(&position_list[2], z_pos);

outlet_list(x->position_out, &s_list, 3, position_list);

if (x->debug) {
Expand All @@ -186,7 +212,7 @@ static void tetra2pos_relative(t_tetra2pos *x, t_symbol *s, int argc, t_atom *ar
distances[2] - distances[0],
distances[3] - distances[0]);
post("tetra2pos: position (mm): %.1f %.1f %.1f",
position[0], position[1], position[2]);
x_pos, y_pos, z_pos);
}
}

Expand Down

0 comments on commit d9c6f16

Please sign in to comment.