Coverage for src/beamme/cosserat_curve/cosserat_curve.py: 98%
179 statements
« prev ^ index » next coverage.py v7.10.3, created at 2025-08-11 12:17 +0000
1# The MIT License (MIT)
2#
3# Copyright (c) 2018-2025 BeamMe Authors
4#
5# Permission is hereby granted, free of charge, to any person obtaining a copy
6# of this software and associated documentation files (the "Software"), to deal
7# in the Software without restriction, including without limitation the rights
8# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9# copies of the Software, and to permit persons to whom the Software is
10# furnished to do so, subject to the following conditions:
11#
12# The above copyright notice and this permission notice shall be included in
13# all copies or substantial portions of the Software.
14#
15# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21# THE SOFTWARE.
22"""Define a Cosserat curve object that can be used to describe warping of
23curve-like objects."""
25from typing import Optional as _Optional
26from typing import Tuple as _Tuple
28import numpy as _np
29import pyvista as _pv
30import quaternion as _quaternion
31from numpy.typing import NDArray as _NDArray
32from scipy import integrate as _integrate
33from scipy import interpolate as _interpolate
34from scipy import optimize as _optimize
36from beamme.core.conf import bme as _bme
37from beamme.core.rotation import Rotation as _Rotation
38from beamme.core.rotation import rotate_coordinates as _rotate_coordinates
39from beamme.core.rotation import smallest_rotation as _smallest_rotation
42def get_piecewise_linear_arc_length_along_points(
43 coordinates: _np.ndarray,
44) -> _np.ndarray:
45 """Return the accumulated distance between the points.
47 Args
48 ----
49 coordinates:
50 Array containing the point coordinates
51 """
53 n_points = len(coordinates)
54 point_distance = _np.linalg.norm(coordinates[1:] - coordinates[:-1], axis=1)
55 point_arc_length = _np.zeros(n_points)
56 for i in range(1, n_points):
57 point_arc_length[i] = point_arc_length[i - 1] + point_distance[i - 1]
58 return point_arc_length
61def get_spline_interpolation(
62 coordinates: _np.ndarray, point_arc_length: _np.ndarray
63) -> _interpolate.BSpline:
64 """Get a spline interpolation of the given points.
66 Args
67 ----
68 coordinates:
69 Array containing the point coordinates
70 point_arc_length:
71 Arc length for each coordinate
73 Return
74 ----
75 centerline_interpolation:
76 The spline interpolation object
77 """
79 # Interpolate coordinates along arc length
80 # Note: The numeric evaluation of the spline interpolation can depend on the
81 # operating system, thus introducing slight numerical differences (~1e-12).
82 centerline_interpolation = _interpolate.make_interp_spline(
83 point_arc_length, coordinates
84 )
85 return centerline_interpolation
88def get_quaternions_along_curve(
89 centerline: _interpolate.BSpline, point_arc_length: _np.ndarray
90) -> _NDArray[_quaternion.quaternion]:
91 """Get the quaternions along the curve based on smallest rotation mappings.
93 The initial rotation will be calculated based on the largest projection of the initial tangent
94 onto the cartesian basis vectors.
96 Args
97 ----
98 centerline:
99 A function that returns the centerline position for a parameter coordinate t
100 point_arc_length:
101 Array of parameter coordinates for which the quaternions should be calculated
102 """
104 centerline_interpolation_derivative = centerline.derivative()
106 def basis(i):
107 """Return the i-th Cartesian basis vector."""
108 basis = _np.zeros([3])
109 basis[i] = 1.0
110 return basis
112 # Get the reference rotation
113 t0 = centerline_interpolation_derivative(point_arc_length[0])
114 min_projection = _np.argmin(_np.abs([_np.dot(basis(i), t0) for i in range(3)]))
115 last_rotation = _Rotation.from_basis(t0, basis(min_projection))
117 # Get the rotation vectors along the curve. They are calculated with smallest rotation mappings.
118 n_points = len(point_arc_length)
119 quaternions = _np.zeros(n_points, dtype=_quaternion.quaternion)
120 quaternions[0] = last_rotation.q
121 for i in range(1, n_points):
122 rotation = _smallest_rotation(
123 last_rotation,
124 centerline_interpolation_derivative(point_arc_length[i]),
125 )
126 quaternions[i] = rotation.q
127 last_rotation = rotation
128 return quaternions
131def get_relative_distance_and_rotations(
132 coordinates: _np.ndarray, quaternions: _NDArray[_quaternion.quaternion]
133) -> _Tuple[
134 _np.ndarray, _NDArray[_quaternion.quaternion], _NDArray[_quaternion.quaternion]
135]:
136 """Get relative distances and rotations that can be used to evaluate
137 "intermediate" states of the Cosserat curve."""
139 n_points = len(coordinates)
140 relative_distances = _np.zeros(n_points - 1)
141 relative_distances_rotation = _np.zeros(n_points - 1, dtype=_quaternion.quaternion)
142 relative_rotations = _np.zeros(n_points - 1, dtype=_quaternion.quaternion)
144 for i_segment in range(n_points - 1):
145 relative_distance = coordinates[i_segment + 1] - coordinates[i_segment]
146 relative_distance_local = _quaternion.rotate_vectors(
147 quaternions[i_segment].conjugate(), relative_distance
148 )
149 relative_distances[i_segment] = _np.linalg.norm(relative_distance_local)
151 smallest_relative_rotation_onto_distance = _smallest_rotation(
152 _Rotation(),
153 relative_distance_local,
154 )
155 relative_distances_rotation[i_segment] = (
156 smallest_relative_rotation_onto_distance.get_numpy_quaternion()
157 )
159 relative_rotations[i_segment] = (
160 quaternions[i_segment].conjugate() * quaternions[i_segment + 1]
161 )
163 return relative_distances, relative_distances_rotation, relative_rotations
166class CosseratCurve(object):
167 """Represent a Cosserat curve in space."""
169 def __init__(
170 self,
171 point_coordinates: _np.ndarray,
172 *,
173 starting_triad_guess: _Optional[_Rotation] = None,
174 ):
175 """Initialize the Cosserat curve based on points in 3D space.
177 Args:
178 point_coordinates: Array containing the point coordinates
179 starting_triad_guess: Optional initial guess for the starting triad.
180 If provided, this introduces a constant twist angle along the curve.
181 The twist angle is computed between:
182 - The given starting guess triad, and
183 - The automatically calculated triad, rotated onto the first basis vector
184 of the starting guess triad using the smallest rotation.
185 """
187 self.coordinates = point_coordinates.copy()
188 self.n_points = len(self.coordinates)
190 # Interpolate coordinates along piece wise linear arc length
191 point_arc_length_piecewise_linear = (
192 get_piecewise_linear_arc_length_along_points(self.coordinates)
193 )
194 centerline_interpolation_piecewise_linear = get_spline_interpolation(
195 self.coordinates, point_arc_length_piecewise_linear
196 )
197 centerline_interpolation_piecewise_linear_p = (
198 centerline_interpolation_piecewise_linear.derivative(1)
199 )
201 def ds(t):
202 """Arc length along interpolated spline."""
203 return _np.linalg.norm(centerline_interpolation_piecewise_linear_p(t))
205 # Integrate the arc length along the interpolated centerline, this will result
206 # in a more accurate centerline arc length
207 self.point_arc_length = _np.zeros(self.n_points)
208 for i in range(len(point_arc_length_piecewise_linear) - 1):
209 self.point_arc_length[i + 1] = (
210 self.point_arc_length[i]
211 + _integrate.quad(
212 ds,
213 point_arc_length_piecewise_linear[i],
214 point_arc_length_piecewise_linear[i + 1],
215 )[0]
216 )
218 # Set the interpolation of the (positional) centerline
219 self.set_centerline_interpolation()
221 # Get the quaternions along the centerline based on smallest rotation mappings
222 self.quaternions = get_quaternions_along_curve(
223 self.centerline_interpolation, self.point_arc_length
224 )
226 # Get the relative quantities used to warp the curve
227 (
228 self.relative_distances,
229 self.relative_distances_rotation,
230 self.relative_rotations,
231 ) = get_relative_distance_and_rotations(self.coordinates, self.quaternions)
233 # Check if we have to apply a twist for the rotations
234 if starting_triad_guess is not None:
235 first_rotation = _Rotation.from_quaternion(self.quaternions[0])
236 starting_triad_e1 = starting_triad_guess * [1, 0, 0]
237 if _np.dot(first_rotation * [1, 0, 0], starting_triad_e1) < 0.5:
238 raise ValueError(
239 "The angle between the first basis vectors of the guess triad you"
240 " provided and the automatically calculated one is too large,"
241 " please check your input data."
242 )
243 smallest_rotation_to_guess_tangent = _smallest_rotation(
244 first_rotation, starting_triad_e1
245 )
246 relative_rotation = (
247 smallest_rotation_to_guess_tangent.inv() * starting_triad_guess
248 )
249 psi = relative_rotation.get_rotation_vector()
250 if _np.linalg.norm(psi[1:]) > _bme.eps_quaternion:
251 raise ValueError(
252 "The twist angle can not be extracted as the relative rotation is not plane!"
253 )
254 twist_angle = psi[0]
255 self.twist(twist_angle)
257 def set_centerline_interpolation(self):
258 """Set the interpolation of the centerline based on the coordinates and
259 arc length stored in this object."""
260 self.centerline_interpolation = get_spline_interpolation(
261 self.coordinates, self.point_arc_length
262 )
264 def translate(self, vector):
265 """Translate the curve by the given vector."""
267 self.coordinates += vector
268 self.set_centerline_interpolation()
270 def rotate(self, rotation: _Rotation, *, origin=None):
271 """Rotate the curve and the quaternions."""
273 self.quaternions = rotation.get_numpy_quaternion() * self.quaternions
274 self.coordinates = _rotate_coordinates(
275 self.coordinates, rotation, origin=origin
276 )
277 self.set_centerline_interpolation()
279 def twist(self, twist_angle: float) -> None:
280 """Apply a constant twist rotation along the Cosserat curve.
282 Args:
283 twist_angle: The rotation angle (in radiants).
284 """
285 material_twist_rotation = _Rotation(
286 [1, 0, 0], twist_angle
287 ).get_numpy_quaternion()
289 self.quaternions = self.quaternions * material_twist_rotation
290 self.relative_distances_rotation = (
291 material_twist_rotation.conjugate()
292 * self.relative_distances_rotation
293 * material_twist_rotation
294 )
295 self.relative_rotations = (
296 material_twist_rotation.conjugate()
297 * self.relative_rotations
298 * material_twist_rotation
299 )
301 def get_centerline_position_and_rotation(
302 self, arc_length: float, **kwargs
303 ) -> _Tuple[_np.ndarray, _NDArray[_quaternion.quaternion]]:
304 """Return the position and rotation at a given centerline arc
305 length."""
306 pos, rot = self.get_centerline_positions_and_rotations([arc_length], **kwargs)
307 return pos[0], rot[0]
309 def get_centerline_positions_and_rotations(
310 self, points_on_arc_length, *, factor=1.0
311 ) -> _Tuple[_np.ndarray, _NDArray[_quaternion.quaternion]]:
312 """Return the position and rotation at given centerline arc lengths.
314 If the points are outside of the valid interval, a linear extrapolation will be
315 performed for the displacements and the rotations will be held constant.
317 Args
318 ----
319 points_on_arc_length: list(float)
320 A sorted list with the arc lengths along the curve centerline
321 factor: float
322 Factor to scale the curvature along the curve.
323 factor == 1
324 Use the default positions and the triads obtained via a smallest rotation mapping
325 factor < 1
326 Integrate (piecewise constant as evaluated with get_relative_distance_and_rotations)
327 the scaled curvature of the curve to obtain a intuitive wrapping
328 """
330 # Get the points that are within the arc length of the given curve.
331 points_on_arc_length = _np.asarray(points_on_arc_length)
332 points_in_bounds = _np.logical_and(
333 points_on_arc_length > self.point_arc_length[0],
334 points_on_arc_length < self.point_arc_length[-1],
335 )
336 index_in_bound = _np.where(points_in_bounds == True)[0]
337 index_out_of_bound = _np.where(points_in_bounds == False)[0]
338 points_on_arc_length_in_bound = [
339 self.point_arc_length[0],
340 *points_on_arc_length[index_in_bound],
341 self.point_arc_length[-1],
342 ]
344 if factor < (1.0 - _bme.eps_quaternion):
345 coordinates = _np.zeros_like(self.coordinates)
346 quaternions = _np.zeros_like(self.quaternions)
347 coordinates[0] = self.coordinates[0]
348 quaternions[0] = self.quaternions[0]
349 for i_segment in range(self.n_points - 1):
350 relative_distance_rotation = _quaternion.slerp_evaluate(
351 _quaternion.quaternion(1),
352 self.relative_distances_rotation[i_segment],
353 factor,
354 )
355 # In the initial configuration (factor=0) we get a straight curve, so we need
356 # to use the arc length here. In the final configuration (factor=1) we want to
357 # exactly recover the input points, so we need the piecewise linear distance.
358 # Between them, we interpolate.
359 relative_distance = (factor * self.relative_distances[i_segment]) + (
360 1.0 - factor
361 ) * (
362 self.point_arc_length[i_segment + 1]
363 - self.point_arc_length[i_segment]
364 )
365 coordinates[i_segment + 1] = (
366 _quaternion.rotate_vectors(
367 quaternions[i_segment] * relative_distance_rotation,
368 [relative_distance, 0, 0],
369 )
370 + coordinates[i_segment]
371 )
372 quaternions[i_segment + 1] = quaternions[
373 i_segment
374 ] * _quaternion.slerp_evaluate(
375 _quaternion.quaternion(1),
376 self.relative_rotations[i_segment],
377 factor,
378 )
379 else:
380 coordinates = self.coordinates
381 quaternions = self.quaternions
383 sol_r = _np.zeros([len(points_on_arc_length_in_bound), 3])
384 sol_q = _np.zeros(
385 len(points_on_arc_length_in_bound), dtype=_quaternion.quaternion
386 )
387 arc_length_spline_interpolation = get_spline_interpolation(
388 coordinates, self.point_arc_length
389 )
390 for i_point, centerline_arc_length in enumerate(points_on_arc_length_in_bound):
391 if (
392 centerline_arc_length >= self.point_arc_length[0]
393 and centerline_arc_length <= self.point_arc_length[-1]
394 ):
395 for i in range(1, self.n_points):
396 centerline_index = i - 1
397 if self.point_arc_length[i] > centerline_arc_length:
398 break
400 # Get the two rotation vectors and arc length values
401 arc_lengths = self.point_arc_length[
402 centerline_index : centerline_index + 2
403 ]
404 q1 = quaternions[centerline_index]
405 q2 = quaternions[centerline_index + 1]
407 # Linear interpolate the arc length
408 xi = (centerline_arc_length - arc_lengths[0]) / (
409 arc_lengths[1] - arc_lengths[0]
410 )
412 # Perform a spline interpolation for the positions and a slerp
413 # interpolation for the rotations
414 sol_r[i_point] = arc_length_spline_interpolation(centerline_arc_length)
415 sol_q[i_point] = _quaternion.slerp_evaluate(q1, q2, xi)
416 else:
417 raise ValueError("Centerline value out of bounds")
419 # Set the already computed results in the final data structures
420 sol_r_final = _np.zeros([len(points_on_arc_length), 3])
421 sol_q_final = _np.zeros(len(points_on_arc_length), dtype=_quaternion.quaternion)
422 if len(index_in_bound) > 0:
423 sol_r_final[index_in_bound] = sol_r[index_in_bound - index_in_bound[0] + 1]
424 sol_q_final[index_in_bound] = sol_q[index_in_bound - index_in_bound[0] + 1]
426 # Perform the extrapolation at both ends of the curve
427 for i in index_out_of_bound:
428 arc_length = points_on_arc_length[i]
429 if arc_length <= self.point_arc_length[0]:
430 index = 0
431 elif arc_length >= self.point_arc_length[-1]:
432 index = -1
433 else:
434 raise ValueError("Should not happen")
436 length = arc_length - self.point_arc_length[index]
437 r = sol_r[index]
438 q = sol_q[index]
439 sol_r_final[i] = r + _Rotation.from_quaternion(q) * [length, 0, 0]
440 sol_q_final[i] = q
442 return sol_r_final, sol_q_final
444 def project_point(self, p, t0=None) -> float:
445 """Project a point to the curve, return the parameter coordinate for
446 the projection point."""
448 centerline_interpolation_p = self.centerline_interpolation.derivative(1)
449 centerline_interpolation_pp = self.centerline_interpolation.derivative(2)
451 def f(t):
452 """Function to find the root of."""
453 r = self.centerline_interpolation(t)
454 rp = centerline_interpolation_p(t)
455 return _np.dot(r - p, rp)
457 def fp(t):
458 """Derivative of the Function to find the root of."""
459 r = self.centerline_interpolation(t)
460 rp = centerline_interpolation_p(t)
461 rpp = centerline_interpolation_pp(t)
462 return _np.dot(rp, rp) + _np.dot(r - p, rpp)
464 if t0 is None:
465 t0 = 0.0
467 return _optimize.newton(f, t0, fprime=fp)
469 def get_pyvista_polyline(self) -> _pv.PolyData:
470 """Create a pyvista (vtk) representation of the curve with the
471 evaluated triad basis vectors."""
473 poly_line = _pv.PolyData()
474 poly_line.points = self.coordinates
475 cell = _np.arange(0, self.n_points, dtype=int)
476 cell = _np.insert(cell, 0, self.n_points)
477 poly_line.lines = cell
479 rotation_matrices = _np.zeros((len(self.quaternions), 3, 3))
480 for i_quaternion, q in enumerate(self.quaternions):
481 R = _quaternion.as_rotation_matrix(q)
482 rotation_matrices[i_quaternion] = R
484 for i_dir in range(3):
485 poly_line.point_data.set_array(
486 rotation_matrices[:, :, i_dir], f"base_vector_{i_dir + 1}"
487 )
489 return poly_line
491 def write_vtk(self, path) -> None:
492 """Save a vtk representation of the curve."""
493 self.get_pyvista_polyline().save(path)