Coverage for /opt/hostedtoolcache/Python/3.11.10/x64/lib/python3.11/site-packages/hypervehicle/geometry/path.py: 69%

1import numpy as np

2from typing import Optional

3from abc import ABC, abstractmethod

4from hypervehicle.geometry.vector import Vector3, cross_product

7class Path(ABC):

8 """Abstract parameteric path."""

10 @abstractmethod

11 def __repr__(self):

12 pass

14 @abstractmethod

15 def __call__(self, t):

16 pass

18 def length(self, n=20):

19 """Rudimentary evaluation of path length by sampling and summing."""

20 length = 0.0

21 p0 = self.__call__(0.0)

22 dt = 1.0 / n

23 for i in range(n):

24 p1 = self.__call__((i + 1) * dt)

25 length += abs(p1 - p0)

26 p0 = p1

27 return length

30class Line(Path):

31 """Straight line path between two points."""

33 __slots__ = ["p0", "p1"]

35 def __init__(self, p0, p1):

36 self.p0 = Vector3(p0)

37 self.p1 = Vector3(p1)

39 def __repr__(self):

40 return "Line(p0={}, p1={})".format(self.p0, self.p1)

42 def __call__(self, t: float):

43 return self.p0 * (1 - t) + self.p1 * t

45 def length(self):

46 return abs(self.p1 - self.p0)

48 def __add__(self, offset: Vector3):

49 if isinstance(offset, Vector3):

50 p0 = self.p0 + offset

51 p1 = self.p1 + offset

52 return Line(p0=p0, p1=p1)

53 else:

54 raise ValueError(f"Cannot add a {type(offset)} to a line.")

57class Bezier(Path):

58 """Bezier curve defined on a list of points."""

60 __slots__ = ["B"]

62 def __init__(self, B):

63 try:

64 self.B = [Vector3(p) for p in B]

65 except Exception as e:

66 raise ValueError(

67 f"Was expecting to get a list of points for B, but got {B}"

68 )

70 def __repr__(self):

71 return f"Bezier(B={self.B})"

73 def __call__(self, t):

74 if len(self.B) == 1:

75 return self.B[0]

76 n_order = len(self.B) - 1

77 # Apply de Casteljau's algorithm.

78 Q = self.B.copy() # work array will be overwritten

79 for k in range(n_order):

80 for i in range(n_order - k):

81 Q[i] = Q[i] * (1.0 - t) + Q[i + 1] * t

82 return Q[0]

85class Polyline(Path):

86 """Collection of Path segments."""

88 __slots__ = ["segments", "t_values", "isclosed"]

90 def __init__(self, segments: list[Path], closed=False, tolerance=1.0e-10):

91 self.segments: list[Path] = []

92 for seg in segments:

93 self.segments.append(seg)

94 self.isclosed = closed

95 if self.isclosed:

96 p0 = self.segments[0](0.0)

97 p1 = self.segments[-1](1.0)

98 if abs(p1 - p0) > tolerance:

99 self.segments.append(Line(p0, p1))

100 self.reset_breakpoints()

101

102 def reset_breakpoints(self):

103 self.t_values = [0.0]

104 t_total = 0.0

105 for seg in self.segments:

106 t_total += seg.length()

107 self.t_values.append(t_total)

108 for i in range(len(self.t_values)):

109 self.t_values[i] /= t_total

110

111 def __repr__(self):

112 text = "Polyline(segments=["

113 n = len(self.segments)

114 for i in range(n):

115 text += "{}".format(self.segments[i])

116 text += ", " if i < n - 1 else "]"

117 return text

118

119 def __call__(self, t):

120 n = len(self.segments)

121 if n == 1:

122 return self.segments[0](t)

123

124 f = self.segments[0](t)

125 f *= 0.0

126

127 for i, tl, tu in zip(range(n), self.t_values[:-1], self.t_values[1:]):

128 t_local = (t - tl) / (tu - tl)

129 if i == 0:

130 isokay = t_local <= 1.0

131 elif i == n - 1:

132 isokay = t_local > 0.0

133 else:

134 isokay = np.logical_and(t_local > 0.0, t_local <= 1.0)

135 f += self.segments[i](t_local) * isokay

136 return f

137

138 def length(self):

139 L = 0.0

140 for seg in self.segments:

141 L += seg.length()

142 return L

143

144

145class Spline(Polyline):

146 """Construct a spline of Bezier segments from a sequence of points."""

147

148 def __init__(

149 self,

150 points,

151 closed: Optional[bool] = False,

152 tolerance: Optional[float] = 1.0e-10,

153 ):

154 """Given m+1 interpolation points p, determine the m-segment

155 Bezier polyline that interpolates these points as a spline.

156 This is done by first determining the array of weight points

157 which define the spline and then evaluating the cubic

158 Bezier segments.

159

160 For a natural spline, the first and last weight points

161 are also the first and last interpolation points.

162 And, for the initial guess at the remaining weight points,

163 just use the supplied data points.

164 This amounts to copying the whole p collection.

165

166 References

167 ----------

168 G. Engelin & F. Uhlig (1996)

169 Numerical Algorithms with C

170 Springer, Berlin

171 Section 12.3.1

172 """

173 self.points = [Vector3(p) for p in points]

174 if closed and (abs(self.points[0] - self.points[-1]) > tolerance):

175 self.points.append(Vector3(points[0]))

176 self.closed = closed

177

178 m = len(self.points) - 1

179 d = [Vector3(p) for p in self.points]

180

181 # Apply Gauss-Seidel iteration until

182 # the internal weight points converge.

183 for j in range(50):

184 max_diff = 0.0

185 for i in range(1, m):

186 old_p = Vector3(d[i])

187 d[i] = 0.25 * (6.0 * self.points[i] - d[i - 1] - d[i + 1])

188 max_diff = max(max_diff, abs(d[i] - old_p))

189 if max_diff < tolerance:

190 break

191

192 # Final stage; calculate the cubic Bezier segments.

193 segments = [

194 Bezier(

195 [

196 self.points[i],

197 (2.0 * d[i] + d[i + 1]) / 3.0,

198 (d[i] + 2.0 * d[i + 1]) / 3.0,

199 self.points[i + 1],

200 ]

201 )

202 for i in range(m)

203 ]

204 super().__init__(segments, closed, tolerance)

205

206 def __repr__(self):

207 return f"Spline(points={self.points}, closed={self.closed})"

208

209

210class Arc(Path):

211 """An arc from point a to point b, with a centre at point c."""

212

213 __slots__ = ["a", "b", "c"]

214

215 def __init__(self, a: Vector3, b: Vector3, c: Vector3):

216 self.a = Vector3(a)

217 self.b = Vector3(b)

218 self.c = Vector3(c)

219

220 def __repr__(self):

221 return f"Arc(a={self.a}, b={self.b}, c={self.c})"

222

223 def __call__(self, t: float):

224 p, L = self.evaluate_position_and_length(t)

225 return p

226

227 def length(self):

228 p, L = self.evaluate_position_and_length(1.0)

229 return L

230

231 def evaluate_position_and_length(self, t: float):

232 l = 0.0

233 ca = self.a - self.c

234 ca_mag = abs(ca)

235 cb = self.b - self.c

236 cb_mag = abs(cb)

237 if abs(ca_mag - cb_mag) > 1.0e-5:

238 raise Exception(f"Arc: radii do not match |ca|={abs(ca)} |cb|={abs(cb)}")

239

240 # First vector in plane.

241 tangent1 = Vector3(ca)

242 tangent1.normalize()

243

244 # Compute unit normal to plane of all three points

245 n = cross_product(ca, cb)

246 if abs(n) > 0.0:

247 n.normalize()

248 else:

249 raise Exception(

250 "Arc: cannot find plane of three points. Maybe you are trying "

251 + "to define an arc with 180 degrees?"

252 )

253

254 # Third (orthogonal) vector is in the original plane

255 tangent2 = cross_product(n, tangent1)

256

257 # Now transform to local coordinates to do the calculation of the point along

258 # the arc in the local xy-plane, with ca along the x-axis

259 cb_local = Vector3(cb)

260 cb_local.transform_to_local_frame(tangent1, tangent2, n)

261 if np.any(np.absolute(cb_local.z) > 1.0e-6):

262 raise Exception(f"Arc: problem with transformation cb_local={cb_local}")

263

264 # Angle of the final point on the arc is in the range -pi < th <= +pi

265 theta = np.arctan2(cb_local.y, cb_local.x)

266

267 # The length of the circular arc

268 l = theta * cb_mag

269

270 # Move the second point around the arc in the local xy-plane

271 theta *= t

272 loc = Vector3(np.cos(theta) * cb_mag, np.sin(theta) * cb_mag, 0.0 * theta)

273

274 # Transform back to global xyz coordinates and add the centre coordinates

275 loc.transform_to_global_frame(tangent1, tangent2, n, self.c)

276

277 return loc, l

278

279

280class ArcLengthParameterizedPath(Path):

281 """A Path reparameterized such that equal increments in t correspond

282 to approximately equal increments in arc length.

283 """

284

285 __slots__ = ["underlying_path", "arc_lengths", "t_values", "_n"]

286

287 def __init__(self, underlying_path, n=1000):

288 if isinstance(underlying_path, Path):

289 self.underlying_path = underlying_path

290 if n < 1:

291 raise RuntimeError("Should have at least one arc-length sample.")

292 self._n = n

293 self.set_arc_lengths()

294 else:

295 raise NotImplementedError("underlying_path should be a type of Path")

296 return

297

298 def set_arc_lengths(self):

299 """

300 Compute the arc lengths for a number of sample points along the Path

301 (in equally-spaced values of t) so that these can later be used to do

302 a reverse interpolation on the evaluation parameter.

303 """

304 dt = 1.0 / self._n

305 L = 0.0

306 self.arc_lengths = [

307 0.0,

308 ]

309 self.t_values = [

310 0.0,

311 ]

312 p0 = self.underlying_path(0.0)

313 for i in range(1, self._n + 1):

314 p1 = self.underlying_path(dt * i)

315 L += abs(p1 - p0)

316 self.arc_lengths.append(L)

317 self.t_values.append(dt * i)

318 p0 = p1

319 self.t_values = np.array(self.t_values)

320 self.arc_lengths = np.array(self.arc_lengths)

321 return

322

323 def underlying_t(self, t):

324 """

325 Search the pieces of arc length to find the piece containing the

326 desired point and then interpolate the local value of t for that piece.

327 """

328 # The incoming parameter value, t, is proportional to arc_length fraction.

329 L_target = t * self.arc_lengths[-1]

330

331 # Do a single variable linear interpolation to approximate an ordinary t value

332 ut = np.interp(L_target, self.arc_lengths, self.t_values, left=0.0, right=1.0)

333 return ut

334

335 def __repr__(self):

336 return "ArcLengthParameterizedPath(underlying_path={}, n={})".format(

337 self.underlying_path, self._n

338 )

339

340 def __call__(self, t):

341 return self.underlying_path(self.underlying_t(t))

342

343 def length(self):

344 return self.underlying_path.length()

345

346

347def roberts(eta: float, alpha: float, beta: float):

348 """Computes the stretched coordinate in the range [0.0..1.0]

349 using the boundary-layer-like transformation devised by Roberts.

350

351 Parameters

352 ------------

353 eta : float

354 unstretched coordinate, 0 <= eta <= 1.0

355

356 beta : float

357 stretching factor (more stretching as beta --> 1.0)

358

359 alpha : float

360 Location of stretching

361 | alpha = 0.5: clustering of nodes at both extremes of eta

362 | alpha = 0.0: nodes will be clustered near eta=1.0

363 """

364 lmbda = (beta + 1.0) / (beta - 1.0)

365 lmbda = np.power(lmbda, ((eta - alpha) / (1.0 - alpha)))

366 etabar = (beta + 2.0 * alpha) * lmbda - beta + 2.0 * alpha

367 etabar = etabar / ((2.0 * alpha + 1.0) * (1.0 + lmbda))

368 return etabar

369

370

371def clustered_roberts(eta: float, end1: bool, end2: bool, beta: float):

372 """Compute a stretched ordinate.

373

374 Parameters

375 ------------

376 eta : float

377 Unstretched ordinate, scalar or array.

378

379 end1 : bool

380 Cluster flag for end 1. If False, points are not clustered to end 1.

381 If True, points ARE clustered to end 1.

382

383 end2 : bool

384 Cluster flag for end 2. If False, points are not clustered to end 2.

385 If True, points ARE clustered to end 2.

386

387 beta : float

388 Grid stretching parameter:

389 1 < beta < +inf : points are clustered

390 The closer to 1, the more the clustering.

391 beta < 1 for no clustering.

392 """

393 # Decide on stretching parameters for Robert's transform.

394 alpha = 0.5

395 reverse = False

396 cluster = True

397 if ((not end1) and (not end2)) or (beta < 1.0):

398 cluster = False

399 if end1 and end2:

400 alpha = 0.5

401 if end1 and (not end2):

402 reverse = True

403 alpha = 0.0

404 if (not end1) and end2:

405 reverse = False

406 alpha = 0.0

407 if cluster:

408 if reverse:

409 eta = 1.0 - eta

410 etabar = roberts(eta, alpha, beta)

411 if reverse:

412 etabar = 1.0 - etabar

413 else:

414 etabar = eta

415 return etabar

416

417

418def distribute_points(t1: float, t2: float, n: int, end1, end2, beta):

419 """Generate a set of n+1 points nonuniformly distributed from t1 to t2.

420

421 Parameters

422 -----------

423 t1 : float

424 Parameter value 1.

425

426 t2 : float

427 Parameter value 2.

428

429 n : int

430 Number of intervals (the number of points is n+1).

431

432 end1 : bool

433 Cluster flag for end 1. If False, points are not clustered to end 1.

434 If True, points ARE clustered to end 1.

435

436 end2 : bool

437 Cluster flag for end 2. If False, points are not clustered to end 2.

438 If True, points ARE clustered to end 2.

439

440 beta : float

441 Grid stretching parameter:

442 1 < beta < +inf : points are clustered

443 The closer to 1, the more the clustering.

444 beta < 1 for no clustering.

445

446 Returns the array of n+1 distributed values.

447 """

448 # Compute the grid points as an array.

449 etabar = clustered_roberts(np.linspace(0.0, 1.0, n + 1), end1, end2, beta)

450

451 # Compute the parameter value within the given end-points.

452 x = (1.0 - etabar) * t1 + etabar * t2

453 return x

454

455

456class ClusterFunction(ABC):

457 @abstractmethod

458 def __repr__(self):

459 pass

460

461 @abstractmethod

462 def __call__(self, x):

463 pass

464

465 @abstractmethod

466 def distribute_parameter_values(self, nv):

467 pass

468

469

470class LinearFunction(ClusterFunction):

471 def __init__(self):

472 return

473

474 def __repr__(self):

475 return "LinearFunction()"

476

477 def __call__(self, x):

478 """

479 Simply returns the value, be it scalar or array.

480 """

481 return x

482

483 def distribute_parameter_values(self, nv):

484 """

485 Returns an array of uniformly-distributed sample points in range [0.0 ... 1.0].

486 """

487 return np.linspace(0.0, 1.0, nv)

488

489

490class RobertsFunction(ClusterFunction):

491 """Roberts' boundary-layer-like clustering function.

492

493 Note that, compared with the old definition that we delegate the actual work to,

494 the ends are renamed 0, 1 to align with the Eilmer4 notation.

495 """

496

497 _slots_ = ["end0", "end1", "beta"]

498

499 def __init__(self, end0, end1, beta):

500 """

501 Store the cluster parameters for later use.

502 """

503 self.end0 = end0

504 self.end1 = end1

505 self.beta = beta

506 return

507

508 def __repr__(self):

509 return f"RobertsFunction(end0={self.end0}, end1={self.end1}, beta={self.beta})"

510

511 def __call__(self, x):

512 return clustered_roberts(x, self.end0, self.end1, self.beta)

513

514 def distribute_parameter_values(self, nv):

515 """Returns an array of clustered sample points in range [0.0 ... 1.0]."""

516 return distribute_points(0.0, 1.0, nv - 1, self.end0, self.end1, self.beta)