Subnautica map helper

.gitignore

/draw
/map.svg
/map.txt
/trilateration-cache.json

draw.d

import ae.utils.array;
import ae.utils.geometry;
import ae.utils.xmlbuild;

import std.algorithm.comparison;
import std.algorithm.iteration;
import std.algorithm.searching;
import std.conv;
import std.exception;
import std.file;
import std.format;
import std.math;
import std.range;
import std.stdio;
import std.string;

import trilateration : trilateration;

enum mapRadius = 1_500; // meters
enum mapScale = 1; // meters per pixel
enum gridInterval = 100; // meters

version(unittest_only) {} else
void main()
{
	auto svg = newXml().svg();
	svg.xmlns = "http://www.w3.org/2000/svg";
	svg.viewBox = "%s %s %s %s".format(
		(-mapRadius)    / mapScale,
		(-mapRadius)    / mapScale,
		(mapRadius * 2) / mapScale,
		(mapRadius * 2) / mapScale,
	);

	svg.rect([
		"x"      : text((-mapRadius)    / mapScale),
		"y"      : text((-mapRadius)    / mapScale),
		"width"  : text((mapRadius * 2) / mapScale),
		"height" : text((mapRadius * 2) / mapScale),
		"fill"   : "#000820",
	]);

	void grid(int interval, string color)
	{
		string d;
		foreach (dir; 0..2)
		{
			auto dirCmd = "hv"[dir];
			foreach (x; -mapRadius .. mapRadius+1)
				if (x % interval == 0)
				{
					d ~= format("M %d,%d %s %s ",
						(dir ? x : -mapRadius) / mapScale,
						(dir ? -mapRadius : x) / mapScale,
						dirCmd,
						(mapRadius * 2) / mapScale,
					);
				}
		}
		svg.path([
			"d" : d,
			"stroke" : color,
		]);
	}
	grid(gridInterval     , "#002080");
	grid(gridInterval * 10, "#0030c0");

	Point!double[][string] lines;
	double offsetX = 0, offsetY = 0;
	double[3][4] trilaterationAnchors;
	string color = "#60a0ff";

	enum Mode
	{
		coord,
		compass,
		trilateration,
	}
	Mode mode;

	foreach (line; "map.txt".readText.splitLines)
	{
		scope(failure) writeln("Error with line: " ~ line);
		if (!line.length || line[0] == '#')
			continue;
		auto parts = line.split("\t");
		string popPart() { enforce(parts.length, "Insufficient arguments"); return parts.shift(); }
		auto command = popPart();
		switch (command)
		{
			case "mode":
				mode = popPart().to!Mode;
				final switch (mode)
				{
					case Mode.coord:
						break;
					case Mode.compass:
						break;
					case Mode.trilateration:
						foreach (ref anchor; trilaterationAnchors)
							foreach (ref axis; anchor)
								axis = popPart().to!double;
						break;
				}
				break;
			case "offset":
				offsetX = popPart().to!double;
				offsetY = popPart().to!double;
				break;
			case "point":
			case "wreck":
			case "line":
			{
				double x, y, depth;
				final switch (mode)
				{
					case Mode.coord:
						x = popPart().to!double;
						depth = popPart().to!double;
						y = popPart().to!double;
						break;
					case Mode.compass:
						auto dist = popPart().to!int;
						auto dir = parseDir(popPart());
						auto a = (dir + 180) / 180.0 * PI;
						x =  dist * cos(a);
						y = -dist * sin(a);
						break;
					case Mode.trilateration:
						double[3][4] refPoints = trilaterationAnchors;
						double[4] distances;
						foreach (ref distance; distances)
							distance = popPart().to!double;
						auto p = trilateration(refPoints, distances);
						x = p[0];
						y = p[2];
						depth = p[1];
						break;
				}
				x += offsetX;
				y += offsetY;

				final switch (command)
				{
					case "point":
					case "wreck":
						final switch (command)
						{
							case "point":
								svg.circle([
									"cx"      : text(x    / mapScale),
									"cy"      : text(y    / mapScale),
									"r"       : text(6),
									"stroke"  : color,
								]);
								svg.circle([
									"cx"      : text(x    / mapScale),
									"cy"      : text(y    / mapScale),
									"r"       : text(3),
									"fill"    : color,
								]);
								break;
							case "wreck":
								svg.path([
									"d" : "M %s,%s m -6, -6 v 12 h 12 v -12 z".format(x / mapScale, y / mapScale),
									"stroke" : "#ff8000",
								]);
								break;
						}
						auto t = svg.text([
							"x"      : text(x    / mapScale + 8),
							"y"      : text(y    / mapScale + 6),
							"fill"   : color,
							"style"  : "font-family: sans-serif; font-size: 15pt",
						]);
						auto textLines = wrapNodeText(popPart());
						if (!depth.isNaN)
							textLines ~= "\nD=%d".format(cast(int)round(depth));
						foreach (i, l; textLines)
						{
							auto tspan = t.tspan([
								"x"  : text(x    / mapScale + 8),
								"dy" : "%dem".format(i == 0 ? 0 : 1),
							]);
							tspan[] = l;
						}
						break;
					case "line":
						lines[popPart().findSplit(" (")[0]] ~= Point!double(x, y);
						break;
				}
				break;
			}
			case "color":
				color = popPart();
				break;
			default:
				throw new Exception("Unknown command");
		}
		enforce(parts.length == 0, "Too many arguments");
	}

	foreach (name, points; lines)
	{
		svg.path([
			"d" : "M " ~ points.map!(
				point => "%s %s".format(point.x / mapScale, point.y / mapScale)
			).join(" L "),
			"stroke" : "#008000",
			"fill" : "none",
		]);
	}

	svg.toString().toFile("map.svg");
}

double parseDir(string s)
{
	auto p = min(
		s.indexOf('-').size_t,
		s.indexOf('+').size_t,
		s.length,
	);
	auto baseDirIndex = ["E", "NE", "N", "NW", "W", "SW", "S", "SE"].indexOf(s[0..p]);
	enforce(baseDirIndex >= 0, "Unknown base direction: " ~ s[0..p]);
	double a = baseDirIndex * 45;
	if (p < s.length)
	{
		// In-game compass gradations go clockwise, but the trig angle goes CCW
		a -= s[p..$].to!double * (45. / 6);
	}
	return a;
}

unittest
{
	assert(parseDir("E") == 0);
	assert(parseDir("N") == 90);
	assert(parseDir("N+2") == 75);
}

string[] wrapNodeText(string s)
{
	assert(s.length, "Empty node!");

	string[] words = s.split;

	string[] tryWrap(size_t width)
	{
		string[] lines;
		size_t currentWidth;
		foreach (ref word; words)
		{
			if (!lines || (currentWidth > 0 && currentWidth + word.length > width))
			{
				lines ~= null;
				currentWidth = 0;
			}
			if (lines[$-1].length)
				lines[$-1] ~= ' ';
			lines[$-1] ~= word;
			currentWidth += word.length;
		}
		return lines;
	}

	auto initWidth = cast(int)(sqrt(double(s.length)) * 2).clamp(10, 30);

	auto width = initWidth;
	while (width && tryWrap(width - 1).length == tryWrap(width).length)
		width--;

	return tryWrap(width);
}

find_anchors.py

import numpy as np
from scipy.spatial import distance
from scipy.optimize import least_squares
import math



# def oracle():
#     fixed_points = [(1, 2, 3),
#                     (2, 3, 4),
#                     (3, 4, 5),
#                     (4, 5, 6)]
#     secret_random_point = np.random.rand(3) * 10
#     return [distance.euclidean(fixed_point, secret_random_point) for fixed_point in fixed_points]

# Notes:
# T1 is below sea level (Y<0)
# T2 is North-ish (X<0)
# T3 is East-ish (Z>0)
# T4 is SW-ish

distances = [
    (  27,	 345,	 509,	1042),	# Life Pod 5 (origin)
    ( 236,	 559,	 717,	 814),	# (random point on the surface)
    ( 502,	 823,	 951,	 590),	# (random point on the surface)
    ( 263,	 548,	 400,	1111),	# (random point on the surface)
    ( 311,	 380,	 470,	1126),	# (bottom of the pink shroom biome)
    (1399,	1058,	1437,	2143),	# (random point on the surface (about 1387m), almost exactly S of Life Pod 5)
    (1213,	1425,	1709,	 380),	# (big wreck on a flat, with warpers)
    (1270,	1131,	1654,	1404),	# lifepod 13 (D=180)
    (1288,	1120,	1647,	1495),	# "calcified root system" (D=149)
    ( 131,	 441,	 634,	 915),	# Mystery stalker teeth below here (D=1)
    (1441,	1185,	1064,	2406),	# Lifepod 12 (D=268); amp-eels, light bulb plants

]
oracle_point_names = [
    "Life Pod 5 (origin)",
    "(random point on the surface)",
    "(random point on the surface)",
    "(random point on the surface)",
    "(bottom of the pink shroom biome)",
    "(random point on the surface (about 1387m), almost exactly S of Life Pod 5)",
    "(big wreck on a flat, with warpers)",
    "lifepod 13 (D=180)",
    "calcified root system (D=149)",
    "Mystery stalker teeth below here (D=1)",
    "Lifepod 12 (D=268); amp-eels, light bulb plants",
]
# T1<->T2 - 358
# T1<->T3 - 509
# T1<->T4 - 1027

oracle_random_point_distances = distances.copy()

def oracle():
	return oracle_random_point_distances.pop(0)

# def trilateration_residuals(fixed_points_flat, random_points, distances):
#     fixed_points = fixed_points_flat.reshape(4, 3)
#     residuals = []
#     for i, random_point in enumerate(random_points):
#         for j, fixed_point in enumerate(fixed_points):
#             residuals.append(distance.euclidean(fixed_point, random_point) - distances[i, j])
#     return residuals

# def find_fixed_points(oracle, num_random_points=5):
#     random_points = [np.random.rand(3) * 10 for _ in range(num_random_points)]
#     distances = np.array([oracle() for _ in range(num_random_points)])

#     initial_guess = np.random.rand(12) * 10
#     result = least_squares(trilateration_residuals, initial_guess, args=(random_points, distances))

#     return result.x.reshape(4, 3)

# def trilateration_loss(params, fixed_distances):
#     coords = np.array(params).reshape(4, 3)
#     distances = [distance.euclidean(coords[i], coords[j]) for i in range(4) for j in range(i + 1, 4)]
#     return [distances[i] - fixed_distances[i] for i in range(6)]

NUM_FIXED_POINTS = 4
NUM_AXES = 3

loss_names_recorded = False
loss_names = []

def trilateration_loss(params, fixed_distances):
    coords = np.array(params).reshape(len(params)//NUM_AXES, NUM_AXES)
    fixed_coords = coords[:NUM_FIXED_POINTS]
    oracle_coords = coords[NUM_FIXED_POINTS:]
    assert len(fixed_distances) == len(oracle_coords)
    num_oracle_coords = len(oracle_coords)

    loss = []
    global loss_names_recorded
    def add_loss(name, value):
        loss.append(value)
        if not loss_names_recorded:
            loss_names.append(name)

    ROUNDING_IGNORE_BOOST = 0.5

    def add_distance_loss(name1, name2, p1, p2, expected):
        assert round(expected) == expected
        dist = distance.euclidean(p1, p2)
        loss = dist - expected
        if round(dist) == expected: loss *= ROUNDING_IGNORE_BOOST
        add_loss('Distance between ' + name1 + ' and ' + name2, loss)

    def add_depth_loss(name, point, expected):
        assert math.trunc(expected) == expected
        loss = point[1] - expected  # Y
        if math.trunc(point[1]) == expected: loss *= ROUNDING_IGNORE_BOOST
        add_loss(name + ' depth', loss)

    for oracle_coord_index in range(num_oracle_coords):
        oracle_coord = oracle_coords[oracle_coord_index]
        # est_distances = [distance.euclidean(fixed_coord, oracle_coord) for fixed_coord in fixed_coords]
        for fixed_coord_index in range(NUM_FIXED_POINTS):
            fixed_coord = fixed_coords[fixed_coord_index]
            expected = fixed_distances[oracle_coord_index][fixed_coord_index]
            add_distance_loss(f'T{fixed_coord_index + 1}', f'P{oracle_coord_index + 1}', fixed_coord, oracle_coord, expected)

    # T1 is below sea level (Y<0)
    # T2 is North-ish (Z<0)
    # T3 is East-ish (X>0)
    # T4 is SW-ish (X<0, Z>0)
    add_loss('T1 is below sea level', max(0, fixed_coords[0][1]))
    add_loss('T2 is North of origin', max(0, fixed_coords[1][2]))
    add_loss('T3 is East of origin', min(0, fixed_coords[2][0]))
    add_loss('T4 is West of origin', max(0, fixed_coords[3][0]))
    add_loss('T4 is South of origin', min(0, fixed_coords[3][2]))

    add_loss('Lifepod X is 0', oracle_coords[0][0] - 0)
    add_loss('Lifepod Z is 0', oracle_coords[0][2] - 0)
    # add_loss('Point 2 Y is 0', oracle_coords[1][1] - 0)
    # add_loss('Point 3 Y is 0', oracle_coords[2][1] - 0)
    # add_loss('Point 4 Y is 0', oracle_coords[3][1] - 0)
    # add_loss('Point 6 Y is 0', oracle_coords[5][1] - 0)
    add_loss('Point 2 is Y-coplanar with 3', (oracle_coords[1][1] - oracle_coords[2][1]) * 1000)
    add_loss('Point 2 is Y-coplanar with 4', (oracle_coords[1][1] - oracle_coords[3][1]) * 1000)
    add_loss('Point 2 is Y-coplanar with 6', (oracle_coords[1][1] - oracle_coords[5][1]) * 1000)
    add_loss('Point 6 X is 0', oracle_coords[5][0] - 0)

    add_depth_loss('Point 8' , oracle_coords[7], -180)
    add_depth_loss('Point 9' , oracle_coords[8], -149)
    add_depth_loss('Point 10', oracle_coords[9],   -1)
    add_depth_loss('Point 11', oracle_coords[10],-268)

    # # All points except the first oracle point should have a non-positive Y
    # for i, coord in enumerate(coords):
    #     if i == NUM_FIXED_POINTS:
    #         add_loss(f'P{i - NUM_FIXED_POINTS} Y below ground penalty', min(0, coord[1]))
    #     else:
    #         add_loss(f'P{i - NUM_FIXED_POINTS} Y above ground penalty', max(0, coord[1]))
    # # add_loss('P6 Z is negative penalty', min(0, oracle_coords[5][2]))

    loss_names_recorded = True
    return loss

def find_fixed_points(oracle, num_samples=len(distances)):
    fixed_distances = []
    for _ in range(num_samples):
        fixed_distances.append(oracle())

    num_coords = NUM_FIXED_POINTS + num_samples
    seed = 18
    while True:
        np.random.seed(seed)
        initial_guess = np.random.rand(num_coords * NUM_AXES) * 1000 - 500
        result = least_squares(trilateration_loss, initial_guess, args=(fixed_distances,))
        if result.cost < 0.5:
            break
        print(f"Cost is {result.cost}, retrying...")
        seed = seed + 1

    print(result)
    coords = np.array(result.x).reshape(num_coords, NUM_AXES)

    print('Coords:')
    point_names = [
        "T1", "T2", "T3", "T4",
        *(f"P{n+1}" for n in range(len(distances))),
    ]
    for i, coord in enumerate(coords):
        name = point_names[i]
        if i >= NUM_FIXED_POINTS:
            name += ' ' + oracle_point_names[i - NUM_FIXED_POINTS]
        print(f"    X={coord[0]:10.3f} Y={coord[1]:10.3f} Z={coord[2]:10.3f}  {name}")

    print('Residuals:')
    for i, residual in enumerate(result.fun):
        print(f"    {residual:10.3f}  {loss_names[i]}")

    print("T1-T2: ", distance.euclidean(coords[0], coords[1]))
    print("T1-T3: ", distance.euclidean(coords[0], coords[2]))
    print("T1-T4: ", distance.euclidean(coords[0], coords[3]))

    print(f"""
mode	coord
color	#ffff00
point	{coords[0][0]}	{coords[0][1]}	{coords[0][2]}	{point_names[0]}
point	{coords[1][0]}	{coords[1][1]}	{coords[1][2]}	{point_names[1]}
point	{coords[2][0]}	{coords[2][1]}	{coords[2][2]}	{point_names[2]}
point	{coords[3][0]}	{coords[3][1]}	{coords[3][2]}	{point_names[3]}

mode	trilateration	{coords[0][0]}	{coords[0][1]}	{coords[0][2]}	{coords[1][0]}	{coords[1][1]}	{coords[1][2]}	{coords[2][0]}	{coords[2][1]}	{coords[2][2]}	{coords[3][0]}	{coords[3][1]}	{coords[3][2]}
""")

    return coords

fixed_points = find_fixed_points(oracle)
# print(fixed_points)

# print("T1-T2: ", distance.euclidean(fixed_points[0], fixed_points[1]))
# print("T1-T3: ", distance.euclidean(fixed_points[0], fixed_points[2]))
# print("T1-T4: ", distance.euclidean(fixed_points[0], fixed_points[3]))



# if __name__ == "__main__":
# 	fixed_points = find_fixed_points(oracle)
# 	print(f"Estimated fixed points coordinates:\n{fixed_points}")

# from scipy.optimize import minimize

# def trilateration_residuals_transformed(transformed_points_flat, points, distances):
# 	transformed_points = transformed_points_flat.reshape(4, 3)
# 	residuals = []
# 	for i, transformed_point in enumerate(transformed_points):
# 		for j, point in enumerate(points):
# 			residuals.append(distance.euclidean(transformed_point, point) - distances[i, j])
# 	return residuals

# def transform_fixed_points_unknown_points(fixed_points, origin_distances, point_a_distances, point_b_distances):
#     def objective_function(params):
#         point_a, point_b, transformed_points_flat = params[:3], params[3:6], params[6:]
#         transformed_points = transformed_points_flat.reshape(4, 3)
#         points = [np.array([0, 0, 0]), point_a, point_b]
#         distances = np.column_stack((origin_distances, point_a_distances, point_b_distances))

#         residuals = trilateration_residuals_transformed(transformed_points_flat, points, distances)
#         return np.sum(np.array(residuals) ** 2)

#     def y_zero_constraint(params):
#         point_a, point_b = params[:3], params[3:6]
#         return np.array([point_a[1], point_b[1]])

#     initial_guess = np.concatenate((np.random.rand(6) * 10, fixed_points.flatten()))
#     constraints = {'type': 'eq', 'fun': y_zero_constraint}
#     result = minimize(objective_function, initial_guess, constraints=constraints)

#     return result.x[6:].reshape(4, 3)

# origin_distances  = distances[0] #[distance.euclidean(fixed_point, [0, 0, 0]) for fixed_point in fixed_points]
# point_a_distances = distances[1] #[distance.euclidean(fixed_point, point_a) for fixed_point in fixed_points]
# point_b_distances = distances[2] #[distance.euclidean(fixed_point, point_b) for fixed_point in fixed_points]

# transformed_fixed_points = transform_fixed_points_unknown_points(fixed_points, origin_distances, point_a_distances, point_b_distances)
# print(f"Transformed fixed points coordinates:\n{transformed_fixed_points}")


# import numpy as np
# from scipy.spatial import distance
# from scipy.spatial.transform import Rotation as R
# from scipy.optimize import least_squares

# def transform_coordinates(fixed_points, reference_points):
#     # Translate the first reference point to the origin
#     translated_points = fixed_points - reference_points[0]

#     # Find the rotation matrix that aligns the second reference point to the X-axis
#     second_point_normalized = translated_points[1] / np.linalg.norm(translated_points[1])
#     rot_x = R.from_rotvec(np.cross(second_point_normalized, [1, 0, 0]) * np.arccos(np.dot(second_point_normalized, [1, 0, 0])))

#     # Apply the rotation to all points
#     rotated_points = rot_x.apply(translated_points)

#     # Find the rotation matrix that aligns the third reference point to the X-Y plane
#     third_point_xy = np.array([rotated_points[2, 0], rotated_points[2, 1], 0])
#     third_point_xy_normalized = third_point_xy / np.linalg.norm(third_point_xy)
#     rot_y = R.from_rotvec(np.cross(rotated_points[2], third_point_xy_normalized) * np.arccos(np.dot(rotated_points[2], third_point_xy_normalized)))

#     # Apply the rotation to all points
#     transformed_points = rot_y.apply(rotated_points)

#     return transformed_points

# def main():
#     fixed_points = find_fixed_points(oracle)
#     print(f"Estimated fixed points coordinates:\n{fixed_points}")

#     # Get the first three points from the oracle to serve as reference points
#     reference_points = [np.random.rand(3) * 10 for _ in range(3)]
#     distances = np.array([oracle() for _ in range(3)])

#     # Solve for the reference points' coordinates
#     initial_guess = np.random.rand(9) * 10
#     result = least_squares(trilateration_residuals, initial_guess, args=(reference_points, distances[:, :3]))
#     reference_points_coords = result.x.reshape(3, 3)

#     transformed_points = transform_coordinates(fixed_points, reference_points_coords)
#     print(f"Transformed fixed points coordinates:\n{transformed_points}")

# if __name__ == "__main__":
#     main()



# # import numpy as np
# # from scipy.spatial import distance
# # from scipy.optimize import least_squares

# # # ... (oracle, trilateration_residuals, find_fixed_points functions) ...

# def compute_transformation_matrix(origin, y0_point1, y0_point2):
#     x_axis = (y0_point1 - origin) / np.linalg.norm(y0_point1 - origin)
#     temp_y_axis = (y0_point2 - origin) / np.linalg.norm(y0_point2 - origin)
#     z_axis = np.cross(x_axis, temp_y_axis)
#     z_axis /= np.linalg.norm(z_axis)
#     y_axis = np.cross(z_axis, x_axis)
#     rotation_matrix = np.vstack((x_axis, y_axis, z_axis)).T
#     translation_vector = -np.dot(rotation_matrix, origin)
#     return rotation_matrix, translation_vector

# def transform_coordinates(fixed_points, transformation_matrix, translation_vector):
#     return np.dot(fixed_points, transformation_matrix.T) + translation_vector

# # if __name__ == "__main__":
# #     fixed_points = find_fixed_points(oracle)
# #     print(f"Estimated fixed points coordinates:\n{fixed_points}")

# #     # Assuming the first three points chosen by the oracle are origin, y0_point1, and y0_point2
# #     oracle_points = [np.random.rand(3) * 10 for _ in range(3)]
# #     origin, y0_point1, y0_point2 = oracle_points

# #     # Compute the transformation matrix and translation vector
# #     transformation_matrix, translation_vector = compute_transformation_matrix(origin, y0_point1, y0_point2)

# #     # Transform the fixed points' coordinates
# #     transformed_fixed_points = transform_coordinates(fixed_points, transformation_matrix, translation_vector)
# #     print(f"Transformed fixed points coordinates:\n{transformed_fixed_points}")



# import numpy as np
# from scipy.spatial import distance
# from scipy.optimize import least_squares

# # ... (oracle, trilateration_residuals, find_fixed_points functions) ...

# def find_transformation_params(params, fixed_points, distances):
#     origin, y0_point1, y0_point2 = params[:3], params[3:6], params[6:9]
#     rotation_matrix, translation_vector = compute_transformation_matrix(origin, y0_point1, y0_point2)
#     transformed_fixed_points = transform_coordinates(fixed_points, rotation_matrix, translation_vector)
    
#     residuals = []
#     for i, point in enumerate(transformed_fixed_points):
#         residuals.append(distance.euclidean(origin, point) - distances[0][i])
#         residuals.append(distance.euclidean(y0_point1, point) - distances[1][i])
#         residuals.append(distance.euclidean(y0_point2, point) - distances[2][i])
#     return residuals

# def find_transformation_matrix(fixed_points, distances):
#     initial_guess = np.zeros(9)
#     initial_guess[4] = initial_guess[8] = 1
#     result = least_squares(find_transformation_params, initial_guess, args=(fixed_points, distances))
#     origin, y0_point1, y0_point2 = result.x[:3], result.x[3:6], result.x[6:9]
#     return compute_transformation_matrix(origin, y0_point1, y0_point2)

# if __name__ == "__main__":
#     fixed_points = find_fixed_points(oracle)
#     print(f"Estimated fixed points coordinates:\n{fixed_points}")
#     print("T1-T2: ", distance.euclidean(fixed_points[0], fixed_points[1]))
#     print("T1-T3: ", distance.euclidean(fixed_points[0], fixed_points[2]))
#     print("T1-T4: ", distance.euclidean(fixed_points[0], fixed_points[3]))

#     # Get distances from the oracle's first three points to the fixed points
#     oracle_distances = distances[:3]

#     # Find the transformation matrix and translation vector
#     transformation_matrix, translation_vector = find_transformation_matrix(fixed_points, oracle_distances)

#     # Transform the fixed points' coordinates
#     transformed_fixed_points = transform_coordinates(fixed_points, transformation_matrix, translation_vector)
#     print(f"Transformed fixed points coordinates:\n{transformed_fixed_points}")

test_anchors.py

import numpy as np
from scipy.spatial import distance
from scipy.optimize import least_squares
import math

NUM_AXES = 3

fixed_points = [
    (2.2404291416571347,	-19.857832279461935,	15.751490015211678),	# T1
    (71.41422405071685,	-68.42767347406883,	-330.0991786228649),	# T2
    (507.2584377961219,	-13.378438265725853,	-38.94151302226673),	# T3
    (-813.8121936246888,	-300.9619060047596,	575.8983392400755),	# T4
]

distances = [1270, 1131, 1654, 1404]  # Lifepod 13 (D=180)
# distances = [1288, 1120, 1647, 1495]  # "calcified root system" (D=149)

def trilateration_loss(params):
    coords = np.array(params).reshape(len(params)//NUM_AXES, NUM_AXES)
    assert len(coords) == 1
    oracle_point = coords[0]
    return [
        distance.euclidean(fixed_points[i], oracle_point) - distances[i] for i in range(len(fixed_points))
    ]

initial_guess = np.random.rand(NUM_AXES) * 1000 - 500
result = least_squares(trilateration_loss, initial_guess)
print(result)

trilateration.d

import ae.sys.cmd;
import ae.sys.persistence.core;
import ae.sys.persistence.memoize;

import std.conv;
import std.format;
import std.string;

double[3] trilateration(double[3][4] refPoints, double[4] distances) {
    string program = q"EOF
import numpy as np
from scipy.spatial import distance
from scipy.optimize import least_squares
import math

NUM_AXES = 3

fixed_points = [%((%(%18g, %))%|, %)]

distances = [%(%18g, %)]

def trilateration_loss(params):
    coords = np.array(params).reshape(len(params)//NUM_AXES, NUM_AXES)
    assert len(coords) == 1
    oracle_point = coords[0]
    return [
        distance.euclidean(fixed_points[i], oracle_point) - distances[i] for i in range(len(fixed_points))
    ]

import sys
seed = 0
while True:
    np.random.seed(seed)
    initial_guess = np.random.rand(NUM_AXES) * 1000 - 500
    result = least_squares(trilateration_loss, initial_guess)
    if result.cost < 0.1:
        break
    print(f"Cost is {result.cost}, retrying...", file=sys.stderr)
    seed = seed + 1

print(list(result.x))
EOF".format(refPoints[], distances[]);
    static memoizedQuery = PersistentMemoized!(query!(), FlushPolicy.atThreadExit)("trilateration-cache.json");
    auto result = memoizedQuery(["python", "-c", program]);
    return result.chomp().to!(double[3]);
    // assert(false, "TODO");
}