mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 14:14:30 +02:00
257 lines
8.0 KiB
Python
257 lines
8.0 KiB
Python
#
|
|
# catmullRom.py
|
|
# shotmanager
|
|
#
|
|
# A Catmull-Rom spline class.
|
|
#
|
|
# Created by Will Silva on 11/22/2015.
|
|
# Copyright (c) 2016 3D Robotics.
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
from vector3 import Vector3
|
|
|
|
TOL = 1.0e-3
|
|
|
|
class CatmullRom:
|
|
# Derivation: https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline
|
|
# Class inspiration:
|
|
# https://code.google.com/p/gamekernel/source/browse/kgraphics/math/CatmullRom.cpp
|
|
|
|
def __init__(self, Pts):
|
|
# internal point storage
|
|
self.points = Pts
|
|
|
|
# make sure we have enough points
|
|
if len(Pts) < 4:
|
|
raise ValueError(
|
|
'Not enough points provided to generate a spline.')
|
|
|
|
# calculate total length of the spline
|
|
self.splineCoefficients = []
|
|
self.arcLengths = []
|
|
self.totalArcLength = 0.0
|
|
for seg in range(0, len(self.points) - 3):
|
|
self.splineCoefficients.append(self.updateSplineCoefficients(seg))
|
|
self.arcLengths.append(self.arcLength(seg, 0, 1))
|
|
|
|
self.totalArcLength = sum(self.arcLengths)
|
|
|
|
def updateSplineCoefficients(self, seg):
|
|
if seg < 0 or seg > len(self.points) - 4:
|
|
raise ValueError(
|
|
'Invalid segment number received (%d). Check the number of spline control points.' % seg)
|
|
|
|
P0 = self.points[seg]
|
|
P1 = self.points[seg + 1]
|
|
P2 = self.points[seg + 2]
|
|
P3 = self.points[seg + 3]
|
|
|
|
A = 3.0 * P1 \
|
|
- P0 \
|
|
- 3.0 * P2 \
|
|
+ P3
|
|
B = 2.0 * P0 \
|
|
- 5.0 * P1 \
|
|
+ 4.0 * P2 \
|
|
- P3
|
|
C = P2 - P0
|
|
|
|
return P0,P1,P2,P3,A,B,C
|
|
|
|
def position(self, seg, u):
|
|
'''Returns x,y,z position of spline at parameter u'''
|
|
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
|
|
|
|
return P1 + (0.5 * u) * (C + u * (B + u * A))
|
|
|
|
def velocity(self, seg, u):
|
|
'''Returns x,y,z velocity of spline at parameter u'''
|
|
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
|
|
|
|
return (0.5 * C + u * (B + 1.5 * u * A))
|
|
|
|
def acceleration(self, seg, u):
|
|
'''Returns x,y,z acceleration of spline at parameter u'''
|
|
P0,P1,P2,P3,A,B,C = self.splineCoefficients[seg]
|
|
|
|
return B + (3.0 * u) * A
|
|
|
|
def curvature(self, seg, u):
|
|
'''Returns Frenet curvature of spline at parameter u'''
|
|
|
|
# https://en.wikipedia.org/wiki/Frenet-Serret_formulas
|
|
vel = self.velocity(seg,u)
|
|
return Vector3.cross(vel, self.acceleration(seg, u)).length() / (vel.length())**3
|
|
|
|
def arcLength(self, seg, u1, u2):
|
|
'''Calculates arc length between u1 and u2 using Gaussian Quadrature'''
|
|
|
|
# sanitize u1,u2
|
|
if u2 <= u1 or u1 > 1.0 or u2 < 0.0:
|
|
return 0.0
|
|
|
|
if u1 < 0.0:
|
|
u1 = 0.0
|
|
|
|
if u2 > 1.0:
|
|
u2 = 1.0
|
|
|
|
# Gaussian Quadrature weights and abscissae for n = 5
|
|
|
|
abscissae = [
|
|
0.0000000000, 0.5384693101, -0.5384693101, 0.9061798459, -0.9061798459]
|
|
weights = [0.5688888889, 0.4786286705,
|
|
0.4786286705, 0.2369268850, 0.2369268850]
|
|
|
|
# Gaussian Quadrature
|
|
length = 0.0
|
|
for j in range(0, 5):
|
|
u = 0.5 * ((u2 - u1) * abscissae[j] + u2 + u1)
|
|
length += weights[j] * self.velocity(seg, u).length()
|
|
|
|
length *= 0.5 * (u2 - u1)
|
|
|
|
return length
|
|
|
|
def findParameterByDistance(self, seg, u1, s, newtonIterations = 32):
|
|
'''Returns a parameter u that is s meters ahead of u1'''
|
|
|
|
'''From CatmullRom.cpp: This extends the approach in the text and uses a mixture of bisection and
|
|
Newton-Raphson to find the root. The result is more stable than Newton-
|
|
Raphson alone because a) we won't end up with a situation where we divide by
|
|
zero in the Newton-Raphson step and b) the end result converges faster.
|
|
|
|
See Numerical Recipes or http://www.essentialmath.com/blog for more details.'''
|
|
|
|
# if desired arclength is beyond the end of the current segment then
|
|
# just return end of segment
|
|
if s >= self.arcLength(seg, u1, 1.0):
|
|
return 1.0
|
|
|
|
# can't do negative distances
|
|
if s <= 0.0:
|
|
return u1
|
|
|
|
a = u1
|
|
b = 1.0
|
|
|
|
# make first guess
|
|
p = u1 + s * (1 / self.arcLengths[seg])
|
|
|
|
# iterate and look for zeros
|
|
for i in range(0, newtonIterations):
|
|
# compute function value and test against zero
|
|
func = self.arcLength(seg, u1, p) - s
|
|
|
|
# if within tolerance, return root
|
|
if abs(func) < TOL:
|
|
return p
|
|
|
|
# if result less than zero then adjust lower limit
|
|
if func < 0.0:
|
|
a = p
|
|
# if result greater than zero then adjust upper limit
|
|
else:
|
|
b = p
|
|
|
|
# Use speed to do a bisection method (will accelerate convergence)
|
|
# get speed along the curve
|
|
speed = self.velocity(seg, p).length()
|
|
|
|
# if result lies outside [a,b]
|
|
if ((p - a) * speed - func) * ((p - b) * speed - func) > -TOL:
|
|
# do bisection
|
|
p = 0.5 * (a + b)
|
|
else:
|
|
p -= func / speed
|
|
|
|
return float("inf")
|
|
|
|
def arclengthToNonDimensional(self, p, dp=None):
|
|
'''
|
|
inputs:
|
|
p - spline position reparameterized to total spline arcLength
|
|
dp - spline velocity reparameterized to total spline arclength
|
|
|
|
returns:
|
|
seg - current spline segment
|
|
u - current non-dimensional spline parameter on segment "seg"
|
|
v - velocity along spline in meters per second
|
|
'''
|
|
|
|
# sanitize p
|
|
if p > 1:
|
|
p = 1
|
|
elif p < 0:
|
|
p = 0
|
|
|
|
# calculate target arc length
|
|
targetArcLength = p * self.totalArcLength
|
|
|
|
# find out what segment that's in
|
|
for seg in range(1, len(self.arcLengths) + 1):
|
|
if sum(self.arcLengths[0:seg]) > targetArcLength:
|
|
break
|
|
# increment down one
|
|
seg -= 1
|
|
|
|
# calculate distance in that segment
|
|
dist = targetArcLength - sum(self.arcLengths[0:seg])
|
|
|
|
# calculate dist ahead of u = 0 on seg
|
|
u = self.findParameterByDistance(seg, 0, dist)
|
|
|
|
if dp is not None:
|
|
v = dp * self.totalArcLength
|
|
return (seg, u, v)
|
|
else:
|
|
return (seg, u)
|
|
|
|
def nonDimensionalToArclength(self, seg, u, v=None):
|
|
'''
|
|
inputs:
|
|
seg - current spline segment
|
|
u - current non-dimensional spline parameter on segment "seg"
|
|
v - velocity along spline in meters per second
|
|
|
|
returns:
|
|
p - spline position reparameterized to total spline arcLength
|
|
dp - spline velocity reparameterized to total spline arcLength
|
|
'''
|
|
|
|
# sanitize seg
|
|
if seg < 0:
|
|
seg = 0
|
|
elif seg > len(self.points) - 4:
|
|
seg = len(self.points) - 4
|
|
|
|
# sanitize u
|
|
if u < 0:
|
|
u = 0
|
|
elif u > 1:
|
|
u = 1
|
|
|
|
# calculate distance along in segment
|
|
dist = self.arcLength(seg, 0, u)
|
|
|
|
# add all previous segment distances
|
|
dist += sum(self.arcLengths[0:seg])
|
|
|
|
# convert to arclength parameter
|
|
p = dist / self.totalArcLength
|
|
|
|
if v is not None:
|
|
dp = v / self.totalArcLength
|
|
return (p, dp)
|
|
else:
|
|
return (p,) |