import?math
import?numpy?as?np
import?bisect


class?Spline:
????u“““
????Cubic?Spline?class
????“““

????def?__init__（self?x?y）:
????????self.b?self.c?self.d?self.w?=?[]?[]?[]?[]

????????self.x?=?x
????????self.y?=?y

????????self.nx?=?len（x）??#?dimension?of?x
????????h?=?np.diff（x）

????????#?calc?coefficient?c
????????self.a?=?[iy?for?iy?in?y]

????????#?calc?coefficient?c
????????A?=?self.__calc_A（h）
????????B?=?self.__calc_B（h）
????????self.c?=?np.linalg.solve（A?B）
????????#??print（self.c1）

????????#?calc?spline?coefficient?b?and?d
????????for?i?in?range（self.nx?-?1）:
????????????self.d.append（（self.c[i?+?1]?-?self.c[i]）?/?（3.0?*?h[i]））
????????????tb?=?（self.a[i?+?1]?-?self.a[i]）?/?h[i]?-?h[i]?*?\
????????????????（self.c[i?+?1]?+?2.0?*?self.c[i]）?/?3.0
????????????self.b.append（tb）

????def?calc（self?t）:
????????u“““
????????Calc?position
????????if?t?is?outside?of?the?input?x?return?None
????????“““

????????if?t?????????????return?None
????????elif?t?>?self.x[-1]:
????????????return?None

????????i?=?self.__search_index（t）
????????dx?=?t?-?self.x[i]
????????result?=?self.a[i]?+?self.b[i]?*?dx?+?\
????????????self.c[i]?*?dx?**?2.0?+?self.d[i]?*?dx?**?3.0

????????return?result

????def?calcd（self?t）:
????????u“““
????????Calc?first?derivative
????????if?t?is?outside?of?the?input?x?return?None
????????“““

????????if?t?????????????return?None
????????elif?t?>?self.x[-1]:
????????????return?None

????????i?=?self.__search_index（t）
????????dx?=?t?-?self.x[i]
????????result?=?self.b[i]?+?2.0?*?self.c[i]?*?dx?+?3.0?*?self.d[i]?*?dx?**?2.0
????????return?result

????def?calcdd（self?t）:
????????u“““
????????Calc?second?derivative
????????“““

????????if?t?????????????return?None
????????elif?t?>?self.x[-1]:
????????????return?None

????????i?=?self.__search_index（t）
????????dx?=?t?-?self.x[i]
????????result?=?2.0?*?self.c[i]?+?6.0?*?self.d[i]?*?dx
????????return?result

????def?__search_index（self?x）:
????????u“““
????????search?data?segment?index
????????“““
????????return?bisect.bisect（self.x?x）?-?1

????def?__calc_A（self?h）:
????????u“““
????????calc?matrix?A?for?spline?coefficient?c
????????“““
????????A?=?np.zeros（（self.nx?self.nx））
????????A[0?0]?=?1.0
????????for?i?in?range（self.nx?-?1）:
????????????if?i?!=?（self.nx?-?2）:
????????????????A[i?+?1?i?+?1]?=?2.0?*?（h[i]?+?h[i?+?1]）
????????????A[i?+?1?i]?=?h[i]
????????????A[i?i?+?1]?=?h[i]

????????A[0?1]?=?0.0
????????A[self.nx?-?1?self.nx?-?2]?=?0.0
????????A[self.nx?-?1?self.nx?-?1]?=?1.0
????????#??print（A）
????????return?A

????def?__calc_B（self?h）:
????????u“““
????????calc?matrix?B?for?spline?coefficient?c
????????“““
????????B?=?np.zeros（self.nx）
????????for?i?in?range（self.nx?-?2）:
????????????B[i?+?1]?=?3.0?*?（self.a[i?+?2]?-?self.a[i?+?1]）?/?\
????????????????h[i?+?1]?-?3.0?*?（self.a[i?+?1]?-?self.a[i]）?/?h[i]
????????#??print（B）
????????return?B


class?Spline2D:
????u“““
????2D?Cubic?Spline?class
????“““

????def?

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-06-22?22:58??frenet_optimal\
?????文件????????6446??2018-06-22?22:58??frenet_optimal\cubic_spline.pyc
?????文件????????9773??2018-06-22?22:57??frenet_optimal\frenet_optimal_trajectory.py
?????文件????????4354??2018-06-22?22:58??frenet_optimal\cubic_spline.py
?????目錄???????????0??2018-06-22?22:59??frenet_optimal\.idea\
?????文件?????????398??2018-06-17?18:54??frenet_optimal\.idea\frenet_optimal.iml
?????目錄???????????0??2018-06-17?18:53??frenet_optimal\.idea\inspectionProfiles\
?????文件?????????228??2018-06-17?18:53??frenet_optimal\.idea\inspectionProfiles\profiles_settings.xml
?????文件???????21875??2018-06-22?22:59??frenet_optimal\.idea\workspace.xml
?????文件?????????280??2018-06-17?18:53??frenet_optimal\.idea\modules.xml
?????文件?????????206??2018-06-17?18:54??frenet_optimal\.idea\misc.xml