User:Caron/memo
Jump to navigation
Jump to search
This page contain the Python code that I plan to implement in Pykmp module.
All codes are tested in Python 3.9.4.
Automated_height_correction
User:Caron/memo/Automated_height_correction
Visualize paths using Graphviz
This code can visualize ENPT/ITPT/CKPT paths using Graphviz.
An Example in Neon Dimension (Enemy routes). The name of each node is the path index and the value of the edge is the number of points the path has.
import os
import numpy as np
import graphviz
def _gn(index) -> str:
return 'Group_{:02X}'.format(index)
def to_graph(data: np.ndarray, render: bool=False, view: bool=True, **kwargs):
"""
Visualize paths using Graphviz.
"""
if 'format' not in kwargs:
kwargs['format'] = 'png'
dgraph = graphviz.Digraph(**kwargs)
# define
dgraph.body.append(r"{rank=min; " + _gn(0) + r";}")
for i, _data in enumerate(data):
kwg = {'shape': 'box'}
uniques = np.unique(_data[8:14])
uniques = uniques[:-1] if 255 in uniques else uniques
if len(uniques) >1:
dgraph.body.append(
r"{rank=same; " + '; '.join([_gn(j) for j in uniques])
+ ';}')
kwg['shape'] = 'ellipse'
dgraph.node(_gn(i), **kwg)
# create node
for i, _data in enumerate(data):
for nxt in np.unique(_data[8:14]):
if nxt == 255:
break
dgraph.edge(_gn(i), _gn(nxt), label=' {}'.format(_data[1]))
if render or view:
try:
if render:
dgraph.render(view=view)
dgraph.view()
except Exception as e:
raise RuntimeError(e)
print('Saved graph data/image to {}.'.format(os.path.join(os.getcwd(), dgraph.directory)))
else:
raise ValueError('Argument "render" or "view" must be True.')
# Example
data = np.array([
[0x00, 0x05, 0x10, 0xff, 0xff, 0xff, 0xff, 0xff, 0x01, 0x04, 0xff, 0xff, 0xff, 0xff],
[0x05, 0x09, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0x02, 0x03, 0xff, 0xff, 0xff, 0xff],
[0x0e, 0x05, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x13, 0x06, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x19, 0x11, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x2a, 0x0a, 0x02, 0x03, 0x04, 0xff, 0xff, 0xff, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b],
[0x34, 0x16, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x4a, 0x16, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x60, 0x16, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x76, 0x0f, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x85, 0x10, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff],
[0x95, 0x08, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0c, 0x0d, 0xff, 0xff, 0xff, 0xff],
[0x9d, 0x0a, 0x0b, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xa7, 0x0a, 0x0b, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xb1, 0x14, 0x06, 0x08, 0x09, 0xff, 0xff, 0xff, 0x11, 0x12, 0xff, 0xff, 0xff, 0xff],
[0xc5, 0x25, 0x07, 0x0a, 0x0c, 0x0d, 0xff, 0xff, 0x10, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xea, 0x02, 0x0f, 0x13, 0xff, 0xff, 0xff, 0xff, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xec, 0x05, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff, 0x13, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xf1, 0x03, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff, 0x13, 0xff, 0xff, 0xff, 0xff, 0xff],
[0xf4, 0x0b, 0x11, 0x12, 0xff, 0xff, 0xff, 0xff, 0x10, 0xff, 0xff, 0xff, 0xff, 0xff]], np.uint8)
to_graph(data)
Smoothing:Catmull–Clark
faceは不要なのでCatmull-clarkの途中処理まで
not tested yet
counts = round(counts) if isinstance(counts, float) else counts
for _ in range(counts):
data_size = len(target)
smoothed = create_new_data(self._sectionname, 2, 2*data_size-1)
smoothed[::2,:] = target # type: ignore
# Center position
smoothed[1::2,0:3] = ((target[:-1,0:3] + target[1:,0:3])/2)
# Average((cur + center_1 + center_2)/3)
smoothed[2:-2:2,0:3] = (smoothed[1:-2:2,0:3] + smoothed[3::2,0:3] + smoothed[2:-2:2,0:3])/3
# update settings
smoothed = update_settings(
iter = range(1, len(smoothed)-1),
old_array=target,
new_array=smoothed
)
# clone array for next iteration.
target = smoothed.copy()
Smoothing:Spline interpolation
こっちの方が元の曲線に近くフィットしてくれる
not tested yet
Neon Dimension ENPH #1. Blue : old positions, Green(star) : smoothed positions
pos = target[:, :3].T
m = len(pos[0])
# create knots vector
tck, _ = interpolate.splprep(pos, s=m+np.sqrt(m), k=4)
u_counts = np.linspace(0,1,round(m*counts))
# fitting
fitted = np.vstack(interpolate.splev(u_counts, tck)).T
### update settings: start
data_size = len(fitted)
update_idx = np.zeros(data_size)
# find nearest positions
for idx in range(m):
old_vec = np.tile(pos.T[idx], (data_size, 1))
norm = np.linalg.norm(old_vec - fitted, axis=-1)
update_idx[idx] = np.argmin(norm)
smoothed = create_new_data(self._sectionname, 2, data_size)
smoothed[:,:3] = fitted
# POTI Setting is start at target[3:], but ENPT, ITPT are target[4:].
start = 4 if self._sectionname != 'POTI' else 3
smoothed[update_idx,start:] = target[:,start:]
smoothed = update_settings(
iter=range(1, m),
old_array=target,
new_array=smoothed,
spline_index=update_idx
)
Convex Quadrilaterals or not
緑の角度の総和が2πになればよい。4πの場合はConvex Quadrilateralsではない。
from typing import *
import numpy as np
def is_convex(
pt1: np.ndarray,
pt2: np.ndarray,
return_data: bool=False
)-> Union[bool, Tuple[bool, Optional[np.ndarray]]]:
pos = np.vstack([pt1.reshape(2,2), pt2.reshape(2,2)[::-1]])
flat = pos.flatten()
if flat.shape[0] != np.unique(flat).shape[0]:
return False if not return_data else (False, None)
vector = np.vstack(
[np.roll(pos, 2)-pos, np.roll(pos, -2)-pos]
).T
res = np.arctan2(vector[1], vector[0])*180/np.pi
res = 180 - (res - np.roll(res, 4))[:4]
res = np.where(res>=360, res-360, res)
if not int(np.sum(res)) == 360:
return False if not return_data else (False, res)
return True if not return_data else (True, res)
# ok
ncq1 = np.array([6.32, -2.02, 16.19, 1.14])
ncq2 = np.array([7.25, 4.36, 15.43, 5.01]))
print(is_convex(ncq1, ncq2))
# >>> True
# Bad(1)
ncq2 = np.array([7.25, 4.36, 16.8, -3.52])
print(is_convex(ncq1, ncq2))
# >>> False
# Bad(2)
ncq1 = np.array([21.13, -8.89, 33.1, -7.91]) # idx-1
ncq2 = np.array([25.29, -2.39, 36.9, -8.54]) # idx
print(is_convex(ncq1, ncq2))
# >>> False
Fix Non-convex Quadrilaterals
これでいけそう
非凸の頂点...簡単。反時計回りで行くなら時計回りになっている点を見つければそれが非凸の頂点。
二等分線...非凸の頂点と隣の2点から二等分線を求める
移動距離...少しづつ移動させる
# python 3.8 or higher
import numpy as np
def fix_non_convex(pt1: np.ndarray, pt2: np.ndarray):
isconvec, res = is_convex(pt1, pt2, True)
if isconvec or res is None:
return pt1, pt2
pos = np.vstack([pt1.reshape(2,2), pt2.reshape(2,2)[::-1]])
vector = np.vstack(
[np.roll(pos, 2)-pos, np.roll(pos, -2)-pos]
)
uvec, vvec = vector[:4], vector[4:]
a, b = np.linalg.norm(uvec, axis=1), np.linalg.norm(vvec, axis=1)
apb = a+b
a_apb, b_apb = (a/apb), (b/apb)
uvec_i2 = np.tile(b_apb, (2,1)).T * uvec
vvec_i2 = np.tile(a_apb, (2,1)).T * vvec
vector_i2 = uvec_i2 + vvec_i2
f_index = np.where(res>180)[0][0]
target_index = [f_index, f_index+1] if f_index != 3 else [f_index, 0]
param = 0.1
for i in range(1, 20):
k = pos.copy()
k[target_index] = k[target_index] + (vector_i2[target_index] * param * i)
p0, p1 = np.vsplit(k, 2)
p0 = p0.flatten()
p1 = p1[::-1].flatten()
if is_convex(p0, p1):
break
return p0, p1
# non-convex
ncq1 = np.array([21.13, -8.89, 33.1, -7.91]) # idx-1
ncq2 = np.array([25.29, -2.39, 36.9, -8.54]) # idx
print(ncq1, ncq2)
# >>> [21.13 -8.89 33.1 -7.91] [25.29 -2.39 36.9 -8.54]
p0, p1 = fix_non_convex(ncq1, ncq2)
print(p0, p1)
# >>> [21.13 -8.89 33.09113556 -8.12449794] [25.29 -2.39 35.22881418 -7.97556521]
Internal nodes
チェックポイントのパス構造には内部ノードは存在してはいけない(自身のコース製作で経験済み)。CPUとアイテムルートは別にパス関係が木構造になっていてもok.