Coverage for src/meshpy/cosserat_curve/cosserat_curve.py: 98%
178 statements
« prev ^ index » next coverage.py v7.9.0, created at 2025-06-13 04:26 +0000
1# The MIT License (MIT)
2#
3# Copyright (c) 2018-2025 MeshPy 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 meshpy.core.conf import mpy as _mpy
37from meshpy.core.rotation import Rotation as _Rotation
38from meshpy.core.rotation import rotate_coordinates as _rotate_coordinates
39from meshpy.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 centerline_interpolation = _interpolate.make_interp_spline(
81 point_arc_length, coordinates
82 )
83 return centerline_interpolation
86def get_quaternions_along_curve(
87 centerline: _interpolate.BSpline, point_arc_length: _np.ndarray
88) -> _NDArray[_quaternion.quaternion]:
89 """Get the quaternions along the curve based on smallest rotation mappings.
91 The initial rotation will be calculated based on the largest projection of the initial tangent
92 onto the cartesian basis vectors.
94 Args
95 ----
96 centerline:
97 A function that returns the centerline position for a parameter coordinate t
98 point_arc_length:
99 Array of parameter coordinates for which the quaternions should be calculated
100 """
102 centerline_interpolation_derivative = centerline.derivative()
104 def basis(i):
105 """Return the i-th Cartesian basis vector."""
106 basis = _np.zeros([3])
107 basis[i] = 1.0
108 return basis
110 # Get the reference rotation
111 t0 = centerline_interpolation_derivative(point_arc_length[0])
112 min_projection = _np.argmin(_np.abs([_np.dot(basis(i), t0) for i in range(3)]))
113 last_rotation = _Rotation.from_basis(t0, basis(min_projection))
115 # Get the rotation vectors along the curve. They are calculated with smallest rotation mappings.
116 n_points = len(point_arc_length)
117 quaternions = _np.zeros(n_points, dtype=_quaternion.quaternion)
118 quaternions[0] = last_rotation.q
119 for i in range(1, n_points):
120 rotation = _smallest_rotation(
121 last_rotation,
122 centerline_interpolation_derivative(point_arc_length[i]),
123 )
124 quaternions[i] = rotation.q
125 last_rotation = rotation
126 return quaternions
129def get_relative_distance_and_rotations(
130 coordinates: _np.ndarray, quaternions: _NDArray[_quaternion.quaternion]
131) -> _Tuple[
132 _np.ndarray, _NDArray[_quaternion.quaternion], _NDArray[_quaternion.quaternion]
133]:
134 """Get relative distances and rotations that can be used to evaluate
135 "intermediate" states of the Cosserat curve."""
137 n_points = len(coordinates)
138 relative_distances = _np.zeros(n_points - 1)
139 relative_distances_rotation = _np.zeros(n_points - 1, dtype=_quaternion.quaternion)
140 relative_rotations = _np.zeros(n_points - 1, dtype=_quaternion.quaternion)
142 for i_segment in range(n_points - 1):
143 relative_distance = coordinates[i_segment + 1] - coordinates[i_segment]
144 relative_distance_local = _quaternion.rotate_vectors(
145 quaternions[i_segment].conjugate(), relative_distance
146 )
147 relative_distances[i_segment] = _np.linalg.norm(relative_distance_local)
149 smallest_relative_rotation_onto_distance = _smallest_rotation(
150 _Rotation(),
151 relative_distance_local,
152 )
153 relative_distances_rotation[i_segment] = (
154 smallest_relative_rotation_onto_distance.get_numpy_quaternion()
155 )
157 relative_rotations[i_segment] = (
158 quaternions[i_segment].conjugate() * quaternions[i_segment + 1]
159 )
161 return relative_distances, relative_distances_rotation, relative_rotations
164class CosseratCurve(object):
165 """Represent a Cosserat curve in space."""
167 def __init__(
168 self,
169 point_coordinates: _np.ndarray,
170 *,
171 starting_triad_guess: _Optional[_Rotation] = None,
172 ):
173 """Initialize the Cosserat curve based on points in 3D space.
175 Args:
176 point_coordinates: Array containing the point coordinates
177 starting_triad_guess: Optional initial guess for the starting triad.
178 If provided, this introduces a constant twist angle along the curve.
179 The twist angle is computed between:
180 - The given starting guess triad, and
181 - The automatically calculated triad, rotated onto the first basis vector
182 of the starting guess triad using the smallest rotation.
183 """
185 self.coordinates = point_coordinates.copy()
186 self.n_points = len(self.coordinates)
188 # Interpolate coordinates along piece wise linear arc length
189 point_arc_length_piecewise_linear = (
190 get_piecewise_linear_arc_length_along_points(self.coordinates)
191 )
192 centerline_interpolation_piecewise_linear = get_spline_interpolation(
193 self.coordinates, point_arc_length_piecewise_linear
194 )
195 centerline_interpolation_piecewise_linear_p = (
196 centerline_interpolation_piecewise_linear.derivative(1)
197 )
199 def ds(t):
200 """Arc length along interpolated spline."""
201 return _np.linalg.norm(centerline_interpolation_piecewise_linear_p(t))
203 # Integrate the arc length along the interpolated centerline, this will result
204 # in a more accurate centerline arc length
205 self.point_arc_length = _np.zeros(self.n_points)
206 for i in range(len(point_arc_length_piecewise_linear) - 1):
207 self.point_arc_length[i + 1] = (
208 self.point_arc_length[i]
209 + _integrate.quad(
210 ds,
211 point_arc_length_piecewise_linear[i],
212 point_arc_length_piecewise_linear[i + 1],
213 )[0]
214 )
216 # Set the interpolation of the (positional) centerline
217 self.set_centerline_interpolation()
219 # Get the quaternions along the centerline based on smallest rotation mappings
220 self.quaternions = get_quaternions_along_curve(
221 self.centerline_interpolation, self.point_arc_length
222 )
224 # Get the relative quantities used to warp the curve
225 (
226 self.relative_distances,
227 self.relative_distances_rotation,
228 self.relative_rotations,
229 ) = get_relative_distance_and_rotations(self.coordinates, self.quaternions)
231 # Check if we have to apply a twist for the rotations
232 if starting_triad_guess is not None:
233 first_rotation = _Rotation.from_quaternion(self.quaternions[0])
234 starting_triad_e1 = starting_triad_guess * [1, 0, 0]
235 if _np.dot(first_rotation * [1, 0, 0], starting_triad_e1) < 0.5:
236 raise ValueError(
237 "The angle between the first basis vectors of the guess triad you"
238 " provided and the automatically calculated one is too large,"
239 " please check your input data."
240 )
241 smallest_rotation_to_guess_tangent = _smallest_rotation(
242 first_rotation, starting_triad_e1
243 )
244 relative_rotation = (
245 smallest_rotation_to_guess_tangent.inv() * starting_triad_guess
246 )
247 psi = relative_rotation.get_rotation_vector()
248 if _np.linalg.norm(psi[1:]) > _mpy.eps_quaternion:
249 raise ValueError(
250 "The twist angle can not be extracted as the relative rotation is not plane!"
251 )
252 twist_angle = psi[0]
253 self.twist(twist_angle)
255 def set_centerline_interpolation(self):
256 """Set the interpolation of the centerline based on the coordinates and
257 arc length stored in this object."""
258 self.centerline_interpolation = get_spline_interpolation(
259 self.coordinates, self.point_arc_length
260 )
262 def translate(self, vector):
263 """Translate the curve by the given vector."""
265 self.coordinates += vector
266 self.set_centerline_interpolation()
268 def rotate(self, rotation: _Rotation, *, origin=None):
269 """Rotate the curve and the quaternions."""
271 self.quaternions = rotation.get_numpy_quaternion() * self.quaternions
272 self.coordinates = _rotate_coordinates(
273 self.coordinates, rotation, origin=origin
274 )
275 self.set_centerline_interpolation()
277 def twist(self, twist_angle: float) -> None:
278 """Apply a constant twist rotation along the Cosserat curve.
280 Args:
281 twist_angle: The rotation angle (in radiants).
282 """
283 material_twist_rotation = _Rotation(
284 [1, 0, 0], twist_angle
285 ).get_numpy_quaternion()
287 self.quaternions = self.quaternions * material_twist_rotation
288 self.relative_distances_rotation = (
289 material_twist_rotation.conjugate()
290 * self.relative_distances_rotation
291 * material_twist_rotation
292 )
293 self.relative_rotations = (
294 material_twist_rotation.conjugate()
295 * self.relative_rotations
296 * material_twist_rotation
297 )
299 def get_centerline_position_and_rotation(
300 self, arc_length: float, **kwargs
301 ) -> _Tuple[_np.ndarray, _NDArray[_quaternion.quaternion]]:
302 """Return the position and rotation at a given centerline arc
303 length."""
304 pos, rot = self.get_centerline_positions_and_rotations([arc_length], **kwargs)
305 return pos[0], rot[0]
307 def get_centerline_positions_and_rotations(
308 self, points_on_arc_length, *, factor=1.0
309 ) -> _Tuple[_np.ndarray, _NDArray[_quaternion.quaternion]]:
310 """Return the position and rotation at given centerline arc lengths.
312 If the points are outside of the valid interval, a linear extrapolation will be
313 performed for the displacements and the rotations will be held constant.
315 Args
316 ----
317 points_on_arc_length: list(float)
318 A sorted list with the arc lengths along the curve centerline
319 factor: float
320 Factor to scale the curvature along the curve.
321 factor == 1
322 Use the default positions and the triads obtained via a smallest rotation mapping
323 factor < 1
324 Integrate (piecewise constant as evaluated with get_relative_distance_and_rotations)
325 the scaled curvature of the curve to obtain a intuitive wrapping
326 """
328 # Get the points that are within the arc length of the given curve.
329 points_on_arc_length = _np.asarray(points_on_arc_length)
330 points_in_bounds = _np.logical_and(
331 points_on_arc_length > self.point_arc_length[0],
332 points_on_arc_length < self.point_arc_length[-1],
333 )
334 index_in_bound = _np.where(points_in_bounds == True)[0]
335 index_out_of_bound = _np.where(points_in_bounds == False)[0]
336 points_on_arc_length_in_bound = [
337 self.point_arc_length[0],
338 *points_on_arc_length[index_in_bound],
339 self.point_arc_length[-1],
340 ]
342 if factor < (1.0 - _mpy.eps_quaternion):
343 coordinates = _np.zeros_like(self.coordinates)
344 quaternions = _np.zeros_like(self.quaternions)
345 coordinates[0] = self.coordinates[0]
346 quaternions[0] = self.quaternions[0]
347 for i_segment in range(self.n_points - 1):
348 relative_distance_rotation = _quaternion.slerp_evaluate(
349 _quaternion.quaternion(1),
350 self.relative_distances_rotation[i_segment],
351 factor,
352 )
353 # In the initial configuration (factor=0) we get a straight curve, so we need
354 # to use the arc length here. In the final configuration (factor=1) we want to
355 # exactly recover the input points, so we need the piecewise linear distance.
356 # Between them, we interpolate.
357 relative_distance = (factor * self.relative_distances[i_segment]) + (
358 1.0 - factor
359 ) * (
360 self.point_arc_length[i_segment + 1]
361 - self.point_arc_length[i_segment]
362 )
363 coordinates[i_segment + 1] = (
364 _quaternion.rotate_vectors(
365 quaternions[i_segment] * relative_distance_rotation,
366 [relative_distance, 0, 0],
367 )
368 + coordinates[i_segment]
369 )
370 quaternions[i_segment + 1] = quaternions[
371 i_segment
372 ] * _quaternion.slerp_evaluate(
373 _quaternion.quaternion(1),
374 self.relative_rotations[i_segment],
375 factor,
376 )
377 else:
378 coordinates = self.coordinates
379 quaternions = self.quaternions
381 sol_r = _np.zeros([len(points_on_arc_length_in_bound), 3])
382 sol_q = _np.zeros(
383 len(points_on_arc_length_in_bound), dtype=_quaternion.quaternion
384 )
385 for i_point, centerline_arc_length in enumerate(points_on_arc_length_in_bound):
386 if (
387 centerline_arc_length >= self.point_arc_length[0]
388 and centerline_arc_length <= self.point_arc_length[-1]
389 ):
390 for i in range(1, self.n_points):
391 centerline_index = i - 1
392 if self.point_arc_length[i] > centerline_arc_length:
393 break
395 # Get the two rotation vectors and arc length values
396 arc_lengths = self.point_arc_length[
397 centerline_index : centerline_index + 2
398 ]
399 q1 = quaternions[centerline_index]
400 q2 = quaternions[centerline_index + 1]
402 # Linear interpolate the arc length
403 xi = (centerline_arc_length - arc_lengths[0]) / (
404 arc_lengths[1] - arc_lengths[0]
405 )
407 # Perform a spline interpolation for the positions and a slerp
408 # interpolation for the rotations
409 sol_r[i_point] = get_spline_interpolation(
410 coordinates, self.point_arc_length
411 )(centerline_arc_length)
412 sol_q[i_point] = _quaternion.slerp_evaluate(q1, q2, xi)
413 else:
414 raise ValueError("Centerline value out of bounds")
416 # Set the already computed results in the final data structures
417 sol_r_final = _np.zeros([len(points_on_arc_length), 3])
418 sol_q_final = _np.zeros(len(points_on_arc_length), dtype=_quaternion.quaternion)
419 if len(index_in_bound) > 0:
420 sol_r_final[index_in_bound] = sol_r[index_in_bound - index_in_bound[0] + 1]
421 sol_q_final[index_in_bound] = sol_q[index_in_bound - index_in_bound[0] + 1]
423 # Perform the extrapolation at both ends of the curve
424 for i in index_out_of_bound:
425 arc_length = points_on_arc_length[i]
426 if arc_length <= self.point_arc_length[0]:
427 index = 0
428 elif arc_length >= self.point_arc_length[-1]:
429 index = -1
430 else:
431 raise ValueError("Should not happen")
433 length = arc_length - self.point_arc_length[index]
434 r = sol_r[index]
435 q = sol_q[index]
436 sol_r_final[i] = r + _Rotation.from_quaternion(q) * [length, 0, 0]
437 sol_q_final[i] = q
439 return sol_r_final, sol_q_final
441 def project_point(self, p, t0=None) -> float:
442 """Project a point to the curve, return the parameter coordinate for
443 the projection point."""
445 centerline_interpolation_p = self.centerline_interpolation.derivative(1)
446 centerline_interpolation_pp = self.centerline_interpolation.derivative(2)
448 def f(t):
449 """Function to find the root of."""
450 r = self.centerline_interpolation(t)
451 rp = centerline_interpolation_p(t)
452 return _np.dot(r - p, rp)
454 def fp(t):
455 """Derivative of the Function to find the root of."""
456 r = self.centerline_interpolation(t)
457 rp = centerline_interpolation_p(t)
458 rpp = centerline_interpolation_pp(t)
459 return _np.dot(rp, rp) + _np.dot(r - p, rpp)
461 if t0 is None:
462 t0 = 0.0
464 return _optimize.newton(f, t0, fprime=fp)
466 def get_pyvista_polyline(self) -> _pv.PolyData:
467 """Create a pyvista (vtk) representation of the curve with the
468 evaluated triad basis vectors."""
470 poly_line = _pv.PolyData()
471 poly_line.points = self.coordinates
472 cell = _np.arange(0, self.n_points, dtype=int)
473 cell = _np.insert(cell, 0, self.n_points)
474 poly_line.lines = cell
476 rotation_matrices = _np.zeros((len(self.quaternions), 3, 3))
477 for i_quaternion, q in enumerate(self.quaternions):
478 R = _quaternion.as_rotation_matrix(q)
479 rotation_matrices[i_quaternion] = R
481 for i_dir in range(3):
482 poly_line.point_data.set_array(
483 rotation_matrices[:, :, i_dir], f"base_vector_{i_dir + 1}"
484 )
486 return poly_line
488 def write_vtk(self, path) -> None:
489 """Save a vtk representation of the curve."""
490 self.get_pyvista_polyline().save(path)