339 lines
12 KiB
Python
339 lines
12 KiB
Python
|
#!/usr/bin/python
|
||
|
|
# -*- coding: utf-8 -*-
|
||
|
|
from dataclasses import dataclass, field
|
||
|
|
from PIL import Image
|
||
|
|
from pathlib import Path
|
||
|
|
import json
|
||
|
|
from math import sqrt
|
||
|
|
from typing import List, Tuple, Union
|
||
|
|
|
||
|
|
|
||
|
|
HBITS = 20
|
||
|
|
VBITS = 19
|
||
|
|
h_mask = 2 ** HBITS - 1
|
||
|
|
v_mask = 2 ** VBITS - 1
|
||
|
|
h_limit = 2 ** (HBITS - 1) - 1
|
||
|
|
h_half = 2 ** (HBITS - 2)
|
||
|
|
v_limit = 2 ** (VBITS - 1) - 1
|
||
|
|
v_half = 2 ** (VBITS - 2)
|
||
|
|
|
||
|
|
# print(f'{h_mask=:_X} {h_limit=:_X} {h_half=:_X} ')
|
||
|
|
|
||
|
|
|
||
|
|
@dataclass
|
||
|
|
class Point2D:
|
||
|
|
x: float = 0.0
|
||
|
|
y: float = 0.0
|
||
|
|
z: float = 0.0
|
||
|
|
|
||
|
|
def distance_to(self, p) -> float:
|
||
|
|
dx = self.x - p.x
|
||
|
|
dy = self.y - p.y
|
||
|
|
dz = self.z - p.z
|
||
|
|
return sqrt(dx * dx + dy * dy + dz * dz)
|
||
|
|
|
||
|
|
def make_key(self) -> int:
|
||
|
|
"""Returns the integer hash key"""
|
||
|
|
# coordinate considerate fino al centimetro
|
||
|
|
# xx = int(500000 + round(100.0 * self.x, 2))
|
||
|
|
# yy = int(500000 + round(100.0 * self.y, 2))
|
||
|
|
# return f'{xx + 1000000 * yy}'
|
||
|
|
xi = int(round(self.x * 100.0, 0)) + h_half
|
||
|
|
yi = int(round(self.y * 100.0, 0)) + h_half
|
||
|
|
zi = int(round(self.z * 100.0, 0)) + v_half
|
||
|
|
if xi < 0:
|
||
|
|
xi = 0
|
||
|
|
if yi < 0:
|
||
|
|
yi = 0
|
||
|
|
if zi < 0:
|
||
|
|
zi = 0
|
||
|
|
if xi > h_limit:
|
||
|
|
xi = h_limit
|
||
|
|
if yi > h_limit:
|
||
|
|
yi = h_limit
|
||
|
|
if zi > v_limit:
|
||
|
|
zi = v_limit
|
||
|
|
xi += yi << HBITS
|
||
|
|
xi += zi << (HBITS + HBITS)
|
||
|
|
return xi
|
||
|
|
|
||
|
|
def from_key(self, key):
|
||
|
|
"""collects x, y and z value from a hash key. the key should be an integer or a string representation of it"""
|
||
|
|
if not isinstance(key, int):
|
||
|
|
key = int(key) # let's say they gave you a string...
|
||
|
|
x_bits = key & h_mask
|
||
|
|
x_bits -= h_half
|
||
|
|
self.x = x_bits / 100.00
|
||
|
|
key >>= HBITS
|
||
|
|
y_bits = key & h_mask
|
||
|
|
y_bits -= h_half
|
||
|
|
self.y = y_bits / 100.00
|
||
|
|
key >>= HBITS
|
||
|
|
z_bits = key & v_mask
|
||
|
|
z_bits -= v_half
|
||
|
|
self.z = z_bits / 100.00
|
||
|
|
|
||
|
|
def to_str_array(self) -> Tuple[str, str, str]:
|
||
|
|
"""returns an array of three strings with the x, y, z values printed as strings with 2 decimal places"""
|
||
|
|
return f'{self.x:.2f}', f'{self.y:.2f}', f'{self.z:.2f}'
|
||
|
|
|
||
|
|
def from_array(self, arr: Union[Tuple, List, dict]):
|
||
|
|
"""Tries to parse the input array to collect the x, y and z value default value for z is 0.0"""
|
||
|
|
if len(arr) < 2:
|
||
|
|
raise ValueError(f'Minimum arr len is 2')
|
||
|
|
if isinstance(arr, dict):
|
||
|
|
if 'x' in arr:
|
||
|
|
self.x = arr['x']
|
||
|
|
if 'y' in arr:
|
||
|
|
self.y = arr['y']
|
||
|
|
if 'z' in arr:
|
||
|
|
self.z = arr['z']
|
||
|
|
elif isinstance(arr, list) or isinstance(arr, tuple):
|
||
|
|
self.x = round(float(arr[0]), 2)
|
||
|
|
self.y = round(float(arr[1]), 2)
|
||
|
|
if len(arr) >= 3:
|
||
|
|
self.z = round(float(arr[2]), 2)
|
||
|
|
|
||
|
|
def round_me(self):
|
||
|
|
self.x = round(self.x, 2)
|
||
|
|
self.y = round(self.y, 2)
|
||
|
|
self.z = round(self.z, 2)
|
||
|
|
|
||
|
|
def __hash__(self):
|
||
|
|
return hash((self.x, self.y, self.z))
|
||
|
|
|
||
|
|
def to_tuple(self):
|
||
|
|
return self.x, self.y, self.z
|
||
|
|
|
||
|
|
|
||
|
|
class ScaledMap:
|
||
|
|
"""Class representing a map that comes along with scaling values between realworld and pixels"""
|
||
|
|
|
||
|
|
def __init__(self, filename=None):
|
||
|
|
"""
|
||
|
|
:param filename: Filename of the _data file
|
||
|
|
"""
|
||
|
|
if filename is None:
|
||
|
|
raise ValueError('You MUST supply a _data file')
|
||
|
|
self.pr1 = None
|
||
|
|
self.pm1 = None
|
||
|
|
self.pr2 = None
|
||
|
|
self.pm2 = None
|
||
|
|
self.x_scale = 1.0
|
||
|
|
self.y_scale = 1.0
|
||
|
|
self.y_is_up = True
|
||
|
|
self.filename = filename
|
||
|
|
self.width, self.height = 0, 0
|
||
|
|
p = Path(filename)
|
||
|
|
self.full_filename = p
|
||
|
|
self.folder = p.parent
|
||
|
|
self.suffix = p.suffix
|
||
|
|
is_data = self.suffix.endswith('_data')
|
||
|
|
is_jd = self.suffix.endswith('_jd')
|
||
|
|
self.image_file = ''
|
||
|
|
if not is_data and not is_jd:
|
||
|
|
raise ValueError('Supplied file suffix MUST end with "_data" or "_jd"')
|
||
|
|
print(self.image_file, self.width, self.height)
|
||
|
|
if is_data:
|
||
|
|
self.image_file = p.parent / f'{p.stem}{p.suffix[:-5]}'
|
||
|
|
with Image.open(self.image_file) as img:
|
||
|
|
self.width, self.height = img.size
|
||
|
|
self.load_from_file(filename)
|
||
|
|
else:
|
||
|
|
self.load_from_jd(filename)
|
||
|
|
|
||
|
|
def __str__(self):
|
||
|
|
s = [f'{k} = {v}' for k, v in self.__dict__.items()]
|
||
|
|
return '\n'.join(s)
|
||
|
|
|
||
|
|
def adj_scales(self):
|
||
|
|
self.x_scale = (self.pr2.x - self.pr1.x) / (self.pm2.x - self.pm1.x)
|
||
|
|
self.y_scale = (self.pr2.y - self.pr1.y) / (self.pm2.y - self.pm1.y)
|
||
|
|
|
||
|
|
def load_from_file(self, infile):
|
||
|
|
n = 0
|
||
|
|
with open(infile, 'r') as fin:
|
||
|
|
for raw_line in fin.readlines():
|
||
|
|
line = raw_line.strip()
|
||
|
|
if line.startswith('#'):
|
||
|
|
continue
|
||
|
|
if ' ' in line:
|
||
|
|
fields = line.split(' ')
|
||
|
|
if len(fields) == 4:
|
||
|
|
vals = [float(v) for v in fields]
|
||
|
|
if n == 0:
|
||
|
|
self.pm1 = Point2D(vals[0], vals[1])
|
||
|
|
self.pr1 = Point2D(vals[2], vals[3])
|
||
|
|
n = 1
|
||
|
|
elif n == 1:
|
||
|
|
self.pm2 = Point2D(vals[0], vals[1])
|
||
|
|
self.pr2 = Point2D(vals[2], vals[3])
|
||
|
|
n = 2
|
||
|
|
elif len(fields) == 2:
|
||
|
|
if 'Y' == fields[0].upper():
|
||
|
|
self.y_is_up = fields[1].upper() in ('UP', 'UPWARD', 'NORD')
|
||
|
|
if not self.y_is_up:
|
||
|
|
self.pm1.y = self.height - self.pm1.y
|
||
|
|
self.pm2.y = self.height - self.pm2.y
|
||
|
|
self.y_is_up = True
|
||
|
|
self.adj_scales()
|
||
|
|
|
||
|
|
def load_from_jd(self, infile):
|
||
|
|
n = 0
|
||
|
|
self.filename = infile
|
||
|
|
with open(infile, 'r') as fin:
|
||
|
|
obj = json.load(fin)
|
||
|
|
self.image_file = obj.get('imagefile', None)
|
||
|
|
h = obj.get('height', None)
|
||
|
|
w = obj.get('width', None)
|
||
|
|
with Image.open(self.image_file) as img:
|
||
|
|
self.width, self.height = img.size
|
||
|
|
if h != self.height:
|
||
|
|
raise ValueError(f'File:{infile} image height={h} realimage({self.image_file}) height={self.height}')
|
||
|
|
if w != self.width:
|
||
|
|
raise ValueError(f'File:{infile} image width={w} realimage({self.image_file}) height={self.width}')
|
||
|
|
self.y_is_up = obj.get('y_is_up', True)
|
||
|
|
pr1 = obj.get('pr1', None)
|
||
|
|
pm1 = obj.get('pm1', None)
|
||
|
|
pr2 = obj.get('pr2', None)
|
||
|
|
pm2 = obj.get('pm2', None)
|
||
|
|
self.pr1 = Point2D(pr1['x'], pr1['y'])
|
||
|
|
self.pm1 = Point2D(pm1['x'], pm1['y'])
|
||
|
|
self.pr2 = Point2D(pr2['x'], pr2['y'])
|
||
|
|
self.pm2 = Point2D(pm2['x'], pm2['y'])
|
||
|
|
if not self.y_is_up:
|
||
|
|
self.pm1.y = self.height - self.pm1.y
|
||
|
|
self.pm2.y = self.height - self.pm2.y
|
||
|
|
self.y_is_up = True
|
||
|
|
self.adj_scales()
|
||
|
|
sx_ratio = self.x_scale / obj['scale_x']
|
||
|
|
sy_ratio = self.y_scale / obj['scale_y']
|
||
|
|
print(f'Scales ratio: {sx_ratio} {sy_ratio}')
|
||
|
|
|
||
|
|
def map_data_to_file(self, outfile):
|
||
|
|
with open(outfile, 'w', encoding='utf-8', newline='\n') as fout:
|
||
|
|
fout.write('# xmap ymap xreal yreal\nY UP\n')
|
||
|
|
fout.write(f'IMAGE {self.image_file}\n')
|
||
|
|
pr = self.get_real_coord(Point2D(0.0, 0.0))
|
||
|
|
fout.write(f'0.0 0.0 {pr.x} {pr.y}\n')
|
||
|
|
pr = self.get_real_coord(Point2D(self.width, self.height))
|
||
|
|
fout.write(f'{self.width} {self.height} {pr.x} {pr.y}\n')
|
||
|
|
|
||
|
|
def get_real_coord(self, map_point: Point2D, round_it=True) -> Point2D:
|
||
|
|
"""
|
||
|
|
Restituisce un Point2D in coordinate reali (metri)
|
||
|
|
:param map_point: Point2D in coordinate pixel/mappa
|
||
|
|
:param round_it: Se Vero, i valori in metri vengono arrotondati a 2 decimali (aka cm)
|
||
|
|
:return: Point2D in coordinate reali (metri)
|
||
|
|
"""
|
||
|
|
if round_it:
|
||
|
|
return Point2D(round(self.pr1.x + self.x_scale * (map_point.x - self.pm1.x), 2),
|
||
|
|
round(self.pr1.y + self.y_scale * (map_point.y - self.pm1.y), 2),
|
||
|
|
map_point.z,
|
||
|
|
)
|
||
|
|
else:
|
||
|
|
return Point2D(self.pr1.x + self.x_scale * (map_point.x - self.pm1.x),
|
||
|
|
self.pr1.y + self.y_scale * (map_point.y - self.pm1.y),
|
||
|
|
map_point.z,
|
||
|
|
)
|
||
|
|
|
||
|
|
def get_map_coord(self, real_point: Point2D, round_it=True) -> Point2D:
|
||
|
|
"""
|
||
|
|
Restituisce un Point2D in coordinate pixel/mappa
|
||
|
|
:param real_point: Point2D in coordinate reali (m)
|
||
|
|
:param round_it: Se Vero restituisce coordinate intere
|
||
|
|
:return: Point2D in coordinate pixel/mappa
|
||
|
|
"""
|
||
|
|
if round_it:
|
||
|
|
return Point2D(round(self.pm1.x + (real_point.x - self.pr1.x) / self.x_scale),
|
||
|
|
round(self.pm1.y + (real_point.y - self.pr1.y) / self.y_scale),
|
||
|
|
real_point.z,
|
||
|
|
)
|
||
|
|
else:
|
||
|
|
return Point2D(self.pm1.x + (real_point.x - self.pr1.x) / self.x_scale,
|
||
|
|
self.pm1.y + (real_point.y - self.pr1.y) / self.y_scale,
|
||
|
|
real_point.z,
|
||
|
|
)
|
||
|
|
|
||
|
|
def to_js_code(self):
|
||
|
|
"""Returns javaScript code to deal with the map coordinates"""
|
||
|
|
ll = self.get_real_coord(Point2D(0.0, 0.0))
|
||
|
|
ur = self.get_real_coord(Point2D(self.width, self.height))
|
||
|
|
return """ const bounds = [L.latLng({lly}, {llx}), L.latLng({ury},{urx})]
|
||
|
|
const map = L.map('map', {{crs: L.CRS.Simple, layers: [], minZoom: -2, zoomDelta: 0.5}});
|
||
|
|
map.fitBounds(bounds);
|
||
|
|
L.DomUtil.addClass(map._container,'crosshair-cursor-enabled');
|
||
|
|
const image = L.imageOverlay('{img_file}', bounds).addTo(map);""".format(llx=ll.x, lly=ll.y, urx=ur.x, ury=ur.y, img_file=self.image_file)
|
||
|
|
|
||
|
|
def to_json(self):
|
||
|
|
return {
|
||
|
|
'name': "",
|
||
|
|
'imagefile': str(self.image_file),
|
||
|
|
'width': self.width,
|
||
|
|
'height': self.height,
|
||
|
|
'y_is_up': self.y_is_up,
|
||
|
|
'pr1': {'x': self.pr1.x, 'y': self.pr1.y},
|
||
|
|
'pm1': {'x': self.pm1.x, 'y': self.pm1.y},
|
||
|
|
'pr2': {'x': self.pr2.x, 'y': self.pr2.y},
|
||
|
|
'pm2': {'x': self.pm2.x, 'y': self.pm2.y},
|
||
|
|
'scale_x': self.x_scale,
|
||
|
|
'scale_y': self.y_scale,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
# FILE = 'Mozz_map.png_data'
|
||
|
|
# # FILE = 'Mappe-ModelPPP.png_data'
|
||
|
|
# my_map = ScaledMap(FILE)
|
||
|
|
# # pm = moz.get_map_coord(Point2D())
|
||
|
|
# # print(pm)
|
||
|
|
# # print(moz.get_real_coord(pm))
|
||
|
|
# # print('-------------')
|
||
|
|
# # pr = my_map.get_real_coord(Point2D(0.0, 0.0))
|
||
|
|
# # print(f'0.0 0.0 {pr}')
|
||
|
|
# # pr = my_map.get_real_coord(Point2D(my_map.width, my_map.height))
|
||
|
|
# # print(f'{my_map.width} {my_map.height} {pr}')
|
||
|
|
# print(my_map.to_js_code())
|
||
|
|
# my_map.map_data_to_file('tt.txt')
|
||
|
|
# obj = my_map.to_json()
|
||
|
|
# with open('t._jd', 'w', encoding='utf-8', newline='\n') as fout:
|
||
|
|
# json.dump(obj, fout, indent=4)
|
||
|
|
# print(obj)
|
||
|
|
# new_img = ScaledMap('t._jd')
|
||
|
|
# print(json.dumps(new_img.to_json(), indent=4))
|
||
|
|
# new_img = ScaledMap('fpm1_manual._jd')
|
||
|
|
|
||
|
|
# new_img = ScaledMap('f2._jd')
|
||
|
|
# obj = new_img.to_json()
|
||
|
|
# print(obj)
|
||
|
|
# with open('FPM2_web._jd', 'w', encoding='utf-8', newline='\n') as fout:
|
||
|
|
# json.dump(obj, fout, indent=4)
|
||
|
|
# print(new_img.to_js_code())
|
||
|
|
|
||
|
|
# new_img = ScaledMap('FPM1_web._jd')
|
||
|
|
# obj = new_img.to_json()
|
||
|
|
# print(obj)
|
||
|
|
# # with open('FPM2_web._jd', 'w', encoding='utf-8', newline='\n') as fout:
|
||
|
|
# # json.dump(obj, fout, indent=4)
|
||
|
|
# print(new_img.to_js_code())
|
||
|
|
def check_me(x=0, y=0, z=0):
|
||
|
|
p = Point2D(x, y, z)
|
||
|
|
key = p.make_key()
|
||
|
|
print(f'Input {p} {key:_X} {key}')
|
||
|
|
pr = Point2D()
|
||
|
|
pr.from_key(key)
|
||
|
|
key_r = pr.make_key()
|
||
|
|
print(f'Output:{pr} {key_r:_X} {key_r}')
|
||
|
|
print()
|
||
|
|
return p
|
||
|
|
|
||
|
|
p = check_me()
|
||
|
|
p = check_me(100, 200, 500)
|
||
|
|
|
||
|
|
p = check_me(100, 200, 1000)
|
||
|
|
print(f'{p} {p.make_key():_X} {p.make_key()} {p.to_str_array()}')
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|