8000 add array2str and bresenham · ZombyDogs/spatialmath-python@ac5a79e · GitHub
[go: up one dir, main page]
Skip to content 10000

Commit ac5a79e

Browse files
petercorkepetercorke
committed
add array2str and bresenham
1 parent 068e0f7 commit ac5a79e

File tree

1 file changed

+129
-0
lines changed

1 file changed

+129
-0
lines changed

‎spatialmath/base/numeric.py

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,3 +58,132 @@ def numjac(f, x, dx=1e-8, SO=0, SE=0):
5858
# print(Ji)
5959

6060
return np.c_[Jcol].T
61+
62+
def array2str(X, valuesep=", ", rowsep=" | ", fmt="{:.3g}",
63+
brackets=("[ ", " ]"), suppress_small=True):
64+
"""
65+
Convert array to single line string
66+
67+
:param X: 1D or 2D array to convert
68+
:type X: ndarray(N,M), array_like(N)
69+
:param valuesep: separator between numbers, defaults to ", "
70+
:type valuesep: str, optional
71+
:param rowsep: separator between rows, defaults to " | "
72+
:type rowsep: str, optional
73+
:param format: format string, defaults to "{:.3g}"
74+
:type precision: str, optional
75+
:param brackets: strings to be added to start and end of the string,
76+
defaults to ("[ ", " ]"). Set to None to suppress brackets.
77+
:type brackets: list, tuple of str
78+
:param suppress_small: small values (:math:`|x| < 10^{-12}` are converted
79+
to zero, defaults to True
80+
:type suppress_small: bool, optional
81+
:return: compact string representation of array
82+
:rtype: str
83+
84+
Converts a small array to a compact single line representation.
85+
"""
86+
# convert to ndarray if not already
87+
if isinstance(X, (list, tuple)):
88+
X = base.getvector(X)
89+
90+
def format_row(x):
91+
s = ""
92+
for j, e in enumerate(x):
93+
if abs(e) < 1e-12:
94+
e = 0
95+
if j > 0:
96+
s += valuesep
97+
s += fmt.format(e)
98+
return s
99+
100+
if X.ndim == 1:
101+
# 1D case
102+
s = format_row(X)
103+
else:
104+
# 2D case
105+
s = ""
106+
for i, row in enumerate(X):
107+
if i > 0:
108+
s += rowsep
109+
s += format_row(row)
110+
111+
if brackets is not None and len(brackets) == 2:
112+
s = brackets[0] + s + brackets[1]
113+
return s
114+
115+
def bresenham(p0, p1, array=None):
116+
"""
117+
Line drawing in a grid
118+
119+
:param p0: initial point
120+
:type p0: array_like(2) of int
121+
:param p1: end point
122+
:type p1: array_like(2) of int
123+
:return: arrays of x and y coordinates for points along the line
124+
:rtype: ndarray(N), ndarray(N) of int
125+
126+
Return x and y coordinate vectors for points in a grid that lie on
127+
a line from ``p0`` to ``p1`` inclusive.
128+
129+
The end points, and all points along the line are integers.
130+
131+
.. note:: The API is similar to the Bresenham algorithm but this
132+
implementation uses NumPy vectorised arithmetic which makes it
133+
faster than the Bresenham algorithm in Python.
134+
"""
135+
x0, y0 = p0
136+
x1, y1 = p1
137+
138+
if array is not None:
139+
_ = array[y0, x0] + array[y1, x1]
140+
141+
line = []
142+
143+
dx = x1 - x0
144+
dy = y1 - y0
145+
146+
if abs(dx) >= abs(dy):
147+
# shallow line -45° <= θ <= 45°
148+
# y = mx + c
149+
if dx == 0:
150+
# case p0 == p1
151+
x = np.r_[x0]
152+
y = np.r_[y0]
153+
else:
154+
m = dy / dx
155+
c = y0 - m * x0
156+
if dx > 0:
157+
# line to the right
158+
x = np.arange(x0, x1 + 1)
159+
elif dx < 0:
160+
# line to the left
161+
x = np.arange(x0, x1 - 1, -1)
162+
y = np.round(x * m + c)
163+
164+
else:
165+
# steep line θ < -45°, θ > 45°
166+
# x = my + c
167+
m = dx / dy
168+
c = x0 - m * y0
169+
if dy > 0:
170+
# line to the right
171+
y = np.arange(y0, y1 + 1)
172+
elif dy < 0:
173+
# line to the left
174+
y = np.arange(y0, y1 - 1, -1)
175+
x = np.round(y * m + c)
176+
177+
return x.astype(int), y.astype(int)
178+
179+
if __name__ == "__main__":
180+
181+
print(bresenham([2,2], [2,4]))
182+
print(bresenham([2,2], [2,-4]))
183+
print(bresenham([2,2], [4,2]))
184+
print(bresenham([2,2], [-4,2]))
185+
print(bresenham([2,2], [2,2]))
186+
print(bresenham([2,2], [3,6])) # steep
187+
print(bresenham([2,2], [6,3])) # shallow
188+
print(bresenham([2,2], [3,6])) # steep
189+
print(bresenham([2,2], [6,3])) # shallow

0 commit comments

Comments
 (0)
0