Mirrors/OpenSolo
1
0
Fork 0
mirror of https://github.com/OpenSolo/OpenSolo.git synced 2025-04-29 22:24:32 +02:00
Code Issues Packages Projects Releases Wiki Activity
OpenSolo/shotmanager/catmullRom.py

257 lines
8.0 KiB
Python
Raw Blame History

#
# 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,)
Reference in New Issue View Git Blame Copy Permalink