Compare commits
158 Commits
06c8345fc9
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 01261272b4 | |||
| e61146db69 | |||
| 2eb7b214b2 | |||
| 27b1cecc7b | |||
| 9e84306a94 | |||
| c0c36388db | |||
| ed24d0b9e2 | |||
| bccc7a9b29 | |||
| 873f292a11 | |||
| f7cb279841 | |||
| fd96dc395b | |||
|
|
0aa0113be3 | ||
| 01d5657b1b | |||
| 09cf5f9c19 | |||
| 66c3c7e6c7 | |||
| fd2776084a | |||
|
|
bbc35b02bb | ||
|
|
8ce6ffc41c | ||
| 54d6c25171 | |||
| 2c7d563994 | |||
| bc12777f18 | |||
| 7948321cce | |||
| b12c0ec521 | |||
|
|
d417c03f59 | ||
| a8dd511366 | |||
| a30729fb37 | |||
| a70339c55e | |||
| 6fca94e571 | |||
| 255c34dbb2 | |||
| 3c0c719e37 | |||
|
|
0ddd633bc9 | ||
| 28ecf307a8 | |||
| 57467ce48f | |||
| 196ed3be1b | |||
| acd26edc9c | |||
| da9cd51e5c | |||
| d868810048 | |||
|
|
7c9612c3a2 | ||
| 54419b8e60 | |||
| 7dfb0d5e9b | |||
| bbf7314592 | |||
| 6dde28e8d9 | |||
| 9066b3f091 | |||
| c514d4adcc | |||
| cca58e8ba9 | |||
| bf55a45cf9 | |||
| a825a6935b | |||
| 43638f383f | |||
| a42f6dbc98 | |||
|
|
acced75fa2 | ||
| e548451617 | |||
| e529265a63 | |||
| b2c27a6f5c | |||
| 6214b82a6c | |||
|
|
c6b47d8ad6 | ||
| 273f239abb | |||
|
|
14e583a244 | ||
| 771619b832 | |||
|
|
e0c8d8f39b | ||
|
|
0ce4430668 | ||
|
|
e18066e4a6 | ||
| 1c774d2d69 | |||
| 9e8a7a62c9 | |||
| 1ea5fb623d | |||
| 078e3f38f2 | |||
| 94fb4c6a48 | |||
| b3a257fab6 | |||
| 08ad1e665b | |||
| 90acf1ee8c | |||
| 15cf04f164 | |||
| 6fc0b8bb4e | |||
| c0f274cec0 | |||
|
|
511956531e | ||
| 3f108fcc13 | |||
| 683da9739d | |||
| a7fbae47f0 | |||
| 8a202bd38c | |||
| d69ea51b0c | |||
| a1fa54c636 | |||
|
|
cb362c6e49 | ||
| f2be52d17c | |||
| 3682ccdda6 | |||
|
|
85259fc6ad | ||
| 620f8ba136 | |||
| 50e79aa0ae | |||
| 6a72ead855 | |||
| bc300bb3de | |||
| 78e4bb182c | |||
|
|
85dfa2e9a5 | ||
|
|
bdfc5dbc1c | ||
|
|
ec27cd981a | ||
| 47c57dca72 | |||
| e6b4604bfa | |||
| 412f038eb0 | |||
|
|
80ec2382ea | ||
| 42a4349c76 | |||
|
|
e50fbfc57e | ||
| 49a2e2f9c1 | |||
| fc64642bd6 | |||
|
|
cbe106fe95 | ||
| f618fb64cb | |||
|
|
04272b8e1d | ||
| e0e10395fb | |||
|
|
27dccfe7c0 | ||
| 8b16ac79b8 | |||
| 58d62d85b3 | |||
|
|
9c13def8be | ||
| 5fe2788cd7 | |||
| 962504ed98 | |||
|
|
4d7e830798 | ||
|
|
3d769d79eb | ||
| 0b7261a431 | |||
| c5aa3dc495 | |||
| 98c46c2b8c | |||
| d8fae31406 | |||
| 4a9ecf8639 | |||
|
|
b8383c1375 | ||
| f729799335 | |||
| b1a48d2b77 | |||
| fedc324410 | |||
|
|
1a4d998141 | ||
| 0f955b3c91 | |||
| f5039a329b | |||
| 83b7e0d924 | |||
| 1087e043a8 | |||
| 281ceacf89 | |||
| 4e3a50acba | |||
| 732cd33947 | |||
|
|
bb41710645 | ||
|
|
0b084f44f6 | ||
|
|
92b1a3cfd5 | ||
| 3ae2d41fc8 | |||
| c7dd816481 | |||
| 5f802ff57e | |||
| 8e8cb543ae | |||
| ab49f927fb | |||
| 00354d9b41 | |||
| 2478c1e8d2 | |||
|
|
460fc360c6 | ||
|
|
36ffde80f5 | ||
| fe6492e4fc | |||
|
|
de80acf315 | ||
| 3d3b57a311 | |||
| 227f501d6f | |||
|
|
218da42a1d | ||
|
|
c2627ecd89 | ||
| 4696daa2ee | |||
|
|
2e5e1918e1 | ||
| 45f655dc90 | |||
| 9fb33e5bec | |||
|
|
31cf0a45dd | ||
|
|
4bf95c1043 | ||
| 052d4e2eb4 | |||
| 7556539084 | |||
|
|
8b1873b40b | ||
| 0295d93cd3 | |||
| 60cade4d1f | |||
| dc87ce0fb9 |
@@ -2,15 +2,21 @@ cmake_minimum_required(VERSION 3.14)
|
||||
|
||||
|
||||
#**********************************************
|
||||
# Astrosib(c) BM-700 mount control software *
|
||||
# Astrosib(c) FM-700 mount control software *
|
||||
#**********************************************
|
||||
|
||||
project(ASIB_BM700 LANGUAGES C CXX)
|
||||
project(ASIB_FM700 LANGUAGES C CXX)
|
||||
|
||||
set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
|
||||
|
||||
#
|
||||
# ******* C++ PART OF THE PROJECT *******
|
||||
|
||||
add_subdirectory(cxx)
|
||||
set(EXAMPLES OFF CACHE BOOL "" FORCE)
|
||||
# set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_BUILD_TYPE "Debug")
|
||||
add_subdirectory(LibSidServo)
|
||||
|
||||
# add_subdirectory(cxx)
|
||||
add_subdirectory(mcc)
|
||||
add_subdirectory(asibfm700)
|
||||
|
||||
218
LibSidServo/.qtcreator/libsidservo.creator.user
Normal file
218
LibSidServo/.qtcreator/libsidservo.creator.user
Normal file
@@ -0,0 +1,218 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
218
LibSidServo/.qtcreator/libsidservo.creator.user.cf63021
Normal file
218
LibSidServo/.qtcreator/libsidservo.creator.user.cf63021
Normal file
@@ -0,0 +1,218 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 18.0.0, 2025-11-27T17:22:09. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
@@ -84,49 +84,51 @@ typedef struct{
|
||||
* @return calculated new speed or -1 for max speed
|
||||
*/
|
||||
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
|
||||
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){
|
||||
DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT);
|
||||
double dt = timediff(&tagpos->t, &axis->position.t);
|
||||
if(dt < 0 || dt > Conf.PIDMaxDt){
|
||||
DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt);
|
||||
return axis->speed.val; // data is too old or wrong
|
||||
}
|
||||
double error = tagpos->val - axis->position.val, fe = fabs(error);
|
||||
DBG("error: %g", error);
|
||||
PIDController_t *pid = NULL;
|
||||
switch(axis->state){
|
||||
case AXIS_SLEWING:
|
||||
if(fe < MCC_MAX_POINTING_ERR){
|
||||
if(fe < Conf.MaxPointingErr){
|
||||
axis->state = AXIS_POINTING;
|
||||
DBG("--> Pointing");
|
||||
pid = pidpair->PIDC;
|
||||
}else{
|
||||
DBG("Slewing...");
|
||||
return -1.; // max speed for given axis
|
||||
return NAN; // max speed for given axis
|
||||
}
|
||||
break;
|
||||
case AXIS_POINTING:
|
||||
if(fe < MCC_MAX_GUIDING_ERR){
|
||||
if(fe < Conf.MaxFinePointingErr){
|
||||
axis->state = AXIS_GUIDING;
|
||||
DBG("--> Guiding");
|
||||
pid = pidpair->PIDV;
|
||||
}else if(fe > MCC_MAX_POINTING_ERR){
|
||||
}else if(fe > Conf.MaxPointingErr){
|
||||
DBG("--> Slewing");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return -1.;
|
||||
return NAN;
|
||||
} else pid = pidpair->PIDC;
|
||||
break;
|
||||
case AXIS_GUIDING:
|
||||
pid = pidpair->PIDV;
|
||||
if(fe > MCC_MAX_GUIDING_ERR){
|
||||
if(fe > Conf.MaxFinePointingErr){
|
||||
DBG("--> Pointing");
|
||||
axis->state = AXIS_POINTING;
|
||||
pid = pidpair->PIDC;
|
||||
}else if(fe < MCC_MAX_ATTARGET_ERR){
|
||||
}else if(fe < Conf.MaxGuidingErr){
|
||||
DBG("At target");
|
||||
// TODO: we can point somehow that we are at target or introduce new axis state
|
||||
}else DBG("Current error: %g", fe);
|
||||
break;
|
||||
case AXIS_STOPPED: // start pointing to target; will change speed next time
|
||||
DBG("AXIS STOPPED!!!!");
|
||||
DBG("AXIS STOPPED!!!! --> Slewing");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return -1.;
|
||||
return getspeed(tagpos, pidpair, axis);
|
||||
case AXIS_ERROR:
|
||||
DBG("Can't move from erroneous state");
|
||||
return 0.;
|
||||
@@ -135,16 +137,16 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
|
||||
DBG("WTF? Where is a PID?");
|
||||
return axis->speed.val;
|
||||
}
|
||||
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
|
||||
double dtpid = timediff(&tagpos->t, &pid->prevT);
|
||||
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
|
||||
DBG("time diff too big: clear PID");
|
||||
pid_clear(pid);
|
||||
}
|
||||
double dt = tagpos->t - pid->prevT;
|
||||
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
|
||||
if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
|
||||
pid->prevT = tagpos->t;
|
||||
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state);
|
||||
double tagspeed = pid_calculate(pid, error, dt);
|
||||
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based
|
||||
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
|
||||
double tagspeed = pid_calculate(pid, error, dtpid);
|
||||
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
|
||||
return tagspeed; // coordinate-based
|
||||
}
|
||||
|
||||
@@ -154,22 +156,23 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
|
||||
* @param endpoint - stop point (some far enough point to stop in case of hang)
|
||||
* @return error code
|
||||
*/
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target){
|
||||
static PIDpair_t pidX = {0}, pidY = {0};
|
||||
if(!pidX.PIDC){
|
||||
pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDC) return MCC_E_FATAL;
|
||||
pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
if(!pidY.PIDC){
|
||||
pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDC) return MCC_E_FATAL;
|
||||
pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
mountdata_t m;
|
||||
coordpair_t tagspeed;
|
||||
coordpair_t tagspeed; // absolute value of speed
|
||||
double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
|
||||
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
|
||||
axisdata_t axis;
|
||||
DBG("state: %d/%d", m.Xstate, m.Ystate);
|
||||
@@ -177,20 +180,42 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
|
||||
axis.position = m.encXposition;
|
||||
axis.speed = m.encXspeed;
|
||||
tagspeed.X = getspeed(&target->X, &pidX, &axis);
|
||||
if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X;
|
||||
if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
|
||||
if(isnan(tagspeed.X)){ // max speed
|
||||
if(target->X.val < axis.position.val) Xsign = -1.;
|
||||
tagspeed.X = Xlimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
|
||||
if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
|
||||
}
|
||||
axis_status_t xstate = axis.state;
|
||||
axis.state = m.Ystate;
|
||||
axis.position = m.encYposition;
|
||||
axis.speed = m.encYspeed;
|
||||
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
|
||||
if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y;
|
||||
if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
|
||||
if(isnan(tagspeed.Y)){ // max speed
|
||||
if(target->Y.val < axis.position.val) Ysign = -1.;
|
||||
tagspeed.Y = Ylimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
|
||||
if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
|
||||
}
|
||||
axis_status_t ystate = axis.state;
|
||||
if(m.Xstate != xstate || m.Ystate != ystate){
|
||||
DBG("State changed");
|
||||
setStat(xstate, ystate);
|
||||
}
|
||||
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
|
||||
return Mount.moveWspeed(endpoint, &tagspeed);
|
||||
coordpair_t endpoint;
|
||||
// allow at least PIDMaxDt moving with target speed
|
||||
double dv = fabs(tagspeed.X - m.encXspeed.val);
|
||||
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
|
||||
+ Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
|
||||
+ tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
|
||||
endpoint.X = m.encXposition.val + Xsign * adder;
|
||||
dv = fabs(tagspeed.Y - m.encYspeed.val);
|
||||
adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
|
||||
+ Conf.PIDMaxDt * tagspeed.Y
|
||||
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
|
||||
endpoint.Y = m.encYposition.val + Ysign * adder;
|
||||
DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
|
||||
return Mount.moveWspeed(&endpoint, &tagspeed);
|
||||
}
|
||||
|
||||
@@ -27,7 +27,7 @@ typedef struct {
|
||||
double prev_error; // Previous error
|
||||
double integral; // Integral term
|
||||
double *pidIarray; // array for Integral
|
||||
double prevT; // time of previous correction
|
||||
struct timespec prevT; // time of previous correction
|
||||
size_t pidIarrSize; // it's size
|
||||
size_t curIidx; // and index of current element
|
||||
} PIDController_t;
|
||||
@@ -37,4 +37,4 @@ void pid_clear(PIDController_t *pid);
|
||||
void pid_delete(PIDController_t **pid);
|
||||
double pid_calculate(PIDController_t *pid, double error, double dt);
|
||||
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint);
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target);
|
||||
|
||||
@@ -1,2 +1,3 @@
|
||||
1. PID: slew2
|
||||
|
||||
fix encoders opening for several tries
|
||||
encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request)
|
||||
Read HW config even in model mode
|
||||
|
||||
@@ -1,64 +0,0 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
extern conf_t Conf;
|
||||
|
||||
// unused arguments of functions
|
||||
#define _U_ __attribute__((__unused__))
|
||||
// break absent in `case`
|
||||
#define FALLTHRU __attribute__ ((fallthrough))
|
||||
// and synonym for FALLTHRU
|
||||
#define NOBREAKHERE __attribute__ ((fallthrough))
|
||||
// weak functions
|
||||
#define WEAK __attribute__ ((weak))
|
||||
|
||||
#ifndef DBL_EPSILON
|
||||
#define DBL_EPSILON (2.2204460492503131e-16)
|
||||
#endif
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE (0)
|
||||
#endif
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE (1)
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef EBUG
|
||||
#include <stdio.h>
|
||||
#define COLOR_RED "\033[1;31;40m"
|
||||
#define COLOR_GREEN "\033[1;32;40m"
|
||||
#define COLOR_OLD "\033[0;0;0m"
|
||||
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
||||
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
||||
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
|
||||
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
|
||||
fprintf(stderr, __VA_ARGS__); \
|
||||
fprintf(stderr, "\n");} while(0)
|
||||
|
||||
#else // EBUG
|
||||
#define FNAME()
|
||||
#define DBG(...)
|
||||
#endif // EBUG
|
||||
@@ -34,7 +34,9 @@ typedef struct{
|
||||
|
||||
static hardware_configuration_t HW = {0};
|
||||
|
||||
static parameters G = {0};
|
||||
static parameters G = {
|
||||
.conffile = "servo.conf",
|
||||
};
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
@@ -53,7 +55,7 @@ static sl_option_t confopts[] = {
|
||||
|
||||
static void dumpaxis(char axis, axis_config_t *c){
|
||||
#define STRUCTPAR(p) (c)->p
|
||||
#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
|
||||
#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0)
|
||||
#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
||||
DUMPD(accel);
|
||||
DUMPD(backlash);
|
||||
@@ -64,6 +66,8 @@ static void dumpaxis(char axis, axis_config_t *c){
|
||||
DUMP(outplimit);
|
||||
DUMP(currlimit);
|
||||
DUMP(intlimit);
|
||||
DUMP(motor_stepsperrev);
|
||||
DUMP(axis_stepsperrev);
|
||||
#undef DUMP
|
||||
#undef DUMPD
|
||||
}
|
||||
|
||||
@@ -24,25 +24,32 @@
|
||||
static conf_t Config = {
|
||||
.MountDevPath = "/dev/ttyUSB0",
|
||||
.MountDevSpeed = 19200,
|
||||
.EncoderXDevPath = "/dev/encoderX0",
|
||||
.EncoderYDevPath = "/dev/encoderY0",
|
||||
.EncoderXDevPath = "/dev/encoder_X0",
|
||||
.EncoderYDevPath = "/dev/encoder_Y0",
|
||||
.EncoderDevSpeed = 153000,
|
||||
.MountReqInterval = 0.1,
|
||||
.EncoderReqInterval = 0.05,
|
||||
.EncoderReqInterval = 0.001,
|
||||
.SepEncoder = 2,
|
||||
.EncoderSpeedInterval = 0.1,
|
||||
.XPIDC.P = 0.8,
|
||||
.EncoderSpeedInterval = 0.05,
|
||||
.EncodersDisagreement = 1e-5, // 2''
|
||||
.PIDMaxDt = 1.,
|
||||
.PIDRefreshDt = 0.1,
|
||||
.PIDCycleDt = 5.,
|
||||
.XPIDC.P = 0.5,
|
||||
.XPIDC.I = 0.1,
|
||||
.XPIDC.D = 0.3,
|
||||
.XPIDV.P = 1.,
|
||||
.XPIDV.I = 0.01,
|
||||
.XPIDV.D = 0.2,
|
||||
.YPIDC.P = 0.8,
|
||||
.XPIDC.D = 0.2,
|
||||
.XPIDV.P = 0.09,
|
||||
.XPIDV.I = 0.0,
|
||||
.XPIDV.D = 0.05,
|
||||
.YPIDC.P = 0.5,
|
||||
.YPIDC.I = 0.1,
|
||||
.YPIDC.D = 0.3,
|
||||
.YPIDV.P = 0.5,
|
||||
.YPIDV.I = 0.2,
|
||||
.YPIDV.D = 0.5,
|
||||
.YPIDC.D = 0.2,
|
||||
.YPIDV.P = 0.09,
|
||||
.YPIDV.I = 0.0,
|
||||
.YPIDV.D = 0.05,
|
||||
.MaxPointingErr = 0.13962634,
|
||||
.MaxFinePointingErr = 0.026179939,
|
||||
.MaxGuidingErr = 4.8481368e-7,
|
||||
};
|
||||
|
||||
static sl_option_t opts[] = {
|
||||
@@ -50,13 +57,17 @@ static sl_option_t opts[] = {
|
||||
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
|
||||
{"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"},
|
||||
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
|
||||
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
|
||||
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
|
||||
{"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
|
||||
{"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
|
||||
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
|
||||
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
|
||||
{"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
|
||||
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
|
||||
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
|
||||
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
|
||||
{"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
|
||||
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
|
||||
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
|
||||
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
|
||||
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
|
||||
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
|
||||
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
|
||||
@@ -69,6 +80,12 @@ static sl_option_t opts[] = {
|
||||
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
|
||||
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
|
||||
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
|
||||
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
|
||||
{"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
|
||||
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
|
||||
{"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"},
|
||||
{"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"},
|
||||
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
|
||||
end_option
|
||||
};
|
||||
|
||||
@@ -93,5 +110,19 @@ void dumpConf(){
|
||||
}
|
||||
|
||||
void confHelp(){
|
||||
sl_showhelp(-1, opts);
|
||||
sl_conf_showhelp(-1, opts);
|
||||
}
|
||||
|
||||
const char* errcodes[MCC_E_AMOUNT] = {
|
||||
[MCC_E_OK] = "OK",
|
||||
[MCC_E_FATAL] = "Fatal error",
|
||||
[MCC_E_BADFORMAT] = "Wrong data format",
|
||||
[MCC_E_ENCODERDEV] = "Encoder error",
|
||||
[MCC_E_MOUNTDEV] = "Mount error",
|
||||
[MCC_E_FAILED] = "Failed to run"
|
||||
};
|
||||
// return string with error code
|
||||
const char *EcodeStr(mcc_errcodes_t e){
|
||||
if(e >= MCC_E_AMOUNT) return "Wrong error code";
|
||||
return errcodes[e];
|
||||
}
|
||||
|
||||
@@ -25,3 +25,4 @@
|
||||
void confHelp();
|
||||
conf_t *readServoConf(const char *filename);
|
||||
void dumpConf();
|
||||
const char *EcodeStr(mcc_errcodes_t e);
|
||||
|
||||
@@ -23,6 +23,9 @@
|
||||
#include "dump.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
// starting dump time (to conform different logs)
|
||||
static struct timespec dumpT0 = {0};
|
||||
|
||||
#if 0
|
||||
// amount of elements used for encoders' data filtering
|
||||
#define NFILT (10)
|
||||
@@ -59,6 +62,12 @@ static double filter(double val, int idx){
|
||||
}
|
||||
#endif
|
||||
|
||||
// return starting time of dump
|
||||
void dumpt0(struct timespec *t){
|
||||
if(t) *t = dumpT0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief logmnt - log mount data into file
|
||||
* @param fcoords - file to dump
|
||||
@@ -68,12 +77,12 @@ void logmnt(FILE *fcoords, mountdata_t *m){
|
||||
if(!fcoords) return;
|
||||
//DBG("LOG %s", m ? "data" : "header");
|
||||
if(!m){ // write header
|
||||
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
|
||||
fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
|
||||
return;
|
||||
}
|
||||
}else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
|
||||
// write data
|
||||
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
|
||||
m->encXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
|
||||
Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
|
||||
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
|
||||
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
|
||||
m->millis);
|
||||
@@ -99,16 +108,17 @@ void dumpmoving(FILE *fcoords, double t, int N){
|
||||
LOGWARN("Can't get mount data");
|
||||
}
|
||||
uint32_t mdmillis = mdata.millis;
|
||||
double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
|
||||
struct timespec encXt = mdata.encXposition.t;
|
||||
int ctr = -1;
|
||||
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
|
||||
double t0 = Mount.currentT();
|
||||
while(Mount.currentT() - t0 < t && ctr < N){
|
||||
double t0 = Mount.timeFromStart();
|
||||
while(Mount.timeFromStart() - t0 < t && ctr < N){
|
||||
usleep(1000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
|
||||
if(tmsr == enct) continue;
|
||||
enct = tmsr;
|
||||
//double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
|
||||
struct timespec msrt = mdata.encXposition.t;
|
||||
if(msrt.tv_nsec == encXt.tv_nsec) continue;
|
||||
encXt = msrt;
|
||||
if(fcoords) logmnt(fcoords, &mdata);
|
||||
if(mdata.millis == mdmillis) continue;
|
||||
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
|
||||
@@ -119,7 +129,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
|
||||
ctr = 0;
|
||||
}else ++ctr;
|
||||
}
|
||||
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0);
|
||||
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -130,17 +140,15 @@ void waitmoving(int N){
|
||||
mountdata_t mdata;
|
||||
int ctr = -1;
|
||||
uint32_t millis = 0;
|
||||
double xlast = 0., ylast = 0.;
|
||||
//double xlast = 0., ylast = 0.;
|
||||
DBG("Wait moving for %d stopped times", N);
|
||||
while(ctr < N){
|
||||
usleep(10000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
if(mdata.millis == millis) continue;
|
||||
millis = mdata.millis;
|
||||
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
||||
xlast = mdata.motXposition.val;
|
||||
ylast = mdata.motYposition.val;
|
||||
ctr = 0;
|
||||
}else ++ctr;
|
||||
if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
|
||||
else ++ctr;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N);
|
||||
void waitmoving(int N);
|
||||
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
|
||||
void chk0(int ncycles);
|
||||
void dumpt0(struct timespec *t);
|
||||
|
||||
@@ -73,6 +73,7 @@ int main(int argc, char **argv){
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
confHelp();
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
|
||||
@@ -139,8 +139,10 @@ static mcc_errcodes_t return2zero(){
|
||||
short_command_t cmd = {0};
|
||||
DBG("Try to move to zero");
|
||||
cmd.Xmot = 0.; cmd.Ymot = 0.;
|
||||
cmd.Xspeed = MCC_MAX_X_SPEED;
|
||||
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
||||
coordpair_t maxspd;
|
||||
if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
|
||||
cmd.Xspeed = maxspd.X;
|
||||
cmd.Yspeed = maxspd.Y;
|
||||
/*cmd.xychange = 1;
|
||||
cmd.XBits = 100;
|
||||
cmd.YBits = 20;*/
|
||||
@@ -216,7 +218,7 @@ int main(int argc, char **argv){
|
||||
sleep(5);
|
||||
// return to zero and wait
|
||||
green("Return 2 zero and wait\n");
|
||||
if(!return2zero()) ERRX("Can't return");
|
||||
if(MCC_E_OK != return2zero()) ERRX("Can't return");
|
||||
Wait(0., 0);
|
||||
Wait(0., 1);
|
||||
// wait moving ends
|
||||
|
||||
@@ -83,7 +83,7 @@ void waithalf(double t){
|
||||
uint32_t millis = 0;
|
||||
double xlast = 0., ylast = 0.;
|
||||
while(ctr < 5){
|
||||
if(Mount.currentT() >= t) return;
|
||||
if(Mount.timeFromStart() >= t) return;
|
||||
usleep(1000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
if(mdata.millis == millis) continue;
|
||||
@@ -110,16 +110,28 @@ int main(int argc, char **argv){
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w"))){
|
||||
WARNX("Can't open %s", G.coordsoutput);
|
||||
return 1;
|
||||
}
|
||||
}else fcoords = stdout;
|
||||
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
|
||||
if(G.Ncycles < 2){
|
||||
WARNX("Ncycles should be >2");
|
||||
return 1;
|
||||
}
|
||||
double absamp = fabs(G.amplitude);
|
||||
if(absamp < 0.01 || absamp > 45.)
|
||||
ERRX("Amplitude should be from 0.01 to 45 degrees");
|
||||
if(G.period < 0.1 || G.period > 900.)
|
||||
ERRX("Period should be from 0.1 to 900s");
|
||||
if(G.Nswings < 1) ERRX("Nswings should be more than 0");
|
||||
if(absamp < 0.01 || absamp > 45.){
|
||||
WARNX("Amplitude should be from 0.01 to 45 degrees");
|
||||
return 1;
|
||||
}
|
||||
if(G.period < 0.1 || G.period > 900.){
|
||||
WARNX("Period should be from 0.1 to 900s");
|
||||
return 1;
|
||||
}
|
||||
if(G.Nswings < 1){
|
||||
WARNX("Nswings should be more than 0");
|
||||
return 1;
|
||||
}
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
@@ -146,24 +158,24 @@ int main(int argc, char **argv){
|
||||
}else{
|
||||
tagX = 0.; tagY = DEG2RAD(G.amplitude);
|
||||
}
|
||||
double t = Mount.currentT(), t0 = t;
|
||||
double t = Mount.timeFromStart(), t0 = t;
|
||||
coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
|
||||
double divide = 2.;
|
||||
for(int i = 0; i < G.Nswings; ++i){
|
||||
Mount.moveTo(&tag);
|
||||
DBG("CMD: %g", Mount.currentT()-t0);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
t += G.period / divide;
|
||||
divide = 1.;
|
||||
waithalf(t);
|
||||
DBG("Moved to +, t=%g", t-t0);
|
||||
DBG("CMD: %g", Mount.currentT()-t0);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
Mount.moveTo(&rtag);
|
||||
t += G.period;
|
||||
waithalf(t);
|
||||
DBG("Moved to -, t=%g", t-t0);
|
||||
DBG("CMD: %g", Mount.currentT()-t0);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
}
|
||||
green("Move to zero @ %g\n", Mount.currentT());
|
||||
green("Move to zero @ %g\n", Mount.timeFromStart());
|
||||
tag = (coordpair_t){0};
|
||||
// be sure to move @ 0,0
|
||||
if(MCC_E_OK != Mount.moveTo(&tag)){
|
||||
|
||||
@@ -91,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){
|
||||
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
|
||||
coordval_pair_t M, E;
|
||||
if(!getPos(&M, &E)) ERRX("Can't get current position");
|
||||
printf("Current time: %.10f\n", Mount.timeFromStart());
|
||||
if(G.coordsoutput){
|
||||
if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
|
||||
if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
|
||||
G.wait = 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
logmnt(fcoords, NULL);
|
||||
@@ -121,7 +120,11 @@ int main(int _U_ argc, char _U_ **argv){
|
||||
}
|
||||
printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
|
||||
tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
|
||||
Mount.moveTo(&tag);
|
||||
mcc_errcodes_t e = Mount.moveTo(&tag);
|
||||
if(MCC_E_OK != e){
|
||||
WARNX("Cant go to given coordinates: %s\n", EcodeStr(e));
|
||||
goto out;
|
||||
}
|
||||
if(G.wait){
|
||||
sleep(1);
|
||||
waitmoving(G.Ncycles);
|
||||
@@ -133,7 +136,9 @@ out:
|
||||
if(G.coordsoutput) pthread_join(dthr, NULL);
|
||||
DBG("QUIT");
|
||||
if(G.wait){
|
||||
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
|
||||
usleep(250000); // pause to refresh coordinates
|
||||
if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val),
|
||||
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
|
||||
Mount.quit();
|
||||
}
|
||||
return 0;
|
||||
|
||||
@@ -44,6 +44,7 @@ typedef struct{
|
||||
char *conffile;
|
||||
} parameters;
|
||||
|
||||
static conf_t *Config = NULL;
|
||||
static FILE *fcoords = NULL, *errlog = NULL;
|
||||
static pthread_t dthr;
|
||||
static parameters G = {
|
||||
@@ -96,35 +97,35 @@ static void runtraectory(traectory_fn tfn){
|
||||
if(!tfn) return;
|
||||
coordval_pair_t telXY;
|
||||
coordval_pair_t target;
|
||||
coordpair_t traectXY, endpoint;
|
||||
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
|
||||
double t0 = Mount.currentT(), tlast = 0.;
|
||||
double tlastX = 0., tlastY = 0.;
|
||||
coordpair_t traectXY;
|
||||
double tlast = 0., tstart = Mount.timeFromStart();
|
||||
long tlastXnsec = 0, tlastYnsec = 0;
|
||||
struct timespec tcur, t0 = {0};
|
||||
dumpt0(&t0);
|
||||
while(1){
|
||||
if(!telpos(&telXY)){
|
||||
WARNX("No next telescope position");
|
||||
return;
|
||||
}
|
||||
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
|
||||
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t);
|
||||
tlastX = telXY.X.t; tlastY = telXY.Y.t;
|
||||
double t = Mount.currentT();
|
||||
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
|
||||
if(!Mount.currentT(&tcur)) continue;
|
||||
if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
|
||||
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
|
||||
tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
|
||||
double t = Mount.timeFromStart();
|
||||
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
|
||||
if(!traectory_point(&traectXY, t)) break;
|
||||
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
|
||||
target.X.t = target.Y.t = t;
|
||||
// check whether we should change direction
|
||||
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
|
||||
else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
|
||||
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
|
||||
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
|
||||
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
||||
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
|
||||
if(errlog)
|
||||
fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
|
||||
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
|
||||
target.X.t = target.Y.t = tcur;
|
||||
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
|
||||
else{
|
||||
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
||||
DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
|
||||
if(errlog)
|
||||
fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
}
|
||||
if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
|
||||
while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
|
||||
tlast = t;
|
||||
}
|
||||
WARNX("No next traectory point or emulation ends");
|
||||
@@ -150,7 +151,7 @@ int main(int argc, char **argv){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
Config = readServoConf(G.conffile);
|
||||
if(!Config || G.dumpconf){
|
||||
dumpConf();
|
||||
return 1;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
Current configuration:
|
||||
# Current configuration
|
||||
MountDevPath=/dev/ttyUSB0
|
||||
MountDevSpeed=19200
|
||||
EncoderDevPath=(null)
|
||||
|
||||
@@ -41,7 +41,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
|
||||
if(!f || !XY0) return FALSE;
|
||||
cur_traectory = f;
|
||||
XYstart = *XY0;
|
||||
tstart = Mount.currentT();
|
||||
tstart = Mount.timeFromStart();
|
||||
mountdata_t mdata;
|
||||
int ntries = 0;
|
||||
for(; ntries < 10; ++ntries){
|
||||
@@ -98,7 +98,7 @@ int Linear(coordpair_t *nextpt, double t){
|
||||
int SinCos(coordpair_t *nextpt, double t){
|
||||
coordpair_t pt;
|
||||
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
|
||||
pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
|
||||
pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI);
|
||||
if(nextpt) *nextpt = pt;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
|
||||
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
@@ -86,6 +86,7 @@
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
@@ -110,8 +111,8 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
@@ -123,8 +124,8 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
@@ -139,8 +140,8 @@
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
@@ -164,6 +165,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@@ -173,8 +175,8 @@
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
@@ -198,6 +200,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@@ -208,10 +211,6 @@
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
|
||||
@@ -22,6 +22,7 @@ examples/traectories.h
|
||||
main.h
|
||||
movingmodel.c
|
||||
movingmodel.h
|
||||
polltest/main.c
|
||||
ramp.c
|
||||
ramp.h
|
||||
serial.h
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
#include <time.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "movingmodel.h"
|
||||
@@ -32,40 +33,82 @@
|
||||
#include "ssii.h"
|
||||
#include "PID.h"
|
||||
|
||||
// adder for monotonic time by realtime: inited any call of init()
|
||||
static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC
|
||||
t0 = {0}, // curtime() for initstarttime() call
|
||||
starttime = {0}; // starting time by monotonic (for timefromstart())
|
||||
|
||||
conf_t Conf = {0};
|
||||
// parameters for model
|
||||
static movemodel_t *Xmodel, *Ymodel;
|
||||
// limits for model and/or real mount (in latter case data should be read from mount on init)
|
||||
// radians, rad/sec, rad/sec^2
|
||||
static limits_t
|
||||
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||
limits_t
|
||||
Xlimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
|
||||
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
|
||||
Ylimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
|
||||
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
|
||||
;
|
||||
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
||||
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
|
||||
|
||||
/**
|
||||
* @brief nanotime - monotonic time from first run
|
||||
* @return time in seconds
|
||||
* @brief curtime - monotonic time from first run
|
||||
* @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
|
||||
* @return TRUE if all OK
|
||||
* FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
|
||||
*/
|
||||
double nanotime(){
|
||||
static struct timespec *start = NULL;
|
||||
int curtime(struct timespec *t){
|
||||
struct timespec now;
|
||||
if(!start){
|
||||
start = malloc(sizeof(struct timespec));
|
||||
if(!start) return -1.;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
|
||||
now.tv_sec += timeadder.tv_sec;
|
||||
now.tv_nsec += timeadder.tv_nsec;
|
||||
if(now.tv_nsec > 999999999L){
|
||||
++now.tv_sec;
|
||||
now.tv_nsec -= 1000000000L;
|
||||
}
|
||||
if(t) *t = now;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// init starttime; @return TRUE if all OK
|
||||
static int initstarttime(){
|
||||
struct timespec start;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
|
||||
if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
|
||||
timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
|
||||
timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
|
||||
if(timeadder.tv_nsec < 0){
|
||||
--timeadder.tv_sec;
|
||||
timeadder.tv_nsec += 1000000000L;
|
||||
}
|
||||
curtime(&t0);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return difference (in seconds) between time1 and time0
|
||||
double timediff(const struct timespec *time1, const struct timespec *time0){
|
||||
if(!time1 || !time0) return -1.;
|
||||
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
|
||||
}
|
||||
// difference between given time and last initstarttime() call
|
||||
double timediff0(const struct timespec *time1){
|
||||
return timediff(time1, &t0);
|
||||
}
|
||||
// time from last initstarttime() call
|
||||
double timefromstart(){
|
||||
struct timespec now;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
|
||||
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
||||
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
||||
return sd + nd;
|
||||
return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief quit - close all opened and return to default state
|
||||
* TODO: close serial devices even in "model" mode
|
||||
*/
|
||||
static void quit(){
|
||||
if(Conf.RunModel) return;
|
||||
@@ -75,18 +118,19 @@ static void quit(){
|
||||
DBG("Exit");
|
||||
}
|
||||
|
||||
void getModData(coordval_pair_t *c){
|
||||
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
|
||||
if(!c || !Xmodel || !Ymodel) return;
|
||||
double tnow = nanotime();
|
||||
double tnow = timefromstart();
|
||||
moveparam_t Xp, Yp;
|
||||
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
|
||||
//DBG("Xstate = %d", Xst);
|
||||
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
|
||||
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
|
||||
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
|
||||
c->X.t = c->Y.t = tnow;
|
||||
c->X.val = Xp.coord;
|
||||
c->Y.val = Yp.coord;
|
||||
c->X = Xp.coord;
|
||||
c->Y = Yp.coord;
|
||||
if(xst) *xst = Xst;
|
||||
if(yst) *yst = Yst;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -117,6 +161,8 @@ double LS_calc_slope(less_square_t *l, double x, double t){
|
||||
if(!l) return 0.;
|
||||
size_t idx = l->idx;
|
||||
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
|
||||
/*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
|
||||
l->xsum, l->tsum, l->t2sum, l->xtsum);*/
|
||||
double t2 = t * t, xt = x * t;
|
||||
l->x[idx] = x; l->t2[idx] = t2;
|
||||
l->t[idx] = t; l->xt[idx] = xt;
|
||||
@@ -128,14 +174,13 @@ double LS_calc_slope(less_square_t *l, double x, double t){
|
||||
l->xtsum += xt - oldxt;
|
||||
double n = (double)l->arraysz;
|
||||
double denominator = n * l->t2sum - l->tsum * l->tsum;
|
||||
//DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator);
|
||||
if(fabs(denominator) < 1e-7) return 0.;
|
||||
double numerator = n * l->xtsum - l->xsum * l->tsum;
|
||||
//DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
|
||||
// point: (sum_x - slope * sum_t) / n;
|
||||
return (numerator / denominator);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief init - open serial devices and do other job
|
||||
* @param c - initial configuration
|
||||
@@ -144,15 +189,20 @@ double LS_calc_slope(less_square_t *l, double x, double t){
|
||||
static mcc_errcodes_t init(conf_t *c){
|
||||
FNAME();
|
||||
if(!c) return MCC_E_BADFORMAT;
|
||||
if(!initstarttime()) return MCC_E_FAILED;
|
||||
Conf = *c;
|
||||
mcc_errcodes_t ret = MCC_E_OK;
|
||||
Xmodel = model_init(&Xlimits);
|
||||
Ymodel = model_init(&Ylimits);
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
if(Conf.RunModel){
|
||||
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
|
||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
|
||||
DBG("Define mount device path and speed");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}else if(!openMount()){
|
||||
@@ -168,41 +218,47 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
ret = MCC_E_ENCODERDEV;
|
||||
}
|
||||
}
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
// TODO: read hardware configuration on init
|
||||
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
|
||||
DBG("Wrong speed interval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
//uint8_t buf[1024];
|
||||
//data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
|
||||
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
|
||||
if(ret != MCC_E_OK) return ret;
|
||||
return updateMotorPos();
|
||||
// read HW config to update constants
|
||||
hardware_configuration_t HW;
|
||||
if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
|
||||
// make a pause for actual encoder's values
|
||||
double t0 = timefromstart();
|
||||
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
|
||||
mcc_errcodes_t e = updateMotorPos();
|
||||
// and refresh data after updating
|
||||
DBG("Wait for next mount reading");
|
||||
t0 = timefromstart();
|
||||
while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
|
||||
return e;
|
||||
}
|
||||
|
||||
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
|
||||
// TODO fix to real limits!!!
|
||||
static int chkX(double X){
|
||||
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
|
||||
if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkY(double Y){
|
||||
if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
|
||||
if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkXs(double s){
|
||||
if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE;
|
||||
if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkYs(double s){
|
||||
if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE;
|
||||
if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// set SLEWING state if axis was stopped later
|
||||
// set SLEWING state if axis was stopped
|
||||
static void setslewingstate(){
|
||||
//FNAME();
|
||||
mountdata_t d;
|
||||
@@ -218,19 +274,6 @@ static void setslewingstate(){
|
||||
}else DBG("CAN't GET MOUNT DATA!");
|
||||
}
|
||||
|
||||
/*
|
||||
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
||||
(void)target;
|
||||
(void)flags;
|
||||
//if(Conf.RunModel) return ... ;
|
||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||
//...
|
||||
setStat(AXIS_SLEWING, AXIS_SLEWING);
|
||||
//...
|
||||
return MCC_E_FAILED;
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief move2 - simple move to given point and stop
|
||||
* @param X - new X coordinate (radians: -pi..pi) or NULL
|
||||
@@ -245,12 +288,13 @@ static mcc_errcodes_t move2(const coordpair_t *target){
|
||||
DBG("x,y: %g, %g", target->X, target->Y);
|
||||
cmd.Xmot = target->X;
|
||||
cmd.Ymot = target->Y;
|
||||
cmd.Xspeed = MCC_MAX_X_SPEED;
|
||||
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
||||
mcc_errcodes_t r = shortcmd(&cmd);
|
||||
cmd.Xspeed = Xlimits.max.speed;
|
||||
cmd.Yspeed = Ylimits.max.speed;
|
||||
/*mcc_errcodes_t r = shortcmd(&cmd);
|
||||
if(r != MCC_E_OK) return r;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
return MCC_E_OK;*/
|
||||
return shortcmd(&cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -279,6 +323,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
||||
if(!target || !speed) return MCC_E_BADFORMAT;
|
||||
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
||||
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
|
||||
// updateMotorPos() here can make a problem; TODO: remove?
|
||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||
short_command_t cmd = {0};
|
||||
cmd.Xmot = target->X;
|
||||
@@ -298,7 +343,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
||||
static mcc_errcodes_t emstop(){
|
||||
FNAME();
|
||||
if(Conf.RunModel){
|
||||
double curt = nanotime();
|
||||
double curt = timefromstart();
|
||||
Xmodel->emergency_stop(Xmodel, curt);
|
||||
Ymodel->emergency_stop(Ymodel, curt);
|
||||
return MCC_E_OK;
|
||||
@@ -310,7 +355,7 @@ static mcc_errcodes_t emstop(){
|
||||
static mcc_errcodes_t stop(){
|
||||
FNAME();
|
||||
if(Conf.RunModel){
|
||||
double curt = nanotime();
|
||||
double curt = timefromstart();
|
||||
Xmodel->stop(Xmodel, curt);
|
||||
Ymodel->stop(Ymodel,curt);
|
||||
return MCC_E_OK;
|
||||
@@ -327,7 +372,7 @@ static mcc_errcodes_t stop(){
|
||||
static mcc_errcodes_t shortcmd(short_command_t *cmd){
|
||||
if(!cmd) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel){
|
||||
double curt = nanotime();
|
||||
double curt = timefromstart();
|
||||
moveparam_t param = {0};
|
||||
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
@@ -359,7 +404,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
|
||||
static mcc_errcodes_t longcmd(long_command_t *cmd){
|
||||
if(!cmd) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel){
|
||||
double curt = nanotime();
|
||||
double curt = timefromstart();
|
||||
moveparam_t param = {0};
|
||||
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
@@ -386,6 +431,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
||||
if(!hwConfig) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel) return MCC_E_FAILED;
|
||||
SSconfig config;
|
||||
DBG("Read HW configuration");
|
||||
if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
|
||||
// Convert acceleration (ticks per loop^2 to rad/s^2)
|
||||
hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
|
||||
@@ -425,8 +471,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
||||
// Copy ticks per revolution
|
||||
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
|
||||
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
|
||||
hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less
|
||||
hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4;
|
||||
hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
|
||||
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
|
||||
// Convert slew rates (ticks per loop to rad/s)
|
||||
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
||||
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
||||
@@ -444,6 +490,30 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
||||
hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
|
||||
// Convert backlash speed (ticks per loop to rad/s)
|
||||
hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
|
||||
// now read text commands
|
||||
int64_t i64;
|
||||
double Xticks, Yticks;
|
||||
DBG("SERIAL");
|
||||
// motor's encoder ticks per rev
|
||||
if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED;
|
||||
Xticks = ((double) i64); // divide by 4 as these values stored ???
|
||||
if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED;
|
||||
Yticks = ((double) i64);
|
||||
X_ENC_ZERO = Conf.XEncZero;
|
||||
Y_ENC_ZERO = Conf.YEncZero;
|
||||
DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
|
||||
X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
|
||||
Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
|
||||
DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
|
||||
// axis encoder ticks per rev
|
||||
if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
|
||||
Xticks = (double) i64;
|
||||
if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
|
||||
Yticks = (double) i64;
|
||||
DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
|
||||
X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
|
||||
Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
|
||||
DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
@@ -499,17 +569,37 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
||||
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
|
||||
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
|
||||
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
|
||||
// todo - also write text params
|
||||
// TODO - next
|
||||
(void) config;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// getters of max/min speed and acceleration
|
||||
mcc_errcodes_t maxspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.max.speed;
|
||||
v->Y = Ylimits.max.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t minspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.min.speed;
|
||||
v->Y = Ylimits.min.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t acceleration(coordpair_t *a){
|
||||
if(!a) return MCC_E_BADFORMAT;
|
||||
a->X = Xlimits.max.accel;
|
||||
a->Y = Ylimits.max.accel;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// init mount class
|
||||
mount_t Mount = {
|
||||
.init = init,
|
||||
.quit = quit,
|
||||
.getMountData = getMD,
|
||||
// .slewTo = slew2,
|
||||
.moveTo = move2,
|
||||
.moveWspeed = move2s,
|
||||
.setSpeed = setspeed,
|
||||
@@ -519,7 +609,13 @@ mount_t Mount = {
|
||||
.longCmd = longcmd,
|
||||
.getHWconfig = get_hwconf,
|
||||
.saveHWconfig = write_hwconf,
|
||||
.currentT = nanotime,
|
||||
.currentT = curtime,
|
||||
.timeFromStart = timefromstart,
|
||||
.timeDiff = timediff,
|
||||
.timeDiff0 = timediff0,
|
||||
.correctTo = correct2,
|
||||
.getMaxSpeed = maxspeed,
|
||||
.getMinSpeed = minspeed,
|
||||
.getAcceleration = acceleration,
|
||||
};
|
||||
|
||||
|
||||
@@ -24,11 +24,16 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "movingmodel.h"
|
||||
#include "sidservo.h"
|
||||
|
||||
extern conf_t Conf;
|
||||
double nanotime();
|
||||
void getModData(coordval_pair_t *c);
|
||||
extern limits_t Xlimits, Ylimits;
|
||||
int curtime(struct timespec *t);
|
||||
double timediff(const struct timespec *time1, const struct timespec *time0);
|
||||
double timediff0(const struct timespec *time1);
|
||||
double timefromstart();
|
||||
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst);
|
||||
typedef struct{
|
||||
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
|
||||
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
|
||||
@@ -42,10 +47,6 @@ double LS_calc_slope(less_square_t *l, double x, double t);
|
||||
|
||||
// unused arguments of functions
|
||||
#define _U_ __attribute__((__unused__))
|
||||
// break absent in `case`
|
||||
#define FALLTHRU __attribute__ ((fallthrough))
|
||||
// and synonym for FALLTHRU
|
||||
#define NOBREAKHERE __attribute__ ((fallthrough))
|
||||
// weak functions
|
||||
#define WEAK __attribute__ ((weak))
|
||||
|
||||
|
||||
@@ -60,9 +60,14 @@ movemodel_t *model_init(limits_t *l){
|
||||
|
||||
int model_move2(movemodel_t *model, moveparam_t *target, double t){
|
||||
if(!target || !model) return FALSE;
|
||||
//DBG("MOVE to %g at speed %g", target->coord, target->speed);
|
||||
DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
|
||||
// only positive velocity
|
||||
if(target->speed < 0.) target->speed = -target->speed;
|
||||
if(fabs(target->speed) < model->Min.speed){
|
||||
DBG("STOP");
|
||||
model->stop(model, t);
|
||||
return TRUE;
|
||||
}
|
||||
// don't mind about acceleration - user cannot set it now
|
||||
return model->calculate(model, target, t);
|
||||
}
|
||||
|
||||
@@ -44,7 +44,7 @@ typedef struct{
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
double acceleration;
|
||||
//double acceleration;
|
||||
} limits_t;
|
||||
|
||||
typedef enum{
|
||||
|
||||
197
LibSidServo/polltest/main.c
Normal file
197
LibSidServo/polltest/main.c
Normal file
@@ -0,0 +1,197 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
// suppose that we ONLY poll data
|
||||
#define XYBUFSZ (128)
|
||||
|
||||
struct{
|
||||
int help;
|
||||
char *Xpath;
|
||||
char *Ypath;
|
||||
double dt;
|
||||
} G = {
|
||||
.Xpath = "/dev/encoder_X0",
|
||||
.Ypath = "/dev/encoder_Y0",
|
||||
.dt = 0.001,
|
||||
};
|
||||
|
||||
sl_option_t options[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"Xpath", NEED_ARG, NULL, 'X', arg_string, APTR(&G.Xpath), "path to X encoder"},
|
||||
{"Ypath", NEED_ARG, NULL, 'Y', arg_string, APTR(&G.Ypath), "path to Y encoder"},
|
||||
{"dt", NEED_ARG, NULL, 'd', arg_double, APTR(&G.dt), "request interval (1e-4..10s)"},
|
||||
};
|
||||
|
||||
typedef struct{
|
||||
char buf[XYBUFSZ+1];
|
||||
int len;
|
||||
} buf_t;
|
||||
|
||||
static int Xfd = -1, Yfd = -1;
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
DBG("close");
|
||||
if(Xfd > 0){ close(Xfd); Xfd = -1; }
|
||||
if(Yfd > 0){ close(Yfd); Yfd = -1; }
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
static int op(const char *nm){
|
||||
int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK);
|
||||
if(fd < 0) ERR("Can't open %s", nm);
|
||||
struct termios2 tty;
|
||||
if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings");
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
|
||||
tty.c_ispeed = 1000000;
|
||||
tty.c_ospeed = 1000000;
|
||||
if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings");
|
||||
// try to set exclusive
|
||||
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
|
||||
return fd;
|
||||
}
|
||||
|
||||
// write to buffer next data portion; return FALSE in case of error
|
||||
static int readstrings(buf_t *buf, int fd){
|
||||
FNAME();
|
||||
if(!buf){WARNX("Empty buffer"); return FALSE;}
|
||||
int L = XYBUFSZ - buf->len;
|
||||
if(L == 0){
|
||||
DBG("buffer overfull!", buf->len);
|
||||
char *lastn = strrchr(buf->buf, '\n');
|
||||
if(lastn){
|
||||
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
|
||||
++lastn;
|
||||
buf->len = XYBUFSZ - (lastn - buf->buf);
|
||||
DBG("Memmove %d", buf->len);
|
||||
memmove(lastn, buf->buf, buf->len);
|
||||
buf->buf[buf->len] = 0;
|
||||
}else buf->len = 0;
|
||||
L = XYBUFSZ - buf->len;
|
||||
}
|
||||
int got = read(fd, &buf->buf[buf->len], L);
|
||||
if(got < 0){
|
||||
WARN("read()");
|
||||
return FALSE;
|
||||
}else if(got == 0){ DBG("NO data"); return TRUE; }
|
||||
buf->len += got;
|
||||
buf->buf[buf->len] = 0;
|
||||
DBG("buf[%d]: %s", buf->len, buf->buf);
|
||||
return TRUE;
|
||||
}
|
||||
// return TRUE if got, FALSE if no data found
|
||||
static int getdata(buf_t *buf, long *out){
|
||||
if(!buf || buf->len < 1) return FALSE;
|
||||
// read record between last '\n' and previous (or start of string)
|
||||
char *last = &buf->buf[buf->len - 1];
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
if(*last != '\n') return FALSE;
|
||||
*last = 0;
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
char *prev = strrchr(buf->buf, '\n');
|
||||
if(!prev) prev = buf->buf;
|
||||
else{
|
||||
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
|
||||
++prev; // after last '\n'
|
||||
}
|
||||
if(out) *out = atol(prev);
|
||||
// clear buffer
|
||||
buf->len = 0;
|
||||
return TRUE;
|
||||
}
|
||||
// try to write '\n' asking new data portion; return FALSE if failed
|
||||
static int asknext(int fd){
|
||||
FNAME();
|
||||
if(fd < 0) return FALSE;
|
||||
int i = 0;
|
||||
for(; i < 5; ++i){
|
||||
int l = write(fd, "\n", 1);
|
||||
//DBG("l=%d", l);
|
||||
if(1 == l) return TRUE;
|
||||
usleep(100);
|
||||
}
|
||||
DBG("5 tries... failed!");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
buf_t xbuf, ybuf;
|
||||
long xlast, ylast;
|
||||
double xtlast, ytlast;
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, options);
|
||||
if(G.help) sl_showhelp(-1, options);
|
||||
if(G.dt < 1e-4) ERRX("dx too small");
|
||||
if(G.dt > 10.) ERRX("dx too big");
|
||||
Xfd = op(G.Xpath);
|
||||
Yfd = op(G.Ypath);
|
||||
struct pollfd pfds[2];
|
||||
pfds[0].fd = Xfd; pfds[0].events = POLLIN;
|
||||
pfds[1].fd = Yfd; pfds[1].events = POLLIN;
|
||||
double t0x, t0y, tstart;
|
||||
asknext(Xfd); asknext(Yfd);
|
||||
t0x = t0y = tstart = sl_dtime();
|
||||
DBG("Start");
|
||||
do{ // main cycle
|
||||
if(poll(pfds, 2, 0) < 0){
|
||||
WARN("poll()");
|
||||
break;
|
||||
}
|
||||
if(pfds[0].revents && POLLIN){
|
||||
DBG("got X");
|
||||
if(!readstrings(&xbuf, Xfd)) break;
|
||||
}
|
||||
if(pfds[1].revents && POLLIN){
|
||||
DBG("got Y");
|
||||
if(!readstrings(&ybuf, Yfd)) break;
|
||||
}
|
||||
double curt = sl_dtime();
|
||||
if(getdata(&xbuf, &xlast)) xtlast = curt;
|
||||
if(curt - t0x >= G.dt){ // get last records
|
||||
if(curt - xtlast < 1.5*G.dt)
|
||||
printf("%-14.4fX=%ld\n", xtlast-tstart, xlast);
|
||||
if(!asknext(Xfd)) break;
|
||||
t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt;
|
||||
}
|
||||
curt = sl_dtime();
|
||||
if(getdata(&ybuf, &ylast)) ytlast = curt;
|
||||
if(curt - t0y >= G.dt){ // get last records
|
||||
if(curt - ytlast < 1.5*G.dt)
|
||||
printf("%-14.4fY=%ld\n", ytlast-tstart, ylast);
|
||||
if(!asknext(Yfd)) break;
|
||||
t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt;
|
||||
}
|
||||
}while(Xfd > 0 && Yfd > 0);
|
||||
DBG("OOps: disconnected");
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
@@ -23,12 +23,14 @@
|
||||
|
||||
#include "main.h"
|
||||
#include "ramp.h"
|
||||
/*
|
||||
|
||||
#ifdef EBUG
|
||||
#undef DBG
|
||||
#define DBG(...)
|
||||
#undef FNAME
|
||||
#define FNAME()
|
||||
#endif
|
||||
*/
|
||||
|
||||
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
|
||||
static void emstop(movemodel_t *m, double _U_ t){
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdint.h>
|
||||
@@ -48,7 +49,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
||||
// encoders thread and mount thread
|
||||
static pthread_t encthread, mntthread;
|
||||
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
|
||||
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
|
||||
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
|
||||
// encoders raw data
|
||||
typedef struct __attribute__((packed)){
|
||||
uint8_t magick;
|
||||
@@ -64,20 +65,12 @@ void getXspeed(){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
|
||||
double dt = timediff0(&mountdata.encXposition.t);
|
||||
double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
|
||||
if(fabs(speed) < 1.5 * Xlimits.max.speed){
|
||||
mountdata.encXspeed.val = speed;
|
||||
mountdata.encXspeed.t = mountdata.encXposition.t;
|
||||
}
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("Xspeed=%g", mountdata.encXspeed.val);
|
||||
#if 0
|
||||
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
|
||||
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
|
||||
lastXenc.val = mountdata.encXposition.val;
|
||||
lastXenc.t = t;
|
||||
#endif
|
||||
}
|
||||
void getYspeed(){
|
||||
static less_square_t *ls = NULL;
|
||||
@@ -85,19 +78,12 @@ void getYspeed(){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
|
||||
double dt = timediff0(&mountdata.encYposition.t);
|
||||
double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
|
||||
if(fabs(speed) < 1.5 * Ylimits.max.speed){
|
||||
mountdata.encYspeed.val = speed;
|
||||
mountdata.encYspeed.t = mountdata.encYposition.t;
|
||||
}
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
#if 0
|
||||
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
|
||||
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
|
||||
lastYenc.val = mountdata.encYposition.val;
|
||||
lastYenc.t = t;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -105,7 +91,8 @@ void getYspeed(){
|
||||
* @param databuf - input buffer with 13 bytes of data
|
||||
* @param t - time when databuf[0] got
|
||||
*/
|
||||
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
|
||||
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
|
||||
if(!t) return;
|
||||
enc_t *edata = (enc_t*) databuf;
|
||||
/*
|
||||
#ifdef EBUG
|
||||
@@ -140,18 +127,17 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
|
||||
return;
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encXposition.val = X_ENC2RAD(edata->encX);
|
||||
mountdata.encYposition.val = Y_ENC2RAD(edata->encY);
|
||||
mountdata.encXposition.val = Xenc2rad(edata->encX);
|
||||
mountdata.encYposition.val = Yenc2rad(edata->encY);
|
||||
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
|
||||
mountdata.encXposition.t = t;
|
||||
mountdata.encYposition.t = t;
|
||||
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
|
||||
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
|
||||
mountdata.encXposition.t = *t;
|
||||
mountdata.encYposition.t = *t;
|
||||
getXspeed(); getYspeed();
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
|
||||
}
|
||||
|
||||
#if 0
|
||||
/**
|
||||
* @brief getencval - get uint64_t data from encoder
|
||||
* @param fd - encoder fd
|
||||
@@ -159,33 +145,53 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
|
||||
* @param t - measurement time
|
||||
* @return amount of data read or 0 if problem
|
||||
*/
|
||||
static int getencval(int fd, double *val, double *t){
|
||||
if(fd < 0) return FALSE;
|
||||
static int getencval(int fd, double *val, struct timespec *t){
|
||||
if(fd < 0){
|
||||
DBG("Encoder fd < 0!");
|
||||
return FALSE;
|
||||
}
|
||||
char buf[128];
|
||||
int got = 0, Lmax = 127;
|
||||
double t0 = nanotime();
|
||||
double t0 = timefromstart();
|
||||
//DBG("start: %.6g", t0);
|
||||
do{
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(fd, &rfds);
|
||||
struct timeval tv = encRtmout;
|
||||
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval) continue;
|
||||
if(!retval){
|
||||
//DBG("select()==0 - timeout, %.6g", timefromstart());
|
||||
break;
|
||||
}
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
if(errno == EINTR){
|
||||
DBG("EINTR");
|
||||
continue;
|
||||
}
|
||||
DBG("select() < 0");
|
||||
return 0;
|
||||
}
|
||||
if(FD_ISSET(fd, &rfds)){
|
||||
ssize_t l = read(fd, &buf[got], Lmax);
|
||||
if(l < 1) return 0; // disconnected ??
|
||||
if(l < 1){
|
||||
DBG("read() < 0");
|
||||
return 0; // disconnected ??
|
||||
}
|
||||
got += l; Lmax -= l;
|
||||
buf[got] = 0;
|
||||
} else continue;
|
||||
if(strchr(buf, '\n')) break;
|
||||
}while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval);
|
||||
if(got == 0) return 0; // WTF?
|
||||
if(buf[got-1] == '\n') break; // got EOL as last symbol
|
||||
}while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
|
||||
if(got == 0){
|
||||
//DBG("No data from encoder, tfs=%.6g", timefromstart());
|
||||
return 0;
|
||||
}
|
||||
char *estr = strrchr(buf, '\n');
|
||||
if(!estr) return 0;
|
||||
if(!estr){
|
||||
DBG("No EOL");
|
||||
return 0;
|
||||
}
|
||||
*estr = 0;
|
||||
char *bgn = strrchr(buf, '\n');
|
||||
if(bgn) ++bgn;
|
||||
@@ -197,9 +203,11 @@ static int getencval(int fd, double *val, double *t){
|
||||
return 0; // wrong number
|
||||
}
|
||||
if(val) *val = (double) data;
|
||||
if(t) *t = t0;
|
||||
if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
|
||||
return got;
|
||||
}
|
||||
#endif
|
||||
|
||||
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
|
||||
static int getencbyte(){
|
||||
if(encfd[0] < 0) return -1;
|
||||
@@ -261,8 +269,6 @@ static void clrmntbuf(){
|
||||
if(mntfd < 0) return;
|
||||
uint8_t byte;
|
||||
fd_set rfds;
|
||||
//double t0 = nanotime();
|
||||
//int n = 0;
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(mntfd, &rfds);
|
||||
@@ -276,10 +282,8 @@ static void clrmntbuf(){
|
||||
if(FD_ISSET(mntfd, &rfds)){
|
||||
ssize_t l = read(mntfd, &byte, 1);
|
||||
if(l != 1) break;
|
||||
//++n;
|
||||
} else break;
|
||||
}while(1);
|
||||
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
|
||||
}
|
||||
|
||||
// main encoder thread (for separate encoder): read next data and make parsing
|
||||
@@ -287,7 +291,7 @@ static void *encoderthread1(void _U_ *u){
|
||||
if(Conf.SepEncoder != 1) return NULL;
|
||||
uint8_t databuf[ENC_DATALEN];
|
||||
int wridx = 0, errctr = 0;
|
||||
double t = 0.;
|
||||
struct timespec tcur;
|
||||
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
|
||||
int b = getencbyte();
|
||||
if(b == -2) ++errctr;
|
||||
@@ -298,13 +302,14 @@ static void *encoderthread1(void _U_ *u){
|
||||
if((uint8_t)b == ENC_MAGICK){
|
||||
// DBG("Got magic -> start filling packet");
|
||||
databuf[wridx++] = (uint8_t) b;
|
||||
t = nanotime();
|
||||
}
|
||||
continue;
|
||||
}else databuf[wridx++] = (uint8_t) b;
|
||||
if(wridx == ENC_DATALEN){
|
||||
parse_encbuf(databuf, t);
|
||||
wridx = 0;
|
||||
if(curtime(&tcur)){
|
||||
parse_encbuf(databuf, &tcur);
|
||||
wridx = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(encfd[0] > -1){
|
||||
@@ -314,53 +319,138 @@ static void *encoderthread1(void _U_ *u){
|
||||
return NULL;
|
||||
}
|
||||
|
||||
#define XYBUFSZ (128)
|
||||
typedef struct{
|
||||
char buf[XYBUFSZ+1];
|
||||
int len;
|
||||
} buf_t;
|
||||
|
||||
// write to buffer next data portion; return FALSE in case of error
|
||||
static int readstrings(buf_t *buf, int fd){
|
||||
if(!buf){DBG("Empty buffer"); return FALSE;}
|
||||
int L = XYBUFSZ - buf->len;
|
||||
if(L < 0){
|
||||
DBG("buf not initialized!");
|
||||
buf->len = 0;
|
||||
}
|
||||
if(L == 0){
|
||||
DBG("buffer overfull: %d!", buf->len);
|
||||
char *lastn = strrchr(buf->buf, '\n');
|
||||
if(lastn){
|
||||
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
|
||||
++lastn;
|
||||
buf->len = XYBUFSZ - (lastn - buf->buf);
|
||||
DBG("Memmove %d", buf->len);
|
||||
memmove(lastn, buf->buf, buf->len);
|
||||
buf->buf[buf->len] = 0;
|
||||
}else buf->len = 0;
|
||||
L = XYBUFSZ - buf->len;
|
||||
}
|
||||
//DBG("read %d bytes from %d", L, fd);
|
||||
int got = read(fd, &buf->buf[buf->len], L);
|
||||
if(got < 0){
|
||||
DBG("read()");
|
||||
return FALSE;
|
||||
}else if(got == 0){ DBG("NO data"); return TRUE; }
|
||||
buf->len += got;
|
||||
buf->buf[buf->len] = 0;
|
||||
//DBG("buf[%d]: %s", buf->len, buf->buf);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return TRUE if got, FALSE if no data found
|
||||
static int getdata(buf_t *buf, long *out){
|
||||
if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE;
|
||||
// read record between last '\n' and previous (or start of string)
|
||||
char *last = &buf->buf[buf->len - 1];
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
if(*last != '\n') return FALSE;
|
||||
*last = 0;
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
char *prev = strrchr(buf->buf, '\n');
|
||||
if(!prev) prev = buf->buf;
|
||||
else{
|
||||
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
|
||||
++prev; // after last '\n'
|
||||
}
|
||||
if(out) *out = atol(prev);
|
||||
// clear buffer
|
||||
buf->len = 0;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// try to write '\n' asking new data portion; return FALSE if failed
|
||||
static int asknext(int fd){
|
||||
//FNAME();
|
||||
if(fd < 0) return FALSE;
|
||||
int i = 0;
|
||||
for(; i < 5; ++i){
|
||||
int l = write(fd, "\n", 1);
|
||||
//DBG("l=%d", l);
|
||||
if(1 == l) return TRUE;
|
||||
usleep(100);
|
||||
}
|
||||
DBG("5 tries... failed!");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
|
||||
static void *encoderthread2(void _U_ *u){
|
||||
if(Conf.SepEncoder != 2) return NULL;
|
||||
DBG("Thread started");
|
||||
struct pollfd pfds[2];
|
||||
pfds[0].fd = encfd[0]; pfds[0].events = POLLIN;
|
||||
pfds[1].fd = encfd[1]; pfds[1].events = POLLIN;
|
||||
double t0[2], tstart;
|
||||
buf_t strbuf[2] = {0};
|
||||
long msrlast[2]; // last encoder data
|
||||
double mtlast[2]; // last measurement time
|
||||
asknext(encfd[0]); asknext(encfd[1]);
|
||||
t0[0] = t0[1] = tstart = timefromstart();
|
||||
int errctr = 0;
|
||||
double t0 = nanotime();
|
||||
const char *req = "\n";
|
||||
int need2ask = 0; // need or not to ask encoder for new data
|
||||
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
|
||||
if(need2ask){
|
||||
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
|
||||
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
|
||||
do{ // main cycle
|
||||
if(poll(pfds, 2, 0) < 0){
|
||||
DBG("poll()");
|
||||
break;
|
||||
}
|
||||
double v, t;
|
||||
if(getencval(encfd[0], &v, &t)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encXposition.val = X_ENC2RAD(v);
|
||||
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
|
||||
mountdata.encXposition.t = t;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
|
||||
getXspeed();
|
||||
if(getencval(encfd[1], &v, &t)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encYposition.val = Y_ENC2RAD(v);
|
||||
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
|
||||
mountdata.encYposition.t = t;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
|
||||
getYspeed();
|
||||
errctr = 0;
|
||||
need2ask = 0;
|
||||
} else {
|
||||
if(need2ask) ++errctr;
|
||||
else need2ask = 1;
|
||||
continue;
|
||||
int got = 0;
|
||||
for(int i = 0; i < 2; ++i){
|
||||
if(pfds[i].revents && POLLIN){
|
||||
if(!readstrings(&strbuf[i], encfd[i])){
|
||||
++errctr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
double curt = timefromstart();
|
||||
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
|
||||
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
|
||||
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
if(i == 0){
|
||||
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
|
||||
curtime(&mountdata.encXposition.t);
|
||||
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
|
||||
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
|
||||
X_ENC_ZERO, X_ENC_STEPSPERREV);*/
|
||||
getXspeed();
|
||||
}else{
|
||||
mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
|
||||
curtime(&mountdata.encYposition.t);
|
||||
getYspeed();
|
||||
}
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
}
|
||||
if(!asknext(encfd[i])){
|
||||
++errctr;
|
||||
break;
|
||||
}
|
||||
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
|
||||
++got;
|
||||
}
|
||||
} else {
|
||||
if(need2ask) ++errctr;
|
||||
else need2ask = 1;
|
||||
continue;
|
||||
}
|
||||
while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); }
|
||||
//DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval);
|
||||
t0 = nanotime();
|
||||
}
|
||||
DBG("ERRCTR=%d", errctr);
|
||||
if(got == 2) errctr = 0;
|
||||
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
|
||||
DBG("\n\nEXIT: ERRCTR=%d", errctr);
|
||||
for(int i = 0; i < 2; ++i){
|
||||
if(encfd[i] > -1){
|
||||
close(encfd[i]);
|
||||
@@ -386,33 +476,67 @@ void data_free(data_t **x){
|
||||
*x = NULL;
|
||||
}
|
||||
|
||||
static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
|
||||
if(!prev || !nstopped || !stat) return;
|
||||
if(isnan(*prev)){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("START");
|
||||
}else if(*stat != AXIS_STOPPED){
|
||||
if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("AXIS stopped; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped);
|
||||
}
|
||||
}else if(*prev != cur){
|
||||
DBG("AXIS moving");
|
||||
*nstopped = 0;
|
||||
}
|
||||
*prev = cur;
|
||||
}
|
||||
|
||||
// main mount thread
|
||||
static void *mountthread(void _U_ *u){
|
||||
int errctr = 0;
|
||||
uint8_t buf[2*sizeof(SSstat)];
|
||||
SSstat *status = (SSstat*) buf;
|
||||
bzero(&mountdata, sizeof(mountdata));
|
||||
double t0 = nanotime();
|
||||
static double oldmt = -100.; // old `millis measurement` time
|
||||
double t0 = timefromstart(), tstart = t0, tcur = t0;
|
||||
double oldmt = -100.; // old `millis measurement` time
|
||||
static uint32_t oldmillis = 0;
|
||||
if(Conf.RunModel) while(1){
|
||||
coordval_pair_t c;
|
||||
// now change data
|
||||
getModData(&c);
|
||||
pthread_mutex_lock(&datamutex);
|
||||
double tnow = c.X.t;
|
||||
mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow;
|
||||
mountdata.motXposition.val = mountdata.encXposition.val = c.X.val;
|
||||
mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val;
|
||||
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
|
||||
if(tnow - oldmt > Conf.MountReqInterval){
|
||||
oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3);
|
||||
oldmt = tnow;
|
||||
}else mountdata.millis = oldmillis;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
getXspeed(); getYspeed();
|
||||
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
|
||||
t0 = nanotime();
|
||||
if(Conf.RunModel){
|
||||
double Xprev = NAN, Yprev = NAN; // previous coordinates
|
||||
int xcnt = 0, ycnt = 0;
|
||||
while(1){
|
||||
coordpair_t c;
|
||||
movestate_t xst, yst;
|
||||
// now change data
|
||||
getModData(&c, &xst, &yst);
|
||||
struct timespec tnow;
|
||||
if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
|
||||
mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
|
||||
mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
|
||||
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
|
||||
if(tcur - oldmt > Conf.MountReqInterval){
|
||||
oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
|
||||
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
|
||||
if(xst == ST_MOVE)
|
||||
mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
|
||||
//else
|
||||
// mountdata.motXposition.val = c.X;
|
||||
if(yst == ST_MOVE)
|
||||
mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
|
||||
//else
|
||||
// mountdata.motYposition.val = c.Y;
|
||||
oldmt = tcur;
|
||||
}else mountdata.millis = oldmillis;
|
||||
chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
|
||||
chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
|
||||
getXspeed(); getYspeed();
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
|
||||
t0 = timefromstart();
|
||||
}
|
||||
}
|
||||
// data to get
|
||||
data_t d = {.buf = buf, .maxlen = sizeof(buf)};
|
||||
@@ -421,31 +545,8 @@ static void *mountthread(void _U_ *u){
|
||||
if(!cmd_getstat) goto failed;
|
||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||
// read data to status
|
||||
double t0 = nanotime();
|
||||
#if 0
|
||||
// 127 milliseconds to get answer on X/Y commands!!!
|
||||
int64_t ans;
|
||||
int ctr = 0;
|
||||
if(SSgetint(CMD_MOTX, &ans)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.motXposition.t = tgot;
|
||||
mountdata.motXposition.val = X_MOT2RAD(ans);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
++ctr;
|
||||
}
|
||||
tgot = nanotime();
|
||||
if(SSgetint(CMD_MOTY, &ans)){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.motXposition.t = tgot;
|
||||
mountdata.motXposition.val = X_MOT2RAD(ans);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
++ctr;
|
||||
}
|
||||
if(ctr == 2){
|
||||
mountdata.millis = (uint32_t)(1e3 * tgot);
|
||||
DBG("Got both coords; millis=%d", mountdata.millis);
|
||||
}
|
||||
#endif
|
||||
struct timespec tcur;
|
||||
if(!curtime(&tcur)) continue;
|
||||
// 80 milliseconds to get answer on GETSTAT
|
||||
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
|
||||
#ifdef EBUG
|
||||
@@ -462,14 +563,13 @@ static void *mountthread(void _U_ *u){
|
||||
errctr = 0;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
// now change data
|
||||
SSconvstat(status, &mountdata, t0);
|
||||
SSconvstat(status, &mountdata, &tcur);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("GOT FULL stat by %g", nanotime() - t0);
|
||||
// allow writing & getters
|
||||
do{
|
||||
usleep(500);
|
||||
}while(nanotime() - t0 < Conf.MountReqInterval);
|
||||
t0 = nanotime();
|
||||
}while(timefromstart() - t0 < Conf.MountReqInterval);
|
||||
t0 = timefromstart();
|
||||
}
|
||||
data_free(&cmd_getstat);
|
||||
failed:
|
||||
@@ -485,8 +585,15 @@ static int ttyopen(const char *path, speed_t speed){
|
||||
int fd = -1;
|
||||
struct termios2 tty;
|
||||
DBG("Try to open %s @ %d", path, speed);
|
||||
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
|
||||
if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
|
||||
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
|
||||
DBG("Can't open device %s: %s", path, strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
if(ioctl(fd, TCGETS2, &tty)){
|
||||
DBG("Can't read TTY settings");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
@@ -495,7 +602,11 @@ static int ttyopen(const char *path, speed_t speed){
|
||||
tty.c_ospeed = speed;
|
||||
//tty.c_cc[VMIN] = 0; // non-canonical mode
|
||||
//tty.c_cc[VTIME] = 5;
|
||||
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
|
||||
if(ioctl(fd, TCSETS2, &tty)){
|
||||
DBG("Can't set TTY settings");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
|
||||
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
|
||||
// try to set exclusive
|
||||
@@ -528,7 +639,7 @@ int openEncoder(){
|
||||
if(encfd[i] < 0) return FALSE;
|
||||
}
|
||||
encRtmout.tv_sec = 0;
|
||||
encRtmout.tv_usec = 1000; // 1ms
|
||||
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
|
||||
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
|
||||
for(int i = 0; i < 2; ++i){
|
||||
close(encfd[i]);
|
||||
@@ -575,6 +686,7 @@ create_thread:
|
||||
|
||||
// close all opened serial devices and quit threads
|
||||
void closeSerial(){
|
||||
// TODO: close devices in "model" mode too!
|
||||
if(Conf.RunModel) return;
|
||||
if(mntfd > -1){
|
||||
DBG("Cancel mount thread");
|
||||
@@ -606,6 +718,8 @@ mcc_errcodes_t getMD(mountdata_t *d){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
*d = mountdata;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val);
|
||||
//DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL));
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
@@ -624,30 +738,24 @@ static int wr(const data_t *out, data_t *in, int needeol){
|
||||
return FALSE;
|
||||
}
|
||||
clrmntbuf();
|
||||
//double t0 = nanotime();
|
||||
if(out){
|
||||
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
|
||||
DBG("written bytes not equal to need");
|
||||
return FALSE;
|
||||
}
|
||||
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
|
||||
if(needeol){
|
||||
int g = write(mntfd, "\r", 1); // add EOL
|
||||
(void) g;
|
||||
}
|
||||
usleep(50000); // add little pause so that the idiot has time to swallow
|
||||
}
|
||||
//DBG("sent by %g", nanotime() - t0);
|
||||
//uint8_t buf[256];
|
||||
//data_t dumb = {.buf = buf, .maxlen = 256};
|
||||
if(!in) return TRUE;
|
||||
//if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
|
||||
in->len = 0;
|
||||
for(size_t i = 0; i < in->maxlen; ++i){
|
||||
int b = getmntbyte();
|
||||
if(b < 0) break; // nothing to read -> go out
|
||||
in->buf[in->len++] = (uint8_t) b;
|
||||
}
|
||||
//DBG("got %zd bytes by %g", in->len, nanotime() - t0);
|
||||
while(getmntbyte() > -1);
|
||||
return TRUE;
|
||||
}
|
||||
@@ -751,16 +859,23 @@ int cmdC(SSconfig *conf, int rw){
|
||||
}else{ // read
|
||||
data_t d;
|
||||
d.buf = (uint8_t *) conf;
|
||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||
d.len = 0; d.maxlen = 0;
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("write command: %s", ret ? "TRUE" : "FALSE");
|
||||
if(!ret) goto rtn;
|
||||
// make a huge pause for stupid SSII
|
||||
usleep(100000);
|
||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
||||
if(d.len != d.maxlen) return FALSE;
|
||||
if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
|
||||
// simplest checksum
|
||||
uint16_t sum = 0;
|
||||
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
||||
if(sum != conf->checksum){
|
||||
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
||||
return FALSE;
|
||||
ret = FALSE;
|
||||
goto rtn;
|
||||
}
|
||||
}
|
||||
rtn:
|
||||
|
||||
@@ -32,38 +32,13 @@ extern "C"
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
// acceptable position error - 0.1''
|
||||
#define MCC_POSITION_ERROR (5e-7)
|
||||
// acceptable disagreement between motor and axis encoders - 2''
|
||||
#define MCC_ENCODERS_ERROR (1e-7)
|
||||
|
||||
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||
#define MCC_MAX_X_SPEED (0.174533)
|
||||
#define MCC_MAX_Y_SPEED (0.139626)
|
||||
// accelerations by both axis (for model); TODO: move speeds/accelerations into config?
|
||||
// xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||
#define MCC_X_ACCELERATION (0.219911)
|
||||
#define MCC_Y_ACCELERATION (0.165806)
|
||||
|
||||
// minimal serial speed of mount device
|
||||
#define MOUNT_BAUDRATE_MIN (1200)
|
||||
// max speed interval, seconds
|
||||
#define MCC_CONF_MAX_SPEEDINT (2.)
|
||||
// minimal speed interval in parts of EncoderReqInterval
|
||||
#define MCC_CONF_MIN_SPEEDC (3.)
|
||||
// PID I cycle time (analog of "RC" for PID on opamps)
|
||||
#define MCC_PID_CYCLE_TIME (5.)
|
||||
// maximal PID refresh time interval (if larger all old data will be cleared)
|
||||
#define MCC_PID_MAX_DT (1.)
|
||||
// normal PID refresh interval
|
||||
#define MCC_PID_REFRESH_DT (0.1)
|
||||
// boundary conditions for axis state: "slewing/pointing/guiding"
|
||||
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees
|
||||
//#define MCC_MAX_POINTING_ERR (0.20943951)
|
||||
//#define MCC_MAX_POINTING_ERR (0.08726646)
|
||||
#define MCC_MAX_POINTING_ERR (0.13962634)
|
||||
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
|
||||
#define MCC_MAX_GUIDING_ERR (0.026179939)
|
||||
// if error less than this value we suppose that target is captured and guiding is good: 0.1''
|
||||
#define MCC_MAX_ATTARGET_ERR (4.8481368e-7)
|
||||
|
||||
|
||||
// error codes
|
||||
typedef enum{
|
||||
@@ -73,6 +48,7 @@ typedef enum{
|
||||
MCC_E_ENCODERDEV, // encoder device error or can't open
|
||||
MCC_E_MOUNTDEV, // mount device error or can't open
|
||||
MCC_E_FAILED, // failed to run command - protocol error
|
||||
MCC_E_AMOUNT // Just amount of errors
|
||||
} mcc_errcodes_t;
|
||||
|
||||
typedef struct{
|
||||
@@ -87,14 +63,23 @@ typedef struct{
|
||||
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
||||
char* EncoderXDevPath; // paths to new controller devices
|
||||
char* EncoderYDevPath;
|
||||
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
|
||||
double MountReqInterval; // interval between subsequent mount requests (seconds)
|
||||
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
|
||||
double EncoderSpeedInterval; // interval between speed calculations
|
||||
int RunModel; // == 1 if you want to use model instead of real mount
|
||||
double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared)
|
||||
double PIDRefreshDt; // normal PID refresh interval
|
||||
double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps)
|
||||
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
|
||||
PIDpar_t XPIDV;
|
||||
PIDpar_t YPIDC;
|
||||
PIDpar_t YPIDV;
|
||||
double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
|
||||
double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
|
||||
double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
|
||||
int XEncZero; // encoders' zero position
|
||||
int YEncZero;
|
||||
} conf_t;
|
||||
|
||||
// coordinates/speeds in degrees or d/s: X, Y
|
||||
@@ -105,7 +90,7 @@ typedef struct{
|
||||
// coordinate/speed and time of last measurement
|
||||
typedef struct{
|
||||
double val;
|
||||
double t;
|
||||
struct timespec t;
|
||||
} coordval_t;
|
||||
|
||||
typedef struct{
|
||||
@@ -206,6 +191,9 @@ typedef struct{
|
||||
double outplimit; // Output Limit, percent (0..100)
|
||||
double currlimit; // Current Limit (A)
|
||||
double intlimit; // Integral Limit (???)
|
||||
// these params are taken from mount by text commands (don't save negative values - better save these marks in xybits
|
||||
double motor_stepsperrev;// encoder's steps per revolution: motor and axis
|
||||
double axis_stepsperrev; // negative sign of these values means reverse direction
|
||||
} __attribute__((packed)) axis_config_t;
|
||||
|
||||
// hardware configuration
|
||||
@@ -247,7 +235,7 @@ typedef struct{
|
||||
void (*quit)(); // deinit
|
||||
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
||||
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
|
||||
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
|
||||
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target);
|
||||
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
|
||||
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
|
||||
mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
|
||||
@@ -257,7 +245,13 @@ typedef struct{
|
||||
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
|
||||
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
||||
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
|
||||
double (*currentT)(); // current time
|
||||
int (*currentT)(struct timespec *t); // current time
|
||||
double (*timeFromStart)(); // amount of seconds from last init
|
||||
double (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times
|
||||
double (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time
|
||||
mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
|
||||
mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//-
|
||||
mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
|
||||
} mount_t;
|
||||
|
||||
extern mount_t Mount;
|
||||
|
||||
@@ -26,6 +26,13 @@
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
|
||||
// defaults until read from controller
|
||||
double X_MOT_STEPSPERREV = 13312000.,
|
||||
Y_MOT_STEPSPERREV = 17578668.,
|
||||
X_ENC_STEPSPERREV = 67108864.,
|
||||
Y_ENC_STEPSPERREV = 67108864.;
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len){
|
||||
uint16_t checksum = 0;
|
||||
for(int i = 0; i < len; i++){
|
||||
@@ -67,17 +74,18 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
|
||||
* @param m (o) - output
|
||||
* @param t - measurement time
|
||||
*/
|
||||
void SSconvstat(const SSstat *s, mountdata_t *m, double t){
|
||||
if(!s || !m) return;
|
||||
void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
|
||||
if(!s || !m || !t) return;
|
||||
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
||||
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
||||
ChkStopped(s, m);
|
||||
m->motXposition.t = m->motYposition.t = t;
|
||||
m->motXposition.t = m->motYposition.t = *t;
|
||||
// fill encoder data from here, as there's no separate enc thread
|
||||
if(!Conf.SepEncoder){
|
||||
m->encXposition.val = X_ENC2RAD(s->Xenc);
|
||||
m->encYposition.val = Y_ENC2RAD(s->Yenc);
|
||||
m->encXposition.t = m->encYposition.t = t;
|
||||
m->encXposition.val = Xenc2rad(s->Xenc);
|
||||
DBG("encx: %g", m->encXposition.val);
|
||||
m->encYposition.val = Yenc2rad(s->Yenc);
|
||||
m->encXposition.t = m->encYposition.t = *t;
|
||||
getXspeed(); getYspeed();
|
||||
}
|
||||
m->keypad = s->keypad;
|
||||
@@ -176,33 +184,39 @@ int SSstop(int emerg){
|
||||
mcc_errcodes_t updateMotorPos(){
|
||||
mountdata_t md = {0};
|
||||
if(Conf.RunModel) return MCC_E_OK;
|
||||
double t0 = nanotime(), t = 0.;
|
||||
double t0 = timefromstart(), t = 0.;
|
||||
struct timespec curt;
|
||||
DBG("start @ %g", t0);
|
||||
do{
|
||||
t = nanotime();
|
||||
t = timefromstart();
|
||||
if(!curtime(&curt)){
|
||||
usleep(10000);
|
||||
continue;
|
||||
}
|
||||
//DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
|
||||
if(MCC_E_OK == getMD(&md)){
|
||||
if(md.encXposition.t == 0 || md.encYposition.t == 0){
|
||||
DBG("Just started, t-t0 = %g!", t - t0);
|
||||
sleep(1);
|
||||
DBG("t-t0 = %g", nanotime() - t0);
|
||||
//usleep(10000);
|
||||
if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
|
||||
DBG("Just started? t-t0 = %g!", t - t0);
|
||||
usleep(10000);
|
||||
continue;
|
||||
}
|
||||
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
|
||||
if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
|
||||
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
|
||||
mcc_errcodes_t OK = MCC_E_OK;
|
||||
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
|
||||
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
|
||||
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
|
||||
DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
|
||||
DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
|
||||
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
|
||||
DBG("Xpos sync failed!");
|
||||
OK = MCC_E_FAILED;
|
||||
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
|
||||
}else DBG("Xpos sync OK, Dt=%g", t - t0);
|
||||
}
|
||||
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
|
||||
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
|
||||
if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
|
||||
DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
|
||||
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
|
||||
DBG("Ypos sync failed!");
|
||||
OK = MCC_E_FAILED;
|
||||
}else DBG("Ypos sync OK, Dt=%g", nanotime() - t0);
|
||||
}else DBG("Ypos sync OK, Dt=%g", t - t0);
|
||||
}
|
||||
if(MCC_E_OK == OK){
|
||||
DBG("Encoders synced");
|
||||
|
||||
@@ -173,64 +173,74 @@
|
||||
#define SITECH_LOOP_FREQUENCY (1953.)
|
||||
|
||||
// amount of consequent same coordinates to detect stop
|
||||
#define MOTOR_STOPPED_CNT (4)
|
||||
#define MOTOR_STOPPED_CNT (19)
|
||||
|
||||
// replace macros with global variables inited when config read
|
||||
extern int X_ENC_ZERO, Y_ENC_ZERO;
|
||||
extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
|
||||
|
||||
// TODO: take it from settings?
|
||||
// steps per revolution (SSI - x4 - for SSI)
|
||||
#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||
// -> hwconf.Xconf.mot/enc_stepsperrev
|
||||
//#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||
// 13312000 / 4 = 3328000
|
||||
#define X_MOT_STEPSPERREV (3328000.)
|
||||
#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
||||
//#define X_MOT_STEPSPERREV (3328000.)
|
||||
//#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
||||
// 17578668 / 4 = 4394667
|
||||
#define Y_MOT_STEPSPERREV (4394667.)
|
||||
//#define Y_MOT_STEPSPERREV (4394667.)
|
||||
|
||||
// encoder per revolution
|
||||
#define X_ENC_STEPSPERREV (67108864.)
|
||||
#define Y_ENC_STEPSPERREV (67108864.)
|
||||
//#define X_ENC_STEPSPERREV (67108864.)
|
||||
//#define Y_ENC_STEPSPERREV (67108864.)
|
||||
// encoder zero position
|
||||
#define X_ENC_ZERO (61245239)
|
||||
#define Y_ENC_ZERO (36999830)
|
||||
// encoder reversed (no: +1)
|
||||
#define X_ENC_SIGN (-1.)
|
||||
#define Y_ENC_SIGN (-1.)
|
||||
// -> conf.XEncZero/YEncZero
|
||||
//#define X_ENC_ZERO (61245239)
|
||||
//#define Y_ENC_ZERO (36999830)
|
||||
// encoder reversed (no: +1) -> sign of ...stepsperrev
|
||||
//#define X_ENC_SIGN (-1.)
|
||||
//#define Y_ENC_SIGN (-1.)
|
||||
|
||||
|
||||
// encoder position to radians and back
|
||||
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
|
||||
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
|
||||
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
|
||||
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
|
||||
#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
|
||||
#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
|
||||
#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
|
||||
#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
|
||||
|
||||
// convert angle in radians to +-pi
|
||||
static inline double ang2half(double ang){
|
||||
static inline __attribute__((always_inline)) double ang2half(double ang){
|
||||
ang = fmod(ang, 2.*M_PI);
|
||||
if(ang < -M_PI) ang += 2.*M_PI;
|
||||
else if(ang > M_PI) ang -= 2.*M_PI;
|
||||
return ang;
|
||||
}
|
||||
// convert to only positive: 0..2pi
|
||||
static inline double ang2full(double ang){
|
||||
static inline __attribute__((always_inline)) double ang2full(double ang){
|
||||
ang = fmod(ang, 2.*M_PI);
|
||||
if(ang < 0.) ang += 2.*M_PI;
|
||||
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
|
||||
return ang;
|
||||
}
|
||||
|
||||
// motor position to radians and back
|
||||
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
|
||||
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
|
||||
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
|
||||
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
|
||||
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
|
||||
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
|
||||
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
|
||||
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
|
||||
// motor speed in rad/s and back
|
||||
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
|
||||
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY)
|
||||
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
|
||||
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY))
|
||||
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||
// motor acceleration -//-
|
||||
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
|
||||
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY)
|
||||
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
|
||||
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY))
|
||||
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||
|
||||
// adder time to seconds vice versa
|
||||
#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY)
|
||||
#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY)
|
||||
#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY))
|
||||
#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY))
|
||||
|
||||
// encoder's tolerance (ticks)
|
||||
#define YencTOL (25.)
|
||||
@@ -331,7 +341,7 @@ typedef struct{
|
||||
} __attribute__((packed)) SSconfig;
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, double t);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
|
||||
int SStextcmd(const char *cmd, data_t *answer);
|
||||
int SSrawcmd(const char *cmd, data_t *answer);
|
||||
int SSgetint(const char *cmd, int64_t *ans);
|
||||
|
||||
88
asibfm700/CMakeLists.txt
Normal file
88
asibfm700/CMakeLists.txt
Normal file
@@ -0,0 +1,88 @@
|
||||
cmake_minimum_required(VERSION 3.14)
|
||||
|
||||
# set(CMAKE_BUILD_TYPE Release)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 23)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
|
||||
|
||||
|
||||
# find_package(ASIO QUIET CONFIG)
|
||||
find_package(ASIO QUIET)
|
||||
if (ASIO_FOUND)
|
||||
message(STATUS "ASIO library was found in the host system")
|
||||
else()
|
||||
message(STATUS "ASIO library was not found! Try to download it!")
|
||||
|
||||
include(FetchContent)
|
||||
include(ExternalProject)
|
||||
|
||||
FetchContent_Declare(asio_lib
|
||||
SOURCE_DIR ${CMAKE_BINARY_DIR}/asio_lib
|
||||
BINARY_DIR ${CMAKE_BINARY_DIR}
|
||||
GIT_REPOSITORY "https://github.com/chriskohlhoff/asio"
|
||||
GIT_TAG "asio-1-32-0"
|
||||
GIT_SHALLOW TRUE
|
||||
GIT_SUBMODULES ""
|
||||
GIT_PROGRESS TRUE
|
||||
)
|
||||
FetchContent_MakeAvailable(asio_lib)
|
||||
# FetchContent_GetProperties(asio_lib SOURCE_DIR asio_SOURCE_DIR)
|
||||
|
||||
set(ASIO_INSTALL_DIR ${CMAKE_BINARY_DIR}/asio_lib/asio)
|
||||
find_package(ASIO)
|
||||
endif()
|
||||
|
||||
find_package(cxxopts QUIET CONFIG)
|
||||
if (cxxopts_FOUND)
|
||||
message(STATUS "CXXOPTS library was found in the host system")
|
||||
else()
|
||||
message(STATUS "CXXOPTS library was not found! Try to download it!")
|
||||
|
||||
include(FetchContent)
|
||||
include(ExternalProject)
|
||||
|
||||
FetchContent_Declare(cxxopts_lib
|
||||
PREFIX ${CMAKE_BINARY_DIR}/cxxopts_lib
|
||||
# SOURCE_DIR ${CMAKE_BINARY_DIR}/cxxopts_lib
|
||||
# BINARY_DIR ${CMAKE_BINARY_DIR}
|
||||
GIT_REPOSITORY "https://github.com/jarro2783/cxxopts.git"
|
||||
GIT_TAG "v3.3.1"
|
||||
GIT_SHALLOW TRUE
|
||||
GIT_SUBMODULES ""
|
||||
GIT_PROGRESS TRUE
|
||||
OVERRIDE_FIND_PACKAGE
|
||||
)
|
||||
FetchContent_MakeAvailable(cxxopts_lib)
|
||||
find_package(cxxopts CONFIG)
|
||||
endif()
|
||||
|
||||
set(ASIBFM700_LIB_SRC asibfm700_common.h asibfm700_servocontroller.h asibfm700_servocontroller.cpp)
|
||||
|
||||
set(ASIBFM700_LIB asibfm700mount)
|
||||
add_library(${ASIBFM700_LIB} STATIC ${ASIBFM700_LIB_SRC}
|
||||
asibfm700_mount.h asibfm700_mount.cpp
|
||||
asibfm700_configfile.h
|
||||
asibfm700_netserver.cpp
|
||||
asibfm700_netserver.h
|
||||
)
|
||||
|
||||
target_include_directories(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_INCLUDE_DIR})
|
||||
# target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_LIBFILE})
|
||||
target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc ASIO::ASIO spdlog ERFA_LIB bsplines sidservo)
|
||||
|
||||
|
||||
set(ASIBFM700_NETSERVER_APP asibfm700_netserver)
|
||||
add_executable(${ASIBFM700_NETSERVER_APP} asibfm700_netserver_main.cpp)
|
||||
target_link_libraries(${ASIBFM700_NETSERVER_APP} PRIVATE cxxopts::cxxopts ${ASIBFM700_LIB})
|
||||
|
||||
option(WITH_TESTS "Build tests" ON)
|
||||
|
||||
if (WITH_TESTS)
|
||||
set(CFG_TEST_APP cfg_test)
|
||||
add_executable(${CFG_TEST_APP} tests/cfg_test.cpp)
|
||||
target_link_libraries(${CFG_TEST_APP} PRIVATE mcc)
|
||||
|
||||
enable_testing()
|
||||
endif()
|
||||
30
asibfm700/asibfm700_common.h
Normal file
30
asibfm700/asibfm700_common.h
Normal file
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
/* AstroSib FORK MOUNT FM-700 CONTROL LIBRARY */
|
||||
|
||||
|
||||
/* COMMON LIBRARY DEFINITIONS */
|
||||
|
||||
|
||||
#include <mcc_moving_model_common.h>
|
||||
#include <mcc_pcm.h>
|
||||
#include <mcc_pzone_container.h>
|
||||
#include <mcc_spdlog.h>
|
||||
|
||||
#include "mcc_ccte_erfa.h"
|
||||
#include "mcc_slewing_model.h"
|
||||
#include "mcc_tracking_model.h"
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
static constexpr mcc::MccMountType asibfm700MountType = mcc::MccMountType::FORK_TYPE;
|
||||
|
||||
typedef mcc::ccte::erfa::MccCCTE_ERFA Asibfm700CCTE;
|
||||
typedef mcc::MccDefaultPCM<asibfm700MountType> Asibfm700PCM;
|
||||
typedef mcc::MccPZoneContainer<mcc::MccTimeDuration> Asibfm700PZoneContainer;
|
||||
typedef mcc::utils::MccSpdlogLogger Asibfm700Logger;
|
||||
typedef mcc::MccSimpleSlewingModel Asibfm700SlewingModel;
|
||||
typedef mcc::MccSimpleTrackingModel Asibfm700TrackingModel;
|
||||
|
||||
} // namespace asibfm700
|
||||
860
asibfm700/asibfm700_configfile.h
Normal file
860
asibfm700/asibfm700_configfile.h
Normal file
@@ -0,0 +1,860 @@
|
||||
#pragma once
|
||||
|
||||
/**/
|
||||
|
||||
#include <expected>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
|
||||
#include <mcc_angle.h>
|
||||
#include <mcc_moving_model_common.h>
|
||||
#include <mcc_pcm.h>
|
||||
#include <mcc_utils.h>
|
||||
|
||||
#include "asibfm700_common.h"
|
||||
#include "asibfm700_servocontroller.h"
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
|
||||
/* A SIMPLE "KEYWORD - VALUE" HOLDER CLASS SUITABLE TO STORE SOME APPLICATION CONFIGURATION */
|
||||
|
||||
|
||||
// to follow std::variant requirements (not references, not array, not void)
|
||||
template <typename T>
|
||||
concept config_record_valid_type_c = requires { !std::is_array_v<T> && !std::is_void_v<T> && !std::is_reference_v<T>; };
|
||||
|
||||
// simple minimal-requirement configuration record class
|
||||
template <config_record_valid_type_c T>
|
||||
struct simple_config_record_t {
|
||||
std::string_view key;
|
||||
T value;
|
||||
std::vector<std::string_view> comment;
|
||||
};
|
||||
|
||||
|
||||
/* ASTOROSIB FM700 MOUNT CONFIGURATION CLASS */
|
||||
|
||||
// configuration description and its defaults
|
||||
static auto Asibfm700MountConfigDefaults = std::make_tuple(
|
||||
// main cycle period in millisecs
|
||||
simple_config_record_t{"hardwarePollingPeriod", std::chrono::milliseconds{100}, {"main cycle period in millisecs"}},
|
||||
|
||||
/* geographic coordinates of the observation site */
|
||||
|
||||
// site latitude in degrees
|
||||
simple_config_record_t{"siteLatitude", mcc::MccAngle(43.646711_degs), {"site latitude in degrees"}},
|
||||
|
||||
// site longitude in degrees
|
||||
simple_config_record_t{"siteLongitude", mcc::MccAngle(41.440732_degs), {"site longitude in degrees"}},
|
||||
|
||||
// site elevation in meters
|
||||
simple_config_record_t{"siteElevation", 2070.0, {"site elevation in meters"}},
|
||||
|
||||
/* celestial coordinate transformation */
|
||||
|
||||
// wavelength at which refraction is calculated (in mkm)
|
||||
simple_config_record_t{"refractWavelength", 0.55, {"wavelength at which refraction is calculated (in mkm)"}},
|
||||
|
||||
// an empty filename means default precompiled string
|
||||
simple_config_record_t{"leapSecondFilename", std::string(), {"an empty filename means default precompiled string"}},
|
||||
|
||||
// an empty filename means default precompiled string
|
||||
simple_config_record_t{"bulletinAFilename", std::string(), {"an empty filename means default precompiled string"}},
|
||||
|
||||
/* pointing correction model */
|
||||
|
||||
// PCM default type
|
||||
simple_config_record_t{"pcmType",
|
||||
mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY,
|
||||
{"PCM type:", "GEOMETRY - 'classic' geometry-based correction coefficients",
|
||||
"GEOMETRY-BSPLINE - previous one and additional 2D B-spline corrections",
|
||||
"BSPLINE - pure 2D B-spline corrections"}},
|
||||
|
||||
// PCM geometrical coefficients
|
||||
simple_config_record_t{"pcmGeomCoeffs",
|
||||
std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{"PCM geometrical coefficients"}},
|
||||
|
||||
// PCM B-spline degrees
|
||||
simple_config_record_t{"pcmBsplineDegree", std::vector<size_t>{3, 3}, {"PCM B-spline degrees"}},
|
||||
|
||||
// PCM B-spline knots along X-axis (HA-angle or azimuth). By default from 0 to 2*PI radians
|
||||
// NOTE: The first and last values are interpretated as border knots!!!
|
||||
// Thus the array length must be equal to or greater than 2!
|
||||
simple_config_record_t{"pcmBsplineXknots",
|
||||
std::vector<double>{0.0, 0.6981317, 1.3962634, 2.0943951, 2.7925268, 3.4906585, 4.1887902,
|
||||
4.88692191, 5.58505361, 6.28318531},
|
||||
{"PCM B-spline knots along X-axis (HA-angle or azimuth). By default from 0 to 2*PI radians",
|
||||
"NOTE: The first and last values are interpretated as border knots!!!",
|
||||
" Thus the array length must be equal to or greater than 2!"}},
|
||||
|
||||
// PCM B-spline knots along Y-axis (declination or zenithal distance). By default from -PI/6 to PI/2 radians
|
||||
// NOTE: The first and last values are interpretated as border knots!!!
|
||||
// Thus the array length must be equal to or greater than 2!
|
||||
simple_config_record_t{
|
||||
"pcmBsplineYknots",
|
||||
std::vector<double>{-0.52359878, -0.29088821, -0.05817764, 0.17453293, 0.40724349, 0.63995406, 0.87266463,
|
||||
1.10537519, 1.33808576, 1.57079633},
|
||||
{"PCM B-spline knots along Y-axis (declination or zenithal distance). By default from -PI/6 to PI/2 radians",
|
||||
"NOTE: The first and last values are interpretated as border knots!!!",
|
||||
" Thus the array length must be equal to or greater than 2!"}},
|
||||
|
||||
// PCM B-spline coeffs for along X-axis (HA-angle or azimuth)
|
||||
simple_config_record_t{"pcmBsplineXcoeffs",
|
||||
std::vector<double>{},
|
||||
{"PCM B-spline coeffs for along X-axis (HA-angle)"}},
|
||||
|
||||
// PCM B-spline coeffs for along Y-axis (declination or zenithal distance)
|
||||
simple_config_record_t{"pcmBsplineYcoeffs",
|
||||
std::vector<double>{},
|
||||
{"PCM B-spline coeffs for along Y-axis (declination angle)"}},
|
||||
|
||||
|
||||
/* slewing and tracking parameters */
|
||||
|
||||
// // arcseconds per second
|
||||
// simple_config_record_t{"sideralRate", 15.0410686},
|
||||
|
||||
// timeout for telemetry updating in milliseconds
|
||||
simple_config_record_t{"telemetryTimeout",
|
||||
std::chrono::milliseconds(3000),
|
||||
{"timeout for telemetry updating in milliseconds"}},
|
||||
|
||||
// minimal allowed time in seconds to prohibited zone
|
||||
simple_config_record_t{"minTimeToPZone",
|
||||
std::chrono::seconds(10),
|
||||
{"minimal allowed time in seconds to prohibited zone"}},
|
||||
|
||||
// a time interval to update prohibited zones related quantities (millisecs)
|
||||
simple_config_record_t{"updatingPZoneInterval",
|
||||
std::chrono::milliseconds(5000),
|
||||
{"a time interval to update prohibited zones related quantities (millisecs)"}},
|
||||
|
||||
// coordinates difference in arcsecs to stop slewing
|
||||
simple_config_record_t{"slewToleranceRadius", 5.0, {"coordinates difference in arcsecs to stop slewing"}},
|
||||
|
||||
simple_config_record_t{"slewingTelemetryInterval",
|
||||
std::chrono::milliseconds(100),
|
||||
{"telemetry request interval (in millisecs) in slewing mode"}},
|
||||
|
||||
simple_config_record_t{"slewingPathFilename",
|
||||
std::string(),
|
||||
{"slewing trajectory filename", "if it is an empty - just skip saving"}},
|
||||
|
||||
// target-mount coordinate difference in arcsecs to start adjusting of slewing
|
||||
simple_config_record_t{"adjustCoordDiff",
|
||||
50.0,
|
||||
{"target-mount coordinate difference in arcsecs to start adjusting of slewing"}},
|
||||
|
||||
// minimum time in millisecs between two successive adjustments
|
||||
simple_config_record_t{"adjustCycleInterval",
|
||||
std::chrono::milliseconds(300),
|
||||
{"minimum time in millisecs between two successive adjustments"}},
|
||||
|
||||
// slew process timeout in seconds
|
||||
simple_config_record_t{"slewTimeout", std::chrono::seconds(3600), {"slew process timeout in seconds"}},
|
||||
|
||||
// a time shift into future to compute target position in future (UT1-scale time duration, millisecs)
|
||||
simple_config_record_t{
|
||||
"timeShiftToTargetPoint",
|
||||
std::chrono::milliseconds(10000),
|
||||
{"a time shift into future to compute target position in future (UT1-scale time duration, millisecs)"}},
|
||||
|
||||
|
||||
simple_config_record_t{"trackingTelemetryInterval",
|
||||
std::chrono::milliseconds(100),
|
||||
{"telemetry request interval (in millisecs) in tracking mode"}},
|
||||
|
||||
|
||||
// minimum time in millisecs between two successive tracking corrections
|
||||
simple_config_record_t{"trackingCycleInterval",
|
||||
std::chrono::milliseconds(300),
|
||||
{"minimum time in millisecs between two successive tracking corrections"}},
|
||||
|
||||
// maximal valid target-to-mount distance for tracking process (arcsecs)
|
||||
// if current distance is greater than assume current mount coordinate as target point
|
||||
simple_config_record_t{"trackingMaxCoordDiff",
|
||||
20.0,
|
||||
{"maximal valid target-to-mount distance for tracking process (arcsecs)",
|
||||
"if current distance is greater than assume current mount coordinate as target point"}},
|
||||
|
||||
simple_config_record_t{"trackingPathFilename",
|
||||
std::string(),
|
||||
{"tracking trajectory filename", "if it is an empty - just skip saving"}},
|
||||
|
||||
|
||||
/* prohibited zones */
|
||||
|
||||
// minimal altitude
|
||||
simple_config_record_t{"pzMinAltitude", mcc::MccAngle(10.0_degs), {"minimal altitude"}},
|
||||
|
||||
// HA-axis limit switch minimal value
|
||||
simple_config_record_t{"pzLimitSwitchHAMin", mcc::MccAngle(-270.0_degs), {"HA-axis limit switch minimal value"}},
|
||||
|
||||
// HA-axis limit switch maximal value
|
||||
simple_config_record_t{"pzLimitSwitchHAMax", mcc::MccAngle(270.0_degs), {"HA-axis limit switch maximal value"}},
|
||||
|
||||
// DEC-axis limit switch minimal value
|
||||
simple_config_record_t{"pzLimitSwitchDecMin", mcc::MccAngle(-90.0_degs), {"DEC-axis limit switch minimal value"}},
|
||||
|
||||
// DEC-axis limit switch maximal value
|
||||
simple_config_record_t{"pzLimitSwitchDecMax", mcc::MccAngle(90.0_degs), {"DEC-axis limit switch maximal value"}},
|
||||
|
||||
|
||||
/* hardware-related */
|
||||
|
||||
// hardware mode: 1 - model mode, otherwise real mode
|
||||
simple_config_record_t{"RunModel", 0, {"hardware mode: 1 - model mode, otherwise real mode"}},
|
||||
|
||||
// mount serial device paths
|
||||
simple_config_record_t{"MountDevPath", std::string("/dev/ttyUSB0"), {"mount serial device paths"}},
|
||||
|
||||
// mount serial device speed
|
||||
simple_config_record_t{"MountDevSpeed", 19200, {"mount serial device speed"}},
|
||||
|
||||
// motor encoders serial device path
|
||||
simple_config_record_t{"EncoderDevPath", std::string(""), {"motor encoders serial device path"}},
|
||||
|
||||
// X-axis encoder serial device path
|
||||
simple_config_record_t{"EncoderXDevPath", std::string("/dev/encoderX0"), {"X-axis encoder serial device path"}},
|
||||
|
||||
// Y-axis encoder serial device path
|
||||
simple_config_record_t{"EncoderYDevPath", std::string("/dev/encoderY0"), {"Y-axis encoder serial device path"}},
|
||||
|
||||
// encoders serial device speed
|
||||
simple_config_record_t{"EncoderDevSpeed", 153000, {"encoders serial device speed"}},
|
||||
|
||||
// ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
||||
simple_config_record_t{
|
||||
"SepEncoder",
|
||||
2,
|
||||
{"==1 if encoder works as separate serial device, ==2 if there's new version with two devices"}},
|
||||
|
||||
|
||||
// mount polling interval in millisecs
|
||||
simple_config_record_t{"MountReqInterval", std::chrono::milliseconds(100), {"mount polling interval in millisecs"}},
|
||||
|
||||
// encoders polling interval in millisecs
|
||||
simple_config_record_t{"EncoderReqInterval",
|
||||
std::chrono::milliseconds(1),
|
||||
{"encoders polling interval in millisecs"}},
|
||||
|
||||
// mount axes rate calculation interval in millisecs
|
||||
simple_config_record_t{"EncoderSpeedInterval",
|
||||
std::chrono::milliseconds(50),
|
||||
{"mount axes rate calculation interval in millisecs"}},
|
||||
|
||||
simple_config_record_t{"PIDMaxDt",
|
||||
std::chrono::milliseconds(1000),
|
||||
{"maximal PID refresh time interval in millisecs",
|
||||
"NOTE: if PID data will be refreshed with interval longer than this value (e.g. user polls "
|
||||
"encoder data too rarely)",
|
||||
"then the PID 'expired' data will be cleared and new computing loop is started"}},
|
||||
|
||||
simple_config_record_t{"PIDRefreshDt", std::chrono::milliseconds(100), {"PID refresh interval"}},
|
||||
|
||||
simple_config_record_t{"PIDCycleDt",
|
||||
std::chrono::milliseconds(5000),
|
||||
{"PID I cycle time (analog of 'RC' for PID on opamps)"}},
|
||||
|
||||
|
||||
|
||||
// X-axis coordinate PID P,I,D-params
|
||||
simple_config_record_t{"XPIDC", std::vector<double>{0.5, 0.1, 0.2}, {"X-axis coordinate PID P,I,D-params"}},
|
||||
|
||||
// X-axis rate PID P,I,D-params
|
||||
simple_config_record_t{"XPIDV", std::vector<double>{0.09, 0.0, 0.05}, {"X-axis rate PID P,I,D-params"}},
|
||||
|
||||
// Y-axis coordinate PID P, I, D-params
|
||||
simple_config_record_t{"YPIDC", std::vector<double>{0.5, 0.1, 0.2}, {"Y-axis coordinate PID P, I, D-params"}},
|
||||
|
||||
// Y-axis rate PID P,I,D-params
|
||||
simple_config_record_t{"YPIDV", std::vector<double>{0.09, 0.0, 0.05}, {"Y-axis rate PID P,I,D-params"}},
|
||||
|
||||
|
||||
// maximal moving rate (degrees per second) along HA-axis (Y-axis of Sidereal servo microcontroller)
|
||||
simple_config_record_t{
|
||||
"hwMaxRateHA",
|
||||
mcc::MccAngle(8.0_degs),
|
||||
{"maximal moving rate (degrees per second) along HA-axis (Y-axis of Sidereal servo microcontroller)"}},
|
||||
|
||||
// maximal moving rate (degrees per second) along DEC-axis (X-axis of Sidereal servo microcontroller)
|
||||
simple_config_record_t{
|
||||
"hwMaxRateDEC",
|
||||
mcc::MccAngle(10.0_degs),
|
||||
{"maximal moving rate (degrees per second) along DEC-axis (X-axis of Sidereal servo microcontroller)"}},
|
||||
|
||||
simple_config_record_t{"MaxPointingErr",
|
||||
mcc::MccAngle(8.0_degs),
|
||||
{"slewing-to-pointing mode angular limit in degrees"}},
|
||||
|
||||
simple_config_record_t{"MaxFinePointingErr",
|
||||
mcc::MccAngle(1.5_degs),
|
||||
{"pointing-to-guiding mode angular limit in degrees"}},
|
||||
|
||||
simple_config_record_t{"MaxGuidingErr",
|
||||
mcc::MccAngle(0.5_arcsecs),
|
||||
{"guiding 'good'-flag error cirle radius (mount-to-target distance) in degrees"}},
|
||||
|
||||
simple_config_record_t{"XEncZero", mcc::MccAngle(0.0_degs), {"X-axis encoder zero-point in degrees"}},
|
||||
|
||||
simple_config_record_t{"YEncZero", mcc::MccAngle(0.0_degs), {"Y-axis encoder zero-point in degrees"}}
|
||||
|
||||
);
|
||||
|
||||
|
||||
class Asibfm700MountConfig : public mcc::utils::KeyValueHolder<decltype(Asibfm700MountConfigDefaults)>
|
||||
{
|
||||
using base_t = mcc::utils::KeyValueHolder<decltype(Asibfm700MountConfigDefaults)>;
|
||||
|
||||
protected:
|
||||
inline static auto deserializer = []<typename VT>(std::string_view str, VT& value) {
|
||||
std::error_code ec{};
|
||||
|
||||
mcc::utils::MccSimpleDeserializer deser;
|
||||
deser.setRangeDelim(base_t::VALUE_ARRAY_DELIM);
|
||||
|
||||
if constexpr (std::is_arithmetic_v<VT> || mcc::traits::mcc_output_char_range<VT> || std::ranges::range<VT> ||
|
||||
mcc::traits::mcc_time_duration_c<VT>) {
|
||||
// ec = base_t::defaultDeserializeFunc(str, value);
|
||||
ec = deser(str, value);
|
||||
} else if constexpr (std::same_as<VT, mcc::MccAngle>) { // assume here all angles are in degrees
|
||||
double vd;
|
||||
// ec = base_t::defaultDeserializeFunc(str, vd);
|
||||
ec = deser(str, vd);
|
||||
if (!ec) {
|
||||
value = mcc::MccAngle(vd, mcc::MccDegreeTag{});
|
||||
}
|
||||
} else if constexpr (std::same_as<VT, mcc::MccDefaultPCMType>) {
|
||||
std::string vstr;
|
||||
// ec = base_t::defaultDeserializeFunc(str, vstr);
|
||||
ec = deser(str, vstr);
|
||||
|
||||
if (!ec) {
|
||||
auto s = mcc::utils::trimSpaces(vstr);
|
||||
|
||||
if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY>) {
|
||||
value = mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY;
|
||||
} else if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE>) {
|
||||
value = mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY;
|
||||
} else if (s == mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE>) {
|
||||
value = mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE;
|
||||
} else {
|
||||
ec = std::make_error_code(std::errc::invalid_argument);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ec = std::make_error_code(std::errc::invalid_argument);
|
||||
}
|
||||
|
||||
return ec;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
/* the most usefull config fields */
|
||||
|
||||
template <mcc::traits::mcc_time_duration_c DT>
|
||||
DT hardwarePollingPeriod() const
|
||||
{
|
||||
return std::chrono::duration_cast<DT>(
|
||||
getValue<std::chrono::milliseconds>("hardwarePollingPeriod").value_or(std::chrono::milliseconds{}));
|
||||
};
|
||||
|
||||
std::chrono::milliseconds hardwarePollingPeriod() const
|
||||
{
|
||||
return hardwarePollingPeriod<std::chrono::milliseconds>();
|
||||
};
|
||||
|
||||
template <mcc::mcc_angle_c T>
|
||||
T siteLatitude() const
|
||||
{
|
||||
return static_cast<double>(getValue<mcc::MccAngle>("siteLatitude").value_or(mcc::MccAngle{}));
|
||||
};
|
||||
|
||||
mcc::MccAngle siteLatitude() const
|
||||
{
|
||||
return siteLatitude<mcc::MccAngle>();
|
||||
};
|
||||
|
||||
template <mcc::mcc_angle_c T>
|
||||
T siteLongitude() const
|
||||
{
|
||||
return static_cast<double>(getValue<mcc::MccAngle>("siteLongitude").value_or(mcc::MccAngle{}));
|
||||
};
|
||||
|
||||
mcc::MccAngle siteLongitude() const
|
||||
{
|
||||
return siteLongitude<mcc::MccAngle>();
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
T siteElevation() const
|
||||
requires std::is_arithmetic_v<T>
|
||||
{
|
||||
return getValue<double>("siteElevation").value_or(0.0);
|
||||
}
|
||||
|
||||
double siteElevation() const
|
||||
{
|
||||
return getValue<double>("siteElevation").value_or(0.0);
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
T refractWavelength() const
|
||||
requires std::is_arithmetic_v<T>
|
||||
{
|
||||
return getValue<double>("refractWavelength").value_or(0.0);
|
||||
}
|
||||
|
||||
double refractWavelength() const
|
||||
{
|
||||
return getValue<double>("refractWavelength").value_or(0.0);
|
||||
};
|
||||
|
||||
template <mcc::traits::mcc_output_char_range R>
|
||||
R leapSecondFilename() const
|
||||
{
|
||||
R r;
|
||||
|
||||
std::string val = getValue<std::string>("leapSecondFilename").value_or("");
|
||||
std::ranges::copy(val, std::back_inserter(r));
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
std::string leapSecondFilename() const
|
||||
{
|
||||
return leapSecondFilename<std::string>();
|
||||
};
|
||||
|
||||
template <mcc::traits::mcc_output_char_range R>
|
||||
R bulletinAFilename() const
|
||||
{
|
||||
R r;
|
||||
std::string val = getValue<std::string>("bulletinAFilename").value_or("");
|
||||
std::ranges::copy(val, std::back_inserter(r));
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
std::string bulletinAFilename() const
|
||||
{
|
||||
return bulletinAFilename<std::string>();
|
||||
};
|
||||
|
||||
|
||||
template <mcc::mcc_angle_c T>
|
||||
T pzMinAltitude() const
|
||||
{
|
||||
return static_cast<double>(getValue<mcc::MccAngle>("pzMinAltitude").value_or(mcc::MccAngle{}));
|
||||
};
|
||||
|
||||
mcc::MccAngle pzMinAltitude() const
|
||||
{
|
||||
return pzMinAltitude<mcc::MccAngle>();
|
||||
};
|
||||
|
||||
template <mcc::mcc_angle_c T>
|
||||
T pzLimitSwitchHAMin() const
|
||||
{
|
||||
return static_cast<double>(getValue<mcc::MccAngle>("pzLimitSwitchHAMin").value_or(mcc::MccAngle{}));
|
||||
};
|
||||
|
||||
mcc::MccAngle pzLimitSwitchHAMin() const
|
||||
{
|
||||
return pzLimitSwitchHAMin<mcc::MccAngle>();
|
||||
};
|
||||
|
||||
template <mcc::mcc_angle_c T>
|
||||
T pzLimitSwitchHAMax() const
|
||||
{
|
||||
return static_cast<double>(getValue<mcc::MccAngle>("pzLimitSwitchHAMax").value_or(mcc::MccAngle{}));
|
||||
};
|
||||
|
||||
mcc::MccAngle pzLimitSwitchHAMax() const
|
||||
{
|
||||
return pzLimitSwitchHAMax<mcc::MccAngle>();
|
||||
};
|
||||
|
||||
|
||||
AsibFM700ServoController::hardware_config_t servoControllerConfig() const
|
||||
{
|
||||
AsibFM700ServoController::hardware_config_t hw_cfg;
|
||||
|
||||
hw_cfg.hwConfig = {};
|
||||
|
||||
hw_cfg.MountDevPath = getValue<std::string>("MountDevPath").value_or(std::string{});
|
||||
hw_cfg.EncoderDevPath = getValue<std::string>("EncoderDevPath").value_or(std::string{});
|
||||
hw_cfg.EncoderXDevPath = getValue<std::string>("EncoderXDevPath").value_or(std::string{});
|
||||
hw_cfg.EncoderYDevPath = getValue<std::string>("EncoderYDevPath").value_or(std::string{});
|
||||
|
||||
hw_cfg.devConfig.MountDevPath = hw_cfg.MountDevPath.data();
|
||||
hw_cfg.devConfig.EncoderDevPath = hw_cfg.EncoderDevPath.data();
|
||||
hw_cfg.devConfig.EncoderXDevPath = hw_cfg.EncoderXDevPath.data();
|
||||
hw_cfg.devConfig.EncoderYDevPath = hw_cfg.EncoderYDevPath.data();
|
||||
|
||||
hw_cfg.devConfig.RunModel = getValue<int>("RunModel").value_or(int{});
|
||||
hw_cfg.devConfig.MountDevSpeed = getValue<int>("MountDevSpeed").value_or(int{});
|
||||
hw_cfg.devConfig.EncoderDevSpeed = getValue<int>("EncoderDevSpeed").value_or(int{});
|
||||
hw_cfg.devConfig.SepEncoder = getValue<int>("SepEncoder").value_or(int{});
|
||||
|
||||
std::chrono::duration<double> secs; // seconds as floating-point
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("MountReqInterval").value_or(std::chrono::milliseconds{});
|
||||
hw_cfg.devConfig.MountReqInterval = secs.count();
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("EncoderReqInterval").value_or(std::chrono::milliseconds{});
|
||||
hw_cfg.devConfig.EncoderReqInterval = secs.count();
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("EncoderSpeedInterval").value_or(std::chrono::milliseconds{});
|
||||
hw_cfg.devConfig.EncoderSpeedInterval = secs.count();
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("PIDMaxDt").value_or(std::chrono::milliseconds{1000});
|
||||
hw_cfg.devConfig.PIDMaxDt = secs.count();
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("PIDRefreshDt").value_or(std::chrono::milliseconds{100});
|
||||
hw_cfg.devConfig.PIDRefreshDt = secs.count();
|
||||
|
||||
secs = getValue<std::chrono::milliseconds>("PIDCycleDt").value_or(std::chrono::milliseconds{5000});
|
||||
hw_cfg.devConfig.PIDCycleDt = secs.count();
|
||||
|
||||
|
||||
std::vector<double> pid = getValue<std::vector<double>>("XPIDC").value_or(std::vector<double>{});
|
||||
if (pid.size() > 2) {
|
||||
hw_cfg.devConfig.XPIDC.P = pid[0];
|
||||
hw_cfg.devConfig.XPIDC.I = pid[1];
|
||||
hw_cfg.devConfig.XPIDC.D = pid[2];
|
||||
}
|
||||
|
||||
pid = getValue<std::vector<double>>("XPIDV").value_or(std::vector<double>{});
|
||||
if (pid.size() > 2) {
|
||||
hw_cfg.devConfig.XPIDV.P = pid[0];
|
||||
hw_cfg.devConfig.XPIDV.I = pid[1];
|
||||
hw_cfg.devConfig.XPIDV.D = pid[2];
|
||||
}
|
||||
|
||||
pid = getValue<std::vector<double>>("YPIDC").value_or(std::vector<double>{});
|
||||
if (pid.size() > 2) {
|
||||
hw_cfg.devConfig.YPIDC.P = pid[0];
|
||||
hw_cfg.devConfig.YPIDC.I = pid[1];
|
||||
hw_cfg.devConfig.YPIDC.D = pid[2];
|
||||
}
|
||||
|
||||
pid = getValue<std::vector<double>>("YPIDV").value_or(std::vector<double>{});
|
||||
if (pid.size() > 2) {
|
||||
hw_cfg.devConfig.YPIDV.P = pid[0];
|
||||
hw_cfg.devConfig.YPIDV.I = pid[1];
|
||||
hw_cfg.devConfig.YPIDV.D = pid[2];
|
||||
}
|
||||
|
||||
double ang = getValue<mcc::MccAngle>("MaxPointingErr").value_or(mcc::MccAngle(8.0_degs));
|
||||
hw_cfg.devConfig.MaxPointingErr = ang;
|
||||
|
||||
ang = getValue<mcc::MccAngle>("MaxFinePointingErr").value_or(mcc::MccAngle(1.5_degs));
|
||||
hw_cfg.devConfig.MaxFinePointingErr = ang;
|
||||
|
||||
ang = getValue<mcc::MccAngle>("MaxGuidingErr").value_or(mcc::MccAngle(0.5_arcsecs));
|
||||
hw_cfg.devConfig.MaxGuidingErr = ang;
|
||||
|
||||
ang = getValue<mcc::MccAngle>("XEncZero").value_or(mcc::MccAngle(0.0_degs));
|
||||
hw_cfg.devConfig.XEncZero = ang;
|
||||
|
||||
|
||||
ang = getValue<mcc::MccAngle>("YEncZero").value_or(mcc::MccAngle(0.0_degs));
|
||||
hw_cfg.devConfig.YEncZero = ang;
|
||||
|
||||
|
||||
|
||||
return hw_cfg;
|
||||
}
|
||||
|
||||
|
||||
mcc::MccSimpleMovingModelParams movingModelParams() const
|
||||
{
|
||||
static constexpr double arcsecs2rad = std::numbers::pi / 180.0 / 3600.0; // arcseconds to radians
|
||||
|
||||
mcc::MccSimpleMovingModelParams pars;
|
||||
|
||||
auto get_value = [&pars, this]<typename VT>(std::string_view name, VT& val) {
|
||||
val = getValue<VT>(name).value_or(val);
|
||||
};
|
||||
|
||||
pars.telemetryTimeout =
|
||||
getValue<decltype(pars.telemetryTimeout)>("telemetryTimeout").value_or(pars.telemetryTimeout);
|
||||
|
||||
pars.minTimeToPZone = getValue<decltype(pars.minTimeToPZone)>("minTimeToPZone").value_or(pars.minTimeToPZone);
|
||||
|
||||
pars.updatingPZoneInterval = getValue<decltype(pars.updatingPZoneInterval)>("updatingPZoneInterval")
|
||||
.value_or(pars.updatingPZoneInterval);
|
||||
|
||||
pars.slewToleranceRadius =
|
||||
getValue<decltype(pars.slewToleranceRadius)>("slewToleranceRadius").value_or(pars.slewToleranceRadius) *
|
||||
arcsecs2rad;
|
||||
|
||||
get_value("slewingTelemetryInterval", pars.slewingTelemetryInterval);
|
||||
|
||||
pars.slewRateX = getValue<decltype(pars.slewRateX)>("hwMaxRateHA").value_or(pars.slewRateX);
|
||||
pars.slewRateY = getValue<decltype(pars.slewRateY)>("hwMaxRateDEC").value_or(pars.slewRateY);
|
||||
|
||||
pars.adjustCoordDiff =
|
||||
getValue<decltype(pars.adjustCoordDiff)>("adjustCoordDiff").value_or(pars.adjustCoordDiff) * arcsecs2rad;
|
||||
|
||||
pars.adjustCycleInterval =
|
||||
getValue<decltype(pars.adjustCycleInterval)>("adjustCycleInterval").value_or(pars.adjustCycleInterval);
|
||||
|
||||
pars.slewTimeout = getValue<decltype(pars.slewTimeout)>("slewTimeout").value_or(pars.slewTimeout);
|
||||
|
||||
pars.slewingPathFilename =
|
||||
getValue<decltype(pars.slewingPathFilename)>("slewingPathFilename").value_or(std::string());
|
||||
|
||||
get_value("trackingTelemetryInterval", pars.trackingTelemetryInterval);
|
||||
|
||||
pars.timeShiftToTargetPoint = getValue<decltype(pars.timeShiftToTargetPoint)>("timeShiftToTargetPoint")
|
||||
.value_or(pars.timeShiftToTargetPoint);
|
||||
|
||||
pars.trackingCycleInterval = getValue<decltype(pars.trackingCycleInterval)>("trackingCycleInterval")
|
||||
.value_or(pars.trackingCycleInterval);
|
||||
|
||||
pars.trackingMaxCoordDiff =
|
||||
getValue<decltype(pars.trackingMaxCoordDiff)>("trackingMaxCoordDiff").value_or(pars.trackingMaxCoordDiff) *
|
||||
arcsecs2rad;
|
||||
|
||||
pars.trackingPathFilename =
|
||||
getValue<decltype(pars.trackingPathFilename)>("trackingPathFilename").value_or(std::string());
|
||||
|
||||
return pars;
|
||||
}
|
||||
|
||||
Asibfm700PCM::pcm_data_t pcmData() const
|
||||
{
|
||||
Asibfm700PCM::pcm_data_t pcm_data;
|
||||
|
||||
std::vector<double> empty_vec;
|
||||
|
||||
pcm_data.type = getValue<decltype(pcm_data.type)>("pcmType").value_or(pcm_data.type);
|
||||
|
||||
pcm_data.siteLatitude = getValue<mcc::MccAngle>("siteLatitude").value_or(pcm_data.siteLatitude);
|
||||
|
||||
std::vector<double> vec = getValue<std::vector<double>>("pcmGeomCoeffs").value_or(empty_vec);
|
||||
if (vec.size() >= 9) { // must be 9 coefficients
|
||||
pcm_data.geomCoefficients = {.zeroPointX = vec[0],
|
||||
.zeroPointY = vec[1],
|
||||
.collimationErr = vec[2],
|
||||
.nonperpendErr = vec[3],
|
||||
.misalignErr1 = vec[4],
|
||||
.misalignErr2 = vec[5],
|
||||
.tubeFlexure = vec[6],
|
||||
.forkFlexure = vec[7],
|
||||
.DECaxisFlexure = vec[8]};
|
||||
}
|
||||
|
||||
std::vector<size_t> dd = getValue<decltype(dd)>("pcmBsplineDegree").value_or(dd);
|
||||
if (dd.size() >= 2) {
|
||||
pcm_data.bspline.bsplDegreeX = dd[0] > 0 ? dd[0] : 3;
|
||||
pcm_data.bspline.bsplDegreeY = dd[1] > 0 ? dd[1] : 3;
|
||||
}
|
||||
|
||||
vec = getValue<std::vector<double>>("pcmBsplineXknots").value_or(empty_vec);
|
||||
// pid must contains interior and border (single point for each border) knots so minimal length must be 2
|
||||
if (vec.size() >= 2) {
|
||||
// generate full knots array (with border knots)
|
||||
size_t Nknots = vec.size() + pcm_data.bspline.bsplDegreeX * 2 - 2;
|
||||
pcm_data.bspline.knotsX.resize(Nknots);
|
||||
|
||||
for (size_t i = 0; i <= pcm_data.bspline.bsplDegreeX; ++i) { // border knots
|
||||
pcm_data.bspline.knotsX[i] = vec[0];
|
||||
pcm_data.bspline.knotsX[Nknots - i - 1] = vec.back();
|
||||
}
|
||||
for (size_t i = 0; i < (vec.size() - 2); ++i) { // interior knots
|
||||
pcm_data.bspline.knotsX[i + pcm_data.bspline.bsplDegreeX] = vec[1 + i];
|
||||
}
|
||||
}
|
||||
|
||||
vec = getValue<std::vector<double>>("pcmBsplineYknots").value_or(empty_vec);
|
||||
// pid must contains interior and border (single point for each border) knots so minimal length must be 2
|
||||
if (vec.size() >= 2) {
|
||||
// generate full knots array (with border knots)
|
||||
size_t Nknots = vec.size() + pcm_data.bspline.bsplDegreeY * 2 - 2;
|
||||
pcm_data.bspline.knotsY.resize(Nknots);
|
||||
|
||||
for (size_t i = 0; i <= pcm_data.bspline.bsplDegreeY; ++i) { // border knots
|
||||
pcm_data.bspline.knotsY[i] = vec[0];
|
||||
pcm_data.bspline.knotsY[Nknots - i - 1] = vec.back();
|
||||
}
|
||||
for (size_t i = 0; i < (vec.size() - 2); ++i) { // interior knots
|
||||
pcm_data.bspline.knotsY[i + pcm_data.bspline.bsplDegreeY] = vec[1 + i];
|
||||
}
|
||||
}
|
||||
|
||||
// minimal allowed number of B-spline coefficients
|
||||
size_t Ncoeffs = pcm_data.type == mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY
|
||||
? 0
|
||||
: (pcm_data.bspline.knotsX.size() - pcm_data.bspline.bsplDegreeX - 1) *
|
||||
(pcm_data.bspline.knotsY.size() - pcm_data.bspline.bsplDegreeY - 1);
|
||||
|
||||
vec = getValue<std::vector<double>>("pcmBsplineXcoeffs").value_or(empty_vec);
|
||||
|
||||
if (vec.size() >= Ncoeffs) {
|
||||
pcm_data.bspline.coeffsX.resize(Ncoeffs);
|
||||
for (size_t i = 0; i < Ncoeffs; ++i) {
|
||||
pcm_data.bspline.coeffsX[i] = vec[i];
|
||||
}
|
||||
}
|
||||
|
||||
vec = getValue<std::vector<double>>("pcmBsplineYcoeffs").value_or(empty_vec);
|
||||
|
||||
if (vec.size() >= Ncoeffs) {
|
||||
pcm_data.bspline.coeffsY.resize(Ncoeffs);
|
||||
for (size_t i = 0; i < Ncoeffs; ++i) {
|
||||
pcm_data.bspline.coeffsY[i] = vec[i];
|
||||
}
|
||||
}
|
||||
|
||||
return pcm_data;
|
||||
}
|
||||
|
||||
|
||||
Asibfm700MountConfig() : base_t(Asibfm700MountConfigDefaults) {}
|
||||
|
||||
~Asibfm700MountConfig() = default;
|
||||
|
||||
std::error_code load(const std::filesystem::path& path)
|
||||
{
|
||||
std::string buffer;
|
||||
|
||||
std::error_code ec;
|
||||
auto sz = std::filesystem::file_size(path, ec);
|
||||
|
||||
if (!ec && sz) {
|
||||
std::ifstream fst(path);
|
||||
|
||||
try {
|
||||
buffer.resize(sz);
|
||||
|
||||
fst.read(buffer.data(), sz);
|
||||
|
||||
fst.close();
|
||||
|
||||
ec = base_t::fromCharRange(buffer, deserializer);
|
||||
if (!ec) {
|
||||
// remove possible spaces in filenames
|
||||
|
||||
std::string val = getValue<std::string>("leapSecondFilename").value_or("");
|
||||
auto fname = mcc::utils::trimSpaces(val);
|
||||
setValue("leapSecondFilename", fname);
|
||||
|
||||
|
||||
val = getValue<std::string>("bulletinAFilename").value_or("");
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("bulletinAFilename", fname);
|
||||
|
||||
val = getValue<std::string>("MountDevPath").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("MountDevPath", fname);
|
||||
|
||||
val = getValue<std::string>("EncoderDevPath").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("EncoderDevPath", fname);
|
||||
|
||||
val = getValue<std::string>("EncoderXDevPath").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("EncoderXDevPath", fname);
|
||||
|
||||
val = getValue<std::string>("EncoderYDevPath").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("EncoderYDevPath", fname);
|
||||
|
||||
val = getValue<std::string>("slewingPathFilename").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("slewingPathFilename", fname);
|
||||
|
||||
val = getValue<std::string>("trackingPathFilename").value_or(std::string{});
|
||||
fname = mcc::utils::trimSpaces(val);
|
||||
setValue("trackingPathFilename", fname);
|
||||
}
|
||||
} catch (std::ios_base::failure const& ex) {
|
||||
ec = ex.code();
|
||||
} catch (std::length_error const& ex) {
|
||||
ec = std::make_error_code(std::errc::no_buffer_space);
|
||||
} catch (std::bad_alloc const& ex) {
|
||||
ec = std::make_error_code(std::errc::not_enough_memory);
|
||||
} catch (...) {
|
||||
ec = std::make_error_code(std::errc::operation_canceled);
|
||||
}
|
||||
}
|
||||
|
||||
return ec;
|
||||
}
|
||||
|
||||
bool dumpDefaultsToFile(const std::filesystem::path& path)
|
||||
{
|
||||
std::ofstream fst(path);
|
||||
if (!fst.is_open()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
fst << "#\n";
|
||||
fst << "# ASTROSIB FM-700 MOUNT CONFIGURATION\n" << "#\n";
|
||||
fst << "# (created at " << std::format("{:%FT%T UTC}", std::chrono::system_clock::now()) << ")\n";
|
||||
fst << "#\n";
|
||||
|
||||
auto wrec = [&fst, this]<size_t I>() {
|
||||
fst << "\n";
|
||||
for (size_t i = 0; i < std::get<I>(_keyValue).comment.size(); ++i) {
|
||||
fst << "# " << std::get<I>(_keyValue).comment[i] << "\n";
|
||||
}
|
||||
fst << std::get<I>(_keyValue).key << " = ";
|
||||
auto v = std::get<I>(_keyValue).value;
|
||||
using v_t = std::remove_cvref_t<decltype(v)>;
|
||||
|
||||
if constexpr (std::is_arithmetic_v<v_t> || mcc::traits::mcc_char_range<v_t>) {
|
||||
fst << std::format("{}", v);
|
||||
} else if constexpr (mcc::traits::mcc_time_duration_c<v_t>) {
|
||||
fst << std::format("{}", v.count());
|
||||
} else if constexpr (mcc::mcc_angle_c<v_t>) {
|
||||
fst << std::format("{}", mcc::MccAngle(static_cast<double>(v)).degrees());
|
||||
} else if constexpr (std::same_as<v_t, mcc::MccDefaultPCMType>) {
|
||||
if (v == mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY) {
|
||||
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY>;
|
||||
} else if (v == mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
|
||||
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE>;
|
||||
} else if (v == mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE) {
|
||||
fst << mcc::MccDefaultPCMTypeString<mcc::MccDefaultPCMType::PCM_TYPE_BSPLINE>;
|
||||
}
|
||||
} else if constexpr (std::ranges::range<v_t> && std::formattable<std::ranges::range_value_t<v_t>, char>) {
|
||||
size_t sz = std::ranges::size(v);
|
||||
if (!sz) {
|
||||
return;
|
||||
}
|
||||
--sz;
|
||||
|
||||
auto it = v.begin();
|
||||
for (size_t j = 0; j < sz; ++j, ++it) {
|
||||
fst << std::format("{}", *it) << base_t::VALUE_ARRAY_DELIM;
|
||||
}
|
||||
fst << std::format("{}", *it);
|
||||
} else if constexpr (std::formattable<v_t, char>) {
|
||||
fst << std::format("{}", v);
|
||||
} else {
|
||||
static_assert(false, "INVALID TYPE!");
|
||||
}
|
||||
|
||||
fst << "\n";
|
||||
};
|
||||
|
||||
[&wrec]<size_t... Is>(std::index_sequence<Is...>) {
|
||||
(wrec.operator()<Is>(), ...);
|
||||
}(std::make_index_sequence<std::tuple_size_v<decltype(Asibfm700MountConfigDefaults)>>());
|
||||
|
||||
fst.close();
|
||||
|
||||
return true;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
|
||||
} // namespace asibfm700
|
||||
355
asibfm700/asibfm700_mount.cpp
Normal file
355
asibfm700/asibfm700_mount.cpp
Normal file
@@ -0,0 +1,355 @@
|
||||
#include "asibfm700_mount.h"
|
||||
|
||||
#include <mcc_pzone.h>
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
|
||||
/* CONSTRUCTOR AND DESTRUCTOR */
|
||||
|
||||
Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
|
||||
: mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
|
||||
.wavelength = config.refractWavelength(),
|
||||
.lat = config.siteLatitude(),
|
||||
.lon = config.siteLongitude(),
|
||||
.elev = config.siteElevation()}),
|
||||
Asibfm700PCM(config.pcmData()),
|
||||
gm_class_t(std::make_tuple(config.servoControllerConfig()),
|
||||
std::make_tuple(this),
|
||||
std::make_tuple(),
|
||||
std::make_tuple(this, Asibfm700Logger{logger}),
|
||||
std::make_tuple(this, Asibfm700Logger{logger}),
|
||||
std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
|
||||
// base_gm_class_t(Asibfm700StartState{},
|
||||
// std::make_tuple(config.servoControllerConfig()),
|
||||
// std::make_tuple(this),
|
||||
// std::make_tuple(),
|
||||
// std::make_tuple(this),
|
||||
// std::make_tuple(this),
|
||||
// std::make_tuple(logger, Asibfm700Logger::LOGGER_DEFAULT_FORMAT)),
|
||||
_mountConfig(config),
|
||||
_mountConfigMutex(new std::mutex)
|
||||
{
|
||||
gm_class_t::addMarkToPatternIdx("[ASIB-MOUNT]");
|
||||
|
||||
logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId());
|
||||
|
||||
initMount();
|
||||
}
|
||||
// Asibfm700Mount::Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger)
|
||||
// : mcc::ccte::erfa::MccCCTE_ERFA({.meteo{.temperature = 10.0, .humidity = 0.5, .pressure = 1010.0},
|
||||
// .wavelength = config.refractWavelength(),
|
||||
// .lat = config.siteLatitude(),
|
||||
// .lon = config.siteLongitude(),
|
||||
// .elev = config.siteElevation()}),
|
||||
// Asibfm700PCM(config.pcmData()),
|
||||
// base_gm_class_t(
|
||||
// gm_class_t{AsibFM700ServoController{config.servoControllerConfig()}, mcc::MccTelemetry{this},
|
||||
// Asibfm700PZoneContainer{}, mcc::MccSimpleSlewingModel{this}, mcc::MccSimpleTrackingModel{this},
|
||||
// Asibfm700Logger{std::move(logger), Asibfm700Logger::LOGGER_DEFAULT_FORMAT}},
|
||||
// Asibfm700StartState{}),
|
||||
// _mountConfig(config),
|
||||
// _mountConfigMutex(new std::mutex)
|
||||
// {
|
||||
// addMarkToPatternIdx("ASIB-MOUNT");
|
||||
|
||||
// logDebug("Create Asibfm700Mount class instance ({})", this->getThreadId());
|
||||
|
||||
// initMount();
|
||||
// }
|
||||
|
||||
|
||||
Asibfm700Mount::~Asibfm700Mount()
|
||||
{
|
||||
logDebug("Delete Asibfm700Mount class instance ({})", this->getThreadId());
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* PUBIC METHODS */
|
||||
|
||||
Asibfm700Mount::error_t Asibfm700Mount::initMount()
|
||||
{
|
||||
std::lock_guard lock{*_mountConfigMutex};
|
||||
|
||||
logInfo("Stop telemetry data updating");
|
||||
stopInternalTelemetryDataUpdating();
|
||||
|
||||
logInfo("Init AstroSib FM-700 mount with configuration:");
|
||||
logInfo(" site latitude: {}", _mountConfig.siteLatitude().sexagesimal());
|
||||
logInfo(" site longitude: {}", _mountConfig.siteLongitude().sexagesimal());
|
||||
logInfo(" site elevation: {} meters", _mountConfig.siteElevation());
|
||||
logInfo(" refraction wavelength: {} mkm", _mountConfig.refractWavelength());
|
||||
logInfo(" leap seconds filename: {}", _mountConfig.leapSecondFilename());
|
||||
logInfo(" IERS Bulletin A filename: {}", _mountConfig.bulletinAFilename());
|
||||
|
||||
logInfo("");
|
||||
logDebug("Delete previously defined prohobited zones");
|
||||
clearPZones();
|
||||
logInfo("Add prohibited zones ...");
|
||||
|
||||
logInfo(" Add MccAltLimitPZ zone: min alt = {}, lat = {} (pzone type: '{}')",
|
||||
_mountConfig.pzMinAltitude().degrees(), _mountConfig.siteLatitude().degrees(),
|
||||
"Minimal altitude prohibited zone");
|
||||
addPZone(mcc::MccAltLimitPZ<mcc::MccAltLimitKind::MIN_ALT_LIMIT>{_mountConfig.pzMinAltitude(),
|
||||
_mountConfig.siteLatitude(), this});
|
||||
|
||||
logInfo(" Add MccAxisLimitSwitchPZ zone: min value = {}, max value = {} (pzone type: '{}')",
|
||||
_mountConfig.pzLimitSwitchHAMin().degrees(), _mountConfig.pzLimitSwitchHAMax().degrees(),
|
||||
"HA-axis limit switch");
|
||||
size_t pz_num = addPZone(mcc::MccAxisLimitSwitchPZ<mcc::MccCoordKind::COORDS_KIND_HA>{
|
||||
_mountConfig.pzLimitSwitchHAMin(), _mountConfig.pzLimitSwitchHAMax(), this});
|
||||
|
||||
logInfo("{} prohibited zones were added successfully", pz_num);
|
||||
|
||||
auto mpars = _mountConfig.movingModelParams();
|
||||
|
||||
using secs_t = std::chrono::duration<double>;
|
||||
auto to_msecs = [](double secs) {
|
||||
auto s = secs_t{secs};
|
||||
return std::chrono::duration_cast<std::chrono::milliseconds>(s);
|
||||
};
|
||||
|
||||
auto hw_cfg = _mountConfig.servoControllerConfig();
|
||||
logInfo("");
|
||||
logInfo("Hardware initialization ...");
|
||||
logInfo(" set hardware configuration:");
|
||||
logInfo(" RunModel: {}", hw_cfg.devConfig.RunModel == 1 ? "MODEL-MODE" : "REAL-MODE");
|
||||
logInfo(" mount dev path: {}", hw_cfg.MountDevPath);
|
||||
logInfo(" encoder dev path: {}", hw_cfg.EncoderDevPath);
|
||||
logInfo(" encoder X-dev path: {}", hw_cfg.EncoderXDevPath);
|
||||
logInfo(" encoder Y-dev path: {}", hw_cfg.EncoderYDevPath);
|
||||
|
||||
logInfo(" EncoderDevSpeed: {}", hw_cfg.devConfig.EncoderDevSpeed);
|
||||
logInfo(" SepEncoder: {}", hw_cfg.devConfig.SepEncoder);
|
||||
logInfo(" MountReqInterval: {}", to_msecs(hw_cfg.devConfig.MountReqInterval));
|
||||
logInfo(" EncoderReqInterval: {}", to_msecs(hw_cfg.devConfig.EncoderReqInterval));
|
||||
logInfo(" EncoderSpeedInterval: {}", to_msecs(hw_cfg.devConfig.EncoderSpeedInterval));
|
||||
logInfo(" PIDMaxDt: {}", to_msecs(hw_cfg.devConfig.PIDMaxDt));
|
||||
logInfo(" PIDRefreshDt: {}", to_msecs(hw_cfg.devConfig.PIDRefreshDt));
|
||||
logInfo(" PIDCycleDt: {}", to_msecs(hw_cfg.devConfig.PIDCycleDt));
|
||||
|
||||
logInfo(" XPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDC.P, hw_cfg.devConfig.XPIDC.I,
|
||||
hw_cfg.devConfig.XPIDC.D);
|
||||
logInfo(" XPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.XPIDV.P, hw_cfg.devConfig.XPIDV.I,
|
||||
hw_cfg.devConfig.XPIDV.D);
|
||||
logInfo(" YPIDC: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDC.P, hw_cfg.devConfig.YPIDC.I,
|
||||
hw_cfg.devConfig.YPIDC.D);
|
||||
logInfo(" YPIDV: [P: {}, I: {}, D: {}]", hw_cfg.devConfig.YPIDV.P, hw_cfg.devConfig.YPIDV.I,
|
||||
hw_cfg.devConfig.YPIDV.D);
|
||||
logInfo(" XEncZero: {}", hw_cfg.devConfig.XEncZero);
|
||||
logInfo(" YEncZero: {}", hw_cfg.devConfig.YEncZero);
|
||||
|
||||
// actually, only set this->_hardwareConfig.devConfig part and paths!!!
|
||||
this->_hardwareConfig = hw_cfg;
|
||||
|
||||
logInfo("");
|
||||
logInfo(" EEPROM data:");
|
||||
|
||||
if (hw_cfg.devConfig.RunModel != 1) { // load EEPROM only in REAL HARDWARE mode
|
||||
// load EEPROM part
|
||||
auto cfg_err = this->hardwareUpdateConfig();
|
||||
if (cfg_err) {
|
||||
errorLogging("Cannot load EEPROM data:", cfg_err);
|
||||
return cfg_err;
|
||||
}
|
||||
|
||||
mcc::MccAngle ang{_hardwareConfig.hwConfig.Yconf.accel}; // Sidereal defines HA-axis as Y-axis
|
||||
|
||||
logInfo(" HA-axis accel: {} degs/s^2", ang.degrees());
|
||||
ang = _hardwareConfig.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis
|
||||
logInfo(" DEC-axis accel: {} degs/s^2", ang.degrees());
|
||||
logInfo(" HA-axis backlash: {}", (double)_hardwareConfig.hwConfig.Yconf.backlash);
|
||||
logInfo(" DEC-axis backlash: {}", (double)_hardwareConfig.hwConfig.Xconf.backlash);
|
||||
|
||||
logInfo(" HA-axis encoder ticks per revolution: {}",
|
||||
_hardwareConfig.hwConfig.Ysetpr); // Sidereal defines HA-axis as Y-axis
|
||||
logInfo(" DEC-axis encoder ticks per revolution: {}",
|
||||
_hardwareConfig.hwConfig.Xsetpr); // Sidereal defines DEC-axis as X-axis
|
||||
logInfo(" HA-motor encoder ticks per revolution: {}",
|
||||
_hardwareConfig.hwConfig.Ymetpr); // Sidereal defines HA-axis as Y-axis
|
||||
logInfo(" DEC-motor encoder ticks per revolution: {}",
|
||||
_hardwareConfig.hwConfig.Xmetpr); // Sidereal defines DEC-axis as X-axis
|
||||
|
||||
ang = _hardwareConfig.hwConfig.Yslewrate; // Sidereal defines HA-axis as Y-axis
|
||||
logInfo(" HA-axis slew rate: {} degs/s", ang.degrees());
|
||||
ang = _hardwareConfig.hwConfig.Xslewrate; // Sidereal defines DEC-axis as X-axis
|
||||
logInfo(" DEC-axis slew rate: {} degs/s", ang.degrees());
|
||||
} else {
|
||||
logWarn(" MODEL-MODE, no EEPROM data!");
|
||||
}
|
||||
|
||||
logInfo("");
|
||||
logInfo("Setup slewing and tracking parameters ...");
|
||||
mpars.slewRateX = _mountConfig.getValue<mcc::MccAngle>("hwMaxRateHA").value_or(0.0);
|
||||
mpars.slewRateY = _mountConfig.getValue<mcc::MccAngle>("hwMaxRateDEC").value_or(0.0);
|
||||
if (hw_cfg.devConfig.RunModel != 1) {
|
||||
mpars.brakingAccelX = _hardwareConfig.hwConfig.Yconf.accel; // Sidereal defines HA-axis as Y-axis
|
||||
mpars.brakingAccelY = _hardwareConfig.hwConfig.Xconf.accel; // Sidereal defines DEC-axis as X-axis
|
||||
} else {
|
||||
mpars.brakingAccelX = 0.165806; // Sidereal defines HA-axis as Y-axis
|
||||
mpars.brakingAccelY = 0.219911; // Sidereal defines DEC-axis as X-axis
|
||||
}
|
||||
|
||||
auto max_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDMaxDt").value_or({});
|
||||
auto min_dt_intvl = _mountConfig.getValue<std::chrono::milliseconds>("PIDRefreshDt").value_or({});
|
||||
|
||||
// check for polling interval consistency
|
||||
auto intvl = mpars.slewingTelemetryInterval;
|
||||
if (intvl > max_dt_intvl) {
|
||||
mpars.slewingTelemetryInterval = max_dt_intvl;
|
||||
logWarn(
|
||||
" slewingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
|
||||
"allowed one: {} ms",
|
||||
intvl.count(), max_dt_intvl.count());
|
||||
}
|
||||
if (intvl < min_dt_intvl) {
|
||||
mpars.slewingTelemetryInterval = min_dt_intvl;
|
||||
logWarn(
|
||||
" slewingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal allowed "
|
||||
"one: {} ms",
|
||||
intvl.count(), min_dt_intvl.count());
|
||||
}
|
||||
|
||||
intvl = mpars.trackingTelemetryInterval;
|
||||
if (intvl > max_dt_intvl) {
|
||||
mpars.trackingTelemetryInterval = max_dt_intvl;
|
||||
logWarn(
|
||||
" trackingTelemetryInterval user value ({} ms) is greater than allowed! Set it to maximal "
|
||||
"allowed one: {} ms",
|
||||
intvl.count(), max_dt_intvl.count());
|
||||
}
|
||||
if (intvl < min_dt_intvl) {
|
||||
mpars.trackingTelemetryInterval = min_dt_intvl;
|
||||
logWarn(
|
||||
" trackingTelemetryInterval user value ({} ms) is lesser than allowed! Set it to minimal "
|
||||
"allowed one: {} ms",
|
||||
intvl.count(), min_dt_intvl.count());
|
||||
}
|
||||
|
||||
|
||||
auto st_err = setSlewingParams(mpars);
|
||||
if (st_err) {
|
||||
errorLogging(" An error occured while setting slewing parameters: ", st_err);
|
||||
} else {
|
||||
logInfo(" Max HA-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateX).degrees());
|
||||
logInfo(" Max DEC-axis speed: {} degs/s", mcc::MccAngle(mpars.slewRateY).degrees());
|
||||
logInfo(" HA-axis stop acceleration braking: {} degs/s^2", mcc::MccAngle(mpars.brakingAccelX).degrees());
|
||||
logInfo(" DEC-axis stop acceleration braking: {} degs/s^2", mcc::MccAngle(mpars.brakingAccelY).degrees());
|
||||
|
||||
logInfo(" Slewing telemetry polling interval: {} millisecs", mpars.slewingTelemetryInterval.count());
|
||||
}
|
||||
st_err = setTrackingParams(_mountConfig.movingModelParams());
|
||||
if (st_err) {
|
||||
errorLogging(" An error occured while setting tracking parameters: ", st_err);
|
||||
} else {
|
||||
logInfo(" Tracking telemetry polling interval: {} millisecs", mpars.trackingTelemetryInterval.count());
|
||||
}
|
||||
logInfo("Slewing and tracking parameters have been set successfully");
|
||||
|
||||
|
||||
|
||||
// call base class initMount method
|
||||
auto hw_err = gm_class_t::initMount();
|
||||
// auto hw_err = base_gm_class_t::initMount();
|
||||
if (hw_err) {
|
||||
errorLogging("", hw_err);
|
||||
return hw_err;
|
||||
} else {
|
||||
logInfo("Hardware initialization was performed sucessfully!");
|
||||
}
|
||||
|
||||
logInfo("ERFA engine initialization ...");
|
||||
|
||||
|
||||
// set ERFA state
|
||||
Asibfm700CCTE::engine_state_t ccte_state{
|
||||
.meteo = Asibfm700CCTE::_currentState.meteo, // just use of previous values
|
||||
.wavelength = _mountConfig.refractWavelength(),
|
||||
.lat = _mountConfig.siteLatitude(),
|
||||
.lon = _mountConfig.siteLongitude(),
|
||||
.elev = _mountConfig.siteElevation()};
|
||||
|
||||
|
||||
if (_mountConfig.leapSecondFilename().size()) { // load leap seconds file
|
||||
logInfo("Loading leap second file: '{}' ...", _mountConfig.leapSecondFilename());
|
||||
bool ok = ccte_state._leapSeconds.load(_mountConfig.leapSecondFilename());
|
||||
if (ok) {
|
||||
logInfo("Leap second file was loaded successfully (expire date: {})", ccte_state._leapSeconds.expireDate());
|
||||
} else {
|
||||
logError("Leap second file loading failed! Using hardcoded defauls (expire date: {})",
|
||||
ccte_state._leapSeconds.expireDate());
|
||||
}
|
||||
} else {
|
||||
logInfo("Using hardcoded leap seconds defauls (expire date: {})", ccte_state._leapSeconds.expireDate());
|
||||
}
|
||||
|
||||
if (_mountConfig.bulletinAFilename().size()) { // load IERS Bulletin A file
|
||||
logInfo("Loading IERS Bulletin A file: '{}' ...", _mountConfig.bulletinAFilename());
|
||||
bool ok = ccte_state._bulletinA.load(_mountConfig.bulletinAFilename());
|
||||
if (ok) {
|
||||
logInfo("IERS Bulletin A file was loaded successfully (date range: {} - {})",
|
||||
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
||||
} else {
|
||||
logError("IERS Bulletin A file loading failed! Using hardcoded defauls (date range: {} - {})",
|
||||
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
||||
}
|
||||
} else {
|
||||
logInfo("Using hardcoded IERS Bulletin A defauls (date range: {} - {})",
|
||||
ccte_state._bulletinA.dateRange().begin, ccte_state._bulletinA.dateRange().end);
|
||||
}
|
||||
|
||||
setStateERFA(std::move(ccte_state));
|
||||
|
||||
// setTelemetryDataUpdateInterval(_mountConfig.hardwarePollingPeriod());
|
||||
setTelemetryUpdateTimeout(_mountConfig.movingModelParams().telemetryTimeout);
|
||||
startInternalTelemetryDataUpdating();
|
||||
|
||||
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
|
||||
bool ok = isInternalTelemetryDataUpdating();
|
||||
if (ok) {
|
||||
logInfo("Start updating telemetry data ...");
|
||||
mcc::MccTelemetryData tdata;
|
||||
auto err = waitForTelemetryData(&tdata, _mountConfig.movingModelParams().telemetryTimeout);
|
||||
if (err) {
|
||||
logError("Cannot update telemetry data (err = {} [{}, {}])!", err.message(), err.value(),
|
||||
err.category().name());
|
||||
}
|
||||
} else {
|
||||
auto err = lastUpdateError();
|
||||
logError("Cannot update telemetry data (err = {} [{}, {}])!", err.message(), err.value(),
|
||||
err.category().name());
|
||||
}
|
||||
|
||||
return mcc::MccGenericMountErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
|
||||
Asibfm700Mount::error_t Asibfm700Mount::updateMountConfig(const Asibfm700MountConfig& cfg)
|
||||
{
|
||||
std::lock_guard lock{*_mountConfigMutex};
|
||||
|
||||
_mountConfig = cfg;
|
||||
|
||||
auto hw_cfg = _mountConfig.servoControllerConfig();
|
||||
hardwareUpdateConfig(hw_cfg.devConfig);
|
||||
hardwareUpdateConfig(hw_cfg.hwConfig);
|
||||
|
||||
return AsibFM700ServoControllerErrorCode::ERROR_OK;
|
||||
}
|
||||
|
||||
|
||||
/* PROTECTED METHODS */
|
||||
|
||||
void Asibfm700Mount::errorLogging(const std::string& msg, const std::error_code& err)
|
||||
{
|
||||
if (msg.empty()) {
|
||||
logError("{}::{} ({})", err.category().name(), err.value(), err.message());
|
||||
} else {
|
||||
logError("{}: {}::{} ({})", msg, err.category().name(), err.value(), err.message());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace asibfm700
|
||||
189
asibfm700/asibfm700_mount.h
Normal file
189
asibfm700/asibfm700_mount.h
Normal file
@@ -0,0 +1,189 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <mcc_generic_mount.h>
|
||||
#include <mcc_pzone_container.h>
|
||||
#include <mcc_slewing_model.h>
|
||||
#include <mcc_spdlog.h>
|
||||
#include <mcc_telemetry.h>
|
||||
#include <mcc_tracking_model.h>
|
||||
|
||||
#include "asibfm700_common.h"
|
||||
#include "asibfm700_configfile.h"
|
||||
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
|
||||
class Asibfm700Mount : public Asibfm700CCTE,
|
||||
public Asibfm700PCM,
|
||||
public mcc::MccGenericMount<AsibFM700ServoController,
|
||||
mcc::MccTelemetry,
|
||||
Asibfm700PZoneContainer,
|
||||
Asibfm700SlewingModel,
|
||||
Asibfm700TrackingModel,
|
||||
Asibfm700Logger>
|
||||
{
|
||||
typedef mcc::MccGenericMount<AsibFM700ServoController,
|
||||
mcc::MccTelemetry,
|
||||
Asibfm700PZoneContainer,
|
||||
Asibfm700SlewingModel,
|
||||
Asibfm700TrackingModel,
|
||||
Asibfm700Logger>
|
||||
gm_class_t;
|
||||
|
||||
public:
|
||||
using gm_class_t::error_t;
|
||||
|
||||
using Asibfm700CCTE::setStateERFA;
|
||||
using Asibfm700CCTE::updateBulletinA;
|
||||
using Asibfm700CCTE::updateLeapSeconds;
|
||||
using Asibfm700CCTE::updateMeteoERFA;
|
||||
|
||||
using gm_class_t::logCritical;
|
||||
using gm_class_t::logDebug;
|
||||
using gm_class_t::logError;
|
||||
using gm_class_t::logInfo;
|
||||
using gm_class_t::logWarn;
|
||||
// using Asibfm700Logger::logCritical;
|
||||
// using Asibfm700Logger::logDebug;
|
||||
// using Asibfm700Logger::logError;
|
||||
// using Asibfm700Logger::logInfo;
|
||||
// using Asibfm700Logger::logWarn;
|
||||
|
||||
// using Asibfm700PZoneContainer::addPZone;
|
||||
|
||||
Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger);
|
||||
|
||||
~Asibfm700Mount();
|
||||
|
||||
Asibfm700Mount(Asibfm700Mount&&) = default;
|
||||
Asibfm700Mount& operator=(Asibfm700Mount&&) = default;
|
||||
|
||||
Asibfm700Mount(const Asibfm700Mount&) = delete;
|
||||
Asibfm700Mount& operator=(const Asibfm700Mount&) = delete;
|
||||
|
||||
error_t initMount();
|
||||
|
||||
error_t updateMountConfig(Asibfm700MountConfig const&);
|
||||
Asibfm700MountConfig currentMountConfig();
|
||||
|
||||
protected:
|
||||
Asibfm700MountConfig _mountConfig;
|
||||
std::unique_ptr<std::mutex> _mountConfigMutex;
|
||||
|
||||
void errorLogging(const std::string&, const std::error_code&);
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
class Asibfm700Mount : public Asibfm700CCTE,
|
||||
public Asibfm700PCM,
|
||||
public mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
|
||||
mcc::MccTelemetry,
|
||||
Asibfm700PZoneContainer,
|
||||
mcc::MccSimpleSlewingModel,
|
||||
mcc::MccSimpleTrackingModel,
|
||||
Asibfm700Logger>>
|
||||
{
|
||||
typedef mcc::MccGenericMount<AsibFM700ServoController,
|
||||
mcc::MccTelemetry,
|
||||
Asibfm700PZoneContainer,
|
||||
mcc::MccSimpleSlewingModel,
|
||||
mcc::MccSimpleTrackingModel,
|
||||
Asibfm700Logger>
|
||||
gm_class_t;
|
||||
|
||||
typedef mcc::MccGenericFsmMount<mcc::MccGenericMount<AsibFM700ServoController,
|
||||
mcc::MccTelemetry,
|
||||
Asibfm700PZoneContainer,
|
||||
mcc::MccSimpleSlewingModel,
|
||||
mcc::MccSimpleTrackingModel,
|
||||
Asibfm700Logger>>
|
||||
base_gm_class_t;
|
||||
|
||||
protected:
|
||||
struct Asibfm700ErrorState : base_gm_class_t::MccGenericFsmMountBaseState {
|
||||
static constexpr std::string_view ID{"ASIBFM700-MOUNT-ERROR-STATE"};
|
||||
|
||||
// void exit(MccGenericFsmMountErrorEvent& event)
|
||||
// {
|
||||
// event.mount()->logWarn("The mount already in error state!");
|
||||
// }
|
||||
|
||||
void enter(MccGenericFsmMountErrorEvent& event)
|
||||
{
|
||||
enterLog(event);
|
||||
|
||||
// event.mount()->logWarn("The mount already in error state!");
|
||||
auto err = event.eventData();
|
||||
event.mount()->logError("An error occured: {} [{} {}]", err.message(), err.value(), err.category().name());
|
||||
}
|
||||
|
||||
void exit(mcc::fsm::traits::fsm_event_c auto& event)
|
||||
{
|
||||
exitLog(event);
|
||||
}
|
||||
|
||||
void enter(mcc::fsm::traits::fsm_event_c auto& event)
|
||||
{
|
||||
enterLog(event);
|
||||
|
||||
// ...
|
||||
}
|
||||
|
||||
using transition_t = mcc::fsm::fsm_transition_table_t<
|
||||
std::pair<MccGenericFsmMountErrorEvent, Asibfm700ErrorState>,
|
||||
std::pair<MccGenericFsmMountInitEvent, MccGenericFsmMountInitState<Asibfm700ErrorState>>,
|
||||
std::pair<MccGenericFsmMountIdleEvent, MccGenericFsmMountIdleState<Asibfm700ErrorState>>>;
|
||||
};
|
||||
|
||||
|
||||
typedef base_gm_class_t::MccGenericFsmMountStartState<Asibfm700ErrorState> Asibfm700StartState;
|
||||
|
||||
public:
|
||||
using base_gm_class_t::error_t;
|
||||
|
||||
using Asibfm700CCTE::setStateERFA;
|
||||
using Asibfm700CCTE::updateBulletinA;
|
||||
using Asibfm700CCTE::updateLeapSeconds;
|
||||
using Asibfm700CCTE::updateMeteoERFA;
|
||||
|
||||
using Asibfm700Logger::logCritical;
|
||||
using Asibfm700Logger::logDebug;
|
||||
using Asibfm700Logger::logError;
|
||||
using Asibfm700Logger::logInfo;
|
||||
using Asibfm700Logger::logWarn;
|
||||
|
||||
// using Asibfm700PZoneContainer::addPZone;
|
||||
|
||||
Asibfm700Mount(Asibfm700MountConfig const& config, std::shared_ptr<spdlog::logger> logger);
|
||||
|
||||
~Asibfm700Mount();
|
||||
|
||||
Asibfm700Mount(Asibfm700Mount&&) = default;
|
||||
Asibfm700Mount& operator=(Asibfm700Mount&&) = default;
|
||||
|
||||
Asibfm700Mount(const Asibfm700Mount&) = delete;
|
||||
Asibfm700Mount& operator=(const Asibfm700Mount&) = delete;
|
||||
|
||||
error_t initMount();
|
||||
|
||||
error_t updateMountConfig(Asibfm700MountConfig const&);
|
||||
Asibfm700MountConfig currentMountConfig();
|
||||
|
||||
protected:
|
||||
Asibfm700MountConfig _mountConfig;
|
||||
std::unique_ptr<std::mutex> _mountConfigMutex;
|
||||
|
||||
void errorLogging(const std::string&, const std::error_code&);
|
||||
};
|
||||
*/
|
||||
|
||||
static_assert(mcc::mcc_position_controls_c<Asibfm700Mount>, "");
|
||||
static_assert(mcc::mcc_all_controls_c<Asibfm700Mount>, "");
|
||||
|
||||
static_assert(mcc::mcc_generic_mount_c<Asibfm700Mount>, "");
|
||||
|
||||
} // namespace asibfm700
|
||||
59
asibfm700/asibfm700_netserver.cpp
Normal file
59
asibfm700/asibfm700_netserver.cpp
Normal file
@@ -0,0 +1,59 @@
|
||||
#include "asibfm700_netserver.h"
|
||||
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
Asibfm700MountNetServer::Asibfm700MountNetServer(asio::io_context& ctx,
|
||||
Asibfm700Mount& mount,
|
||||
std::shared_ptr<spdlog::logger> logger)
|
||||
: base_t(ctx, mount, std::move(logger), Asibfm700Logger::LOGGER_DEFAULT_FORMAT)
|
||||
{
|
||||
addMarkToPatternIdx("[ASIB-NETSERVER]");
|
||||
|
||||
// to avoid possible compiler optimization (one needs to catch 'mount' strictly by reference)
|
||||
auto* mount_ptr = &mount;
|
||||
|
||||
base_t::_handleMessageFunc = [mount_ptr, this](std::string_view command) {
|
||||
// using mount_error_t = typename Asibfm700Mount::error_t;
|
||||
std::error_code err{};
|
||||
|
||||
Asibfm700NetMessage input_msg;
|
||||
using output_msg_t = Asibfm700NetMessage<handle_message_func_result_t>;
|
||||
output_msg_t output_msg;
|
||||
|
||||
auto nn = std::this_thread::get_id();
|
||||
|
||||
auto ec = parseMessage(command, input_msg);
|
||||
|
||||
if (ec) {
|
||||
output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, ec);
|
||||
} else {
|
||||
if (input_msg.withKey(ASIBFM700_COMMPROTO_KEYWORD_METEO_STR)) {
|
||||
// what is operation type (set or get)?
|
||||
if (input_msg.paramSize()) { // set operation
|
||||
auto vp = input_msg.paramValue<Asibfm700CCTE::meteo_t>(0);
|
||||
if (vp) {
|
||||
mount_ptr->updateMeteoERFA(vp.value());
|
||||
|
||||
output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR, input_msg.byteRepr());
|
||||
} else {
|
||||
output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ERROR_STR, vp.error());
|
||||
}
|
||||
} else { // get operation
|
||||
output_msg.construct(mcc::network::MCC_COMMPROTO_KEYWORD_SERVER_ACK_STR,
|
||||
ASIBFM700_COMMPROTO_KEYWORD_METEO_STR, mount_ptr->getStateERFA().meteo);
|
||||
}
|
||||
} else {
|
||||
// basic network message processing
|
||||
output_msg = base_t::handleMessage<output_msg_t>(input_msg, mount_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
return output_msg.template byteRepr<typename base_t::handle_message_func_result_t>();
|
||||
};
|
||||
}
|
||||
|
||||
Asibfm700MountNetServer::~Asibfm700MountNetServer() {}
|
||||
|
||||
} // namespace asibfm700
|
||||
146
asibfm700/asibfm700_netserver.h
Normal file
146
asibfm700/asibfm700_netserver.h
Normal file
@@ -0,0 +1,146 @@
|
||||
#pragma once
|
||||
|
||||
#include <mcc_netserver.h>
|
||||
#include <mcc_netserver_proto.h>
|
||||
|
||||
#include "asibfm700_common.h"
|
||||
#include "asibfm700_mount.h"
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
namespace details
|
||||
{
|
||||
|
||||
template <typename VT, size_t N1, size_t N2>
|
||||
static constexpr auto merge_arrays(const std::array<VT, N1>& arr1, const std::array<VT, N2>& arr2)
|
||||
{
|
||||
constexpr auto N = N1 + N2;
|
||||
std::array<VT, N> res;
|
||||
|
||||
for (size_t i = 0; i < N1; ++i) {
|
||||
res[i] = arr1[i];
|
||||
}
|
||||
|
||||
for (size_t i = N1; i < N; ++i) {
|
||||
res[i] = arr2[i - N1];
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
} // namespace details
|
||||
|
||||
constexpr static std::string_view ASIBFM700_COMMPROTO_KEYWORD_METEO_STR{"METEO"};
|
||||
|
||||
struct Asibfm700NetMessageValidKeywords {
|
||||
static constexpr std::array NETMSG_VALID_KEYWORDS =
|
||||
details::merge_arrays(mcc::network::MccNetMessageValidKeywords::NETMSG_VALID_KEYWORDS,
|
||||
std::array{ASIBFM700_COMMPROTO_KEYWORD_METEO_STR});
|
||||
|
||||
// hashes of valid keywords
|
||||
static constexpr std::array NETMSG_VALID_KEYWORD_HASHES = []<size_t... Is>(std::index_sequence<Is...>) {
|
||||
return std::array{mcc::utils::FNV1aHash(NETMSG_VALID_KEYWORDS[Is])...};
|
||||
}(std::make_index_sequence<NETMSG_VALID_KEYWORDS.size()>());
|
||||
|
||||
constexpr static const size_t* isKeywordValid(std::string_view key)
|
||||
{
|
||||
const auto hash = mcc::utils::FNV1aHash(key);
|
||||
|
||||
for (auto const& h : NETMSG_VALID_KEYWORD_HASHES) {
|
||||
if (h == hash) {
|
||||
return &h;
|
||||
}
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
template <mcc::traits::mcc_char_range BYTEREPR_T = std::string_view>
|
||||
class Asibfm700NetMessage : public mcc::network::MccNetMessage<BYTEREPR_T, Asibfm700NetMessageValidKeywords>
|
||||
{
|
||||
protected:
|
||||
using base_t = mcc::network::MccNetMessage<BYTEREPR_T, Asibfm700NetMessageValidKeywords>;
|
||||
|
||||
class serializer_t : public base_t::DefaultSerializer
|
||||
{
|
||||
public:
|
||||
template <typename T, mcc::traits::mcc_output_char_range OR>
|
||||
void operator()(const T& value, OR& bytes)
|
||||
{
|
||||
if constexpr (std::same_as<T, Asibfm700CCTE::meteo_t>) {
|
||||
// serialize just like a vector
|
||||
std::vector<double> meteo{value.temperature, value.humidity, value.pressure};
|
||||
base_t::DefaultSerializer::operator()(meteo, bytes);
|
||||
} else {
|
||||
base_t::DefaultSerializer::operator()(value, bytes);
|
||||
}
|
||||
}
|
||||
} _serializer;
|
||||
|
||||
|
||||
class deserializer_t : public base_t::DefaultDeserializer
|
||||
{
|
||||
public:
|
||||
template <mcc::traits::mcc_input_char_range IR, typename VT>
|
||||
std::error_code operator()(IR&& bytes, VT& value) const
|
||||
{
|
||||
if constexpr (std::same_as<VT, Asibfm700CCTE::meteo_t>) {
|
||||
// deserialize just like a vector
|
||||
|
||||
std::vector<double> v;
|
||||
auto ec = base_t::DefaultDeserializer::operator()(std::forward<IR>(bytes), v);
|
||||
if (ec) {
|
||||
return ec;
|
||||
}
|
||||
|
||||
if (v.size() < 3) {
|
||||
return std::make_error_code(std::errc::invalid_argument);
|
||||
}
|
||||
|
||||
value.temperature = v[0];
|
||||
value.humidity = v[1];
|
||||
value.pressure = v[2];
|
||||
|
||||
return {};
|
||||
} else {
|
||||
return base_t::DefaultDeserializer::operator()(std::forward<IR>(bytes), value);
|
||||
}
|
||||
}
|
||||
} _deserializer;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
using base_t::base_t;
|
||||
|
||||
template <typename T>
|
||||
std::expected<T, std::error_code> paramValue(size_t idx) const
|
||||
{
|
||||
return base_t::template paramValue<T>(idx, _deserializer);
|
||||
}
|
||||
|
||||
|
||||
|
||||
template <mcc::traits::mcc_input_char_range KT, typename... PTs>
|
||||
std::error_code construct(KT&& key, PTs&&... params)
|
||||
requires mcc::traits::mcc_output_char_range<BYTEREPR_T>
|
||||
{
|
||||
return base_t::construct(_serializer, std::forward<KT>(key), std::forward<PTs>(params)...);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
class Asibfm700MountNetServer : public mcc::network::MccGenericMountNetworkServer<Asibfm700Logger>
|
||||
{
|
||||
using base_t = mcc::network::MccGenericMountNetworkServer<Asibfm700Logger>;
|
||||
|
||||
public:
|
||||
Asibfm700MountNetServer(asio::io_context& ctx, Asibfm700Mount& mount, std::shared_ptr<spdlog::logger> logger);
|
||||
|
||||
~Asibfm700MountNetServer();
|
||||
};
|
||||
|
||||
} // namespace asibfm700
|
||||
507
asibfm700/asibfm700_netserver_endpoint.h
Normal file
507
asibfm700/asibfm700_netserver_endpoint.h
Normal file
@@ -0,0 +1,507 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <charconv>
|
||||
#include <cstdint>
|
||||
#include <filesystem>
|
||||
#include <ranges>
|
||||
#include <string_view>
|
||||
|
||||
#include "mcc_traits.h"
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
namespace utils
|
||||
{
|
||||
|
||||
static constexpr bool charSubrangeCompare(const mcc::traits::mcc_char_view auto& what,
|
||||
const mcc::traits::mcc_char_view auto& where,
|
||||
bool case_insensitive = false)
|
||||
{
|
||||
if (std::ranges::size(what) == std::ranges::size(where)) {
|
||||
if (case_insensitive) {
|
||||
auto f = std::ranges::search(where,
|
||||
std::views::transform(what, [](const char& ch) { return std::tolower(ch); }));
|
||||
return !f.empty();
|
||||
} else {
|
||||
auto f = std::ranges::search(where, what);
|
||||
return !f.empty();
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace utils
|
||||
|
||||
/*
|
||||
* Very simple various protocols endpoint parser and holder class
|
||||
*
|
||||
* endpoint: proto_mark://host_name:port_num/path
|
||||
* where "part" is optional for all non-local protocol kinds;
|
||||
*
|
||||
* for local kind of protocols the endpoint must be given as:
|
||||
* local://stream/PATH
|
||||
* local://seqpacket/PATH
|
||||
* local://serial/PATH
|
||||
* where 'stream' and 'seqpacket' "host_name"-field marks the
|
||||
* stream-type and seqpacket-type UNIX domain sockets protocols;
|
||||
* 'serial' marks a serial (RS232/485) protocol.
|
||||
* here, possible "port_num" field is allowed but ignored.
|
||||
*
|
||||
* NOTE: "proto_mark" and "host_name" (for local kind) fields are parsed in case-insensitive manner!
|
||||
*
|
||||
* EXAMPLES: tcp://192.168.70.130:3131
|
||||
* local://serial/dev/ttyS1
|
||||
* local://seqpacket/tmp/BM70_SERVER_SOCK
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
class Asibfm700NetserverEndpoint
|
||||
{
|
||||
public:
|
||||
static constexpr std::string_view protoHostDelim = "://";
|
||||
static constexpr std::string_view hostPortDelim = ":";
|
||||
static constexpr std::string_view portPathDelim = "/";
|
||||
|
||||
enum proto_id_t : uint8_t {
|
||||
PROTO_ID_LOCAL,
|
||||
PROTO_ID_SEQLOCAL,
|
||||
PROTO_ID_SERLOCAL,
|
||||
PROTO_ID_TCP,
|
||||
PROTO_ID_TLS,
|
||||
PROTO_ID_UNKNOWN
|
||||
};
|
||||
|
||||
static constexpr std::string_view protoMarkLocal{"local"}; // UNIX domain
|
||||
static constexpr std::string_view protoMarkTCP{"tcp"}; // TCP
|
||||
static constexpr std::string_view protoMarkTLS{"tls"}; // TLS
|
||||
|
||||
static constexpr std::array validProtoMarks{protoMarkLocal, protoMarkTCP, protoMarkTLS};
|
||||
|
||||
|
||||
static constexpr std::string_view localProtoTypeStream{"stream"}; // UNIX domain stream
|
||||
static constexpr std::string_view localProtoTypeSeqpacket{"seqpacket"}; // UNIX domain seqpacket
|
||||
static constexpr std::string_view localProtoTypeSerial{"serial"}; // serial (RS232/485)
|
||||
|
||||
static constexpr std::array validLocalProtoTypes{localProtoTypeStream, localProtoTypeSeqpacket,
|
||||
localProtoTypeSerial};
|
||||
|
||||
|
||||
template <mcc::traits::mcc_input_char_range R>
|
||||
Asibfm700NetserverEndpoint(const R& ept)
|
||||
{
|
||||
fromRange(ept);
|
||||
}
|
||||
|
||||
Asibfm700NetserverEndpoint(const Asibfm700NetserverEndpoint& other)
|
||||
{
|
||||
copyInst(other);
|
||||
}
|
||||
|
||||
Asibfm700NetserverEndpoint(Asibfm700NetserverEndpoint&& other)
|
||||
{
|
||||
moveInst(std::move(other));
|
||||
}
|
||||
|
||||
virtual ~Asibfm700NetserverEndpoint() = default;
|
||||
|
||||
|
||||
Asibfm700NetserverEndpoint& operator=(const Asibfm700NetserverEndpoint& other)
|
||||
{
|
||||
copyInst(other);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
Asibfm700NetserverEndpoint& operator=(Asibfm700NetserverEndpoint&& other)
|
||||
{
|
||||
moveInst(std::move(other));
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_input_char_range R>
|
||||
requires std::ranges::contiguous_range<R>
|
||||
bool fromRange(const R& ept)
|
||||
{
|
||||
_isValid = false;
|
||||
|
||||
// at least 'ws://a' (proto, proto-host delimiter and at least a single character of hostname)
|
||||
if (std::ranges::size(ept) < 6) {
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
if constexpr (std::is_array_v<std::remove_cvref_t<R>>) {
|
||||
_endpoint = ept;
|
||||
} else {
|
||||
_endpoint.clear();
|
||||
std::ranges::copy(ept, std::back_inserter(_endpoint));
|
||||
}
|
||||
|
||||
auto found = std::ranges::search(_endpoint, protoHostDelim);
|
||||
if (found.empty()) {
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
|
||||
ssize_t idx;
|
||||
if ((idx = checkProtoMark(std::string_view{_endpoint.begin(), found.begin()})) < 0) {
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
_proto = validProtoMarks[idx];
|
||||
|
||||
_host = std::string_view{found.end(), _endpoint.end()};
|
||||
|
||||
auto f1 = std::ranges::search(_host, portPathDelim);
|
||||
// std::string_view port_sv;
|
||||
if (f1.empty() && isLocal()) { // no path, but it is mandatory for 'local'!
|
||||
return _isValid;
|
||||
} else {
|
||||
_host = std::string_view(_host.begin(), f1.begin());
|
||||
|
||||
_path = std::string_view(f1.end(), &*_endpoint.end());
|
||||
|
||||
f1 = std::ranges::search(_host, hostPortDelim);
|
||||
if (f1.empty() && !isLocal()) { // no port, but it is mandatory for non-local!
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
_portView = std::string_view(f1.end(), _host.end());
|
||||
if (_portView.size()) {
|
||||
_host = std::string_view(_host.begin(), f1.begin());
|
||||
|
||||
if (!isLocal()) {
|
||||
// convert port string to int
|
||||
auto end_ptr = _portView.data() + _portView.size();
|
||||
|
||||
auto [ptr, ec] = std::from_chars(_portView.data(), end_ptr, _port);
|
||||
if (ec != std::errc() || ptr != end_ptr) {
|
||||
return _isValid;
|
||||
}
|
||||
} else { // ignore for local
|
||||
_port = -1;
|
||||
}
|
||||
} else {
|
||||
_port = -1;
|
||||
}
|
||||
|
||||
if (isLocal()) { // check for special values
|
||||
idx = 0;
|
||||
if (std::ranges::any_of(validLocalProtoTypes, [&idx, this](const auto& el) {
|
||||
bool ok = utils::charSubrangeCompare(_host, el, true);
|
||||
if (!ok) {
|
||||
++idx;
|
||||
}
|
||||
return ok;
|
||||
})) {
|
||||
_host = validLocalProtoTypes[idx];
|
||||
} else {
|
||||
return _isValid;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_isValid = true;
|
||||
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
|
||||
bool isValid() const
|
||||
{
|
||||
return _isValid;
|
||||
}
|
||||
|
||||
|
||||
auto endpoint() const
|
||||
{
|
||||
return _endpoint;
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_view_or_output_char_range R>
|
||||
R proto() const
|
||||
{
|
||||
return part<R>(PROTO_PART);
|
||||
}
|
||||
|
||||
std::string_view proto() const
|
||||
{
|
||||
return proto<std::string_view>();
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_view_or_output_char_range R>
|
||||
R host() const
|
||||
{
|
||||
return part<R>(HOST_PART);
|
||||
}
|
||||
|
||||
std::string_view host() const
|
||||
{
|
||||
return host<std::string_view>();
|
||||
}
|
||||
|
||||
int port() const
|
||||
{
|
||||
return _port;
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_view_or_output_char_range R>
|
||||
R portView() const
|
||||
{
|
||||
return part<R>(PORT_PART);
|
||||
}
|
||||
|
||||
std::string_view portView() const
|
||||
{
|
||||
return portView<std::string_view>();
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_output_char_range R, mcc::traits::mcc_input_char_range RR = std::string_view>
|
||||
R path(RR&& root_path) const
|
||||
{
|
||||
if (_path.empty()) {
|
||||
if constexpr (mcc::traits::mcc_output_char_range<R>) {
|
||||
R res;
|
||||
std::ranges::copy(std::forward<RR>(root_path), std::back_inserter(res));
|
||||
|
||||
return res;
|
||||
} else { // can't add root path!!!
|
||||
return part<R>(PATH_PART);
|
||||
}
|
||||
}
|
||||
|
||||
auto N = std::ranges::distance(root_path.begin(), root_path.end());
|
||||
|
||||
if (N) {
|
||||
R res;
|
||||
std::filesystem::path pt(root_path.begin(), root_path.end());
|
||||
|
||||
if (isLocal() && _path[0] == '\0') {
|
||||
std::ranges::copy(std::string_view(" "), std::back_inserter(res));
|
||||
pt /= _path.substr(1);
|
||||
std::ranges::copy(pt.string(), std::back_inserter(res));
|
||||
*res.begin() = '\0';
|
||||
} else {
|
||||
pt /= _path;
|
||||
std::ranges::copy(pt.string(), std::back_inserter(res));
|
||||
}
|
||||
|
||||
return res;
|
||||
} else {
|
||||
return part<R>(PATH_PART);
|
||||
}
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_input_char_range RR = std::string_view>
|
||||
std::string path(RR&& root_path) const
|
||||
{
|
||||
return path<std::string, RR>(std::forward<RR>(root_path));
|
||||
}
|
||||
|
||||
template <mcc::traits::mcc_view_or_output_char_range R>
|
||||
R path() const
|
||||
{
|
||||
return part<R>(PATH_PART);
|
||||
}
|
||||
|
||||
std::string_view path() const
|
||||
{
|
||||
return path<std::string_view>();
|
||||
}
|
||||
|
||||
|
||||
bool isLocal() const
|
||||
{
|
||||
return proto() == protoMarkLocal;
|
||||
}
|
||||
|
||||
bool isLocalStream() const
|
||||
{
|
||||
return host() == localProtoTypeStream;
|
||||
}
|
||||
|
||||
bool isLocalSerial() const
|
||||
{
|
||||
return host() == localProtoTypeSerial;
|
||||
}
|
||||
|
||||
bool isLocalSeqpacket() const
|
||||
{
|
||||
return host() == localProtoTypeSeqpacket;
|
||||
}
|
||||
|
||||
|
||||
bool isTCP() const
|
||||
{
|
||||
return proto() == protoMarkTCP;
|
||||
}
|
||||
|
||||
bool isTLS() const
|
||||
{
|
||||
return proto() == protoMarkTLS;
|
||||
}
|
||||
|
||||
|
||||
// add '\0' char (or replace special-meaning char/char-sequence) to construct UNIX abstract namespace
|
||||
// endpoint path
|
||||
template <typename T = std::nullptr_t>
|
||||
Asibfm700NetserverEndpoint& makeAbstract(const T& mark = nullptr)
|
||||
requires(mcc::traits::mcc_input_char_range<T> || std::same_as<std::remove_cv_t<T>, char> ||
|
||||
std::is_null_pointer_v<std::remove_cv_t<T>>)
|
||||
{
|
||||
if (!(isLocalStream() || isLocalSeqpacket())) { // only local proto is valid!
|
||||
return *this;
|
||||
}
|
||||
|
||||
if constexpr (std::is_null_pointer_v<T>) { // just insert '\0'
|
||||
auto it = _endpoint.insert(std::string::const_iterator(_path.begin()), '\0');
|
||||
_path = std::string_view(it, _endpoint.end());
|
||||
} else if constexpr (std::same_as<std::remove_cv_t<T>, char>) { // replace a character (mark)
|
||||
auto pos = std::distance(_endpoint.cbegin(), std::string::const_iterator(_path.begin()));
|
||||
if (_endpoint[pos] == mark) {
|
||||
_endpoint[pos] = '\0';
|
||||
}
|
||||
} else { // replace a character range (mark)
|
||||
if (std::ranges::equal(_path | std::views::take(std::ranges::size(mark), mark))) {
|
||||
auto pos = std::distance(_endpoint.cbegin(), std::string::const_iterator(_path.begin()));
|
||||
_endpoint.replace(pos, std::ranges::size(mark), 1, '\0');
|
||||
_path = std::string_view(_endpoint.begin() + pos, _endpoint.end());
|
||||
}
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::string _endpoint;
|
||||
std::string_view _proto, _host, _path, _portView;
|
||||
int _port;
|
||||
bool _isValid;
|
||||
|
||||
|
||||
virtual ssize_t checkProtoMark(std::string_view proto_mark)
|
||||
{
|
||||
ssize_t idx = 0;
|
||||
|
||||
// case-insensitive look-up
|
||||
bool found =
|
||||
std::ranges::any_of(Asibfm700NetserverEndpoint::validProtoMarks, [&idx, &proto_mark](const auto& el) {
|
||||
bool ok = utils::charSubrangeCompare(proto_mark, el, true);
|
||||
|
||||
if (!ok) {
|
||||
++idx;
|
||||
}
|
||||
|
||||
return ok;
|
||||
});
|
||||
|
||||
return found ? idx : -1;
|
||||
}
|
||||
|
||||
enum EndpointPart { PROTO_PART, HOST_PART, PATH_PART, PORT_PART };
|
||||
|
||||
template <mcc::traits::mcc_view_or_output_char_range R>
|
||||
R part(EndpointPart what) const
|
||||
{
|
||||
R res;
|
||||
|
||||
// if (!_isValid) {
|
||||
// return res;
|
||||
// }
|
||||
|
||||
auto part = _proto;
|
||||
|
||||
switch (what) {
|
||||
case PROTO_PART:
|
||||
part = _proto;
|
||||
break;
|
||||
case HOST_PART:
|
||||
part = _host;
|
||||
break;
|
||||
case PATH_PART:
|
||||
part = _path;
|
||||
break;
|
||||
case PORT_PART:
|
||||
part = _portView;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if constexpr (std::ranges::view<R>) {
|
||||
return {part.begin(), part.end()};
|
||||
} else {
|
||||
std::ranges::copy(part, std::back_inserter(res));
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void copyInst(const Asibfm700NetserverEndpoint& other)
|
||||
{
|
||||
if (&other != this) {
|
||||
if (other._isValid) {
|
||||
_isValid = other._isValid;
|
||||
_endpoint = other._endpoint;
|
||||
_proto = other._proto;
|
||||
|
||||
std::iterator_traits<const char*>::difference_type idx;
|
||||
if (other.isLocal()) { // for 'local' host is one of static class constants
|
||||
_host = other._host;
|
||||
} else {
|
||||
idx = std::distance(other._endpoint.c_str(), other._host.data());
|
||||
_host = std::string_view(_endpoint.c_str() + idx, other._host.size());
|
||||
}
|
||||
|
||||
idx = std::distance(other._endpoint.c_str(), other._path.data());
|
||||
_path = std::string_view(_endpoint.c_str() + idx, other._path.size());
|
||||
|
||||
idx = std::distance(other._endpoint.c_str(), other._portView.data());
|
||||
_portView = std::string_view(_endpoint.c_str() + idx, other._portView.size());
|
||||
|
||||
_port = other._port;
|
||||
} else {
|
||||
_isValid = false;
|
||||
_endpoint = std::string();
|
||||
_proto = std::string_view();
|
||||
_host = std::string_view();
|
||||
_path = std::string_view();
|
||||
_portView = std::string_view();
|
||||
_port = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void moveInst(Asibfm700NetserverEndpoint&& other)
|
||||
{
|
||||
if (&other != this) {
|
||||
if (other._isValid) {
|
||||
_isValid = std::move(other._isValid);
|
||||
_endpoint = std::move(other._endpoint);
|
||||
_proto = other._proto;
|
||||
_host = std::move(other._host);
|
||||
_path = std::move(other._path);
|
||||
_port = std::move(other._port);
|
||||
_portView = std::move(other._portView);
|
||||
} else {
|
||||
_isValid = false;
|
||||
_endpoint = std::string();
|
||||
_proto = std::string_view();
|
||||
_host = std::string_view();
|
||||
_path = std::string_view();
|
||||
_portView = std::string_view();
|
||||
_port = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace asibfm700
|
||||
172
asibfm700/asibfm700_netserver_main.cpp
Normal file
172
asibfm700/asibfm700_netserver_main.cpp
Normal file
@@ -0,0 +1,172 @@
|
||||
#include <spdlog/sinks/basic_file_sink.h>
|
||||
#include <spdlog/sinks/stdout_color_sinks.h>
|
||||
|
||||
#include <cxxopts.hpp>
|
||||
#include <iostream>
|
||||
|
||||
#include <asio/thread_pool.hpp>
|
||||
|
||||
#include <mcc_netserver_endpoint.h>
|
||||
#include "asibfm700_netserver.h"
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
/* COMMANDLINE OPTS */
|
||||
cxxopts::Options options(argv[0], "Astrosib (c) BM700 mount server\n");
|
||||
|
||||
options.allow_unrecognised_options();
|
||||
|
||||
options.add_options()("h,help", "Print usage");
|
||||
|
||||
options.add_options()("D,daemon", "Demonize server");
|
||||
|
||||
options.add_options()("l,log", "Log filename (use stdout and stderr for standard output and error stream)",
|
||||
cxxopts::value<std::string>()->default_value(""));
|
||||
|
||||
options.add_options()("level", "Log level (see SPDLOG package description for valid values)",
|
||||
cxxopts::value<std::string>()->default_value("info"));
|
||||
|
||||
options.add_options()("c,config", "Mount configuration filename (by default use of hardcoded one)",
|
||||
cxxopts::value<std::string>()->default_value(""));
|
||||
|
||||
options.add_options()("dump", "Dump mount default configuration to file and exit",
|
||||
cxxopts::value<std::string>()->default_value(""));
|
||||
|
||||
options.add_options()(
|
||||
"endpoints",
|
||||
"endpoints server will be listening for. For 'local' endpoint the '@' symbol at the beginning of the path "
|
||||
"means "
|
||||
"abstract namespace socket.",
|
||||
cxxopts::value<std::vector<std::string>>()->default_value("local://stream/@FM700_SERVER"));
|
||||
|
||||
|
||||
options.positional_help("[endpoint0] [enpoint1] ... [endpointN]");
|
||||
options.parse_positional({"endpoints"});
|
||||
|
||||
asio::io_context ctx(8);
|
||||
// asio::io_context ctx;
|
||||
|
||||
|
||||
try {
|
||||
auto opt_result = options.parse(argc, argv);
|
||||
|
||||
if (opt_result["help"].count()) {
|
||||
std::cout << options.help();
|
||||
std::cout << "\n";
|
||||
std::cout << "[endpoint0] [enpoint1] ... [endpointN] - endpoints server will be listening for. For 'local' "
|
||||
"endpoint the '@' symbol at the beginning of the path "
|
||||
"means abstract namespace socket (e.g. local://stream/@ASIBFM700_SERVER)."
|
||||
<< "\n";
|
||||
return 0;
|
||||
}
|
||||
|
||||
asibfm700::Asibfm700MountConfig mount_cfg;
|
||||
|
||||
std::string fname = opt_result["dump"].as<std::string>();
|
||||
if (fname.size()) {
|
||||
bool ok = mount_cfg.dumpDefaultsToFile(fname);
|
||||
if (!ok) {
|
||||
return 255;
|
||||
}
|
||||
|
||||
return 0;
|
||||
} else {
|
||||
// just ignore
|
||||
}
|
||||
|
||||
auto logname = opt_result["log"].as<std::string>();
|
||||
|
||||
auto logger = [&logname]() {
|
||||
if (logname == "stdout") {
|
||||
return spdlog::stdout_color_mt("console");
|
||||
} else if (logname == "stderr") {
|
||||
return spdlog::stderr_color_mt("stderr");
|
||||
} else if (logname == "") {
|
||||
return spdlog::null_logger_mt("FM700_SERVER_NULL_LOGGER");
|
||||
} else {
|
||||
return spdlog::basic_logger_mt(logname, logname);
|
||||
}
|
||||
}();
|
||||
|
||||
std::string level_str = opt_result["level"].as<std::string>();
|
||||
std::ranges::transform(level_str, level_str.begin(), [](const char& c) { return std::tolower(c); });
|
||||
|
||||
auto log_level = spdlog::level::from_str(level_str);
|
||||
logger->set_level(log_level);
|
||||
logger->flush_on(spdlog::level::trace);
|
||||
|
||||
|
||||
logger->set_pattern("%v");
|
||||
int w = 90;
|
||||
// const std::string fmt = std::format("{{:*^{}}}", w);
|
||||
constexpr std::string_view fmt = "{:*^90}";
|
||||
logger->info("\n\n\n");
|
||||
logger->info(fmt, "");
|
||||
logger->info(fmt, " ASTROSIB FM700 MOUNT SERVER ");
|
||||
auto zt = std::chrono::zoned_time(std::chrono::current_zone(),
|
||||
std::chrono::floor<std::chrono::seconds>(std::chrono::system_clock::now()));
|
||||
logger->info(fmt, std::format(" {} ", zt));
|
||||
logger->info(fmt, "");
|
||||
logger->info("\n");
|
||||
|
||||
|
||||
logger->set_pattern("[%Y-%m-%d %T.%e][%l]: %v");
|
||||
std::string mount_cfg_fname = opt_result["config"].as<std::string>();
|
||||
if (mount_cfg_fname.size()) {
|
||||
logger->info("Try to load mount configuration from file: {}", mount_cfg_fname);
|
||||
auto err = mount_cfg.load(mount_cfg_fname);
|
||||
if (err) {
|
||||
logger->error("Cannot load mount configuration (err = {})! Use of defaults!", err.message());
|
||||
} else {
|
||||
logger->info("Mount configuration was loaded successfully!");
|
||||
}
|
||||
logger->info("\n");
|
||||
}
|
||||
|
||||
asibfm700::Asibfm700Mount mount(mount_cfg, logger);
|
||||
asibfm700::Asibfm700MountNetServer server(ctx, mount, logger);
|
||||
|
||||
server.setupSignals();
|
||||
|
||||
if (opt_result["daemon"].count()) {
|
||||
server.daemonize();
|
||||
}
|
||||
|
||||
// mcc::MccServerEndpoint epn(std::string_view("local://seqpacket/tmp/BM700_SERVER_SOCK"));
|
||||
// mcc::MccServerEndpoint epn(std::string_view("local://stream/tmp/BM700_SERVER_SOCK"));
|
||||
// mcc::MccServerEndpoint epn(std::string_view("local://stream/@tmp/BM700_SERVER_SOCK"));
|
||||
// mcc::MccServerEndpoint epn(std::string_view("tcp://localhost:12345"));
|
||||
|
||||
// asio::co_spawn(ctx, server.listen(epn), asio::detached);
|
||||
|
||||
auto epnts = opt_result["endpoints"].as<std::vector<std::string>>();
|
||||
|
||||
for (auto& epnt : epnts) {
|
||||
mcc::network::MccNetServerEndpoint ep(epnt);
|
||||
|
||||
if (ep.isValid()) {
|
||||
ep.makeAbstract('@');
|
||||
|
||||
asio::co_spawn(ctx, server.listen(ep), asio::detached);
|
||||
} else {
|
||||
std::cerr << "Unrecognized endpoint: '" << epnt << "'! Ignore!\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// asio::thread_pool pool(5);
|
||||
|
||||
// asio::post(pool, [&ctx]() { ctx.run(); });
|
||||
|
||||
// pool.join();
|
||||
ctx.run();
|
||||
|
||||
} catch (const std::system_error& ex) {
|
||||
std::cerr << "An error occured: " << ex.code().message() << "\n";
|
||||
return ex.code().value();
|
||||
} catch (...) {
|
||||
std::cerr << "Unhandled exceptions!\n";
|
||||
return 255;
|
||||
}
|
||||
}
|
||||
292
asibfm700/asibfm700_servocontroller.cpp
Normal file
292
asibfm700/asibfm700_servocontroller.cpp
Normal file
@@ -0,0 +1,292 @@
|
||||
#include "asibfm700_servocontroller.h"
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
const char* AsibFM700ServoControllerErrorCategory::name() const noexcept
|
||||
{
|
||||
return "ASIBFM700-SERVOCONTROLLER-ERROR-CATEGORY";
|
||||
}
|
||||
|
||||
std::string AsibFM700ServoControllerErrorCategory::message(int ec) const
|
||||
{
|
||||
AsibFM700ServoControllerErrorCode err = static_cast<AsibFM700ServoControllerErrorCode>(ec);
|
||||
|
||||
switch (err) {
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_OK:
|
||||
return "OK";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_FATAL:
|
||||
return "LibSidServo fatal error";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_BADFORMAT:
|
||||
return "LibSidServo wrong arguments of function";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_ENCODERDEV:
|
||||
return "LibSidServo encoder device error or can't open";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_MOUNTDEV:
|
||||
return "LibSidServo mount device error or can't open";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_FAILED:
|
||||
return "LibSidServo failed to run command";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_NULLPTR:
|
||||
return "nullptr argument";
|
||||
case AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT:
|
||||
return "polling timeout";
|
||||
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
const AsibFM700ServoControllerErrorCategory& AsibFM700ServoControllerErrorCategory::get()
|
||||
{
|
||||
static const AsibFM700ServoControllerErrorCategory constInst;
|
||||
return constInst;
|
||||
}
|
||||
|
||||
|
||||
|
||||
AsibFM700ServoController::AsibFM700ServoController() : _hardwareConfig(), _setStateMutex(new std::mutex) {}
|
||||
|
||||
AsibFM700ServoController::AsibFM700ServoController(hardware_config_t config) : AsibFM700ServoController()
|
||||
{
|
||||
_hardwareConfig = std::move(config);
|
||||
|
||||
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
||||
}
|
||||
|
||||
|
||||
AsibFM700ServoController::~AsibFM700ServoController() {}
|
||||
|
||||
|
||||
constexpr std::string_view AsibFM700ServoController::hardwareName() const
|
||||
{
|
||||
return "Sidereal-ServoControllerII";
|
||||
}
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareStop()
|
||||
{
|
||||
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
|
||||
hardware_state_t hw_state;
|
||||
|
||||
auto start_tp = std::chrono::steady_clock::now();
|
||||
|
||||
// poll hardware till stopped-state detected ...
|
||||
while (true) {
|
||||
err = hardwareGetState(&hw_state);
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
|
||||
if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) {
|
||||
break;
|
||||
}
|
||||
|
||||
if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
|
||||
err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit()
|
||||
{
|
||||
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.init(&_hardwareConfig.devConfig));
|
||||
}
|
||||
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state)
|
||||
{
|
||||
std::lock_guard lock{*_setStateMutex};
|
||||
|
||||
if (state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) { // stop!
|
||||
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.stop());
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
|
||||
hardware_state_t hw_state;
|
||||
|
||||
auto start_tp = std::chrono::steady_clock::now();
|
||||
|
||||
// poll hardware till stopped-state detected ...
|
||||
while (true) {
|
||||
err = hardwareGetState(&hw_state);
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
|
||||
if (hw_state.moving_state == hardware_moving_state_t::HW_MOVE_STOPPED) {
|
||||
break;
|
||||
}
|
||||
|
||||
if ((std::chrono::steady_clock::now() - start_tp) > _hardwareConfig.pollingTimeout) {
|
||||
err = AsibFM700ServoControllerErrorCode::ERROR_POLLING_TIMEOUT;
|
||||
break;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(_hardwareConfig.pollingInterval);
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
// static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
|
||||
// static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};
|
||||
|
||||
// cvalpair.X = {.val = state.Y, .t = tp};
|
||||
// cvalpair.Y = {.val = state.X, .t = tp};
|
||||
|
||||
// cpair.X = state.tagY;
|
||||
// cpair.Y = state.tagX;
|
||||
|
||||
// time point from sidservo library is 'double' number represented UNIXTIME with
|
||||
// microseconds/nanoseconds precision
|
||||
// double tp = std::chrono::duration<double>(state.time_point.time_since_epoch()).count();
|
||||
|
||||
|
||||
// 2025-12-04: coordval_pair_t.X.t is now of type struct timespec
|
||||
auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(state.time_point.time_since_epoch());
|
||||
auto secs = std::chrono::floor<std::chrono::seconds>(ns);
|
||||
ns -= secs;
|
||||
std::timespec tp{.tv_sec = secs.count(), .tv_nsec = ns.count()};
|
||||
|
||||
// according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
||||
coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
|
||||
// coordpair_t cpair{.X = state.endptY, .Y = state.endptX};
|
||||
|
||||
|
||||
// correctTo is asynchronous function!!!
|
||||
//
|
||||
// according to the Eddy's implementation of the LibSidServo library it is safe
|
||||
// to pass the addresses of 'cvalpair' and 'cpair' automatic variables
|
||||
// auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair, &cpair));
|
||||
auto err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.correctTo(&cvalpair));
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(hardware_state_t* state)
|
||||
{
|
||||
if (state == nullptr) {
|
||||
return AsibFM700ServoControllerErrorCode::ERROR_NULLPTR;
|
||||
}
|
||||
|
||||
using tp_t = decltype(hardware_state_t::time_point);
|
||||
|
||||
mountdata_t mdata;
|
||||
|
||||
error_t err = static_cast<AsibFM700ServoControllerErrorCode>(Mount.getMountData(&mdata));
|
||||
if (!err) {
|
||||
// time point from sidservo library is 'double' number represented UNIXTIME with
|
||||
// microseconds/nanoseconds precision (must be equal for encXposition and encYposition)
|
||||
|
||||
// using secs_t = std::chrono::duration<double>;
|
||||
|
||||
// secs_t secs = secs_t{mdata.encXposition.t};
|
||||
// state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
|
||||
|
||||
// 2025-12-04: coordval_pair_t.X.t is now of type struct timespec
|
||||
auto dr = std::chrono::duration_cast<decltype(state->time_point)::duration>(
|
||||
std::chrono::seconds(mdata.encXposition.t.tv_sec) + std::chrono::nanoseconds(mdata.encXposition.t.tv_nsec));
|
||||
state->time_point = decltype(state->time_point){dr};
|
||||
|
||||
|
||||
// if (mcc::utils::isEqual(secs.count(), 0.0)) { // model mode?
|
||||
// state->time_point = decltype(state->time_point)::clock::now();
|
||||
// } else {
|
||||
// state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
|
||||
// }
|
||||
// WARNING: TEMPORARY (WAIT FOR Eddy fix its implementation of LibSidServo)!!!
|
||||
// state->time_point = decltype(state->time_point)::clock::now();
|
||||
|
||||
// according to "SiTech protocol notes" X is DEC-axis and Y is HA-axis
|
||||
state->X = mdata.encYposition.val;
|
||||
state->Y = mdata.encXposition.val;
|
||||
|
||||
state->speedX = mdata.encYspeed.val;
|
||||
state->speedY = mdata.encXspeed.val;
|
||||
|
||||
state->stateX = mdata.Ystate;
|
||||
state->stateY = mdata.Xstate;
|
||||
|
||||
if (mdata.Xstate == AXIS_ERROR || mdata.Ystate == AXIS_ERROR) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_ERROR;
|
||||
} else {
|
||||
if (mdata.Xstate == AXIS_STOPPED) {
|
||||
if (mdata.Ystate == AXIS_STOPPED) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_STOPPED;
|
||||
} else if (mdata.Ystate == AXIS_SLEWING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
||||
} else if (mdata.Ystate == AXIS_POINTING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
|
||||
} else if (mdata.Ystate == AXIS_GUIDING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
|
||||
} else {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
|
||||
}
|
||||
} else if (mdata.Xstate == AXIS_SLEWING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
||||
} else if (mdata.Xstate == AXIS_POINTING) {
|
||||
if (mdata.Ystate == AXIS_SLEWING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
||||
} else {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
|
||||
}
|
||||
} else if (mdata.Xstate == AXIS_GUIDING) {
|
||||
if (mdata.Ystate == AXIS_SLEWING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_SLEWING;
|
||||
} else if (mdata.Ystate == AXIS_POINTING) {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_ADJUSTING;
|
||||
} else {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_GUIDING;
|
||||
}
|
||||
} else {
|
||||
state->moving_state = hardware_moving_state_t::HW_MOVE_UNKNOWN;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
|
||||
void AsibFM700ServoController::hardwareUpdateConfig(conf_t cfg)
|
||||
{
|
||||
_hardwareConfig.devConfig = std::move(cfg);
|
||||
|
||||
_hardwareConfig.devConfig.MountDevPath = const_cast<char*>(_hardwareConfig.MountDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderDevPath = const_cast<char*>(_hardwareConfig.EncoderDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderXDevPath = const_cast<char*>(_hardwareConfig.EncoderXDevPath.c_str());
|
||||
_hardwareConfig.devConfig.EncoderYDevPath = const_cast<char*>(_hardwareConfig.EncoderYDevPath.c_str());
|
||||
}
|
||||
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig(hardware_configuration_t cfg)
|
||||
{
|
||||
_hardwareConfig.hwConfig = std::move(cfg);
|
||||
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.saveHWconfig(&_hardwareConfig.hwConfig));
|
||||
}
|
||||
|
||||
|
||||
AsibFM700ServoController::error_t AsibFM700ServoController::hardwareUpdateConfig()
|
||||
{
|
||||
return static_cast<AsibFM700ServoControllerErrorCode>(Mount.getHWconfig(&_hardwareConfig.hwConfig));
|
||||
}
|
||||
|
||||
|
||||
AsibFM700ServoController::hardware_config_t AsibFM700ServoController::getHardwareConfig() const
|
||||
{
|
||||
return _hardwareConfig;
|
||||
}
|
||||
|
||||
} // namespace asibfm700
|
||||
144
asibfm700/asibfm700_servocontroller.h
Normal file
144
asibfm700/asibfm700_servocontroller.h
Normal file
@@ -0,0 +1,144 @@
|
||||
#pragma once
|
||||
|
||||
#include <mcc_defaults.h>
|
||||
#include <mcc_generics.h>
|
||||
|
||||
#include "../LibSidServo/sidservo.h"
|
||||
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
/* error codes enum definition */
|
||||
|
||||
enum class AsibFM700ServoControllerErrorCode : int {
|
||||
// error codes from sidservo library
|
||||
ERROR_OK = MCC_E_OK,
|
||||
ERROR_FATAL = MCC_E_FATAL,
|
||||
ERROR_BADFORMAT = MCC_E_BADFORMAT,
|
||||
ERROR_ENCODERDEV = MCC_E_ENCODERDEV,
|
||||
ERROR_MOUNTDEV = MCC_E_MOUNTDEV,
|
||||
ERROR_FAILED = MCC_E_FAILED,
|
||||
// my codes ...
|
||||
ERROR_POLLING_TIMEOUT,
|
||||
ERROR_NULLPTR
|
||||
};
|
||||
|
||||
// error category
|
||||
struct AsibFM700ServoControllerErrorCategory : public std::error_category {
|
||||
const char* name() const noexcept;
|
||||
std::string message(int ec) const;
|
||||
|
||||
static const AsibFM700ServoControllerErrorCategory& get();
|
||||
};
|
||||
|
||||
|
||||
static inline std::error_code make_error_code(AsibFM700ServoControllerErrorCode ec)
|
||||
{
|
||||
return std::error_code(static_cast<int>(ec), AsibFM700ServoControllerErrorCategory::get());
|
||||
}
|
||||
|
||||
} // namespace asibfm700
|
||||
|
||||
|
||||
namespace std
|
||||
{
|
||||
|
||||
template <>
|
||||
class is_error_code_enum<asibfm700::AsibFM700ServoControllerErrorCode> : public true_type
|
||||
{
|
||||
};
|
||||
|
||||
} // namespace std
|
||||
|
||||
|
||||
namespace asibfm700
|
||||
{
|
||||
|
||||
class AsibFM700ServoController
|
||||
{
|
||||
public:
|
||||
typedef std::error_code error_t;
|
||||
|
||||
enum class hardware_moving_state_t : int {
|
||||
HW_MOVE_ERROR = -1,
|
||||
HW_MOVE_STOPPED = 0,
|
||||
HW_MOVE_SLEWING,
|
||||
HW_MOVE_ADJUSTING,
|
||||
HW_MOVE_TRACKING,
|
||||
HW_MOVE_GUIDING,
|
||||
HW_MOVE_UNKNOWN
|
||||
};
|
||||
|
||||
struct hardware_state_t {
|
||||
static constexpr mcc::MccCoordPairKind pair_kind = mcc::MccCoordPairKind::COORDS_KIND_HADEC_APP;
|
||||
mcc::MccTimePoint time_point;
|
||||
|
||||
double X, Y, speedX, speedY;
|
||||
axis_status_t stateX, stateY; // Eddy's LibSidServo axis state
|
||||
|
||||
hardware_moving_state_t moving_state;
|
||||
|
||||
// endpoint: a point on the trajectory of movement behind the guidance point (X,Y), taking into account
|
||||
// the movement vector (i.e. sign of movement speed)
|
||||
// this point is needed as Sidereal controller commands require not only moving speed but
|
||||
// also 'target' point (point at which mount will stop)
|
||||
// double endptX, endptY;
|
||||
};
|
||||
|
||||
|
||||
struct hardware_config_t {
|
||||
// the 'char*' fields from conf_t:
|
||||
// wrap it to std::string
|
||||
std::string MountDevPath;
|
||||
std::string EncoderDevPath;
|
||||
std::string EncoderXDevPath;
|
||||
std::string EncoderYDevPath;
|
||||
|
||||
conf_t devConfig; // devices paths and PIDs parameters
|
||||
hardware_configuration_t hwConfig; // EEPROM-located configuration
|
||||
|
||||
std::chrono::milliseconds pollingInterval{300}; // hardware polling interval
|
||||
std::chrono::milliseconds pollingTimeout{30000}; // hardware polling timeout
|
||||
};
|
||||
|
||||
/* constructors and destructor */
|
||||
|
||||
AsibFM700ServoController();
|
||||
|
||||
AsibFM700ServoController(hardware_config_t config);
|
||||
|
||||
AsibFM700ServoController(const AsibFM700ServoController&) = delete;
|
||||
AsibFM700ServoController& operator=(const AsibFM700ServoController&) = delete;
|
||||
|
||||
AsibFM700ServoController(AsibFM700ServoController&&) = default;
|
||||
AsibFM700ServoController& operator=(AsibFM700ServoController&&) = default;
|
||||
|
||||
virtual ~AsibFM700ServoController();
|
||||
|
||||
/* public methods */
|
||||
|
||||
constexpr std::string_view hardwareName() const;
|
||||
|
||||
error_t hardwareSetState(hardware_state_t state);
|
||||
error_t hardwareGetState(hardware_state_t* state);
|
||||
|
||||
error_t hardwareStop();
|
||||
error_t hardwareInit();
|
||||
|
||||
void hardwareUpdateConfig(conf_t cfg);
|
||||
|
||||
// save config to EEPROM
|
||||
error_t hardwareUpdateConfig(hardware_configuration_t cfg);
|
||||
// load config from EEPROM
|
||||
error_t hardwareUpdateConfig();
|
||||
|
||||
hardware_config_t getHardwareConfig() const;
|
||||
|
||||
protected:
|
||||
hardware_config_t _hardwareConfig;
|
||||
|
||||
std::unique_ptr<std::mutex> _setStateMutex;
|
||||
};
|
||||
|
||||
} // namespace asibfm700
|
||||
69
asibfm700/tests/cfg_test.cpp
Normal file
69
asibfm700/tests/cfg_test.cpp
Normal file
@@ -0,0 +1,69 @@
|
||||
#include <iostream>
|
||||
|
||||
#include "../asibfm700_configfile.h"
|
||||
|
||||
template <typename VT>
|
||||
struct rec_t {
|
||||
std::string_view key;
|
||||
VT value;
|
||||
};
|
||||
|
||||
static std::string_view cfg_str = R"--(A = 11
|
||||
B=3.3
|
||||
# this is comment
|
||||
C = WWWWWeeeWWWW
|
||||
|
||||
E = 10,20, 40, 32
|
||||
)--";
|
||||
|
||||
int main()
|
||||
{
|
||||
auto desc = std::make_tuple(rec_t{"A", 1}, rec_t{"B", 2.2}, rec_t{"C", std::string("EEE")}, rec_t{"D", 3.3},
|
||||
rec_t{"E", std::vector<int>{1, 2, 3}});
|
||||
|
||||
std::error_code err;
|
||||
|
||||
asibfm700::Asibfm700MountConfig acfg;
|
||||
bool ok = acfg.dumpDefaultsToFile("/tmp/cfg.cfg");
|
||||
if (!ok) {
|
||||
std::cerr << "Cannot dump default configuration!\n";
|
||||
exit(10);
|
||||
}
|
||||
|
||||
auto ec = acfg.load("/tmp/cfg.cfg");
|
||||
std::cout << "EC (load) = " << ec.message() << "\n";
|
||||
|
||||
std::cout << "refr w: " << acfg.refractWavelength() << "\n";
|
||||
|
||||
acfg.setValue("refractWavelength", 0.3);
|
||||
|
||||
auto e = acfg.getValue<double>("refractWavelength");
|
||||
std::cout << "refr w: " << e.value_or(0.0) << "\n";
|
||||
std::cout << "refr w: " << acfg.refractWavelength() << "\n";
|
||||
|
||||
|
||||
mcc::utils::KeyValueHolder kvh(desc);
|
||||
err = kvh.setValue("C", "ewlkjfde");
|
||||
if (err) {
|
||||
std::cout << "cannot set value: " << err.message() << "\n";
|
||||
} else {
|
||||
auto vs = kvh.getValue<std::string>("C");
|
||||
std::cout << "kvh[C] = " << vs.value_or("<no value>") << "\n";
|
||||
}
|
||||
|
||||
ec = kvh.fromCharRange(cfg_str);
|
||||
if (ec) {
|
||||
std::cout << "EC = " << ec.message() << "\n";
|
||||
} else {
|
||||
auto v3 = kvh.getValue<std::vector<int>>("E");
|
||||
std::cout << "[";
|
||||
for (auto& el : v3.value_or(std::vector<int>{0, 0, 0})) {
|
||||
std::cout << el << " ";
|
||||
}
|
||||
std::cout << "]\n";
|
||||
}
|
||||
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -6,13 +6,22 @@ set(ASIO_FOUND FALSE)
|
||||
|
||||
find_package(Threads REQUIRED)
|
||||
|
||||
find_path(ASIO_DIR asio.hpp HINTS ${ASIO_INSTALL_DIR} PATH_SUFFIXES include)
|
||||
set(ASIO_INSTALL_DIR "" CACHE STRING "ASIO install dir")
|
||||
set(ASIO_INSTALL_DIR_INTERNAL "" CACHE STRING "ASIO install dir")
|
||||
if(NOT "${ASIO_INSTALL_DIR}" STREQUAL "${ASIO_INSTALL_DIR_INTERNAL}") # ASIO_INSTALL_DIR is given in command-line
|
||||
unset(ASIO_INCLUDE_DIR CACHE)
|
||||
find_path(ASIO_DIR asio.hpp HINTS ${ASIO_INSTALL_DIR} PATH_SUFFIXES include)
|
||||
else() # in system path
|
||||
find_path(ASIO_DIR asio.hpp PATH_SUFFIXES include)
|
||||
endif()
|
||||
|
||||
|
||||
|
||||
if (NOT ASIO_DIR)
|
||||
message(WARNING "Cannot find ASIO library headers!")
|
||||
set(ASIO_FOUND FALSE)
|
||||
else()
|
||||
message(STATUS "Found ASIO: TRUE (${ASIO_DIR})")
|
||||
message(STATUS "Found ASIO: (${ASIO_DIR})")
|
||||
|
||||
# ASIO is header-only library so it is IMPORTED target
|
||||
add_library(ASIO::ASIO INTERFACE IMPORTED GLOBAL)
|
||||
|
||||
@@ -52,7 +52,10 @@ namespace mcc
|
||||
struct MccSimpleSlewModelCategory : public std::error_category {
|
||||
MccSimpleSlewModelCategory() : std::error_category() {}
|
||||
|
||||
const char* name() const noexcept { return "ADC_GENERIC_DEVICE"; }
|
||||
const char* name() const noexcept
|
||||
{
|
||||
return "ADC_GENERIC_DEVICE";
|
||||
}
|
||||
|
||||
std::string message(int ec) const
|
||||
{
|
||||
@@ -164,7 +167,7 @@ public:
|
||||
}
|
||||
|
||||
_stopRequested = other._stopRequested.load();
|
||||
_slewFunc = std::move(_slewFunc);
|
||||
_slewFunc = std::move(other._slewFunc);
|
||||
|
||||
return *this;
|
||||
};
|
||||
|
||||
@@ -24,7 +24,7 @@ public:
|
||||
|
||||
template <traits::mcc_range_of_input_char_range R = decltype(LOGGER_DEFAULT_FORMAT)>
|
||||
MccSpdlogLogger(std::shared_ptr<spdlog::logger> logger, const R& pattern_range = LOGGER_DEFAULT_FORMAT)
|
||||
: _loggerSPtr(logger), _currentLogPatternRange(), _currentLogPattern()
|
||||
: _currentLogPatternRange(), _currentLogPattern(), _loggerSPtr(logger)
|
||||
{
|
||||
if (std::distance(pattern_range.begin(), pattern_range.end())) {
|
||||
std::ranges::copy(
|
||||
@@ -46,25 +46,52 @@ public:
|
||||
virtual ~MccSpdlogLogger() = default;
|
||||
|
||||
|
||||
void setLogLevel(loglevel_t log_level) { _loggerSPtr->set_level(log_level); }
|
||||
void setLogLevel(loglevel_t log_level)
|
||||
{
|
||||
_loggerSPtr->set_level(log_level);
|
||||
}
|
||||
|
||||
loglevel_t getLogLevel() const { return _loggerSPtr->level(); }
|
||||
loglevel_t getLogLevel() const
|
||||
{
|
||||
return _loggerSPtr->level();
|
||||
}
|
||||
|
||||
void logMessage(loglevel_t level, const std::string& msg) { _loggerSPtr->log(level, msg); }
|
||||
void logMessage(loglevel_t level, const std::string& msg)
|
||||
{
|
||||
_loggerSPtr->log(level, msg);
|
||||
}
|
||||
|
||||
// specialized for given level methods
|
||||
|
||||
void logCritical(const std::string& msg) { logMessage(spdlog::level::critical, msg); }
|
||||
void logCritical(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::critical, msg);
|
||||
}
|
||||
|
||||
void logError(const std::string& msg) { logMessage(spdlog::level::err, msg); }
|
||||
void logError(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::err, msg);
|
||||
}
|
||||
|
||||
void logWarn(const std::string& msg) { logMessage(spdlog::level::warn, msg); }
|
||||
void logWarn(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::warn, msg);
|
||||
}
|
||||
|
||||
void logInfo(const std::string& msg) { logMessage(spdlog::level::info, msg); }
|
||||
void logInfo(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::info, msg);
|
||||
}
|
||||
|
||||
void logDebug(const std::string& msg) { logMessage(spdlog::level::debug, msg); }
|
||||
void logDebug(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::debug, msg);
|
||||
}
|
||||
|
||||
void logTrace(const std::string& msg) { logMessage(spdlog::level::trace, msg); }
|
||||
void logTrace(const std::string& msg)
|
||||
{
|
||||
logMessage(spdlog::level::trace, msg);
|
||||
}
|
||||
|
||||
template <traits::mcc_formattable... ArgTs>
|
||||
void logMessage(spdlog::level::level_enum level, spdlog::format_string_t<ArgTs...> fmt, ArgTs&&... args)
|
||||
@@ -149,7 +176,10 @@ protected:
|
||||
_loggerSPtr->set_pattern(_currentLogPattern);
|
||||
}
|
||||
|
||||
void addMarkToPatternIdx(const char* mark, size_t after_idx = 1) { addMarkToPatternIdx(std::string_view{mark}); }
|
||||
void addMarkToPatternIdx(const char* mark, size_t after_idx = 1)
|
||||
{
|
||||
addMarkToPatternIdx(std::string_view{mark}, after_idx);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mcc::utils
|
||||
|
||||
@@ -7,6 +7,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
|
||||
|
||||
find_package(Threads REQUIRED)
|
||||
|
||||
# ******* SPDLOG LIBRARY *******
|
||||
|
||||
@@ -16,64 +17,77 @@ include(ExternalProject)
|
||||
set(SPDLOG_USE_STD_FORMAT ON CACHE INTERNAL "Use of C++20 std::format")
|
||||
set(SPDLOG_FMT_EXTERNAL OFF CACHE INTERNAL "Turn off external fmt library")
|
||||
|
||||
FetchContent_Declare(spdlog
|
||||
# ExternalProject_Add(spdlog
|
||||
# SOURCE_DIR ${CMAKE_BINARY_DIR}/spdlog_lib
|
||||
# BINARY_DIR ${CMAKE_BINARY_DIR}/spdlog_lib/build
|
||||
GIT_REPOSITORY "https://github.com/gabime/spdlog.git"
|
||||
GIT_TAG "v1.15.1"
|
||||
GIT_SHALLOW TRUE
|
||||
GIT_SUBMODULES ""
|
||||
GIT_PROGRESS TRUE
|
||||
CMAKE_ARGS "-DSPDLOG_USE_STD_FORMAT=ON -DSPDLOG_FMT_EXTERNAL=OFF"
|
||||
# CONFIGURE_COMMAND ""
|
||||
# BUILD_COMMAND ""
|
||||
# INSTALL_COMMAND ""
|
||||
# UPDATE_COMMAND ""
|
||||
# SOURCE_SUBDIR cmake # turn off building
|
||||
OVERRIDE_FIND_PACKAGE
|
||||
)
|
||||
find_package(spdlog CONFIG)
|
||||
|
||||
|
||||
if (NOT ${spdlog_FOUND})
|
||||
FetchContent_Declare(spdlog
|
||||
# ExternalProject_Add(spdlog
|
||||
# SOURCE_DIR ${CMAKE_BINARY_DIR}/spdlog_lib
|
||||
# BINARY_DIR ${CMAKE_BINARY_DIR}/spdlog_lib/build
|
||||
GIT_REPOSITORY "https://github.com/gabime/spdlog.git"
|
||||
GIT_TAG "v1.15.1"
|
||||
GIT_SHALLOW TRUE
|
||||
GIT_SUBMODULES ""
|
||||
GIT_PROGRESS TRUE
|
||||
CMAKE_ARGS "-DSPDLOG_USE_STD_FORMAT=ON -DSPDLOG_FMT_EXTERNAL=OFF"
|
||||
# CONFIGURE_COMMAND ""
|
||||
# BUILD_COMMAND ""
|
||||
# INSTALL_COMMAND ""
|
||||
# UPDATE_COMMAND ""
|
||||
# SOURCE_SUBDIR cmake # turn off building
|
||||
OVERRIDE_FIND_PACKAGE
|
||||
)
|
||||
find_package(spdlog CONFIG)
|
||||
endif()
|
||||
|
||||
# ******* ERFA LIBRARY *******
|
||||
|
||||
# ExternalProject_Add(erfalib
|
||||
# PREFIX ${CMAKE_BINARY_DIR}/erfa_lib
|
||||
# GIT_REPOSITORY "https://github.com/liberfa/erfa.git"
|
||||
# GIT_TAG "v2.0.1"
|
||||
# UPDATE_COMMAND ""
|
||||
# PATCH_COMMAND ""
|
||||
# # BINARY_DIR erfa_build
|
||||
# # SOURCE_DIR erfa
|
||||
# # INSTALL_DIR
|
||||
# LOG_CONFIGURE 1
|
||||
# CONFIGURE_COMMAND meson setup --reconfigure -Ddefault_library=static -Dbuildtype=release
|
||||
# -Dprefix=${CMAKE_BINARY_DIR}/erfa_lib -Dlibdir= -Dincludedir= -Ddatadir= <SOURCE_DIR>
|
||||
# BUILD_COMMAND ninja -C <BINARY_DIR>
|
||||
# INSTALL_COMMAND meson install -C <BINARY_DIR>
|
||||
# BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a
|
||||
# )
|
||||
add_library(ERFA_LIB STATIC IMPORTED)
|
||||
ExternalProject_Add(erfalib
|
||||
PREFIX ${CMAKE_BINARY_DIR}/erfa_lib
|
||||
GIT_REPOSITORY "https://github.com/liberfa/erfa.git"
|
||||
GIT_TAG "v2.0.1"
|
||||
UPDATE_COMMAND ""
|
||||
PATCH_COMMAND ""
|
||||
# BINARY_DIR erfa_build
|
||||
# SOURCE_DIR erfa
|
||||
# INSTALL_DIR
|
||||
LOG_CONFIGURE 1
|
||||
CONFIGURE_COMMAND meson setup --reconfigure -Ddefault_library=static -Dbuildtype=release
|
||||
-Dprefix=${CMAKE_BINARY_DIR}/erfa_lib -Dlibdir= -Dincludedir= -Ddatadir= <SOURCE_DIR>
|
||||
BUILD_COMMAND ninja -C <BINARY_DIR>
|
||||
INSTALL_COMMAND meson install -C <BINARY_DIR>
|
||||
BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a
|
||||
)
|
||||
add_library(ERFA_LIB STATIC IMPORTED GLOBAL)
|
||||
set_target_properties(ERFA_LIB PROPERTIES IMPORTED_LOCATION ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a)
|
||||
add_dependencies(ERFA_LIB erfalib)
|
||||
set(ERFA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/erfa_lib)
|
||||
# include_directories(${ERFA_INCLUDE_DIR})
|
||||
# set(ERFA_LIBFILE ${CMAKE_BINARY_DIR}/erfa_lib/liberfa.a PARENT_SCOPE)
|
||||
include_directories(${ERFA_INCLUDE_DIR})
|
||||
message(STATUS "ERFA INCLUDE DIR: " ${ERFA_INCLUDE_DIR})
|
||||
|
||||
message(STATUS ${ERFA_INCLUDE_DIR})
|
||||
add_subdirectory(bsplines)
|
||||
message(STATUS "BSPLINES_INCLUDE_DIR: " ${BSPLINES_INCLUDE_DIR})
|
||||
include_directories(${BSPLINES_INCLUDE_DIR})
|
||||
|
||||
set(MCC_LIBRARY_SRC mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h
|
||||
mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_pcm.h mcc_telemetry.h
|
||||
mcc_angle.h mcc_pzone.h mcc_pzone_container.h mcc_finite_state_machine.h
|
||||
mcc_generic_mount.h mcc_tracking_model.h mcc_slewing_model.h mcc_moving_model_common.h
|
||||
mcc_netserver_endpoint.h mcc_netserver.h mcc_netserver_proto.h)
|
||||
|
||||
set(MCC_LIBRARY_SRC1 mcc_generics.h mcc_defaults.h mcc_traits.h mcc_utils.h
|
||||
mcc_ccte_iers.h mcc_ccte_iers_default.h mcc_ccte_erfa.h mcc_telemetry.h
|
||||
mcc_angle.h mcc_pzone.h mcc_pzone_container.h)
|
||||
set(MCC_LIBRARY1 mcc1)
|
||||
add_library(${MCC_LIBRARY1} INTERFACE ${MCC_LIBRARY_SRC1})
|
||||
target_compile_features(${MCC_LIBRARY1} INTERFACE cxx_std_23)
|
||||
target_include_directories(${MCC_LIBRARY1} INTERFACE ${ERFA_INCLUDE_DIR})
|
||||
list(APPEND MCC_LIBRARY_SRC mcc_spdlog.h)
|
||||
|
||||
set(MCC_LIBRARY mcc)
|
||||
add_library(${MCC_LIBRARY} INTERFACE ${MCC_LIBRARY_SRC})
|
||||
target_compile_features(${MCC_LIBRARY} INTERFACE cxx_std_23)
|
||||
target_compile_definitions(${MCC_LIBRARY} INTERFACE SPDLOG_USE_STD_FORMAT=1 SPDLOG_FMT_EXTERNAL=0)
|
||||
target_link_libraries(${MCC_LIBRARY} INTERFACE spdlog Threads::Threads atomic ${ERFA_LIB})
|
||||
target_include_directories(${MCC_LIBRARY} INTERFACE ${ERFA_INCLUDE_DIR} ${BSPLINES_INCLUDE_DIR})
|
||||
target_include_directories(${MCC_LIBRARY} INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/mcc>
|
||||
)
|
||||
|
||||
# add_subdirectory(fitpack)
|
||||
# message(STATUS "FITPACK: " ${FITPACK_INCLUDE_DIR})
|
||||
|
||||
option(WITH_TESTS "Build tests" ON)
|
||||
|
||||
@@ -81,7 +95,16 @@ if (WITH_TESTS)
|
||||
set(CTTE_TEST_APP ccte_test)
|
||||
add_executable(${CTTE_TEST_APP} tests/ccte_test.cpp)
|
||||
target_include_directories(${CTTE_TEST_APP} PRIVATE ${ERFA_INCLUDE_DIR})
|
||||
target_link_libraries(${CTTE_TEST_APP} ERFA_LIB)
|
||||
target_link_libraries(${CTTE_TEST_APP} ERFA_LIB bsplines)
|
||||
|
||||
set(NETMSG_TESTS_APP netmsg_test)
|
||||
add_executable(${NETMSG_TESTS_APP} tests/netmsg_test.cpp)
|
||||
target_link_libraries(${NETMSG_TESTS_APP} mcc)
|
||||
|
||||
|
||||
set(MCCCOORD_TEST_APP mcc_coord_test)
|
||||
add_executable(${MCCCOORD_TEST_APP} tests/mcc_coord_test.cpp)
|
||||
target_link_libraries(${MCCCOORD_TEST_APP} mcc ERFA_LIB)
|
||||
|
||||
enable_testing()
|
||||
endif()
|
||||
endif()
|
||||
|
||||
35
mcc/bsplines/CMakeLists.txt
Normal file
35
mcc/bsplines/CMakeLists.txt
Normal file
@@ -0,0 +1,35 @@
|
||||
cmake_minimum_required(VERSION 3.20)
|
||||
|
||||
set(func_name "")
|
||||
|
||||
file(GLOB src_files "*.f")
|
||||
|
||||
foreach(ff IN LISTS src_files)
|
||||
get_filename_component(sn ${ff} NAME_WE)
|
||||
list(APPEND func_name ${sn})
|
||||
endforeach()
|
||||
|
||||
# message(STATUS "${func_name}")
|
||||
|
||||
string(REPLACE ";" " " func_str "${func_name}")
|
||||
|
||||
# message(STATUS ${func_str})
|
||||
|
||||
enable_language(Fortran CXX)
|
||||
|
||||
include(FortranCInterface)
|
||||
FortranCInterface_HEADER(FortranCInterface.h
|
||||
MACRO_NAMESPACE "FC_"
|
||||
# SYMBOL_NAMESPACE "fp_"
|
||||
SYMBOL_NAMESPACE ""
|
||||
# SYMBOLS ${func_str}
|
||||
SYMBOLS ${func_name}
|
||||
)
|
||||
FortranCInterface_VERIFY(CXX)
|
||||
|
||||
set(BSPLINES_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR} PARENT_SCOPE)
|
||||
include_directories(${BSPLINES_INCLUDE_DIR})
|
||||
|
||||
add_library(bsplines STATIC ${src_files} mcc_bsplines.h)
|
||||
|
||||
|
||||
19
mcc/bsplines/Makefile
Normal file
19
mcc/bsplines/Makefile
Normal file
@@ -0,0 +1,19 @@
|
||||
# Makefile that builts a library lib$(LIB).a from all
|
||||
# of the Fortran files found in the current directory.
|
||||
# Usage: make LIB=<libname>
|
||||
# Pearu
|
||||
|
||||
OBJ=$(patsubst %.f,%.o,$(shell ls *.f))
|
||||
all: lib$(LIB).a
|
||||
$(OBJ):
|
||||
$(FC) -c $(FFLAGS) $(FSHARED) $(patsubst %.o,%.f,$(@F)) -o $@
|
||||
lib$(LIB).a: $(OBJ)
|
||||
$(AR) rus lib$(LIB).a $?
|
||||
clean:
|
||||
rm *.o
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
3
mcc/bsplines/README
Normal file
3
mcc/bsplines/README
Normal file
@@ -0,0 +1,3 @@
|
||||
- ddierckx is a 'real*8' version of dierckx
|
||||
generated by Pearu Peterson <pearu@ioc.ee>.
|
||||
- dierckx (in netlib) is fitpack by P. Dierckx
|
||||
66
mcc/bsplines/bispeu.f
Normal file
66
mcc/bsplines/bispeu.f
Normal file
@@ -0,0 +1,66 @@
|
||||
recursive subroutine bispeu(tx,nx,ty,ny,c,kx,ky,x,y,z,m,wrk,
|
||||
* lwrk, ier)
|
||||
implicit none
|
||||
c subroutine bispeu evaluates on a set of points (x(i),y(i)),i=1,...,m
|
||||
c a bivariate spline s(x,y) of degrees kx and ky, given in the
|
||||
c b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c call bispeu(tx,nx,ty,ny,c,kx,ky,x,y,z,m,wrk,lwrk,
|
||||
c * iwrk,kwrk,ier)
|
||||
c
|
||||
c input parameters:
|
||||
c tx : real array, length nx, which contains the position of the
|
||||
c knots in the x-direction.
|
||||
c nx : integer, giving the total number of knots in the x-direction
|
||||
c ty : real array, length ny, which contains the position of the
|
||||
c knots in the y-direction.
|
||||
c ny : integer, giving the total number of knots in the y-direction
|
||||
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||
c b-spline coefficients.
|
||||
c kx,ky : integer values, giving the degrees of the spline.
|
||||
c x : real array of dimension (mx).
|
||||
c y : real array of dimension (my).
|
||||
c m : on entry m must specify the number points. m >= 1.
|
||||
c wrk : real array of dimension lwrk. used as workspace.
|
||||
c lwrk : integer, specifying the dimension of wrk.
|
||||
c lwrk >= kx+ky+2
|
||||
c
|
||||
c output parameters:
|
||||
c z : real array of dimension m.
|
||||
c on successful exit z(i) contains the value of s(x,y)
|
||||
c at the point (x(i),y(i)), i=1,...,m.
|
||||
c ier : integer error flag
|
||||
c ier=0 : normal return
|
||||
c ier=10: invalid input data (see restrictions)
|
||||
c
|
||||
c restrictions:
|
||||
c m >=1, lwrk>=mx*(kx+1)+my*(ky+1), kwrk>=mx+my
|
||||
c tx(kx+1) <= x(i-1) <= x(i) <= tx(nx-kx), i=2,...,mx
|
||||
c ty(ky+1) <= y(j-1) <= y(j) <= ty(ny-ky), j=2,...,my
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpbisp,fpbspl
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer nx,ny,kx,ky,m,lwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(m),y(m),z(m),
|
||||
* wrk(lwrk)
|
||||
c ..local scalars..
|
||||
integer iwrk(2)
|
||||
integer i, lwest
|
||||
c ..
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
lwest = kx+ky+2
|
||||
if (lwrk.lt.lwest) go to 100
|
||||
if (m.lt.1) go to 100
|
||||
ier = 0
|
||||
do 10 i=1,m
|
||||
call fpbisp(tx,nx,ty,ny,c,kx,ky,x(i),1,y(i),1,z(i),wrk(1),
|
||||
* wrk(kx+2),iwrk(1),iwrk(2))
|
||||
10 continue
|
||||
100 return
|
||||
end
|
||||
104
mcc/bsplines/bispev.f
Normal file
104
mcc/bsplines/bispev.f
Normal file
@@ -0,0 +1,104 @@
|
||||
recursive subroutine bispev(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,
|
||||
* wrk,lwrk,iwrk,kwrk,ier)
|
||||
implicit none
|
||||
c subroutine bispev evaluates on a grid (x(i),y(j)),i=1,...,mx; j=1,...
|
||||
c ,my a bivariate spline s(x,y) of degrees kx and ky, given in the
|
||||
c b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c call bispev(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,wrk,lwrk,
|
||||
c * iwrk,kwrk,ier)
|
||||
c
|
||||
c input parameters:
|
||||
c tx : real array, length nx, which contains the position of the
|
||||
c knots in the x-direction.
|
||||
c nx : integer, giving the total number of knots in the x-direction
|
||||
c ty : real array, length ny, which contains the position of the
|
||||
c knots in the y-direction.
|
||||
c ny : integer, giving the total number of knots in the y-direction
|
||||
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||
c b-spline coefficients.
|
||||
c kx,ky : integer values, giving the degrees of the spline.
|
||||
c x : real array of dimension (mx).
|
||||
c before entry x(i) must be set to the x co-ordinate of the
|
||||
c i-th grid point along the x-axis.
|
||||
c tx(kx+1)<=x(i-1)<=x(i)<=tx(nx-kx), i=2,...,mx.
|
||||
c mx : on entry mx must specify the number of grid points along
|
||||
c the x-axis. mx >=1.
|
||||
c y : real array of dimension (my).
|
||||
c before entry y(j) must be set to the y co-ordinate of the
|
||||
c j-th grid point along the y-axis.
|
||||
c ty(ky+1)<=y(j-1)<=y(j)<=ty(ny-ky), j=2,...,my.
|
||||
c my : on entry my must specify the number of grid points along
|
||||
c the y-axis. my >=1.
|
||||
c wrk : real array of dimension lwrk. used as workspace.
|
||||
c lwrk : integer, specifying the dimension of wrk.
|
||||
c lwrk >= mx*(kx+1)+my*(ky+1)
|
||||
c iwrk : integer array of dimension kwrk. used as workspace.
|
||||
c kwrk : integer, specifying the dimension of iwrk. kwrk >= mx+my.
|
||||
c
|
||||
c output parameters:
|
||||
c z : real array of dimension (mx*my).
|
||||
c on successful exit z(my*(i-1)+j) contains the value of s(x,y)
|
||||
c at the point (x(i),y(j)),i=1,...,mx;j=1,...,my.
|
||||
c ier : integer error flag
|
||||
c ier=0 : normal return
|
||||
c ier=10: invalid input data (see restrictions)
|
||||
c
|
||||
c restrictions:
|
||||
c mx >=1, my >=1, lwrk>=mx*(kx+1)+my*(ky+1), kwrk>=mx+my
|
||||
c tx(kx+1) <= x(i-1) <= x(i) <= tx(nx-kx), i=2,...,mx
|
||||
c ty(ky+1) <= y(j-1) <= y(j) <= ty(ny-ky), j=2,...,my
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpbisp,fpbspl
|
||||
c
|
||||
c references :
|
||||
c de boor c : on calculating with b-splines, j. approximation theory
|
||||
c 6 (1972) 50-62.
|
||||
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||
c applics 10 (1972) 134-149.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer nx,ny,kx,ky,mx,my,lwrk,kwrk,ier
|
||||
c ..array arguments..
|
||||
integer iwrk(kwrk)
|
||||
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(mx),y(my),z(mx*my),
|
||||
* wrk(lwrk)
|
||||
c ..local scalars..
|
||||
integer i,iw,lwest
|
||||
c ..
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
lwest = (kx+1)*mx+(ky+1)*my
|
||||
if(lwrk.lt.lwest) go to 100
|
||||
if(kwrk.lt.(mx+my)) go to 100
|
||||
if (mx.lt.1) go to 100
|
||||
if (mx.eq.1) go to 30
|
||||
go to 10
|
||||
10 do 20 i=2,mx
|
||||
if(x(i).lt.x(i-1)) go to 100
|
||||
20 continue
|
||||
30 if (my.lt.1) go to 100
|
||||
if (my.eq.1) go to 60
|
||||
go to 40
|
||||
40 do 50 i=2,my
|
||||
if(y(i).lt.y(i-1)) go to 100
|
||||
50 continue
|
||||
60 ier = 0
|
||||
iw = mx*(kx+1)+1
|
||||
call fpbisp(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,z,wrk(1),wrk(iw),
|
||||
* iwrk(1),iwrk(mx+1))
|
||||
100 return
|
||||
end
|
||||
353
mcc/bsplines/clocur.f
Normal file
353
mcc/bsplines/clocur.f
Normal file
@@ -0,0 +1,353 @@
|
||||
recursive subroutine clocur(iopt,ipar,idim,m,u,mx,x,w,k,s,nest,
|
||||
* n,t,nc,c,fp,wrk,lwrk,iwrk,ier)
|
||||
implicit none
|
||||
c given the ordered set of m points x(i) in the idim-dimensional space
|
||||
c with x(1)=x(m), and given also a corresponding set of strictly in-
|
||||
c creasing values u(i) and the set of positive numbers w(i),i=1,2,...,m
|
||||
c subroutine clocur determines a smooth approximating closed spline
|
||||
c curve s(u), i.e.
|
||||
c x1 = s1(u)
|
||||
c x2 = s2(u) u(1) <= u <= u(m)
|
||||
c .........
|
||||
c xidim = sidim(u)
|
||||
c with sj(u),j=1,2,...,idim periodic spline functions of degree k with
|
||||
c common knots t(j),j=1,2,...,n.
|
||||
c if ipar=1 the values u(i),i=1,2,...,m must be supplied by the user.
|
||||
c if ipar=0 these values are chosen automatically by clocur as
|
||||
c v(1) = 0
|
||||
c v(i) = v(i-1) + dist(x(i),x(i-1)) ,i=2,3,...,m
|
||||
c u(i) = v(i)/v(m) ,i=1,2,...,m
|
||||
c if iopt=-1 clocur calculates the weighted least-squares closed spline
|
||||
c curve according to a given set of knots.
|
||||
c if iopt>=0 the number of knots of the splines sj(u) and the position
|
||||
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||
c ness of s(u) is then achieved by minimalizing the discontinuity
|
||||
c jumps of the k-th derivative of s(u) at the knots t(j),j=k+2,k+3,...,
|
||||
c n-k-1. the amount of smoothness is determined by the condition that
|
||||
c f(p)=sum((w(i)*dist(x(i),s(u(i))))**2) be <= s, with s a given non-
|
||||
c negative constant, called the smoothing factor.
|
||||
c the fit s(u) is given in the b-spline representation and can be
|
||||
c evaluated by means of subroutine curev.
|
||||
c
|
||||
c calling sequence:
|
||||
c call clocur(iopt,ipar,idim,m,u,mx,x,w,k,s,nest,n,t,nc,c,
|
||||
c * fp,wrk,lwrk,iwrk,ier)
|
||||
c
|
||||
c parameters:
|
||||
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||
c least-squares closed spline curve (iopt=-1) or a smoothing
|
||||
c closed spline curve (iopt=0 or 1) must be determined. if
|
||||
c iopt=0 the routine will start with an initial set of knots
|
||||
c t(i)=u(1)+(u(m)-u(1))*(i-k-1),i=1,2,...,2*k+2. if iopt=1 the
|
||||
c routine will continue with the knots found at the last call.
|
||||
c attention: a call with iopt=1 must always be immediately
|
||||
c preceded by another call with iopt=1 or iopt=0.
|
||||
c unchanged on exit.
|
||||
c ipar : integer flag. on entry ipar must specify whether (ipar=1)
|
||||
c the user will supply the parameter values u(i),or whether
|
||||
c (ipar=0) these values are to be calculated by clocur.
|
||||
c unchanged on exit.
|
||||
c idim : integer. on entry idim must specify the dimension of the
|
||||
c curve. 0 < idim < 11.
|
||||
c unchanged on exit.
|
||||
c m : integer. on entry m must specify the number of data points.
|
||||
c m > 1. unchanged on exit.
|
||||
c u : real array of dimension at least (m). in case ipar=1,before
|
||||
c entry, u(i) must be set to the i-th value of the parameter
|
||||
c variable u for i=1,2,...,m. these values must then be
|
||||
c supplied in strictly ascending order and will be unchanged
|
||||
c on exit. in case ipar=0, on exit,the array will contain the
|
||||
c values u(i) as determined by clocur.
|
||||
c mx : integer. on entry mx must specify the actual dimension of
|
||||
c the array x as declared in the calling (sub)program. mx must
|
||||
c not be too small (see x). unchanged on exit.
|
||||
c x : real array of dimension at least idim*m.
|
||||
c before entry, x(idim*(i-1)+j) must contain the j-th coord-
|
||||
c inate of the i-th data point for i=1,2,...,m and j=1,2,...,
|
||||
c idim. since first and last data point must coincide it
|
||||
c means that x(j)=x(idim*(m-1)+j),j=1,2,...,idim.
|
||||
c unchanged on exit.
|
||||
c w : real array of dimension at least (m). before entry, w(i)
|
||||
c must be set to the i-th value in the set of weights. the
|
||||
c w(i) must be strictly positive. w(m) is not used.
|
||||
c unchanged on exit. see also further comments.
|
||||
c k : integer. on entry k must specify the degree of the splines.
|
||||
c 1<=k<=5. it is recommended to use cubic splines (k=3).
|
||||
c the user is strongly dissuaded from choosing k even,together
|
||||
c with a small s-value. unchanged on exit.
|
||||
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||
c factor. s >=0. unchanged on exit.
|
||||
c for advice on the choice of s see further comments.
|
||||
c nest : integer. on entry nest must contain an over-estimate of the
|
||||
c total number of knots of the splines returned, to indicate
|
||||
c the storage space available to the routine. nest >=2*k+2.
|
||||
c in most practical situation nest=m/2 will be sufficient.
|
||||
c always large enough is nest=m+2*k, the number of knots
|
||||
c needed for interpolation (s=0). unchanged on exit.
|
||||
c n : integer.
|
||||
c unless ier = 10 (in case iopt >=0), n will contain the
|
||||
c total number of knots of the smoothing spline curve returned
|
||||
c if the computation mode iopt=1 is used this value of n
|
||||
c should be left unchanged between subsequent calls.
|
||||
c in case iopt=-1, the value of n must be specified on entry.
|
||||
c t : real array of dimension at least (nest).
|
||||
c on successful exit, this array will contain the knots of the
|
||||
c spline curve,i.e. the position of the interior knots t(k+2),
|
||||
c t(k+3),..,t(n-k-1) as well as the position of the additional
|
||||
c t(1),t(2),..,t(k+1)=u(1) and u(m)=t(n-k),...,t(n) needed for
|
||||
c the b-spline representation.
|
||||
c if the computation mode iopt=1 is used, the values of t(1),
|
||||
c t(2),...,t(n) should be left unchanged between subsequent
|
||||
c calls. if the computation mode iopt=-1 is used, the values
|
||||
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||
c entry. see also the restrictions (ier=10).
|
||||
c nc : integer. on entry nc must specify the actual dimension of
|
||||
c the array c as declared in the calling (sub)program. nc
|
||||
c must not be too small (see c). unchanged on exit.
|
||||
c c : real array of dimension at least (nest*idim).
|
||||
c on successful exit, this array will contain the coefficients
|
||||
c in the b-spline representation of the spline curve s(u),i.e.
|
||||
c the b-spline coefficients of the spline sj(u) will be given
|
||||
c in c(n*(j-1)+i),i=1,2,...,n-k-1 for j=1,2,...,idim.
|
||||
c fp : real. unless ier = 10, fp contains the weighted sum of
|
||||
c squared residuals of the spline curve returned.
|
||||
c wrk : real array of dimension at least m*(k+1)+nest*(7+idim+5*k).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||
c the array wrk as declared in the calling (sub)program. lwrk
|
||||
c must not be too small (see wrk). unchanged on exit.
|
||||
c iwrk : integer array of dimension at least (nest).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c ier : integer. unless the routine detects an error, ier contains a
|
||||
c non-positive value on exit, i.e.
|
||||
c ier=0 : normal return. the close curve returned has a residual
|
||||
c sum of squares fp such that abs(fp-s)/s <= tol with tol a
|
||||
c relative tolerance set to 0.001 by the program.
|
||||
c ier=-1 : normal return. the curve returned is an interpolating
|
||||
c spline curve (fp=0).
|
||||
c ier=-2 : normal return. the curve returned is the weighted least-
|
||||
c squares point,i.e. each spline sj(u) is a constant. in
|
||||
c this extreme case fp gives the upper bound fp0 for the
|
||||
c smoothing factor s.
|
||||
c ier=1 : error. the required storage space exceeds the available
|
||||
c storage space, as specified by the parameter nest.
|
||||
c probably causes : nest too small. if nest is already
|
||||
c large (say nest > m/2), it may also indicate that s is
|
||||
c too small
|
||||
c the approximation returned is the least-squares closed
|
||||
c curve according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||
c the parameter fp gives the corresponding weighted sum of
|
||||
c squared residuals (fp>s).
|
||||
c ier=2 : error. a theoretically impossible result was found during
|
||||
c the iteration process for finding a smoothing curve with
|
||||
c fp = s. probably causes : s too small.
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||
c by the program) allowed for finding a smoothing curve
|
||||
c with fp=s has been reached. probably causes : s too small
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=10 : error. on entry, the input data are controlled on validity
|
||||
c the following restrictions must be satisfied.
|
||||
c -1<=iopt<=1, 1<=k<=5, m>1, nest>2*k+2, w(i)>0,i=1,2,...,m
|
||||
c 0<=ipar<=1, 0<idim<=10, lwrk>=(k+1)*m+nest*(7+idim+5*k),
|
||||
c nc>=nest*idim, x(j)=x(idim*(m-1)+j), j=1,2,...,idim
|
||||
c if ipar=0: sum j=1,idim (x(i*idim+j)-x((i-1)*idim+j))**2>0
|
||||
c i=1,2,...,m-1.
|
||||
c if ipar=1: u(1)<u(2)<...<u(m)
|
||||
c if iopt=-1: 2*k+2<=n<=min(nest,m+2*k)
|
||||
c u(1)<t(k+2)<t(k+3)<...<t(n-k-1)<u(m)
|
||||
c (u(1)=0 and u(m)=1 in case ipar=0)
|
||||
c the schoenberg-whitney conditions, i.e. there
|
||||
c must be a subset of data points uu(j) with
|
||||
c uu(j) = u(i) or u(i)+(u(m)-u(1)) such that
|
||||
c t(j) < uu(j) < t(j+k+1), j=k+1,...,n-k-1
|
||||
c if iopt>=0: s>=0
|
||||
c if s=0 : nest >= m+2*k
|
||||
c if one of these conditions is found to be violated,control
|
||||
c is immediately repassed to the calling program. in that
|
||||
c case there is no approximation returned.
|
||||
c
|
||||
c further comments:
|
||||
c by means of the parameter s, the user can control the tradeoff
|
||||
c between closeness of fit and smoothness of fit of the approximation.
|
||||
c if s is too large, the curve will be too smooth and signal will be
|
||||
c lost ; if s is too small the curve will pick up too much noise. in
|
||||
c the extreme cases the program will return an interpolating curve if
|
||||
c s=0 and the weighted least-squares point if s is very large.
|
||||
c between these extremes, a properly chosen s will result in a good
|
||||
c compromise between closeness of fit and smoothness of fit.
|
||||
c to decide whether an approximation, corresponding to a certain s is
|
||||
c satisfactory the user is highly recommended to inspect the fits
|
||||
c graphically.
|
||||
c recommended values for s depend on the weights w(i). if these are
|
||||
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||
c x(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||
c sqrt(2*m)). if nothing is known about the statistical error in x(i)
|
||||
c each w(i) can be set equal to one and s determined by trial and
|
||||
c error, taking account of the comments above. the best is then to
|
||||
c start with a very large value of s ( to determine the weighted
|
||||
c least-squares point and the upper bound fp0 for s) and then to
|
||||
c progressively decrease the value of s ( say by a factor 10 in the
|
||||
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||
c approximating curve shows more detail) to obtain closer fits.
|
||||
c to economize the search for a good s-value the program provides with
|
||||
c different modes of computation. at the first call of the routine, or
|
||||
c whenever he wants to restart with the initial set of knots the user
|
||||
c must set iopt=0.
|
||||
c if iopt=1 the program will continue with the set of knots found at
|
||||
c the last call of the routine. this will save a lot of computation
|
||||
c time if clocur is called repeatedly for different values of s.
|
||||
c the number of knots of the spline returned and their location will
|
||||
c depend on the value of s and on the complexity of the shape of the
|
||||
c curve underlying the data. but, if the computation mode iopt=1 is
|
||||
c used, the knots returned may also depend on the s-values at previous
|
||||
c calls (if these were smaller). therefore, if after a number of
|
||||
c trials with different s-values and iopt=1, the user can finally
|
||||
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||
c clocur once more with the selected value for s but now with iopt=0.
|
||||
c indeed, clocur may then return an approximation of the same quality
|
||||
c of fit but with fewer knots and therefore better if data reduction
|
||||
c is also an important objective for the user.
|
||||
c
|
||||
c the form of the approximating curve can strongly be affected by
|
||||
c the choice of the parameter values u(i). if there is no physical
|
||||
c reason for choosing a particular parameter u, often good results
|
||||
c will be obtained with the choice of clocur(in case ipar=0), i.e.
|
||||
c v(1)=0, v(i)=v(i-1)+q(i), i=2,...,m, u(i)=v(i)/v(m), i=1,..,m
|
||||
c where
|
||||
c q(i)= sqrt(sum j=1,idim (xj(i)-xj(i-1))**2 )
|
||||
c other possibilities for q(i) are
|
||||
c q(i)= sum j=1,idim (xj(i)-xj(i-1))**2
|
||||
c q(i)= sum j=1,idim abs(xj(i)-xj(i-1))
|
||||
c q(i)= max j=1,idim abs(xj(i)-xj(i-1))
|
||||
c q(i)= 1
|
||||
c
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpbacp,fpbspl,fpchep,fpclos,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||
c
|
||||
c references:
|
||||
c dierckx p. : algorithms for smoothing data with periodic and
|
||||
c parametric splines, computer graphics and image
|
||||
c processing 20 (1982) 171-184.
|
||||
c dierckx p. : algorithms for smoothing data with periodic and param-
|
||||
c etric splines, report tw55, dept. computer science,
|
||||
c k.u.leuven, 1981.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author:
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u. leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c creation date : may 1979
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,fp
|
||||
integer iopt,ipar,idim,m,mx,k,nest,n,nc,lwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 u(m),x(mx),w(m),t(nest),c(nc),wrk(lwrk)
|
||||
integer iwrk(nest)
|
||||
c ..local scalars..
|
||||
real*8 per,tol,dist
|
||||
integer i,ia1,ia2,ib,ifp,ig1,ig2,iq,iz,i1,i2,j1,j2,k1,k2,lwest,
|
||||
* maxit,m1,nmin,ncc,j
|
||||
c ..function references..
|
||||
real*8 sqrt
|
||||
c we set up the parameters tol and maxit
|
||||
maxit = 20
|
||||
tol = 0.1e-02
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(iopt.lt.(-1) .or. iopt.gt.1) go to 90
|
||||
if(ipar.lt.0 .or. ipar.gt.1) go to 90
|
||||
if(idim.le.0 .or. idim.gt.10) go to 90
|
||||
if(k.le.0 .or. k.gt.5) go to 90
|
||||
k1 = k+1
|
||||
k2 = k1+1
|
||||
nmin = 2*k1
|
||||
if(m.lt.2 .or. nest.lt.nmin) go to 90
|
||||
ncc = nest*idim
|
||||
if(mx.lt.m*idim .or. nc.lt.ncc) go to 90
|
||||
lwest = m*k1+nest*(7+idim+5*k)
|
||||
if(lwrk.lt.lwest) go to 90
|
||||
i1 = idim
|
||||
i2 = m*idim
|
||||
do 5 j=1,idim
|
||||
if(x(i1).ne.x(i2)) go to 90
|
||||
i1 = i1-1
|
||||
i2 = i2-1
|
||||
5 continue
|
||||
if(ipar.ne.0 .or. iopt.gt.0) go to 40
|
||||
i1 = 0
|
||||
i2 = idim
|
||||
u(1) = 0.
|
||||
do 20 i=2,m
|
||||
dist = 0.
|
||||
do 10 j1=1,idim
|
||||
i1 = i1+1
|
||||
i2 = i2+1
|
||||
dist = dist+(x(i2)-x(i1))**2
|
||||
10 continue
|
||||
u(i) = u(i-1)+sqrt(dist)
|
||||
20 continue
|
||||
if(u(m).le.0.) go to 90
|
||||
do 30 i=2,m
|
||||
u(i) = u(i)/u(m)
|
||||
30 continue
|
||||
u(m) = 0.1e+01
|
||||
40 if(w(1).le.0.) go to 90
|
||||
m1 = m-1
|
||||
do 50 i=1,m1
|
||||
if(u(i).ge.u(i+1) .or. w(i).le.0.) go to 90
|
||||
50 continue
|
||||
if(iopt.ge.0) go to 70
|
||||
if(n.le.nmin .or. n.gt.nest) go to 90
|
||||
per = u(m)-u(1)
|
||||
j1 = k1
|
||||
t(j1) = u(1)
|
||||
i1 = n-k
|
||||
t(i1) = u(m)
|
||||
j2 = j1
|
||||
i2 = i1
|
||||
do 60 i=1,k
|
||||
i1 = i1+1
|
||||
i2 = i2-1
|
||||
j1 = j1+1
|
||||
j2 = j2-1
|
||||
t(j2) = t(i2)-per
|
||||
t(i1) = t(j1)+per
|
||||
60 continue
|
||||
call fpchep(u,m,t,n,k,ier)
|
||||
if (ier.eq.0) go to 80
|
||||
go to 90
|
||||
70 if(s.lt.0.) go to 90
|
||||
if(s.eq.0. .and. nest.lt.(m+2*k)) go to 90
|
||||
ier = 0
|
||||
c we partition the working space and determine the spline approximation.
|
||||
80 ifp = 1
|
||||
iz = ifp+nest
|
||||
ia1 = iz+ncc
|
||||
ia2 = ia1+nest*k1
|
||||
ib = ia2+nest*k
|
||||
ig1 = ib+nest*k2
|
||||
ig2 = ig1+nest*k2
|
||||
iq = ig2+nest*k1
|
||||
call fpclos(iopt,idim,m,u,mx,x,w,k,s,nest,tol,maxit,k1,k2,n,t,
|
||||
* ncc,c,fp,wrk(ifp),wrk(iz),wrk(ia1),wrk(ia2),wrk(ib),wrk(ig1),
|
||||
* wrk(ig2),wrk(iq),iwrk,ier)
|
||||
90 return
|
||||
end
|
||||
181
mcc/bsplines/cocosp.f
Normal file
181
mcc/bsplines/cocosp.f
Normal file
@@ -0,0 +1,181 @@
|
||||
recursive subroutine cocosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,
|
||||
* sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||
implicit none
|
||||
c given the set of data points (x(i),y(i)) and the set of positive
|
||||
c numbers w(i),i=1,2,...,m, subroutine cocosp determines the weighted
|
||||
c least-squares cubic spline s(x) with given knots t(j),j=1,2,...,n
|
||||
c which satisfies the following concavity/convexity conditions
|
||||
c s''(t(j+3))*e(j) <= 0, j=1,2,...n-6
|
||||
c the fit is given in the b-spline representation( b-spline coef-
|
||||
c ficients c(j),j=1,2,...n-4) and can be evaluated by means of
|
||||
c subroutine splev.
|
||||
c
|
||||
c calling sequence:
|
||||
c call cocosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,bind,wrk,
|
||||
c * lwrk,iwrk,kwrk,ier)
|
||||
c
|
||||
c parameters:
|
||||
c m : integer. on entry m must specify the number of data points.
|
||||
c m > 3. unchanged on exit.
|
||||
c x : real array of dimension at least (m). before entry, x(i)
|
||||
c must be set to the i-th value of the independent variable x,
|
||||
c for i=1,2,...,m. these values must be supplied in strictly
|
||||
c ascending order. unchanged on exit.
|
||||
c y : real array of dimension at least (m). before entry, y(i)
|
||||
c must be set to the i-th value of the dependent variable y,
|
||||
c for i=1,2,...,m. unchanged on exit.
|
||||
c w : real array of dimension at least (m). before entry, w(i)
|
||||
c must be set to the i-th value in the set of weights. the
|
||||
c w(i) must be strictly positive. unchanged on exit.
|
||||
c n : integer. on entry n must contain the total number of knots
|
||||
c of the cubic spline. m+4>=n>=8. unchanged on exit.
|
||||
c t : real array of dimension at least (n). before entry, this
|
||||
c array must contain the knots of the spline, i.e. the position
|
||||
c of the interior knots t(5),t(6),...,t(n-4) as well as the
|
||||
c position of the boundary knots t(1),t(2),t(3),t(4) and t(n-3)
|
||||
c t(n-2),t(n-1),t(n) needed for the b-spline representation.
|
||||
c unchanged on exit. see also the restrictions (ier=10).
|
||||
c e : real array of dimension at least (n). before entry, e(j)
|
||||
c must be set to 1 if s(x) must be locally concave at t(j+3),
|
||||
c to (-1) if s(x) must be locally convex at t(j+3) and to 0
|
||||
c if no convexity constraint is imposed at t(j+3),j=1,2,..,n-6.
|
||||
c e(n-5),...,e(n) are not used. unchanged on exit.
|
||||
c maxtr : integer. on entry maxtr must contain an over-estimate of the
|
||||
c total number of records in the used tree structure, to indic-
|
||||
c ate the storage space available to the routine. maxtr >=1
|
||||
c in most practical situation maxtr=100 will be sufficient.
|
||||
c always large enough is
|
||||
c n-5 n-6
|
||||
c maxtr = ( ) + ( ) with l the greatest
|
||||
c l l+1
|
||||
c integer <= (n-6)/2 . unchanged on exit.
|
||||
c maxbin: integer. on entry maxbin must contain an over-estimate of the
|
||||
c number of knots where s(x) will have a zero second derivative
|
||||
c maxbin >=1. in most practical situation maxbin = 10 will be
|
||||
c sufficient. always large enough is maxbin=n-6.
|
||||
c unchanged on exit.
|
||||
c c : real array of dimension at least (n).
|
||||
c on successful exit, this array will contain the coefficients
|
||||
c c(1),c(2),..,c(n-4) in the b-spline representation of s(x)
|
||||
c sq : real. on successful exit, sq contains the weighted sum of
|
||||
c squared residuals of the spline approximation returned.
|
||||
c sx : real array of dimension at least m. on successful exit
|
||||
c this array will contain the spline values s(x(i)),i=1,...,m
|
||||
c bind : logical array of dimension at least (n). on successful exit
|
||||
c this array will indicate the knots where s''(x)=0, i.e.
|
||||
c s''(t(j+3)) .eq. 0 if bind(j) = .true.
|
||||
c s''(t(j+3)) .ne. 0 if bind(j) = .false., j=1,2,...,n-6
|
||||
c wrk : real array of dimension at least m*4+n*7+maxbin*(maxbin+n+1)
|
||||
c used as working space.
|
||||
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||
c the array wrk as declared in the calling (sub)program.lwrk
|
||||
c must not be too small (see wrk). unchanged on exit.
|
||||
c iwrk : integer array of dimension at least (maxtr*4+2*(maxbin+1))
|
||||
c used as working space.
|
||||
c kwrk : integer. on entry,kwrk must specify the actual dimension of
|
||||
c the array iwrk as declared in the calling (sub)program. kwrk
|
||||
c must not be too small (see iwrk). unchanged on exit.
|
||||
c ier : integer. error flag
|
||||
c ier=0 : successful exit.
|
||||
c ier>0 : abnormal termination: no approximation is returned
|
||||
c ier=1 : the number of knots where s''(x)=0 exceeds maxbin.
|
||||
c probably causes : maxbin too small.
|
||||
c ier=2 : the number of records in the tree structure exceeds
|
||||
c maxtr.
|
||||
c probably causes : maxtr too small.
|
||||
c ier=3 : the algorithm finds no solution to the posed quadratic
|
||||
c programming problem.
|
||||
c probably causes : rounding errors.
|
||||
c ier=10 : on entry, the input data are controlled on validity.
|
||||
c the following restrictions must be satisfied
|
||||
c m>3, maxtr>=1, maxbin>=1, 8<=n<=m+4,w(i) > 0,
|
||||
c x(1)<x(2)<...<x(m), t(1)<=t(2)<=t(3)<=t(4)<=x(1),
|
||||
c x(1)<t(5)<t(6)<...<t(n-4)<x(m)<=t(n-3)<=...<=t(n),
|
||||
c kwrk>=maxtr*4+2*(maxbin+1),
|
||||
c lwrk>=m*4+n*7+maxbin*(maxbin+n+1),
|
||||
c the schoenberg-whitney conditions, i.e. there must
|
||||
c be a subset of data points xx(j) such that
|
||||
c t(j) < xx(j) < t(j+4), j=1,2,...,n-4
|
||||
c if one of these restrictions is found to be violated
|
||||
c control is immediately repassed to the calling program
|
||||
c
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpcosp,fpbspl,fpadno,fpdeno,fpseno,fpfrno,fpchec
|
||||
c
|
||||
c references:
|
||||
c dierckx p. : an algorithm for cubic spline fitting with convexity
|
||||
c constraints, computing 24 (1980) 349-371.
|
||||
c dierckx p. : an algorithm for least-squares cubic spline fitting
|
||||
c with convexity and concavity constraints, report tw39,
|
||||
c dept. computer science, k.u.leuven, 1978.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author:
|
||||
c p. dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c creation date : march 1978
|
||||
c latest update : march 1987.
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 sq
|
||||
integer m,n,maxtr,maxbin,lwrk,kwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),t(n),e(n),c(n),sx(m),wrk(lwrk)
|
||||
integer iwrk(kwrk)
|
||||
logical bind(n)
|
||||
c ..local scalars..
|
||||
integer i,ia,ib,ic,iq,iu,iz,izz,ji,jib,jjb,jl,jr,ju,kwest,
|
||||
* lwest,mb,nm,n6
|
||||
real*8 one
|
||||
c ..
|
||||
c set constant
|
||||
one = 0.1e+01
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(m.lt.4 .or. n.lt.8) go to 40
|
||||
if(maxtr.lt.1 .or. maxbin.lt.1) go to 40
|
||||
lwest = 7*n+m*4+maxbin*(1+n+maxbin)
|
||||
kwest = 4*maxtr+2*(maxbin+1)
|
||||
if(lwrk.lt.lwest .or. kwrk.lt.kwest) go to 40
|
||||
if(w(1).le.0.) go to 40
|
||||
do 10 i=2,m
|
||||
if(x(i-1).ge.x(i) .or. w(i).le.0.) go to 40
|
||||
10 continue
|
||||
call fpchec(x,m,t,n,3,ier)
|
||||
if (ier.eq.0) go to 20
|
||||
go to 40
|
||||
c set numbers e(i)
|
||||
20 n6 = n-6
|
||||
do 30 i=1,n6
|
||||
if(e(i).gt.0.) e(i) = one
|
||||
if(e(i).lt.0.) e(i) = -one
|
||||
30 continue
|
||||
c we partition the working space and determine the spline approximation
|
||||
nm = n+maxbin
|
||||
mb = maxbin+1
|
||||
ia = 1
|
||||
ib = ia+4*n
|
||||
ic = ib+nm*maxbin
|
||||
iz = ic+n
|
||||
izz = iz+n
|
||||
iu = izz+n
|
||||
iq = iu+maxbin
|
||||
ji = 1
|
||||
ju = ji+maxtr
|
||||
jl = ju+maxtr
|
||||
jr = jl+maxtr
|
||||
jjb = jr+maxtr
|
||||
jib = jjb+mb
|
||||
call fpcosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,bind,nm,mb,wrk(ia),
|
||||
*
|
||||
* wrk(ib),wrk(ic),wrk(iz),wrk(izz),wrk(iu),wrk(iq),iwrk(ji),
|
||||
* iwrk(ju),iwrk(jl),iwrk(jr),iwrk(jjb),iwrk(jib),ier)
|
||||
40 return
|
||||
end
|
||||
234
mcc/bsplines/concon.f
Normal file
234
mcc/bsplines/concon.f
Normal file
@@ -0,0 +1,234 @@
|
||||
recursive subroutine concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,
|
||||
* n,t,c,sq,sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||
implicit none
|
||||
c given the set of data points (x(i),y(i)) and the set of positive
|
||||
c numbers w(i), i=1,2,...,m,subroutine concon determines a cubic spline
|
||||
c approximation s(x) which satisfies the following local convexity
|
||||
c constraints s''(x(i))*v(i) <= 0, i=1,2,...,m.
|
||||
c the number of knots n and the position t(j),j=1,2,...n is chosen
|
||||
c automatically by the routine in a way that
|
||||
c sq = sum((w(i)*(y(i)-s(x(i))))**2) be <= s.
|
||||
c the fit is given in the b-spline representation (b-spline coef-
|
||||
c ficients c(j),j=1,2,...n-4) and can be evaluated by means of
|
||||
c subroutine splev.
|
||||
c
|
||||
c calling sequence:
|
||||
c
|
||||
c call concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,
|
||||
c * sx,bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||
c
|
||||
c parameters:
|
||||
c iopt: integer flag.
|
||||
c if iopt=0, the routine will start with the minimal number of
|
||||
c knots to guarantee that the convexity conditions will be
|
||||
c satisfied. if iopt=1, the routine will continue with the set
|
||||
c of knots found at the last call of the routine.
|
||||
c attention: a call with iopt=1 must always be immediately
|
||||
c preceded by another call with iopt=1 or iopt=0.
|
||||
c unchanged on exit.
|
||||
c m : integer. on entry m must specify the number of data points.
|
||||
c m > 3. unchanged on exit.
|
||||
c x : real array of dimension at least (m). before entry, x(i)
|
||||
c must be set to the i-th value of the independent variable x,
|
||||
c for i=1,2,...,m. these values must be supplied in strictly
|
||||
c ascending order. unchanged on exit.
|
||||
c y : real array of dimension at least (m). before entry, y(i)
|
||||
c must be set to the i-th value of the dependent variable y,
|
||||
c for i=1,2,...,m. unchanged on exit.
|
||||
c w : real array of dimension at least (m). before entry, w(i)
|
||||
c must be set to the i-th value in the set of weights. the
|
||||
c w(i) must be strictly positive. unchanged on exit.
|
||||
c v : real array of dimension at least (m). before entry, v(i)
|
||||
c must be set to 1 if s(x) must be locally concave at x(i),
|
||||
c to (-1) if s(x) must be locally convex at x(i) and to 0
|
||||
c if no convexity constraint is imposed at x(i).
|
||||
c s : real. on entry s must specify an over-estimate for the
|
||||
c the weighted sum of squared residuals sq of the requested
|
||||
c spline. s >=0. unchanged on exit.
|
||||
c nest : integer. on entry nest must contain an over-estimate of the
|
||||
c total number of knots of the spline returned, to indicate
|
||||
c the storage space available to the routine. nest >=8.
|
||||
c in most practical situation nest=m/2 will be sufficient.
|
||||
c always large enough is nest=m+4. unchanged on exit.
|
||||
c maxtr : integer. on entry maxtr must contain an over-estimate of the
|
||||
c total number of records in the used tree structure, to indic-
|
||||
c ate the storage space available to the routine. maxtr >=1
|
||||
c in most practical situation maxtr=100 will be sufficient.
|
||||
c always large enough is
|
||||
c nest-5 nest-6
|
||||
c maxtr = ( ) + ( ) with l the greatest
|
||||
c l l+1
|
||||
c integer <= (nest-6)/2 . unchanged on exit.
|
||||
c maxbin: integer. on entry maxbin must contain an over-estimate of the
|
||||
c number of knots where s(x) will have a zero second derivative
|
||||
c maxbin >=1. in most practical situation maxbin = 10 will be
|
||||
c sufficient. always large enough is maxbin=nest-6.
|
||||
c unchanged on exit.
|
||||
c n : integer.
|
||||
c on exit with ier <=0, n will contain the total number of
|
||||
c knots of the spline approximation returned. if the comput-
|
||||
c ation mode iopt=1 is used this value of n should be left
|
||||
c unchanged between subsequent calls.
|
||||
c t : real array of dimension at least (nest).
|
||||
c on exit with ier<=0, this array will contain the knots of the
|
||||
c spline,i.e. the position of the interior knots t(5),t(6),...,
|
||||
c t(n-4) as well as the position of the additional knots
|
||||
c t(1)=t(2)=t(3)=t(4)=x(1) and t(n-3)=t(n-2)=t(n-1)=t(n)=x(m)
|
||||
c needed for the b-spline representation.
|
||||
c if the computation mode iopt=1 is used, the values of t(1),
|
||||
c t(2),...,t(n) should be left unchanged between subsequent
|
||||
c calls.
|
||||
c c : real array of dimension at least (nest).
|
||||
c on successful exit, this array will contain the coefficients
|
||||
c c(1),c(2),..,c(n-4) in the b-spline representation of s(x)
|
||||
c sq : real. unless ier>0 , sq contains the weighted sum of
|
||||
c squared residuals of the spline approximation returned.
|
||||
c sx : real array of dimension at least m. on exit with ier<=0
|
||||
c this array will contain the spline values s(x(i)),i=1,...,m
|
||||
c if the computation mode iopt=1 is used, the values of sx(1),
|
||||
c sx(2),...,sx(m) should be left unchanged between subsequent
|
||||
c calls.
|
||||
c bind: logical array of dimension at least nest. on exit with ier<=0
|
||||
c this array will indicate the knots where s''(x)=0, i.e.
|
||||
c s''(t(j+3)) .eq. 0 if bind(j) = .true.
|
||||
c s''(t(j+3)) .ne. 0 if bind(j) = .false., j=1,2,...,n-6
|
||||
c if the computation mode iopt=1 is used, the values of bind(1)
|
||||
c ,...,bind(n-6) should be left unchanged between subsequent
|
||||
c calls.
|
||||
c wrk : real array of dimension at least (m*4+nest*8+maxbin*(maxbin+
|
||||
c nest+1)). used as working space.
|
||||
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||
c the array wrk as declared in the calling (sub)program.lwrk
|
||||
c must not be too small (see wrk). unchanged on exit.
|
||||
c iwrk : integer array of dimension at least (maxtr*4+2*(maxbin+1))
|
||||
c used as working space.
|
||||
c kwrk : integer. on entry,kwrk must specify the actual dimension of
|
||||
c the array iwrk as declared in the calling (sub)program. kwrk
|
||||
c must not be too small (see iwrk). unchanged on exit.
|
||||
c ier : integer. error flag
|
||||
c ier=0 : normal return, s(x) satisfies the concavity/convexity
|
||||
c constraints and sq <= s.
|
||||
c ier<0 : abnormal termination: s(x) satisfies the concavity/
|
||||
c convexity constraints but sq > s.
|
||||
c ier=-3 : the requested storage space exceeds the available
|
||||
c storage space as specified by the parameter nest.
|
||||
c probably causes: nest too small. if nest is already
|
||||
c large (say nest > m/2), it may also indicate that s
|
||||
c is too small.
|
||||
c the approximation returned is the least-squares cubic
|
||||
c spline according to the knots t(1),...,t(n) (n=nest)
|
||||
c which satisfies the convexity constraints.
|
||||
c ier=-2 : the maximal number of knots n=m+4 has been reached.
|
||||
c probably causes: s too small.
|
||||
c ier=-1 : the number of knots n is less than the maximal number
|
||||
c m+4 but concon finds that adding one or more knots
|
||||
c will not further reduce the value of sq.
|
||||
c probably causes : s too small.
|
||||
c ier>0 : abnormal termination: no approximation is returned
|
||||
c ier=1 : the number of knots where s''(x)=0 exceeds maxbin.
|
||||
c probably causes : maxbin too small.
|
||||
c ier=2 : the number of records in the tree structure exceeds
|
||||
c maxtr.
|
||||
c probably causes : maxtr too small.
|
||||
c ier=3 : the algorithm finds no solution to the posed quadratic
|
||||
c programming problem.
|
||||
c probably causes : rounding errors.
|
||||
c ier=4 : the minimum number of knots (given by n) to guarantee
|
||||
c that the concavity/convexity conditions will be
|
||||
c satisfied is greater than nest.
|
||||
c probably causes: nest too small.
|
||||
c ier=5 : the minimum number of knots (given by n) to guarantee
|
||||
c that the concavity/convexity conditions will be
|
||||
c satisfied is greater than m+4.
|
||||
c probably causes: strongly alternating convexity and
|
||||
c concavity conditions. normally the situation can be
|
||||
c coped with by adding n-m-4 extra data points (found
|
||||
c by linear interpolation e.g.) with a small weight w(i)
|
||||
c and a v(i) number equal to zero.
|
||||
c ier=10 : on entry, the input data are controlled on validity.
|
||||
c the following restrictions must be satisfied
|
||||
c 0<=iopt<=1, m>3, nest>=8, s>=0, maxtr>=1, maxbin>=1,
|
||||
c kwrk>=maxtr*4+2*(maxbin+1), w(i)>0, x(i) < x(i+1),
|
||||
c lwrk>=m*4+nest*8+maxbin*(maxbin+nest+1)
|
||||
c if one of these restrictions is found to be violated
|
||||
c control is immediately repassed to the calling program
|
||||
c
|
||||
c further comments:
|
||||
c as an example of the use of the computation mode iopt=1, the
|
||||
c following program segment will cause concon to return control
|
||||
c each time a spline with a new set of knots has been computed.
|
||||
c .............
|
||||
c iopt = 0
|
||||
c s = 0.1e+60 (s very large)
|
||||
c do 10 i=1,m
|
||||
c call concon(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,sx,
|
||||
c * bind,wrk,lwrk,iwrk,kwrk,ier)
|
||||
c ......
|
||||
c s = sq
|
||||
c iopt=1
|
||||
c 10 continue
|
||||
c .............
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpcoco,fpcosp,fpbspl,fpadno,fpdeno,fpseno,fpfrno
|
||||
c
|
||||
c references:
|
||||
c dierckx p. : an algorithm for cubic spline fitting with convexity
|
||||
c constraints, computing 24 (1980) 349-371.
|
||||
c dierckx p. : an algorithm for least-squares cubic spline fitting
|
||||
c with convexity and concavity constraints, report tw39,
|
||||
c dept. computer science, k.u.leuven, 1978.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author:
|
||||
c p. dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c creation date : march 1978
|
||||
c latest update : march 1987.
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,sq
|
||||
integer iopt,m,nest,maxtr,maxbin,n,lwrk,kwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),v(m),t(nest),c(nest),sx(m),wrk(lwrk)
|
||||
integer iwrk(kwrk)
|
||||
logical bind(nest)
|
||||
c ..local scalars..
|
||||
integer i,lwest,kwest,ie,iw,lww
|
||||
real*8 one
|
||||
c ..
|
||||
c set constant
|
||||
one = 0.1e+01
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(iopt.lt.0 .or. iopt.gt.1) go to 30
|
||||
if(m.lt.4 .or. nest.lt.8) go to 30
|
||||
if(s.lt.0.) go to 30
|
||||
if(maxtr.lt.1 .or. maxbin.lt.1) go to 30
|
||||
lwest = 8*nest+m*4+maxbin*(1+nest+maxbin)
|
||||
kwest = 4*maxtr+2*(maxbin+1)
|
||||
if(lwrk.lt.lwest .or. kwrk.lt.kwest) go to 30
|
||||
if(iopt.gt.0) go to 20
|
||||
if(w(1).le.0.) go to 30
|
||||
if(v(1).gt.0.) v(1) = one
|
||||
if(v(1).lt.0.) v(1) = -one
|
||||
do 10 i=2,m
|
||||
if(x(i-1).ge.x(i) .or. w(i).le.0.) go to 30
|
||||
if(v(i).gt.0.) v(i) = one
|
||||
if(v(i).lt.0.) v(i) = -one
|
||||
10 continue
|
||||
20 ier = 0
|
||||
c we partition the working space and determine the spline approximation
|
||||
ie = 1
|
||||
iw = ie+nest
|
||||
lww = lwrk-nest
|
||||
call fpcoco(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,n,t,c,sq,sx,
|
||||
* bind,wrk(ie),wrk(iw),lww,iwrk,kwrk,ier)
|
||||
30 return
|
||||
end
|
||||
371
mcc/bsplines/concur.f
Normal file
371
mcc/bsplines/concur.f
Normal file
@@ -0,0 +1,371 @@
|
||||
recursive subroutine concur(iopt,idim,m,u,mx,x,xx,w,ib,db,nb,
|
||||
* ie,de,ne,k,s,nest,n,t,nc,c,np,cp,fp,wrk,lwrk,iwrk,ier)
|
||||
implicit none
|
||||
c given the ordered set of m points x(i) in the idim-dimensional space
|
||||
c and given also a corresponding set of strictly increasing values u(i)
|
||||
c and the set of positive numbers w(i),i=1,2,...,m, subroutine concur
|
||||
c determines a smooth approximating spline curve s(u), i.e.
|
||||
c x1 = s1(u)
|
||||
c x2 = s2(u) ub = u(1) <= u <= u(m) = ue
|
||||
c .........
|
||||
c xidim = sidim(u)
|
||||
c with sj(u),j=1,2,...,idim spline functions of odd degree k with
|
||||
c common knots t(j),j=1,2,...,n.
|
||||
c in addition these splines will satisfy the following boundary
|
||||
c constraints (l)
|
||||
c if ib > 0 : sj (u(1)) = db(idim*l+j) ,l=0,1,...,ib-1
|
||||
c and (l)
|
||||
c if ie > 0 : sj (u(m)) = de(idim*l+j) ,l=0,1,...,ie-1.
|
||||
c if iopt=-1 concur calculates the weighted least-squares spline curve
|
||||
c according to a given set of knots.
|
||||
c if iopt>=0 the number of knots of the splines sj(u) and the position
|
||||
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||
c ness of s(u) is then achieved by minimalizing the discontinuity
|
||||
c jumps of the k-th derivative of s(u) at the knots t(j),j=k+2,k+3,...,
|
||||
c n-k-1. the amount of smoothness is determined by the condition that
|
||||
c f(p)=sum((w(i)*dist(x(i),s(u(i))))**2) be <= s, with s a given non-
|
||||
c negative constant, called the smoothing factor.
|
||||
c the fit s(u) is given in the b-spline representation and can be
|
||||
c evaluated by means of subroutine curev.
|
||||
c
|
||||
c calling sequence:
|
||||
c call concur(iopt,idim,m,u,mx,x,xx,w,ib,db,nb,ie,de,ne,k,s,nest,n,
|
||||
c * t,nc,c,np,cp,fp,wrk,lwrk,iwrk,ier)
|
||||
c
|
||||
c parameters:
|
||||
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||
c least-squares spline curve (iopt=-1) or a smoothing spline
|
||||
c curve (iopt=0 or 1) must be determined.if iopt=0 the routine
|
||||
c will start with an initial set of knots t(i)=ub,t(i+k+1)=ue,
|
||||
c i=1,2,...,k+1. if iopt=1 the routine will continue with the
|
||||
c knots found at the last call of the routine.
|
||||
c attention: a call with iopt=1 must always be immediately
|
||||
c preceded by another call with iopt=1 or iopt=0.
|
||||
c unchanged on exit.
|
||||
c idim : integer. on entry idim must specify the dimension of the
|
||||
c curve. 0 < idim < 11.
|
||||
c unchanged on exit.
|
||||
c m : integer. on entry m must specify the number of data points.
|
||||
c m > k-max(ib-1,0)-max(ie-1,0). unchanged on exit.
|
||||
c u : real array of dimension at least (m). before entry,
|
||||
c u(i) must be set to the i-th value of the parameter variable
|
||||
c u for i=1,2,...,m. these values must be supplied in
|
||||
c strictly ascending order and will be unchanged on exit.
|
||||
c mx : integer. on entry mx must specify the actual dimension of
|
||||
c the arrays x and xx as declared in the calling (sub)program
|
||||
c mx must not be too small (see x). unchanged on exit.
|
||||
c x : real array of dimension at least idim*m.
|
||||
c before entry, x(idim*(i-1)+j) must contain the j-th coord-
|
||||
c inate of the i-th data point for i=1,2,...,m and j=1,2,...,
|
||||
c idim. unchanged on exit.
|
||||
c xx : real array of dimension at least idim*m.
|
||||
c used as working space. on exit xx contains the coordinates
|
||||
c of the data points to which a spline curve with zero deriv-
|
||||
c ative constraints has been determined.
|
||||
c if the computation mode iopt =1 is used xx should be left
|
||||
c unchanged between calls.
|
||||
c w : real array of dimension at least (m). before entry, w(i)
|
||||
c must be set to the i-th value in the set of weights. the
|
||||
c w(i) must be strictly positive. unchanged on exit.
|
||||
c see also further comments.
|
||||
c ib : integer. on entry ib must specify the number of derivative
|
||||
c constraints for the curve at the begin point. 0<=ib<=(k+1)/2
|
||||
c unchanged on exit.
|
||||
c db : real array of dimension nb. before entry db(idim*l+j) must
|
||||
c contain the l-th order derivative of sj(u) at u=u(1) for
|
||||
c j=1,2,...,idim and l=0,1,...,ib-1 (if ib>0).
|
||||
c unchanged on exit.
|
||||
c nb : integer, specifying the dimension of db. nb>=max(1,idim*ib)
|
||||
c unchanged on exit.
|
||||
c ie : integer. on entry ie must specify the number of derivative
|
||||
c constraints for the curve at the end point. 0<=ie<=(k+1)/2
|
||||
c unchanged on exit.
|
||||
c de : real array of dimension ne. before entry de(idim*l+j) must
|
||||
c contain the l-th order derivative of sj(u) at u=u(m) for
|
||||
c j=1,2,...,idim and l=0,1,...,ie-1 (if ie>0).
|
||||
c unchanged on exit.
|
||||
c ne : integer, specifying the dimension of de. ne>=max(1,idim*ie)
|
||||
c unchanged on exit.
|
||||
c k : integer. on entry k must specify the degree of the splines.
|
||||
c k=1,3 or 5.
|
||||
c unchanged on exit.
|
||||
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||
c factor. s >=0. unchanged on exit.
|
||||
c for advice on the choice of s see further comments.
|
||||
c nest : integer. on entry nest must contain an over-estimate of the
|
||||
c total number of knots of the splines returned, to indicate
|
||||
c the storage space available to the routine. nest >=2*k+2.
|
||||
c in most practical situation nest=m/2 will be sufficient.
|
||||
c always large enough is nest=m+k+1+max(0,ib-1)+max(0,ie-1),
|
||||
c the number of knots needed for interpolation (s=0).
|
||||
c unchanged on exit.
|
||||
c n : integer.
|
||||
c unless ier = 10 (in case iopt >=0), n will contain the
|
||||
c total number of knots of the smoothing spline curve returned
|
||||
c if the computation mode iopt=1 is used this value of n
|
||||
c should be left unchanged between subsequent calls.
|
||||
c in case iopt=-1, the value of n must be specified on entry.
|
||||
c t : real array of dimension at least (nest).
|
||||
c on successful exit, this array will contain the knots of the
|
||||
c spline curve,i.e. the position of the interior knots t(k+2),
|
||||
c t(k+3),..,t(n-k-1) as well as the position of the additional
|
||||
c t(1)=t(2)=...=t(k+1)=ub and t(n-k)=...=t(n)=ue needed for
|
||||
c the b-spline representation.
|
||||
c if the computation mode iopt=1 is used, the values of t(1),
|
||||
c t(2),...,t(n) should be left unchanged between subsequent
|
||||
c calls. if the computation mode iopt=-1 is used, the values
|
||||
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||
c entry. see also the restrictions (ier=10).
|
||||
c nc : integer. on entry nc must specify the actual dimension of
|
||||
c the array c as declared in the calling (sub)program. nc
|
||||
c must not be too small (see c). unchanged on exit.
|
||||
c c : real array of dimension at least (nest*idim).
|
||||
c on successful exit, this array will contain the coefficients
|
||||
c in the b-spline representation of the spline curve s(u),i.e.
|
||||
c the b-spline coefficients of the spline sj(u) will be given
|
||||
c in c(n*(j-1)+i),i=1,2,...,n-k-1 for j=1,2,...,idim.
|
||||
c cp : real array of dimension at least 2*(k+1)*idim.
|
||||
c on exit cp will contain the b-spline coefficients of a
|
||||
c polynomial curve which satisfies the boundary constraints.
|
||||
c if the computation mode iopt =1 is used cp should be left
|
||||
c unchanged between calls.
|
||||
c np : integer. on entry np must specify the actual dimension of
|
||||
c the array cp as declared in the calling (sub)program. np
|
||||
c must not be too small (see cp). unchanged on exit.
|
||||
c fp : real. unless ier = 10, fp contains the weighted sum of
|
||||
c squared residuals of the spline curve returned.
|
||||
c wrk : real array of dimension at least m*(k+1)+nest*(6+idim+3*k).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||
c the array wrk as declared in the calling (sub)program. lwrk
|
||||
c must not be too small (see wrk). unchanged on exit.
|
||||
c iwrk : integer array of dimension at least (nest).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c ier : integer. unless the routine detects an error, ier contains a
|
||||
c non-positive value on exit, i.e.
|
||||
c ier=0 : normal return. the curve returned has a residual sum of
|
||||
c squares fp such that abs(fp-s)/s <= tol with tol a relat-
|
||||
c ive tolerance set to 0.001 by the program.
|
||||
c ier=-1 : normal return. the curve returned is an interpolating
|
||||
c spline curve, satisfying the constraints (fp=0).
|
||||
c ier=-2 : normal return. the curve returned is the weighted least-
|
||||
c squares polynomial curve of degree k, satisfying the
|
||||
c constraints. in this extreme case fp gives the upper
|
||||
c bound fp0 for the smoothing factor s.
|
||||
c ier=1 : error. the required storage space exceeds the available
|
||||
c storage space, as specified by the parameter nest.
|
||||
c probably causes : nest too small. if nest is already
|
||||
c large (say nest > m/2), it may also indicate that s is
|
||||
c too small
|
||||
c the approximation returned is the least-squares spline
|
||||
c curve according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||
c the parameter fp gives the corresponding weighted sum of
|
||||
c squared residuals (fp>s).
|
||||
c ier=2 : error. a theoretically impossible result was found during
|
||||
c the iteration process for finding a smoothing spline curve
|
||||
c with fp = s. probably causes : s too small.
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||
c by the program) allowed for finding a smoothing curve
|
||||
c with fp=s has been reached. probably causes : s too small
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=10 : error. on entry, the input data are controlled on validity
|
||||
c the following restrictions must be satisfied.
|
||||
c -1<=iopt<=1, k = 1,3 or 5, m>k-max(0,ib-1)-max(0,ie-1),
|
||||
c nest>=2k+2, 0<idim<=10, lwrk>=(k+1)*m+nest*(6+idim+3*k),
|
||||
c nc >=nest*idim ,u(1)<u(2)<...<u(m),w(i)>0 i=1,2,...,m,
|
||||
c mx>=idim*m,0<=ib<=(k+1)/2,0<=ie<=(k+1)/2,nb>=1,ne>=1,
|
||||
c nb>=ib*idim,ne>=ib*idim,np>=2*(k+1)*idim,
|
||||
c if iopt=-1:2*k+2<=n<=min(nest,mmax) with mmax = m+k+1+
|
||||
c max(0,ib-1)+max(0,ie-1)
|
||||
c u(1)<t(k+2)<t(k+3)<...<t(n-k-1)<u(m)
|
||||
c the schoenberg-whitney conditions, i.e. there
|
||||
c must be a subset of data points uu(j) such that
|
||||
c t(j) < uu(j) < t(j+k+1), j=1+max(0,ib-1),...
|
||||
c ,n+k-1-max(0,ie-1)
|
||||
c if iopt>=0: s>=0
|
||||
c if s=0 : nest >=mmax (see above)
|
||||
c if one of these conditions is found to be violated,control
|
||||
c is immediately repassed to the calling program. in that
|
||||
c case there is no approximation returned.
|
||||
c
|
||||
c further comments:
|
||||
c by means of the parameter s, the user can control the tradeoff
|
||||
c between closeness of fit and smoothness of fit of the approximation.
|
||||
c if s is too large, the curve will be too smooth and signal will be
|
||||
c lost ; if s is too small the curve will pick up too much noise. in
|
||||
c the extreme cases the program will return an interpolating curve if
|
||||
c s=0 and the least-squares polynomial curve of degree k if s is
|
||||
c very large. between these extremes, a properly chosen s will result
|
||||
c in a good compromise between closeness of fit and smoothness of fit.
|
||||
c to decide whether an approximation, corresponding to a certain s is
|
||||
c satisfactory the user is highly recommended to inspect the fits
|
||||
c graphically.
|
||||
c recommended values for s depend on the weights w(i). if these are
|
||||
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||
c x(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||
c sqrt(2*m)). if nothing is known about the statistical error in x(i)
|
||||
c each w(i) can be set equal to one and s determined by trial and
|
||||
c error, taking account of the comments above. the best is then to
|
||||
c start with a very large value of s ( to determine the least-squares
|
||||
c polynomial curve and the upper bound fp0 for s) and then to
|
||||
c progressively decrease the value of s ( say by a factor 10 in the
|
||||
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||
c approximating curve shows more detail) to obtain closer fits.
|
||||
c to economize the search for a good s-value the program provides with
|
||||
c different modes of computation. at the first call of the routine, or
|
||||
c whenever he wants to restart with the initial set of knots the user
|
||||
c must set iopt=0.
|
||||
c if iopt=1 the program will continue with the set of knots found at
|
||||
c the last call of the routine. this will save a lot of computation
|
||||
c time if concur is called repeatedly for different values of s.
|
||||
c the number of knots of the spline returned and their location will
|
||||
c depend on the value of s and on the complexity of the shape of the
|
||||
c curve underlying the data. but, if the computation mode iopt=1 is
|
||||
c used, the knots returned may also depend on the s-values at previous
|
||||
c calls (if these were smaller). therefore, if after a number of
|
||||
c trials with different s-values and iopt=1, the user can finally
|
||||
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||
c concur once more with the selected value for s but now with iopt=0.
|
||||
c indeed, concur may then return an approximation of the same quality
|
||||
c of fit but with fewer knots and therefore better if data reduction
|
||||
c is also an important objective for the user.
|
||||
c
|
||||
c the form of the approximating curve can strongly be affected by
|
||||
c the choice of the parameter values u(i). if there is no physical
|
||||
c reason for choosing a particular parameter u, often good results
|
||||
c will be obtained with the choice
|
||||
c v(1)=0, v(i)=v(i-1)+q(i), i=2,...,m, u(i)=v(i)/v(m), i=1,..,m
|
||||
c where
|
||||
c q(i)= sqrt(sum j=1,idim (xj(i)-xj(i-1))**2 )
|
||||
c other possibilities for q(i) are
|
||||
c q(i)= sum j=1,idim (xj(i)-xj(i-1))**2
|
||||
c q(i)= sum j=1,idim abs(xj(i)-xj(i-1))
|
||||
c q(i)= max j=1,idim abs(xj(i)-xj(i-1))
|
||||
c q(i)= 1
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpback,fpbspl,fpched,fpcons,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||
c curev,fppocu,fpadpo,fpinst
|
||||
c
|
||||
c references:
|
||||
c dierckx p. : algorithms for smoothing data with periodic and
|
||||
c parametric splines, computer graphics and image
|
||||
c processing 20 (1982) 171-184.
|
||||
c dierckx p. : algorithms for smoothing data with periodic and param-
|
||||
c etric splines, report tw55, dept. computer science,
|
||||
c k.u.leuven, 1981.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author:
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u. leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c creation date : may 1979
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,fp
|
||||
integer iopt,idim,m,mx,ib,nb,ie,ne,k,nest,n,nc,np,lwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 u(m),x(mx),xx(mx),db(nb),de(ne),w(m),t(nest),c(nc),wrk(lwrk
|
||||
*)
|
||||
real*8 cp(np)
|
||||
integer iwrk(nest)
|
||||
c ..local scalars..
|
||||
real*8 tol
|
||||
integer i,ib1,ie1,ja,jb,jfp,jg,jq,jz,j,k1,k2,lwest,maxit,nmin,
|
||||
* ncc,kk,mmin,nmax,mxx
|
||||
c ..function references
|
||||
integer max0
|
||||
c ..
|
||||
c we set up the parameters tol and maxit
|
||||
maxit = 20
|
||||
tol = 0.1e-02
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(iopt.lt.(-1) .or. iopt.gt.1) go to 90
|
||||
if(idim.le.0 .or. idim.gt.10) go to 90
|
||||
if(k.le.0 .or. k.gt.5) go to 90
|
||||
k1 = k+1
|
||||
kk = k1/2
|
||||
if(kk*2.ne.k1) go to 90
|
||||
k2 = k1+1
|
||||
if(ib.lt.0 .or. ib.gt.kk) go to 90
|
||||
if(ie.lt.0 .or. ie.gt.kk) go to 90
|
||||
nmin = 2*k1
|
||||
ib1 = max0(0,ib-1)
|
||||
ie1 = max0(0,ie-1)
|
||||
mmin = k1-ib1-ie1
|
||||
if(m.lt.mmin .or. nest.lt.nmin) go to 90
|
||||
if(nb.lt.(idim*ib) .or. ne.lt.(idim*ie)) go to 90
|
||||
if(np.lt.(2*k1*idim)) go to 90
|
||||
mxx = m*idim
|
||||
ncc = nest*idim
|
||||
if(mx.lt.mxx .or. nc.lt.ncc) go to 90
|
||||
lwest = m*k1+nest*(6+idim+3*k)
|
||||
if(lwrk.lt.lwest) go to 90
|
||||
if(w(1).le.0.) go to 90
|
||||
do 10 i=2,m
|
||||
if(u(i-1).ge.u(i) .or. w(i).le.0.) go to 90
|
||||
10 continue
|
||||
if(iopt.ge.0) go to 30
|
||||
if(n.lt.nmin .or. n.gt.nest) go to 90
|
||||
j = n
|
||||
do 20 i=1,k1
|
||||
t(i) = u(1)
|
||||
t(j) = u(m)
|
||||
j = j-1
|
||||
20 continue
|
||||
call fpched(u,m,t,n,k,ib,ie,ier)
|
||||
if (ier.eq.0) go to 40
|
||||
go to 90
|
||||
30 if(s.lt.0.) go to 90
|
||||
nmax = m+k1+ib1+ie1
|
||||
if(s.eq.0. .and. nest.lt.nmax) go to 90
|
||||
ier = 0
|
||||
if(iopt.gt.0) go to 70
|
||||
c we determine a polynomial curve satisfying the boundary constraints.
|
||||
40 call fppocu(idim,k,u(1),u(m),ib,db,nb,ie,de,ne,cp,np)
|
||||
c we generate new data points which will be approximated by a spline
|
||||
c with zero derivative constraints.
|
||||
j = nmin
|
||||
do 50 i=1,k1
|
||||
wrk(i) = u(1)
|
||||
wrk(j) = u(m)
|
||||
j = j-1
|
||||
50 continue
|
||||
c evaluate the polynomial curve
|
||||
call curev(idim,wrk,nmin,cp,np,k,u,m,xx,mxx,ier)
|
||||
c subtract from the old data, the values of the polynomial curve
|
||||
do 60 i=1,mxx
|
||||
xx(i) = x(i)-xx(i)
|
||||
60 continue
|
||||
c we partition the working space and determine the spline curve.
|
||||
70 jfp = 1
|
||||
jz = jfp+nest
|
||||
ja = jz+ncc
|
||||
jb = ja+nest*k1
|
||||
jg = jb+nest*k2
|
||||
jq = jg+nest*k2
|
||||
call fpcons(iopt,idim,m,u,mxx,xx,w,ib,ie,k,s,nest,tol,maxit,k1,
|
||||
* k2,n,t,ncc,c,fp,wrk(jfp),wrk(jz),wrk(ja),wrk(jb),wrk(jg),wrk(jq),
|
||||
*
|
||||
* iwrk,ier)
|
||||
c add the polynomial curve to the calculated spline.
|
||||
call fpadpo(idim,t,n,c,ncc,k,cp,np,wrk(jz),wrk(ja),wrk(jb))
|
||||
90 return
|
||||
end
|
||||
92
mcc/bsplines/cualde.f
Normal file
92
mcc/bsplines/cualde.f
Normal file
@@ -0,0 +1,92 @@
|
||||
recursive subroutine cualde(idim,t,n,c,nc,k1,u,d,nd,ier)
|
||||
implicit none
|
||||
c subroutine cualde evaluates at the point u all the derivatives
|
||||
c (l)
|
||||
c d(idim*l+j) = sj (u) ,l=0,1,...,k, j=1,2,...,idim
|
||||
c of a spline curve s(u) of order k1 (degree k=k1-1) and dimension idim
|
||||
c given in its b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c call cualde(idim,t,n,c,nc,k1,u,d,nd,ier)
|
||||
c
|
||||
c input parameters:
|
||||
c idim : integer, giving the dimension of the spline curve.
|
||||
c t : array,length n, which contains the position of the knots.
|
||||
c n : integer, giving the total number of knots of s(u).
|
||||
c c : array,length nc, which contains the b-spline coefficients.
|
||||
c nc : integer, giving the total number of coefficients of s(u).
|
||||
c k1 : integer, giving the order of s(u) (order=degree+1).
|
||||
c u : real, which contains the point where the derivatives must
|
||||
c be evaluated.
|
||||
c nd : integer, giving the dimension of the array d. nd >= k1*idim
|
||||
c
|
||||
c output parameters:
|
||||
c d : array,length nd,giving the different curve derivatives.
|
||||
c d(idim*l+j) will contain the j-th coordinate of the l-th
|
||||
c derivative of the curve at the point u.
|
||||
c ier : error flag
|
||||
c ier = 0 : normal return
|
||||
c ier =10 : invalid input data (see restrictions)
|
||||
c
|
||||
c restrictions:
|
||||
c nd >= k1*idim
|
||||
c t(k1) <= u <= t(n-k1+1)
|
||||
c
|
||||
c further comments:
|
||||
c if u coincides with a knot, right derivatives are computed
|
||||
c ( left derivatives if u = t(n-k1+1) ).
|
||||
c
|
||||
c other subroutines required: fpader.
|
||||
c
|
||||
c references :
|
||||
c de boor c : on calculating with b-splines, j. approximation theory
|
||||
c 6 (1972) 50-62.
|
||||
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||
c applics 10 (1972) 134-149.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer idim,n,nc,k1,nd,ier
|
||||
real*8 u
|
||||
c ..array arguments..
|
||||
real*8 t(n),c(nc),d(nd)
|
||||
c ..local scalars..
|
||||
integer i,j,kk,l,m,nk1
|
||||
c ..local array..
|
||||
real*8 h(6)
|
||||
c ..
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(nd.lt.(k1*idim)) go to 500
|
||||
nk1 = n-k1
|
||||
if(u.lt.t(k1) .or. u.gt.t(nk1+1)) go to 500
|
||||
c search for knot interval t(l) <= u < t(l+1)
|
||||
l = k1
|
||||
100 if(u.lt.t(l+1) .or. l.eq.nk1) go to 200
|
||||
l = l+1
|
||||
go to 100
|
||||
200 if(t(l).ge.t(l+1)) go to 500
|
||||
ier = 0
|
||||
c calculate the derivatives.
|
||||
j = 1
|
||||
do 400 i=1,idim
|
||||
call fpader(t,n,c(j),k1,u,l,h)
|
||||
m = i
|
||||
do 300 kk=1,k1
|
||||
d(m) = h(kk)
|
||||
m = m+idim
|
||||
300 continue
|
||||
j = j+n
|
||||
400 continue
|
||||
500 return
|
||||
end
|
||||
111
mcc/bsplines/curev.f
Normal file
111
mcc/bsplines/curev.f
Normal file
@@ -0,0 +1,111 @@
|
||||
recursive subroutine curev(idim,t,n,c,nc,k,u,m,x,mx,ier)
|
||||
implicit none
|
||||
c subroutine curev evaluates in a number of points u(i),i=1,2,...,m
|
||||
c a spline curve s(u) of degree k and dimension idim, given in its
|
||||
c b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c call curev(idim,t,n,c,nc,k,u,m,x,mx,ier)
|
||||
c
|
||||
c input parameters:
|
||||
c idim : integer, giving the dimension of the spline curve.
|
||||
c t : array,length n, which contains the position of the knots.
|
||||
c n : integer, giving the total number of knots of s(u).
|
||||
c c : array,length nc, which contains the b-spline coefficients.
|
||||
c nc : integer, giving the total number of coefficients of s(u).
|
||||
c k : integer, giving the degree of s(u).
|
||||
c u : array,length m, which contains the points where s(u) must
|
||||
c be evaluated.
|
||||
c m : integer, giving the number of points where s(u) must be
|
||||
c evaluated.
|
||||
c mx : integer, giving the dimension of the array x. mx >= m*idim
|
||||
c
|
||||
c output parameters:
|
||||
c x : array,length mx,giving the value of s(u) at the different
|
||||
c points. x(idim*(i-1)+j) will contain the j-th coordinate
|
||||
c of the i-th point on the curve.
|
||||
c ier : error flag
|
||||
c ier = 0 : normal return
|
||||
c ier =10 : invalid input data (see restrictions)
|
||||
c
|
||||
c restrictions:
|
||||
c m >= 1
|
||||
c mx >= m*idim
|
||||
c t(k+1) <= u(i) <= u(i+1) <= t(n-k) , i=1,2,...,m-1.
|
||||
c
|
||||
c other subroutines required: fpbspl.
|
||||
c
|
||||
c references :
|
||||
c de boor c : on calculating with b-splines, j. approximation theory
|
||||
c 6 (1972) 50-62.
|
||||
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||
c applics 10 (1972) 134-149.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer idim,n,nc,k,m,mx,ier
|
||||
c ..array arguments..
|
||||
real*8 t(n),c(nc),u(m),x(mx)
|
||||
c ..local scalars..
|
||||
integer i,j,jj,j1,k1,l,ll,l1,mm,nk1
|
||||
real*8 arg,sp,tb,te
|
||||
c ..local array..
|
||||
real*8 h(6)
|
||||
c ..
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if (m.lt.1) go to 100
|
||||
if (m.eq.1) go to 30
|
||||
go to 10
|
||||
10 do 20 i=2,m
|
||||
if(u(i).lt.u(i-1)) go to 100
|
||||
20 continue
|
||||
30 if(mx.lt.(m*idim)) go to 100
|
||||
ier = 0
|
||||
c fetch tb and te, the boundaries of the approximation interval.
|
||||
k1 = k+1
|
||||
nk1 = n-k1
|
||||
tb = t(k1)
|
||||
te = t(nk1+1)
|
||||
l = k1
|
||||
l1 = l+1
|
||||
c main loop for the different points.
|
||||
mm = 0
|
||||
do 80 i=1,m
|
||||
c fetch a new u-value arg.
|
||||
arg = u(i)
|
||||
if(arg.lt.tb) arg = tb
|
||||
if(arg.gt.te) arg = te
|
||||
c search for knot interval t(l) <= arg < t(l+1)
|
||||
40 if(arg.lt.t(l1) .or. l.eq.nk1) go to 50
|
||||
l = l1
|
||||
l1 = l+1
|
||||
go to 40
|
||||
c evaluate the non-zero b-splines at arg.
|
||||
50 call fpbspl(t,n,k,arg,l,h)
|
||||
c find the value of s(u) at u=arg.
|
||||
ll = l-k1
|
||||
do 70 j1=1,idim
|
||||
jj = ll
|
||||
sp = 0.
|
||||
do 60 j=1,k1
|
||||
jj = jj+1
|
||||
sp = sp+c(jj)*h(j)
|
||||
60 continue
|
||||
mm = mm+1
|
||||
x(mm) = sp
|
||||
ll = ll+n
|
||||
70 continue
|
||||
80 continue
|
||||
100 return
|
||||
end
|
||||
261
mcc/bsplines/curfit.f
Normal file
261
mcc/bsplines/curfit.f
Normal file
@@ -0,0 +1,261 @@
|
||||
recursive subroutine curfit(iopt,m,x,y,w,xb,xe,k,s,nest,n,
|
||||
* t,c,fp,wrk,lwrk,iwrk,ier)
|
||||
implicit none
|
||||
c given the set of data points (x(i),y(i)) and the set of positive
|
||||
c numbers w(i),i=1,2,...,m,subroutine curfit determines a smooth spline
|
||||
c approximation of degree k on the interval xb <= x <= xe.
|
||||
c if iopt=-1 curfit calculates the weighted least-squares spline
|
||||
c according to a given set of knots.
|
||||
c if iopt>=0 the number of knots of the spline s(x) and the position
|
||||
c t(j),j=1,2,...,n is chosen automatically by the routine. the smooth-
|
||||
c ness of s(x) is then achieved by minimalizing the discontinuity
|
||||
c jumps of the k-th derivative of s(x) at the knots t(j),j=k+2,k+3,...,
|
||||
c n-k-1. the amount of smoothness is determined by the condition that
|
||||
c f(p)=sum((w(i)*(y(i)-s(x(i))))**2) be <= s, with s a given non-
|
||||
c negative constant, called the smoothing factor.
|
||||
c the fit s(x) is given in the b-spline representation (b-spline coef-
|
||||
c ficients c(j),j=1,2,...,n-k-1) and can be evaluated by means of
|
||||
c subroutine splev.
|
||||
c
|
||||
c calling sequence:
|
||||
c call curfit(iopt,m,x,y,w,xb,xe,k,s,nest,n,t,c,fp,wrk,
|
||||
c * lwrk,iwrk,ier)
|
||||
c
|
||||
c parameters:
|
||||
c iopt : integer flag. on entry iopt must specify whether a weighted
|
||||
c least-squares spline (iopt=-1) or a smoothing spline (iopt=
|
||||
c 0 or 1) must be determined. if iopt=0 the routine will start
|
||||
c with an initial set of knots t(i)=xb, t(i+k+1)=xe, i=1,2,...
|
||||
c k+1. if iopt=1 the routine will continue with the knots
|
||||
c found at the last call of the routine.
|
||||
c attention: a call with iopt=1 must always be immediately
|
||||
c preceded by another call with iopt=1 or iopt=0.
|
||||
c unchanged on exit.
|
||||
c m : integer. on entry m must specify the number of data points.
|
||||
c m > k. unchanged on exit.
|
||||
c x : real array of dimension at least (m). before entry, x(i)
|
||||
c must be set to the i-th value of the independent variable x,
|
||||
c for i=1,2,...,m. these values must be supplied in strictly
|
||||
c ascending order. unchanged on exit.
|
||||
c y : real array of dimension at least (m). before entry, y(i)
|
||||
c must be set to the i-th value of the dependent variable y,
|
||||
c for i=1,2,...,m. unchanged on exit.
|
||||
c w : real array of dimension at least (m). before entry, w(i)
|
||||
c must be set to the i-th value in the set of weights. the
|
||||
c w(i) must be strictly positive. unchanged on exit.
|
||||
c see also further comments.
|
||||
c xb,xe : real values. on entry xb and xe must specify the boundaries
|
||||
c of the approximation interval. xb<=x(1), xe>=x(m).
|
||||
c unchanged on exit.
|
||||
c k : integer. on entry k must specify the degree of the spline.
|
||||
c 1<=k<=5. it is recommended to use cubic splines (k=3).
|
||||
c the user is strongly dissuaded from choosing k even,together
|
||||
c with a small s-value. unchanged on exit.
|
||||
c s : real.on entry (in case iopt>=0) s must specify the smoothing
|
||||
c factor. s >=0. unchanged on exit.
|
||||
c for advice on the choice of s see further comments.
|
||||
c nest : integer. on entry nest must contain an over-estimate of the
|
||||
c total number of knots of the spline returned, to indicate
|
||||
c the storage space available to the routine. nest >=2*k+2.
|
||||
c in most practical situation nest=m/2 will be sufficient.
|
||||
c always large enough is nest=m+k+1, the number of knots
|
||||
c needed for interpolation (s=0). unchanged on exit.
|
||||
c n : integer.
|
||||
c unless ier =10 (in case iopt >=0), n will contain the
|
||||
c total number of knots of the spline approximation returned.
|
||||
c if the computation mode iopt=1 is used this value of n
|
||||
c should be left unchanged between subsequent calls.
|
||||
c in case iopt=-1, the value of n must be specified on entry.
|
||||
c t : real array of dimension at least (nest).
|
||||
c on successful exit, this array will contain the knots of the
|
||||
c spline,i.e. the position of the interior knots t(k+2),t(k+3)
|
||||
c ...,t(n-k-1) as well as the position of the additional knots
|
||||
c t(1)=t(2)=...=t(k+1)=xb and t(n-k)=...=t(n)=xe needed for
|
||||
c the b-spline representation.
|
||||
c if the computation mode iopt=1 is used, the values of t(1),
|
||||
c t(2),...,t(n) should be left unchanged between subsequent
|
||||
c calls. if the computation mode iopt=-1 is used, the values
|
||||
c t(k+2),...,t(n-k-1) must be supplied by the user, before
|
||||
c entry. see also the restrictions (ier=10).
|
||||
c c : real array of dimension at least (nest).
|
||||
c on successful exit, this array will contain the coefficients
|
||||
c c(1),c(2),..,c(n-k-1) in the b-spline representation of s(x)
|
||||
c fp : real. unless ier=10, fp contains the weighted sum of
|
||||
c squared residuals of the spline approximation returned.
|
||||
c wrk : real array of dimension at least (m*(k+1)+nest*(7+3*k)).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used, the values wrk(1),...,wrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c lwrk : integer. on entry,lwrk must specify the actual dimension of
|
||||
c the array wrk as declared in the calling (sub)program.lwrk
|
||||
c must not be too small (see wrk). unchanged on exit.
|
||||
c iwrk : integer array of dimension at least (nest).
|
||||
c used as working space. if the computation mode iopt=1 is
|
||||
c used,the values iwrk(1),...,iwrk(n) should be left unchanged
|
||||
c between subsequent calls.
|
||||
c ier : integer. unless the routine detects an error, ier contains a
|
||||
c non-positive value on exit, i.e.
|
||||
c ier=0 : normal return. the spline returned has a residual sum of
|
||||
c squares fp such that abs(fp-s)/s <= tol with tol a relat-
|
||||
c ive tolerance set to 0.001 by the program.
|
||||
c ier=-1 : normal return. the spline returned is an interpolating
|
||||
c spline (fp=0).
|
||||
c ier=-2 : normal return. the spline returned is the weighted least-
|
||||
c squares polynomial of degree k. in this extreme case fp
|
||||
c gives the upper bound fp0 for the smoothing factor s.
|
||||
c ier=1 : error. the required storage space exceeds the available
|
||||
c storage space, as specified by the parameter nest.
|
||||
c probably causes : nest too small. if nest is already
|
||||
c large (say nest > m/2), it may also indicate that s is
|
||||
c too small
|
||||
c the approximation returned is the weighted least-squares
|
||||
c spline according to the knots t(1),t(2),...,t(n). (n=nest)
|
||||
c the parameter fp gives the corresponding weighted sum of
|
||||
c squared residuals (fp>s).
|
||||
c ier=2 : error. a theoretically impossible result was found during
|
||||
c the iteration process for finding a smoothing spline with
|
||||
c fp = s. probably causes : s too small.
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=3 : error. the maximal number of iterations maxit (set to 20
|
||||
c by the program) allowed for finding a smoothing spline
|
||||
c with fp=s has been reached. probably causes : s too small
|
||||
c there is an approximation returned but the corresponding
|
||||
c weighted sum of squared residuals does not satisfy the
|
||||
c condition abs(fp-s)/s < tol.
|
||||
c ier=10 : error. on entry, the input data are controlled on validity
|
||||
c the following restrictions must be satisfied.
|
||||
c -1<=iopt<=1, 1<=k<=5, m>k, nest>2*k+2, w(i)>0,i=1,2,...,m
|
||||
c xb<=x(1)<x(2)<...<x(m)<=xe, lwrk>=(k+1)*m+nest*(7+3*k)
|
||||
c if iopt=-1: 2*k+2<=n<=min(nest,m+k+1)
|
||||
c xb<t(k+2)<t(k+3)<...<t(n-k-1)<xe
|
||||
c the schoenberg-whitney conditions, i.e. there
|
||||
c must be a subset of data points xx(j) such that
|
||||
c t(j) < xx(j) < t(j+k+1), j=1,2,...,n-k-1
|
||||
c if iopt>=0: s>=0
|
||||
c if s=0 : nest >= m+k+1
|
||||
c if one of these conditions is found to be violated,control
|
||||
c is immediately repassed to the calling program. in that
|
||||
c case there is no approximation returned.
|
||||
c
|
||||
c further comments:
|
||||
c by means of the parameter s, the user can control the tradeoff
|
||||
c between closeness of fit and smoothness of fit of the approximation.
|
||||
c if s is too large, the spline will be too smooth and signal will be
|
||||
c lost ; if s is too small the spline will pick up too much noise. in
|
||||
c the extreme cases the program will return an interpolating spline if
|
||||
c s=0 and the weighted least-squares polynomial of degree k if s is
|
||||
c very large. between these extremes, a properly chosen s will result
|
||||
c in a good compromise between closeness of fit and smoothness of fit.
|
||||
c to decide whether an approximation, corresponding to a certain s is
|
||||
c satisfactory the user is highly recommended to inspect the fits
|
||||
c graphically.
|
||||
c recommended values for s depend on the weights w(i). if these are
|
||||
c taken as 1/d(i) with d(i) an estimate of the standard deviation of
|
||||
c y(i), a good s-value should be found in the range (m-sqrt(2*m),m+
|
||||
c sqrt(2*m)). if nothing is known about the statistical error in y(i)
|
||||
c each w(i) can be set equal to one and s determined by trial and
|
||||
c error, taking account of the comments above. the best is then to
|
||||
c start with a very large value of s ( to determine the least-squares
|
||||
c polynomial and the corresponding upper bound fp0 for s) and then to
|
||||
c progressively decrease the value of s ( say by a factor 10 in the
|
||||
c beginning, i.e. s=fp0/10, fp0/100,...and more carefully as the
|
||||
c approximation shows more detail) to obtain closer fits.
|
||||
c to economize the search for a good s-value the program provides with
|
||||
c different modes of computation. at the first call of the routine, or
|
||||
c whenever he wants to restart with the initial set of knots the user
|
||||
c must set iopt=0.
|
||||
c if iopt=1 the program will continue with the set of knots found at
|
||||
c the last call of the routine. this will save a lot of computation
|
||||
c time if curfit is called repeatedly for different values of s.
|
||||
c the number of knots of the spline returned and their location will
|
||||
c depend on the value of s and on the complexity of the shape of the
|
||||
c function underlying the data. but, if the computation mode iopt=1
|
||||
c is used, the knots returned may also depend on the s-values at
|
||||
c previous calls (if these were smaller). therefore, if after a number
|
||||
c of trials with different s-values and iopt=1, the user can finally
|
||||
c accept a fit as satisfactory, it may be worthwhile for him to call
|
||||
c curfit once more with the selected value for s but now with iopt=0.
|
||||
c indeed, curfit may then return an approximation of the same quality
|
||||
c of fit but with fewer knots and therefore better if data reduction
|
||||
c is also an important objective for the user.
|
||||
c
|
||||
c other subroutines required:
|
||||
c fpback,fpbspl,fpchec,fpcurf,fpdisc,fpgivs,fpknot,fprati,fprota
|
||||
c
|
||||
c references:
|
||||
c dierckx p. : an algorithm for smoothing, differentiation and integ-
|
||||
c ration of experimental data using spline functions,
|
||||
c j.comp.appl.maths 1 (1975) 165-184.
|
||||
c dierckx p. : a fast algorithm for smoothing data on a rectangular
|
||||
c grid while using spline functions, siam j.numer.anal.
|
||||
c 19 (1982) 1286-1304.
|
||||
c dierckx p. : an improved algorithm for curve fitting with spline
|
||||
c functions, report tw54, dept. computer science,k.u.
|
||||
c leuven, 1981.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author:
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u. leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c creation date : may 1979
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 xb,xe,s,fp
|
||||
integer iopt,m,k,nest,n,lwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),t(nest),c(nest),wrk(lwrk)
|
||||
integer iwrk(nest)
|
||||
c ..local scalars..
|
||||
real*8 tol
|
||||
integer i,ia,ib,ifp,ig,iq,iz,j,k1,k2,lwest,maxit,nmin
|
||||
c ..
|
||||
c we set up the parameters tol and maxit
|
||||
maxit = 20
|
||||
tol = 0.1d-02
|
||||
c before starting computations a data check is made. if the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(k.le.0 .or. k.gt.5) go to 50
|
||||
k1 = k+1
|
||||
k2 = k1+1
|
||||
if(iopt.lt.(-1) .or. iopt.gt.1) go to 50
|
||||
nmin = 2*k1
|
||||
if(m.lt.k1 .or. nest.lt.nmin) go to 50
|
||||
lwest = m*k1+nest*(7+3*k)
|
||||
if(lwrk.lt.lwest) go to 50
|
||||
if(xb.gt.x(1) .or. xe.lt.x(m)) go to 50
|
||||
do 10 i=2,m
|
||||
if(x(i-1).gt.x(i)) go to 50
|
||||
10 continue
|
||||
if(iopt.ge.0) go to 30
|
||||
if(n.lt.nmin .or. n.gt.nest) go to 50
|
||||
j = n
|
||||
do 20 i=1,k1
|
||||
t(i) = xb
|
||||
t(j) = xe
|
||||
j = j-1
|
||||
20 continue
|
||||
call fpchec(x,m,t,n,k,ier)
|
||||
if (ier.eq.0) go to 40
|
||||
go to 50
|
||||
30 if(s.lt.0.) go to 50
|
||||
if(s.eq.0. .and. nest.lt.(m+k1)) go to 50
|
||||
c we partition the working space and determine the spline approximation.
|
||||
40 ifp = 1
|
||||
iz = ifp+nest
|
||||
ia = iz+nest
|
||||
ib = ia+nest*k1
|
||||
ig = ib+nest*k2
|
||||
iq = ig+nest*k2
|
||||
call fpcurf(iopt,x,y,w,m,xb,xe,k,s,nest,tol,maxit,k1,k2,n,t,c,fp,
|
||||
* wrk(ifp),wrk(iz),wrk(ia),wrk(ib),wrk(ig),wrk(iq),iwrk,ier)
|
||||
50 return
|
||||
end
|
||||
91
mcc/bsplines/dblint.f
Normal file
91
mcc/bsplines/dblint.f
Normal file
@@ -0,0 +1,91 @@
|
||||
recursive function dblint(tx,nx,ty,ny,c,kx,ky,xb,xe,yb,
|
||||
* ye,wrk) result(dblint_res)
|
||||
implicit none
|
||||
real*8 :: dblint_res
|
||||
c function dblint calculates the double integral
|
||||
c / xe / ye
|
||||
c | | s(x,y) dx dy
|
||||
c xb / yb /
|
||||
c with s(x,y) a bivariate spline of degrees kx and ky, given in the
|
||||
c b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c aint = dblint(tx,nx,ty,ny,c,kx,ky,xb,xe,yb,ye,wrk)
|
||||
c
|
||||
c input parameters:
|
||||
c tx : real array, length nx, which contains the position of the
|
||||
c knots in the x-direction.
|
||||
c nx : integer, giving the total number of knots in the x-direction
|
||||
c ty : real array, length ny, which contains the position of the
|
||||
c knots in the y-direction.
|
||||
c ny : integer, giving the total number of knots in the y-direction
|
||||
c c : real array, length (nx-kx-1)*(ny-ky-1), which contains the
|
||||
c b-spline coefficients.
|
||||
c kx,ky : integer values, giving the degrees of the spline.
|
||||
c xb,xe : real values, containing the boundaries of the integration
|
||||
c yb,ye domain. s(x,y) is considered to be identically zero out-
|
||||
c side the rectangle (tx(kx+1),tx(nx-kx))*(ty(ky+1),ty(ny-ky))
|
||||
c
|
||||
c output parameters:
|
||||
c aint : real , containing the double integral of s(x,y).
|
||||
c wrk : real array of dimension at least (nx+ny-kx-ky-2).
|
||||
c used as working space.
|
||||
c on exit, wrk(i) will contain the integral
|
||||
c / xe
|
||||
c | ni,kx+1(x) dx , i=1,2,...,nx-kx-1
|
||||
c xb /
|
||||
c with ni,kx+1(x) the normalized b-spline defined on
|
||||
c the knots tx(i),...,tx(i+kx+1)
|
||||
c wrk(j+nx-kx-1) will contain the integral
|
||||
c / ye
|
||||
c | nj,ky+1(y) dy , j=1,2,...,ny-ky-1
|
||||
c yb /
|
||||
c with nj,ky+1(y) the normalized b-spline defined on
|
||||
c the knots ty(j),...,ty(j+ky+1)
|
||||
c
|
||||
c other subroutines required: fpintb
|
||||
c
|
||||
c references :
|
||||
c gaffney p.w. : the calculation of indefinite integrals of b-splines
|
||||
c j. inst. maths applics 17 (1976) 37-41.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1989
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer nx,ny,kx,ky
|
||||
real*8 xb,xe,yb,ye
|
||||
c ..array arguments..
|
||||
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),wrk(nx+ny-kx-ky-2)
|
||||
c ..local scalars..
|
||||
integer i,j,l,m,nkx1,nky1
|
||||
real*8 res
|
||||
c ..
|
||||
nkx1 = nx-kx-1
|
||||
nky1 = ny-ky-1
|
||||
c we calculate the integrals of the normalized b-splines ni,kx+1(x)
|
||||
call fpintb(tx,nx,wrk,nkx1,xb,xe)
|
||||
c we calculate the integrals of the normalized b-splines nj,ky+1(y)
|
||||
call fpintb(ty,ny,wrk(nkx1+1),nky1,yb,ye)
|
||||
c calculate the integral of s(x,y)
|
||||
dblint_res = 0.
|
||||
do 200 i=1,nkx1
|
||||
res = wrk(i)
|
||||
if(res.eq.0.) go to 200
|
||||
m = (i-1)*nky1
|
||||
l = nkx1
|
||||
do 100 j=1,nky1
|
||||
m = m+1
|
||||
l = l+1
|
||||
dblint_res = dblint_res + res*wrk(l)*c(m)
|
||||
100 continue
|
||||
200 continue
|
||||
return
|
||||
end
|
||||
84
mcc/bsplines/evapol.f
Normal file
84
mcc/bsplines/evapol.f
Normal file
@@ -0,0 +1,84 @@
|
||||
recursive function evapol(tu,nu,tv,nv,c,rad,x,y) result(e_res)
|
||||
implicit none
|
||||
real*8 :: e_res
|
||||
c function program evacir evaluates the function f(x,y) = s(u,v),
|
||||
c defined through the transformation
|
||||
c x = u*rad(v)*cos(v) y = u*rad(v)*sin(v)
|
||||
c and where s(u,v) is a bicubic spline ( 0<=u<=1 , -pi<=v<=pi ), given
|
||||
c in its standard b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c f = evapol(tu,nu,tv,nv,c,rad,x,y)
|
||||
c
|
||||
c input parameters:
|
||||
c tu : real array, length nu, which contains the position of the
|
||||
c knots in the u-direction.
|
||||
c nu : integer, giving the total number of knots in the u-direction
|
||||
c tv : real array, length nv, which contains the position of the
|
||||
c knots in the v-direction.
|
||||
c nv : integer, giving the total number of knots in the v-direction
|
||||
c c : real array, length (nu-4)*(nv-4), which contains the
|
||||
c b-spline coefficients.
|
||||
c rad : real function subprogram, defining the boundary of the
|
||||
c approximation domain. must be declared external in the
|
||||
c calling (sub)-program
|
||||
c x,y : real values.
|
||||
c before entry x and y must be set to the co-ordinates of
|
||||
c the point where f(x,y) must be evaluated.
|
||||
c
|
||||
c output parameter:
|
||||
c f : real
|
||||
c on exit f contains the value of f(x,y)
|
||||
c
|
||||
c other subroutines required:
|
||||
c bispev,fpbisp,fpbspl
|
||||
c
|
||||
c references :
|
||||
c de boor c : on calculating with b-splines, j. approximation theory
|
||||
c 6 (1972) 50-62.
|
||||
c cox m.g. : the numerical evaluation of b-splines, j. inst. maths
|
||||
c applics 10 (1972) 134-149.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1989
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer nu,nv
|
||||
real*8 x,y
|
||||
c ..array arguments..
|
||||
real*8 tu(nu),tv(nv),c((nu-4)*(nv-4))
|
||||
c ..user specified function
|
||||
real*8 rad
|
||||
c ..local scalars..
|
||||
integer ier
|
||||
real*8 u,v,r,f,one,dist
|
||||
c ..local arrays
|
||||
real*8 wrk(8)
|
||||
integer iwrk(2)
|
||||
c ..function references
|
||||
real*8 atan2,sqrt
|
||||
c ..
|
||||
c calculate the (u,v)-coordinates of the given point.
|
||||
one = 1
|
||||
u = 0.
|
||||
v = 0.
|
||||
dist = x**2+y**2
|
||||
if(dist.le.0.) go to 10
|
||||
v = atan2(y,x)
|
||||
r = rad(v)
|
||||
if(r.le.0.) go to 10
|
||||
u = sqrt(dist)/r
|
||||
if(u.gt.one) u = one
|
||||
c evaluate s(u,v)
|
||||
10 call bispev(tu,nu,tv,nv,c,3,3,u,1,v,1,f,wrk,8,iwrk,2,ier)
|
||||
e_res = f
|
||||
return
|
||||
end
|
||||
|
||||
97
mcc/bsplines/fourco.f
Normal file
97
mcc/bsplines/fourco.f
Normal file
@@ -0,0 +1,97 @@
|
||||
recursive subroutine fourco(t,n,c,alfa,m,ress,resc,wrk1,wrk2,ier)
|
||||
implicit none
|
||||
c subroutine fourco calculates the integrals
|
||||
c /t(n-3)
|
||||
c ress(i) = ! s(x)*sin(alfa(i)*x) dx and
|
||||
c t(4)/
|
||||
c /t(n-3)
|
||||
c resc(i) = ! s(x)*cos(alfa(i)*x) dx, i=1,...,m,
|
||||
c t(4)/
|
||||
c where s(x) denotes a cubic spline which is given in its
|
||||
c b-spline representation.
|
||||
c
|
||||
c calling sequence:
|
||||
c call fourco(t,n,c,alfa,m,ress,resc,wrk1,wrk2,ier)
|
||||
c
|
||||
c input parameters:
|
||||
c t : real array,length n, containing the knots of s(x).
|
||||
c n : integer, containing the total number of knots. n>=10.
|
||||
c c : real array,length n, containing the b-spline coefficients.
|
||||
c alfa : real array,length m, containing the parameters alfa(i).
|
||||
c m : integer, specifying the number of integrals to be computed.
|
||||
c wrk1 : real array,length n. used as working space
|
||||
c wrk2 : real array,length n. used as working space
|
||||
c
|
||||
c output parameters:
|
||||
c ress : real array,length m, containing the integrals ress(i).
|
||||
c resc : real array,length m, containing the integrals resc(i).
|
||||
c ier : error flag:
|
||||
c ier=0 : normal return.
|
||||
c ier=10: invalid input data (see restrictions).
|
||||
c
|
||||
c restrictions:
|
||||
c n >= 10
|
||||
c t(4) < t(5) < ... < t(n-4) < t(n-3).
|
||||
c t(1) <= t(2) <= t(3) <= t(4).
|
||||
c t(n-3) <= t(n-2) <= t(n-1) <= t(n).
|
||||
c
|
||||
c other subroutines required: fpbfou,fpcsin
|
||||
c
|
||||
c references :
|
||||
c dierckx p. : calculation of fouriercoefficients of discrete
|
||||
c functions using cubic splines. j. computational
|
||||
c and applied mathematics 3 (1977) 207-209.
|
||||
c dierckx p. : curve and surface fitting with splines, monographs on
|
||||
c numerical analysis, oxford university press, 1993.
|
||||
c
|
||||
c author :
|
||||
c p.dierckx
|
||||
c dept. computer science, k.u.leuven
|
||||
c celestijnenlaan 200a, b-3001 heverlee, belgium.
|
||||
c e-mail : Paul.Dierckx@cs.kuleuven.ac.be
|
||||
c
|
||||
c latest update : march 1987
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer n,m,ier
|
||||
c ..array arguments..
|
||||
real*8 t(n),c(n),wrk1(n),wrk2(n),alfa(m),ress(m),resc(m)
|
||||
c ..local scalars..
|
||||
integer i,j,n4
|
||||
real*8 rs,rc
|
||||
c ..
|
||||
n4 = n-4
|
||||
c before starting computations a data check is made. in the input data
|
||||
c are invalid, control is immediately repassed to the calling program.
|
||||
ier = 10
|
||||
if(n.lt.10) go to 50
|
||||
j = n
|
||||
do 10 i=1,3
|
||||
if(t(i).gt.t(i+1)) go to 50
|
||||
if(t(j).lt.t(j-1)) go to 50
|
||||
j = j-1
|
||||
10 continue
|
||||
do 20 i=4,n4
|
||||
if(t(i).ge.t(i+1)) go to 50
|
||||
20 continue
|
||||
ier = 0
|
||||
c main loop for the different alfa(i).
|
||||
do 40 i=1,m
|
||||
c calculate the integrals
|
||||
c wrk1(j) = integral(nj,4(x)*sin(alfa*x)) and
|
||||
c wrk2(j) = integral(nj,4(x)*cos(alfa*x)), j=1,2,...,n-4,
|
||||
c where nj,4(x) denotes the normalised cubic b-spline defined on the
|
||||
c knots t(j),t(j+1),...,t(j+4).
|
||||
call fpbfou(t,n,alfa(i),wrk1,wrk2)
|
||||
c calculate the integrals ress(i) and resc(i).
|
||||
rs = 0.
|
||||
rc = 0.
|
||||
do 30 j=1,n4
|
||||
rs = rs+c(j)*wrk1(j)
|
||||
rc = rc+c(j)*wrk2(j)
|
||||
30 continue
|
||||
ress(i) = rs
|
||||
resc(i) = rc
|
||||
40 continue
|
||||
50 return
|
||||
end
|
||||
57
mcc/bsplines/fpader.f
Normal file
57
mcc/bsplines/fpader.f
Normal file
@@ -0,0 +1,57 @@
|
||||
recursive subroutine fpader(t,n,c,k1,x,l,d)
|
||||
c subroutine fpader calculates the derivatives
|
||||
c (j-1)
|
||||
c d(j) = s (x) , j=1,2,...,k1
|
||||
c of a spline of order k1 at the point t(l)<=x<t(l+1), using the
|
||||
c stable recurrence scheme of de boor
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 x
|
||||
integer n,k1,l
|
||||
c ..array arguments..
|
||||
real*8 t(n),c(n),d(k1)
|
||||
c ..local scalars..
|
||||
integer i,ik,j,jj,j1,j2,ki,kj,li,lj,lk
|
||||
real*8 ak,fac,one
|
||||
c ..local array..
|
||||
real*8 h(20)
|
||||
c ..
|
||||
one = 0.1d+01
|
||||
lk = l-k1
|
||||
do 100 i=1,k1
|
||||
ik = i+lk
|
||||
h(i) = c(ik)
|
||||
100 continue
|
||||
kj = k1
|
||||
fac = one
|
||||
do 700 j=1,k1
|
||||
ki = kj
|
||||
j1 = j+1
|
||||
if(j.eq.1) go to 300
|
||||
i = k1
|
||||
do 200 jj=j,k1
|
||||
li = i+lk
|
||||
lj = li+kj
|
||||
h(i) = (h(i)-h(i-1))/(t(lj)-t(li))
|
||||
i = i-1
|
||||
200 continue
|
||||
300 do 400 i=j,k1
|
||||
d(i) = h(i)
|
||||
400 continue
|
||||
if(j.eq.k1) go to 600
|
||||
do 500 jj=j1,k1
|
||||
ki = ki-1
|
||||
i = k1
|
||||
do 500 j2=jj,k1
|
||||
li = i+lk
|
||||
lj = li+ki
|
||||
d(i) = ((x-t(li))*d(i)+(t(lj)-x)*d(i-1))/(t(lj)-t(li))
|
||||
i = i-1
|
||||
500 continue
|
||||
600 d(j) = d(k1)*fac
|
||||
ak = k1-j
|
||||
fac = fac*ak
|
||||
kj = kj-1
|
||||
700 continue
|
||||
return
|
||||
end
|
||||
60
mcc/bsplines/fpadno.f
Normal file
60
mcc/bsplines/fpadno.f
Normal file
@@ -0,0 +1,60 @@
|
||||
recursive subroutine fpadno(maxtr,up,left,right,info,count,
|
||||
* merk,jbind,n1,ier)
|
||||
implicit none
|
||||
c subroutine fpadno adds a branch of length n1 to the triply linked
|
||||
c tree,the information of which is kept in the arrays up,left,right
|
||||
c and info. the information field of the nodes of this new branch is
|
||||
c given in the array jbind. in linking the new branch fpadno takes
|
||||
c account of the property of the tree that
|
||||
c info(k) < info(right(k)) ; info(k) < info(left(k))
|
||||
c if necessary the subroutine calls subroutine fpfrno to collect the
|
||||
c free nodes of the tree. if no computer words are available at that
|
||||
c moment, the error parameter ier is set to 1.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer maxtr,count,merk,n1,ier
|
||||
c ..array arguments..
|
||||
integer up(maxtr),left(maxtr),right(maxtr),info(maxtr),jbind(n1)
|
||||
c ..local scalars..
|
||||
integer k,niveau,point
|
||||
logical bool
|
||||
c ..subroutine references..
|
||||
c fpfrno
|
||||
c ..
|
||||
point = 1
|
||||
niveau = 1
|
||||
10 k = left(point)
|
||||
bool = .true.
|
||||
20 if(k.eq.0) go to 50
|
||||
if (info(k)-jbind(niveau).lt.0) go to 30
|
||||
if (info(k)-jbind(niveau).eq.0) go to 40
|
||||
go to 50
|
||||
30 point = k
|
||||
k = right(point)
|
||||
bool = .false.
|
||||
go to 20
|
||||
40 point = k
|
||||
niveau = niveau+1
|
||||
go to 10
|
||||
50 if(niveau.gt.n1) go to 90
|
||||
count = count+1
|
||||
if(count.le.maxtr) go to 60
|
||||
call fpfrno(maxtr,up,left,right,info,point,merk,n1,count,ier)
|
||||
if(ier.ne.0) go to 100
|
||||
60 info(count) = jbind(niveau)
|
||||
left(count) = 0
|
||||
right(count) = k
|
||||
if(bool) go to 70
|
||||
bool = .true.
|
||||
right(point) = count
|
||||
up(count) = up(point)
|
||||
go to 80
|
||||
70 up(count) = point
|
||||
left(point) = count
|
||||
80 point = count
|
||||
niveau = niveau+1
|
||||
k = 0
|
||||
go to 50
|
||||
90 ier = 0
|
||||
100 return
|
||||
end
|
||||
71
mcc/bsplines/fpadpo.f
Normal file
71
mcc/bsplines/fpadpo.f
Normal file
@@ -0,0 +1,71 @@
|
||||
recursive subroutine fpadpo(idim,t,n,c,nc,k,cp,np,cc,t1,t2)
|
||||
implicit none
|
||||
c given a idim-dimensional spline curve of degree k, in its b-spline
|
||||
c representation ( knots t(j),j=1,...,n , b-spline coefficients c(j),
|
||||
c j=1,...,nc) and given also a polynomial curve in its b-spline
|
||||
c representation ( coefficients cp(j), j=1,...,np), subroutine fpadpo
|
||||
c calculates the b-spline representation (coefficients c(j),j=1,...,nc)
|
||||
c of the sum of the two curves.
|
||||
c
|
||||
c other subroutine required : fpinst
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer idim,k,n,nc,np
|
||||
c ..array arguments..
|
||||
real*8 t(n),c(nc),cp(np),cc(nc),t1(n),t2(n)
|
||||
c ..local scalars..
|
||||
integer i,ii,j,jj,k1,l,l1,n1,n2,nk1,nk2
|
||||
c ..
|
||||
k1 = k+1
|
||||
nk1 = n-k1
|
||||
c initialization
|
||||
j = 1
|
||||
l = 1
|
||||
do 20 jj=1,idim
|
||||
l1 = j
|
||||
do 10 ii=1,k1
|
||||
cc(l1) = cp(l)
|
||||
l1 = l1+1
|
||||
l = l+1
|
||||
10 continue
|
||||
j = j+n
|
||||
l = l+k1
|
||||
20 continue
|
||||
if(nk1.eq.k1) go to 70
|
||||
n1 = k1*2
|
||||
j = n
|
||||
l = n1
|
||||
do 30 i=1,k1
|
||||
t1(i) = t(i)
|
||||
t1(l) = t(j)
|
||||
l = l-1
|
||||
j = j-1
|
||||
30 continue
|
||||
c find the b-spline representation of the given polynomial curve
|
||||
c according to the given set of knots.
|
||||
nk2 = nk1-1
|
||||
do 60 l=k1,nk2
|
||||
l1 = l+1
|
||||
j = 1
|
||||
do 40 i=1,idim
|
||||
call fpinst(0,t1,n1,cc(j),k,t(l1),l,t2,n2,cc(j),n)
|
||||
j = j+n
|
||||
40 continue
|
||||
do 50 i=1,n2
|
||||
t1(i) = t2(i)
|
||||
50 continue
|
||||
n1 = n2
|
||||
60 continue
|
||||
c find the b-spline representation of the resulting curve.
|
||||
70 j = 1
|
||||
do 90 jj=1,idim
|
||||
l = j
|
||||
do 80 i=1,nk1
|
||||
c(l) = cc(l)+c(l)
|
||||
l = l+1
|
||||
80 continue
|
||||
j = j+n
|
||||
90 continue
|
||||
return
|
||||
end
|
||||
32
mcc/bsplines/fpback.f
Normal file
32
mcc/bsplines/fpback.f
Normal file
@@ -0,0 +1,32 @@
|
||||
recursive subroutine fpback(a,z,n,k,c,nest)
|
||||
implicit none
|
||||
c subroutine fpback calculates the solution of the system of
|
||||
c equations a*c = z with a a n x n upper triangular matrix
|
||||
c of bandwidth k.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer n,k,nest
|
||||
c ..array arguments..
|
||||
real*8 a(nest,k),z(n),c(n)
|
||||
c ..local scalars..
|
||||
real*8 store
|
||||
integer i,i1,j,k1,l,m
|
||||
c ..
|
||||
k1 = k-1
|
||||
c(n) = z(n)/a(n,1)
|
||||
i = n-1
|
||||
if(i.eq.0) go to 30
|
||||
do 20 j=2,n
|
||||
store = z(i)
|
||||
i1 = k1
|
||||
if(j.le.k1) i1 = j-1
|
||||
m = i
|
||||
do 10 l=1,i1
|
||||
m = m+1
|
||||
store = store-c(m)*a(i,l+1)
|
||||
10 continue
|
||||
c(i) = store/a(i,1)
|
||||
i = i-1
|
||||
20 continue
|
||||
30 return
|
||||
end
|
||||
59
mcc/bsplines/fpbacp.f
Normal file
59
mcc/bsplines/fpbacp.f
Normal file
@@ -0,0 +1,59 @@
|
||||
recursive subroutine fpbacp(a,b,z,n,k,c,k1,nest)
|
||||
implicit none
|
||||
c subroutine fpbacp calculates the solution of the system of equations
|
||||
c g * c = z with g a n x n upper triangular matrix of the form
|
||||
c ! a ' !
|
||||
c g = ! ' b !
|
||||
c ! 0 ' !
|
||||
c with b a n x k matrix and a a (n-k) x (n-k) upper triangular
|
||||
c matrix of bandwidth k1.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer n,k,k1,nest
|
||||
c ..array arguments..
|
||||
real*8 a(nest,k1),b(nest,k),z(n),c(n)
|
||||
c ..local scalars..
|
||||
integer i,i1,j,l,l0,l1,n2
|
||||
real*8 store
|
||||
c ..
|
||||
n2 = n-k
|
||||
l = n
|
||||
do 30 i=1,k
|
||||
store = z(l)
|
||||
j = k+2-i
|
||||
if(i.eq.1) go to 20
|
||||
l0 = l
|
||||
do 10 l1=j,k
|
||||
l0 = l0+1
|
||||
store = store-c(l0)*b(l,l1)
|
||||
10 continue
|
||||
20 c(l) = store/b(l,j-1)
|
||||
l = l-1
|
||||
if(l.eq.0) go to 80
|
||||
30 continue
|
||||
do 50 i=1,n2
|
||||
store = z(i)
|
||||
l = n2
|
||||
do 40 j=1,k
|
||||
l = l+1
|
||||
store = store-c(l)*b(i,j)
|
||||
40 continue
|
||||
c(i) = store
|
||||
50 continue
|
||||
i = n2
|
||||
c(i) = c(i)/a(i,1)
|
||||
if(i.eq.1) go to 80
|
||||
do 70 j=2,n2
|
||||
i = i-1
|
||||
store = c(i)
|
||||
i1 = k
|
||||
if(j.le.k) i1=j-1
|
||||
l = i
|
||||
do 60 l0=1,i1
|
||||
l = l+1
|
||||
store = store-c(l)*a(i,l0+1)
|
||||
60 continue
|
||||
c(i) = store/a(i,1)
|
||||
70 continue
|
||||
80 return
|
||||
end
|
||||
198
mcc/bsplines/fpbfout.f
Normal file
198
mcc/bsplines/fpbfout.f
Normal file
@@ -0,0 +1,198 @@
|
||||
recursive subroutine fpbfou(t,n,par,ress,resc)
|
||||
implicit none
|
||||
c subroutine fpbfou calculates the integrals
|
||||
c /t(n-3)
|
||||
c ress(j) = ! nj,4(x)*sin(par*x) dx and
|
||||
c t(4)/
|
||||
c /t(n-3)
|
||||
c resc(j) = ! nj,4(x)*cos(par*x) dx , j=1,2,...n-4
|
||||
c t(4)/
|
||||
c where nj,4(x) denotes the cubic b-spline defined on the knots
|
||||
c t(j),t(j+1),...,t(j+4).
|
||||
c
|
||||
c calling sequence:
|
||||
c call fpbfou(t,n,par,ress,resc)
|
||||
c
|
||||
c input parameters:
|
||||
c t : real array,length n, containing the knots.
|
||||
c n : integer, containing the number of knots.
|
||||
c par : real, containing the value of the parameter par.
|
||||
c
|
||||
c output parameters:
|
||||
c ress : real array,length n, containing the integrals ress(j).
|
||||
c resc : real array,length n, containing the integrals resc(j).
|
||||
c
|
||||
c restrictions:
|
||||
c n >= 10, t(4) < t(5) < ... < t(n-4) < t(n-3).
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer n
|
||||
real*8 par
|
||||
c ..array arguments..
|
||||
real*8 t(n),ress(n),resc(n)
|
||||
c ..local scalars..
|
||||
integer i,ic,ipj,is,j,jj,jp1,jp4,k,li,lj,ll,nmj,nm3,nm7
|
||||
real*8 ak,beta,con1,con2,c1,c2,delta,eps,fac,f1,f2,f3,one,quart,
|
||||
* sign,six,s1,s2,term
|
||||
c ..local arrays..
|
||||
real*8 co(5),si(5),hs(5),hc(5),rs(3),rc(3)
|
||||
c ..function references..
|
||||
real*8 cos,sin,abs
|
||||
c ..
|
||||
c initialization.
|
||||
one = 0.1e+01
|
||||
six = 0.6e+01
|
||||
eps = 0.1e-07
|
||||
quart = 0.25e0
|
||||
con1 = 0.5e-01
|
||||
con2 = 0.12e+03
|
||||
nm3 = n-3
|
||||
nm7 = n-7
|
||||
if(par.ne.0.) term = six/par
|
||||
beta = par*t(4)
|
||||
co(1) = cos(beta)
|
||||
si(1) = sin(beta)
|
||||
c calculate the integrals ress(j) and resc(j), j=1,2,3 by setting up
|
||||
c a divided difference table.
|
||||
do 30 j=1,3
|
||||
jp1 = j+1
|
||||
jp4 = j+4
|
||||
beta = par*t(jp4)
|
||||
co(jp1) = cos(beta)
|
||||
si(jp1) = sin(beta)
|
||||
call fpcsin(t(4),t(jp4),par,si(1),co(1),si(jp1),co(jp1),
|
||||
* rs(j),rc(j))
|
||||
i = 5-j
|
||||
hs(i) = 0.
|
||||
hc(i) = 0.
|
||||
do 10 jj=1,j
|
||||
ipj = i+jj
|
||||
hs(ipj) = rs(jj)
|
||||
hc(ipj) = rc(jj)
|
||||
10 continue
|
||||
do 20 jj=1,3
|
||||
if(i.lt.jj) i = jj
|
||||
k = 5
|
||||
li = jp4
|
||||
do 20 ll=i,4
|
||||
lj = li-jj
|
||||
fac = t(li)-t(lj)
|
||||
hs(k) = (hs(k)-hs(k-1))/fac
|
||||
hc(k) = (hc(k)-hc(k-1))/fac
|
||||
k = k-1
|
||||
li = li-1
|
||||
20 continue
|
||||
ress(j) = hs(5)-hs(4)
|
||||
resc(j) = hc(5)-hc(4)
|
||||
30 continue
|
||||
if(nm7.lt.4) go to 160
|
||||
c calculate the integrals ress(j) and resc(j),j=4,5,...,n-7.
|
||||
do 150 j=4,nm7
|
||||
jp4 = j+4
|
||||
beta = par*t(jp4)
|
||||
co(5) = cos(beta)
|
||||
si(5) = sin(beta)
|
||||
delta = t(jp4)-t(j)
|
||||
c the way of computing ress(j) and resc(j) depends on the value of
|
||||
c beta = par*(t(j+4)-t(j)).
|
||||
beta = delta*par
|
||||
if(abs(beta).le.one) go to 60
|
||||
c if !beta! > 1 the integrals are calculated by setting up a divided
|
||||
c difference table.
|
||||
do 40 k=1,5
|
||||
hs(k) = si(k)
|
||||
hc(k) = co(k)
|
||||
40 continue
|
||||
do 50 jj=1,3
|
||||
k = 5
|
||||
li = jp4
|
||||
do 50 ll=jj,4
|
||||
lj = li-jj
|
||||
fac = par*(t(li)-t(lj))
|
||||
hs(k) = (hs(k)-hs(k-1))/fac
|
||||
hc(k) = (hc(k)-hc(k-1))/fac
|
||||
k = k-1
|
||||
li = li-1
|
||||
50 continue
|
||||
s2 = (hs(5)-hs(4))*term
|
||||
c2 = (hc(5)-hc(4))*term
|
||||
go to 130
|
||||
c if !beta! <= 1 the integrals are calculated by evaluating a series
|
||||
c expansion.
|
||||
60 f3 = 0.
|
||||
do 70 i=1,4
|
||||
ipj = i+j
|
||||
hs(i) = par*(t(ipj)-t(j))
|
||||
hc(i) = hs(i)
|
||||
f3 = f3+hs(i)
|
||||
70 continue
|
||||
f3 = f3*con1
|
||||
c1 = quart
|
||||
s1 = f3
|
||||
if(abs(f3).le.eps) go to 120
|
||||
sign = one
|
||||
fac = con2
|
||||
k = 5
|
||||
is = 0
|
||||
do 110 ic=1,20
|
||||
k = k+1
|
||||
ak = k
|
||||
fac = fac*ak
|
||||
f1 = 0.
|
||||
f3 = 0.
|
||||
do 80 i=1,4
|
||||
f1 = f1+hc(i)
|
||||
f2 = f1*hs(i)
|
||||
hc(i) = f2
|
||||
f3 = f3+f2
|
||||
80 continue
|
||||
f3 = f3*six/fac
|
||||
if(is.eq.0) go to 90
|
||||
is = 0
|
||||
s1 = s1+f3*sign
|
||||
go to 100
|
||||
90 sign = -sign
|
||||
is = 1
|
||||
c1 = c1+f3*sign
|
||||
100 if(abs(f3).le.eps) go to 120
|
||||
110 continue
|
||||
120 s2 = delta*(co(1)*s1+si(1)*c1)
|
||||
c2 = delta*(co(1)*c1-si(1)*s1)
|
||||
130 ress(j) = s2
|
||||
resc(j) = c2
|
||||
do 140 i=1,4
|
||||
co(i) = co(i+1)
|
||||
si(i) = si(i+1)
|
||||
140 continue
|
||||
150 continue
|
||||
c calculate the integrals ress(j) and resc(j),j=n-6,n-5,n-4 by setting
|
||||
c up a divided difference table.
|
||||
160 do 190 j=1,3
|
||||
nmj = nm3-j
|
||||
i = 5-j
|
||||
call fpcsin(t(nm3),t(nmj),par,si(4),co(4),si(i-1),co(i-1),
|
||||
* rs(j),rc(j))
|
||||
hs(i) = 0.
|
||||
hc(i) = 0.
|
||||
do 170 jj=1,j
|
||||
ipj = i+jj
|
||||
hc(ipj) = rc(jj)
|
||||
hs(ipj) = rs(jj)
|
||||
170 continue
|
||||
do 180 jj=1,3
|
||||
if(i.lt.jj) i = jj
|
||||
k = 5
|
||||
li = nmj
|
||||
do 180 ll=i,4
|
||||
lj = li+jj
|
||||
fac = t(lj)-t(li)
|
||||
hs(k) = (hs(k-1)-hs(k))/fac
|
||||
hc(k) = (hc(k-1)-hc(k))/fac
|
||||
k = k-1
|
||||
li = li+1
|
||||
180 continue
|
||||
ress(nmj) = hs(4)-hs(5)
|
||||
resc(nmj) = hc(4)-hc(5)
|
||||
190 continue
|
||||
return
|
||||
end
|
||||
81
mcc/bsplines/fpbisp.f
Normal file
81
mcc/bsplines/fpbisp.f
Normal file
@@ -0,0 +1,81 @@
|
||||
recursive subroutine fpbisp(tx,nx,ty,ny,c,kx,ky,x,mx,y,my,
|
||||
* z,wx,wy,lx,ly)
|
||||
implicit none
|
||||
c ..scalar arguments..
|
||||
integer nx,ny,kx,ky,mx,my
|
||||
c ..array arguments..
|
||||
integer lx(mx),ly(my)
|
||||
real*8 tx(nx),ty(ny),c((nx-kx-1)*(ny-ky-1)),x(mx),y(my),z(mx*my),
|
||||
* wx(mx,kx+1),wy(my,ky+1)
|
||||
c ..local scalars..
|
||||
integer kx1,ky1,l,l1,l2,m,nkx1,nky1, i, i1, j, j1
|
||||
real*8 arg,sp,tb,te
|
||||
c ..local arrays..
|
||||
real*8 h(6)
|
||||
c ..subroutine references..
|
||||
c fpbspl
|
||||
c ..
|
||||
kx1 = kx+1
|
||||
nkx1 = nx-kx1
|
||||
tb = tx(kx1)
|
||||
te = tx(nkx1+1)
|
||||
l = kx1
|
||||
l1 = l+1
|
||||
do 40 i=1,mx
|
||||
arg = x(i)
|
||||
if(arg.lt.tb) arg = tb
|
||||
if(arg.gt.te) arg = te
|
||||
10 if(arg.lt.tx(l1) .or. l.eq.nkx1) go to 20
|
||||
l = l1
|
||||
l1 = l+1
|
||||
go to 10
|
||||
20 call fpbspl(tx,nx,kx,arg,l,h)
|
||||
lx(i) = l-kx1
|
||||
do 30 j=1,kx1
|
||||
wx(i,j) = h(j)
|
||||
30 continue
|
||||
40 continue
|
||||
ky1 = ky+1
|
||||
nky1 = ny-ky1
|
||||
tb = ty(ky1)
|
||||
te = ty(nky1+1)
|
||||
l = ky1
|
||||
l1 = l+1
|
||||
do 80 i=1,my
|
||||
arg = y(i)
|
||||
if(arg.lt.tb) arg = tb
|
||||
if(arg.gt.te) arg = te
|
||||
50 if(arg.lt.ty(l1) .or. l.eq.nky1) go to 60
|
||||
l = l1
|
||||
l1 = l+1
|
||||
go to 50
|
||||
60 call fpbspl(ty,ny,ky,arg,l,h)
|
||||
ly(i) = l-ky1
|
||||
do 70 j=1,ky1
|
||||
wy(i,j) = h(j)
|
||||
70 continue
|
||||
80 continue
|
||||
m = 0
|
||||
do 130 i=1,mx
|
||||
l = lx(i)*nky1
|
||||
do 90 i1=1,kx1
|
||||
h(i1) = wx(i,i1)
|
||||
90 continue
|
||||
do 120 j=1,my
|
||||
l1 = l+ly(j)
|
||||
sp = 0.
|
||||
do 110 i1=1,kx1
|
||||
l2 = l1
|
||||
do 100 j1=1,ky1
|
||||
l2 = l2+1
|
||||
sp = sp+c(l2)*h(i1)*wy(j,j1)
|
||||
100 continue
|
||||
l1 = l1+nky1
|
||||
110 continue
|
||||
m = m+1
|
||||
z(m) = sp
|
||||
120 continue
|
||||
130 continue
|
||||
return
|
||||
end
|
||||
|
||||
42
mcc/bsplines/fpbspl.f
Normal file
42
mcc/bsplines/fpbspl.f
Normal file
@@ -0,0 +1,42 @@
|
||||
recursive subroutine fpbspl(t,n,k,x,l,h)
|
||||
c subroutine fpbspl evaluates the (k+1) non-zero b-splines of
|
||||
c degree k at t(l) <= x < t(l+1) using the stable recurrence
|
||||
c relation of de boor and cox.
|
||||
c Travis Oliphant 2007
|
||||
c changed so that weighting of 0 is used when knots with
|
||||
c multiplicity are present.
|
||||
c Also, notice that l+k <= n and 1 <= l+1-k
|
||||
c or else the routine will be accessing memory outside t
|
||||
c Thus it is imperative that that k <= l <= n-k but this
|
||||
c is not checked.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 x
|
||||
integer n,k,l
|
||||
c ..array arguments..
|
||||
real*8 t(n),h(20)
|
||||
c ..local scalars..
|
||||
real*8 f,one
|
||||
integer i,j,li,lj
|
||||
c ..local arrays..
|
||||
real*8 hh(19)
|
||||
c ..
|
||||
one = 0.1d+01
|
||||
h(1) = one
|
||||
do 20 j=1,k
|
||||
do 10 i=1,j
|
||||
hh(i) = h(i)
|
||||
10 continue
|
||||
h(1) = 0.0d0
|
||||
do 20 i=1,j
|
||||
li = l+i
|
||||
lj = li-j
|
||||
if (t(li).ne.t(lj)) goto 15
|
||||
h(i+1) = 0.0d0
|
||||
goto 20
|
||||
15 f = hh(i)/(t(li)-t(lj))
|
||||
h(i) = h(i)+f*(t(li)-x)
|
||||
h(i+1) = f*(x-t(lj))
|
||||
20 continue
|
||||
return
|
||||
end
|
||||
87
mcc/bsplines/fpchec.f
Normal file
87
mcc/bsplines/fpchec.f
Normal file
@@ -0,0 +1,87 @@
|
||||
recursive subroutine fpchec(x,m,t,n,k,ier)
|
||||
implicit none
|
||||
c subroutine fpchec verifies the number and the position of the knots
|
||||
c t(j),j=1,2,...,n of a spline of degree k, in relation to the number
|
||||
c and the position of the data points x(i),i=1,2,...,m. if all of the
|
||||
c following conditions are fulfilled, the error parameter ier is set
|
||||
c to zero. if one of the conditions is violated ier is set to ten.
|
||||
c 1) k+1 <= n-k-1 <= m
|
||||
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||
c 5) the conditions specified by schoenberg and whitney must hold
|
||||
c for at least one subset of data points, i.e. there must be a
|
||||
c subset of data points y(j) such that
|
||||
c t(j) < y(j) < t(j+k+1), j=1,2,...,n-k-1
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer m,n,k,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),t(n)
|
||||
c ..local scalars..
|
||||
integer i,j,k1,k2,l,nk1,nk2,nk3
|
||||
real*8 tj,tl
|
||||
c ..
|
||||
k1 = k+1
|
||||
k2 = k1+1
|
||||
nk1 = n-k1
|
||||
nk2 = nk1+1
|
||||
ier = 10
|
||||
c check condition no 1
|
||||
if (nk1.lt.k1 .or. nk1.gt.m) then
|
||||
ier = 10
|
||||
go to 80
|
||||
endif
|
||||
c check condition no 2
|
||||
j = n
|
||||
do 20 i=1,k
|
||||
if (t(i) .gt. t(i+1)) then
|
||||
ier = 20
|
||||
go to 80
|
||||
endif
|
||||
if (t(j) .lt. t(j-1)) then
|
||||
ier = 20
|
||||
go to 80
|
||||
endif
|
||||
j = j-1
|
||||
20 continue
|
||||
c check condition no 3
|
||||
do 30 i=k2,nk2
|
||||
if (t(i) .le. t(i-1)) then
|
||||
ier = 30
|
||||
go to 80
|
||||
endif
|
||||
30 continue
|
||||
c check condition no 4
|
||||
if (x(1).lt.t(k1) .or. x(m).gt.t(nk2)) then
|
||||
ier = 40
|
||||
go to 80
|
||||
endif
|
||||
c check condition no 5
|
||||
if (x(1).ge.t(k2) .or. x(m).le.t(nk1)) then
|
||||
ier = 50
|
||||
go to 80
|
||||
endif
|
||||
i = 1
|
||||
l = k2
|
||||
nk3 = nk1-1
|
||||
if (nk3 .lt. 2) go to 70
|
||||
do 60 j=2,nk3
|
||||
tj = t(j)
|
||||
l = l+1
|
||||
tl = t(l)
|
||||
40 i = i+1
|
||||
if (i .ge. m) then
|
||||
ier = 50
|
||||
go to 80
|
||||
endif
|
||||
if (x(i) .le. tj) go to 40
|
||||
if (x(i) .ge. tl) then
|
||||
ier = 50
|
||||
go to 80
|
||||
endif
|
||||
60 continue
|
||||
70 ier = 0
|
||||
80 return
|
||||
end
|
||||
70
mcc/bsplines/fpched.f
Normal file
70
mcc/bsplines/fpched.f
Normal file
@@ -0,0 +1,70 @@
|
||||
recursive subroutine fpched(x,m,t,n,k,ib,ie,ier)
|
||||
implicit none
|
||||
c subroutine fpched verifies the number and the position of the knots
|
||||
c t(j),j=1,2,...,n of a spline of degree k,with ib derative constraints
|
||||
c at x(1) and ie constraints at x(m), in relation to the number and
|
||||
c the position of the data points x(i),i=1,2,...,m. if all of the
|
||||
c following conditions are fulfilled, the error parameter ier is set
|
||||
c to zero. if one of the conditions is violated ier is set to ten.
|
||||
c 1) k+1 <= n-k-1 <= m + max(0,ib-1) + max(0,ie-1)
|
||||
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||
c 5) the conditions specified by schoenberg and whitney must hold
|
||||
c for at least one subset of data points, i.e. there must be a
|
||||
c subset of data points y(j) such that
|
||||
c t(j) < y(j) < t(j+k+1), j=1+ib1,2+ib1,...,n-k-1-ie1
|
||||
c with ib1 = max(0,ib-1), ie1 = max(0,ie-1)
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer m,n,k,ib,ie,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),t(n)
|
||||
c ..local scalars..
|
||||
integer i,ib1,ie1,j,jj,k1,k2,l,nk1,nk2,nk3
|
||||
real*8 tj,tl
|
||||
c ..
|
||||
k1 = k+1
|
||||
k2 = k1+1
|
||||
nk1 = n-k1
|
||||
nk2 = nk1+1
|
||||
ib1 = ib-1
|
||||
if(ib1.lt.0) ib1 = 0
|
||||
ie1 = ie-1
|
||||
if(ie1.lt.0) ie1 = 0
|
||||
ier = 10
|
||||
c check condition no 1
|
||||
if(nk1.lt.k1 .or. nk1.gt.(m+ib1+ie1)) go to 80
|
||||
c check condition no 2
|
||||
j = n
|
||||
do 20 i=1,k
|
||||
if(t(i).gt.t(i+1)) go to 80
|
||||
if(t(j).lt.t(j-1)) go to 80
|
||||
j = j-1
|
||||
20 continue
|
||||
c check condition no 3
|
||||
do 30 i=k2,nk2
|
||||
if(t(i).le.t(i-1)) go to 80
|
||||
30 continue
|
||||
c check condition no 4
|
||||
if(x(1).lt.t(k1) .or. x(m).gt.t(nk2)) go to 80
|
||||
c check condition no 5
|
||||
if(x(1).ge.t(k2) .or. x(m).le.t(nk1)) go to 80
|
||||
i = 1
|
||||
jj = 2+ib1
|
||||
l = jj+k
|
||||
nk3 = nk1-1-ie1
|
||||
if(nk3.lt.jj) go to 70
|
||||
do 60 j=jj,nk3
|
||||
tj = t(j)
|
||||
l = l+1
|
||||
tl = t(l)
|
||||
40 i = i+1
|
||||
if(i.ge.m) go to 80
|
||||
if(x(i).le.tj) go to 40
|
||||
if(x(i).ge.tl) go to 80
|
||||
60 continue
|
||||
70 ier = 0
|
||||
80 return
|
||||
end
|
||||
82
mcc/bsplines/fpchep.f
Normal file
82
mcc/bsplines/fpchep.f
Normal file
@@ -0,0 +1,82 @@
|
||||
recursive subroutine fpchep(x,m,t,n,k,ier)
|
||||
implicit none
|
||||
c subroutine fpchep verifies the number and the position of the knots
|
||||
c t(j),j=1,2,...,n of a periodic spline of degree k, in relation to
|
||||
c the number and the position of the data points x(i),i=1,2,...,m.
|
||||
c if all of the following conditions are fulfilled, ier is set
|
||||
c to zero. if one of the conditions is violated ier is set to ten.
|
||||
c 1) k+1 <= n-k-1 <= m+k-1
|
||||
c 2) t(1) <= t(2) <= ... <= t(k+1)
|
||||
c t(n-k) <= t(n-k+1) <= ... <= t(n)
|
||||
c 3) t(k+1) < t(k+2) < ... < t(n-k)
|
||||
c 4) t(k+1) <= x(i) <= t(n-k)
|
||||
c 5) the conditions specified by schoenberg and whitney must hold
|
||||
c for at least one subset of data points, i.e. there must be a
|
||||
c subset of data points y(j) such that
|
||||
c t(j) < y(j) < t(j+k+1), j=k+1,...,n-k-1
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer m,n,k,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),t(n)
|
||||
c ..local scalars..
|
||||
integer i,i1,i2,j,j1,k1,k2,l,l1,l2,mm,m1,nk1,nk2
|
||||
real*8 per,tj,tl,xi
|
||||
c ..
|
||||
k1 = k+1
|
||||
k2 = k1+1
|
||||
nk1 = n-k1
|
||||
nk2 = nk1+1
|
||||
m1 = m-1
|
||||
ier = 10
|
||||
c check condition no 1
|
||||
if(nk1.lt.k1 .or. n.gt.m+2*k) go to 130
|
||||
c check condition no 2
|
||||
j = n
|
||||
do 20 i=1,k
|
||||
if(t(i).gt.t(i+1)) go to 130
|
||||
if(t(j).lt.t(j-1)) go to 130
|
||||
j = j-1
|
||||
20 continue
|
||||
c check condition no 3
|
||||
do 30 i=k2,nk2
|
||||
if(t(i).le.t(i-1)) go to 130
|
||||
30 continue
|
||||
c check condition no 4
|
||||
if(x(1).lt.t(k1) .or. x(m).gt.t(nk2)) go to 130
|
||||
c check condition no 5
|
||||
l1 = k1
|
||||
l2 = 1
|
||||
do 50 l=1,m
|
||||
xi = x(l)
|
||||
40 if(xi.lt.t(l1+1) .or. l.eq.nk1) go to 50
|
||||
l1 = l1+1
|
||||
l2 = l2+1
|
||||
if(l2.gt.k1) go to 60
|
||||
go to 40
|
||||
50 continue
|
||||
l = m
|
||||
60 per = t(nk2)-t(k1)
|
||||
do 120 i1=2,l
|
||||
i = i1-1
|
||||
mm = i+m1
|
||||
do 110 j=k1,nk1
|
||||
tj = t(j)
|
||||
j1 = j+k1
|
||||
tl = t(j1)
|
||||
70 i = i+1
|
||||
if(i.gt.mm) go to 120
|
||||
i2 = i-m1
|
||||
if (i2.le.0) go to 80
|
||||
go to 90
|
||||
80 xi = x(i)
|
||||
go to 100
|
||||
90 xi = x(i2)+per
|
||||
100 if(xi.le.tj) go to 70
|
||||
if(xi.ge.tl) go to 120
|
||||
110 continue
|
||||
ier = 0
|
||||
go to 130
|
||||
120 continue
|
||||
130 return
|
||||
end
|
||||
715
mcc/bsplines/fpclos.f
Normal file
715
mcc/bsplines/fpclos.f
Normal file
@@ -0,0 +1,715 @@
|
||||
recursive subroutine fpclos(iopt,idim,m,u,mx,x,w,k,s,nest,tol,
|
||||
* maxit,k1,k2,n,t,nc,c,fp,fpint,z,a1,a2,b,g1,g2,q,nrdata,ier)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,tol,fp
|
||||
integer iopt,idim,m,mx,k,nest,maxit,k1,k2,n,nc,ier
|
||||
c ..array arguments..
|
||||
real*8 u(m),x(mx),w(m),t(nest),c(nc),fpint(nest),z(nc),a1(nest,k1)
|
||||
*,
|
||||
* a2(nest,k),b(nest,k2),g1(nest,k2),g2(nest,k1),q(m,k1)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 acc,cos,d1,fac,fpart,fpms,fpold,fp0,f1,f2,f3,p,per,pinv,piv
|
||||
*,
|
||||
* p1,p2,p3,sin,store,term,ui,wi,rn,one,con1,con4,con9,half
|
||||
integer i,ich1,ich3,ij,ik,it,iter,i1,i2,i3,j,jj,jk,jper,j1,j2,kk,
|
||||
* kk1,k3,l,l0,l1,l5,mm,m1,new,nk1,nk2,nmax,nmin,nplus,npl1,
|
||||
* nrint,n10,n11,n7,n8
|
||||
c ..local arrays..
|
||||
real*8 h(6),h1(7),h2(6),xi(10)
|
||||
c ..function references..
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpbacp,fpbspl,fpgivs,fpdisc,fpknot,fprota
|
||||
c ..
|
||||
c set constants
|
||||
one = 0.1e+01
|
||||
con1 = 0.1e0
|
||||
con9 = 0.9e0
|
||||
con4 = 0.4e-01
|
||||
half = 0.5e0
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position c
|
||||
c ************************************************************** c
|
||||
c given a set of knots we compute the least-squares closed curve c
|
||||
c sinf(u). if the sum f(p=inf) <= s we accept the choice of knots. c
|
||||
c if iopt=-1 sinf(u) is the requested curve c
|
||||
c if iopt=0 or iopt=1 we check whether we can accept the knots: c
|
||||
c if fp <=s we will continue with the current set of knots. c
|
||||
c if fp > s we will increase the number of knots and compute the c
|
||||
c corresponding least-squares curve until finally fp<=s. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmax = m+2*k. c
|
||||
c if s > 0 and c
|
||||
c iopt=0 we first compute the least-squares polynomial curve of c
|
||||
c degree k; n = nmin = 2*k+2. since s(u) must be periodic we c
|
||||
c find that s(u) reduces to a fixed point. c
|
||||
c iopt=1 we start with the set of knots found at the last c
|
||||
c call of the routine, except for the case that s > fp0; then c
|
||||
c we compute directly the least-squares polynomial curve. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
m1 = m-1
|
||||
kk = k
|
||||
kk1 = k1
|
||||
k3 = 3*k+1
|
||||
nmin = 2*k1
|
||||
c determine the length of the period of the splines.
|
||||
per = u(m)-u(1)
|
||||
if(iopt.lt.0) go to 50
|
||||
c calculation of acc, the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c determine nmax, the number of knots for periodic spline interpolation
|
||||
nmax = m+2*k
|
||||
if(s.gt.0. .or. nmax.eq.nmin) go to 30
|
||||
c if s=0, s(u) is an interpolating curve.
|
||||
n = nmax
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(n.gt.nest) go to 620
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
5 if((k/2)*2 .eq.k) go to 20
|
||||
do 10 i=2,m1
|
||||
j = i+k
|
||||
t(j) = u(i)
|
||||
10 continue
|
||||
if(s.gt.0.) go to 50
|
||||
kk = k-1
|
||||
kk1 = k
|
||||
if(kk.gt.0) go to 50
|
||||
t(1) = t(m)-per
|
||||
t(2) = u(1)
|
||||
t(m+1) = u(m)
|
||||
t(m+2) = t(3)+per
|
||||
jj = 0
|
||||
do 15 i=1,m1
|
||||
j = i
|
||||
do 12 j1=1,idim
|
||||
jj = jj+1
|
||||
c(j) = x(jj)
|
||||
j = j+n
|
||||
12 continue
|
||||
15 continue
|
||||
jj = 1
|
||||
j = m
|
||||
do 17 j1=1,idim
|
||||
c(j) = c(jj)
|
||||
j = j+n
|
||||
jj = jj+n
|
||||
17 continue
|
||||
fp = 0.
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = 0.
|
||||
nrdata(n) = 0
|
||||
go to 630
|
||||
20 do 25 i=2,m1
|
||||
j = i+k
|
||||
t(j) = (u(i)+u(i-1))*half
|
||||
25 continue
|
||||
go to 50
|
||||
c if s > 0 our initial choice depends on the value of iopt.
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c polynomial curve. (i.e. a constant point).
|
||||
c if iopt=1 and fp0>s we start computing the least-squares closed
|
||||
c curve according the set of knots found at the last call of the
|
||||
c routine.
|
||||
30 if(iopt.eq.0) go to 35
|
||||
if(n.eq.nmin) go to 35
|
||||
fp0 = fpint(n)
|
||||
fpold = fpint(n-1)
|
||||
nplus = nrdata(n)
|
||||
if(fp0.gt.s) go to 50
|
||||
c the case that s(u) is a fixed point is treated separetely.
|
||||
c fp0 denotes the corresponding sum of squared residuals.
|
||||
35 fp0 = 0.
|
||||
d1 = 0.
|
||||
do 37 j=1,idim
|
||||
z(j) = 0.
|
||||
37 continue
|
||||
jj = 0
|
||||
do 45 it=1,m1
|
||||
wi = w(it)
|
||||
call fpgivs(wi,d1,cos,sin)
|
||||
do 40 j=1,idim
|
||||
jj = jj+1
|
||||
fac = wi*x(jj)
|
||||
call fprota(cos,sin,fac,z(j))
|
||||
fp0 = fp0+fac**2
|
||||
40 continue
|
||||
45 continue
|
||||
do 47 j=1,idim
|
||||
z(j) = z(j)/d1
|
||||
47 continue
|
||||
c test whether that fixed point is a solution of our problem.
|
||||
fpms = fp0-s
|
||||
if(fpms.lt.acc .or. nmax.eq.nmin) go to 640
|
||||
fpold = fp0
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(n.ge.nest) go to 620
|
||||
c start computing the least-squares closed curve with one
|
||||
c interior knot.
|
||||
nplus = 1
|
||||
n = nmin+1
|
||||
mm = (m+1)/2
|
||||
t(k2) = u(mm)
|
||||
nrdata(1) = mm-2
|
||||
nrdata(2) = m1-mm
|
||||
c main loop for the different sets of knots. m is a save upper
|
||||
c bound for the number of trials.
|
||||
50 do 340 iter=1,m
|
||||
c find nrint, the number of knot intervals.
|
||||
nrint = n-nmin+1
|
||||
c find the position of the additional knots which are needed for
|
||||
c the b-spline representation of s(u). if we take
|
||||
c t(k+1) = u(1), t(n-k) = u(m)
|
||||
c t(k+1-j) = t(n-k-j) - per, j=1,2,...k
|
||||
c t(n-k+j) = t(k+1+j) + per, j=1,2,...k
|
||||
c then s(u) will be a smooth closed curve if the b-spline
|
||||
c coefficients satisfy the following conditions
|
||||
c c((i-1)*n+n7+j) = c((i-1)*n+j), j=1,...k,i=1,2,...,idim (**)
|
||||
c with n7=n-2*k-1.
|
||||
t(k1) = u(1)
|
||||
nk1 = n-k1
|
||||
nk2 = nk1+1
|
||||
t(nk2) = u(m)
|
||||
do 60 j=1,k
|
||||
i1 = nk2+j
|
||||
i2 = nk2-j
|
||||
j1 = k1+j
|
||||
j2 = k1-j
|
||||
t(i1) = t(j1)+per
|
||||
t(j2) = t(i2)-per
|
||||
60 continue
|
||||
c compute the b-spline coefficients of the least-squares closed curve
|
||||
c sinf(u). the observation matrix a is built up row by row while
|
||||
c taking into account condition (**) and is reduced to triangular
|
||||
c form by givens transformations .
|
||||
c at the same time fp=f(p=inf) is computed.
|
||||
c the n7 x n7 triangularised upper matrix a has the form
|
||||
c ! a1 ' !
|
||||
c a = ! ' a2 !
|
||||
c ! 0 ' !
|
||||
c with a2 a n7 x k matrix and a1 a n10 x n10 upper triangular
|
||||
c matrix of bandwidth k+1 ( n10 = n7-k).
|
||||
c initialization.
|
||||
do 65 i=1,nc
|
||||
z(i) = 0.
|
||||
65 continue
|
||||
do 70 i=1,nk1
|
||||
do 70 j=1,kk1
|
||||
a1(i,j) = 0.
|
||||
70 continue
|
||||
n7 = nk1-k
|
||||
n10 = n7-kk
|
||||
jper = 0
|
||||
fp = 0.
|
||||
l = k1
|
||||
jj = 0
|
||||
do 290 it=1,m1
|
||||
c fetch the current data point u(it),x(it)
|
||||
ui = u(it)
|
||||
wi = w(it)
|
||||
do 75 j=1,idim
|
||||
jj = jj+1
|
||||
xi(j) = x(jj)*wi
|
||||
75 continue
|
||||
c search for knot interval t(l) <= ui < t(l+1).
|
||||
80 if(ui.lt.t(l+1)) go to 85
|
||||
l = l+1
|
||||
go to 80
|
||||
c evaluate the (k+1) non-zero b-splines at ui and store them in q.
|
||||
85 call fpbspl(t,n,k,ui,l,h)
|
||||
do 90 i=1,k1
|
||||
q(it,i) = h(i)
|
||||
h(i) = h(i)*wi
|
||||
90 continue
|
||||
l5 = l-k1
|
||||
c test whether the b-splines nj,k+1(u),j=1+n7,...nk1 are all zero at ui
|
||||
if(l5.lt.n10) go to 285
|
||||
if(jper.ne.0) go to 160
|
||||
c initialize the matrix a2.
|
||||
do 95 i=1,n7
|
||||
do 95 j=1,kk
|
||||
a2(i,j) = 0.
|
||||
95 continue
|
||||
jk = n10+1
|
||||
do 110 i=1,kk
|
||||
ik = jk
|
||||
do 100 j=1,kk1
|
||||
if(ik.le.0) go to 105
|
||||
a2(ik,i) = a1(ik,j)
|
||||
ik = ik-1
|
||||
100 continue
|
||||
105 jk = jk+1
|
||||
110 continue
|
||||
jper = 1
|
||||
c if one of the b-splines nj,k+1(u),j=n7+1,...nk1 is not zero at ui
|
||||
c we take account of condition (**) for setting up the new row
|
||||
c of the observation matrix a. this row is stored in the arrays h1
|
||||
c (the part with respect to a1) and h2 (the part with
|
||||
c respect to a2).
|
||||
160 do 170 i=1,kk
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
170 continue
|
||||
h1(kk1) = 0.
|
||||
j = l5-n10
|
||||
do 210 i=1,kk1
|
||||
j = j+1
|
||||
l0 = j
|
||||
180 l1 = l0-kk
|
||||
if(l1.le.0) go to 200
|
||||
if(l1.le.n10) go to 190
|
||||
l0 = l1-n10
|
||||
go to 180
|
||||
190 h1(l1) = h(i)
|
||||
go to 210
|
||||
200 h2(l0) = h2(l0)+h(i)
|
||||
210 continue
|
||||
c rotate the new row of the observation matrix into triangle
|
||||
c by givens transformations.
|
||||
if(n10.le.0) go to 250
|
||||
c rotation with the rows 1,2,...n10 of matrix a.
|
||||
do 240 j=1,n10
|
||||
piv = h1(1)
|
||||
if(piv.ne.0.) go to 214
|
||||
do 212 i=1,kk
|
||||
h1(i) = h1(i+1)
|
||||
212 continue
|
||||
h1(kk1) = 0.
|
||||
go to 240
|
||||
c calculate the parameters of the givens transformation.
|
||||
214 call fpgivs(piv,a1(j,1),cos,sin)
|
||||
c transformation to the right hand side.
|
||||
j1 = j
|
||||
do 217 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),z(j1))
|
||||
j1 = j1+n
|
||||
217 continue
|
||||
c transformations to the left hand side with respect to a2.
|
||||
do 220 i=1,kk
|
||||
call fprota(cos,sin,h2(i),a2(j,i))
|
||||
220 continue
|
||||
if(j.eq.n10) go to 250
|
||||
i2 = min0(n10-j,kk)
|
||||
c transformations to the left hand side with respect to a1.
|
||||
do 230 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h1(i1),a1(j,i1))
|
||||
h1(i) = h1(i1)
|
||||
230 continue
|
||||
h1(i1) = 0.
|
||||
240 continue
|
||||
c rotation with the rows n10+1,...n7 of matrix a.
|
||||
250 do 270 j=1,kk
|
||||
ij = n10+j
|
||||
if(ij.le.0) go to 270
|
||||
piv = h2(j)
|
||||
if(piv.eq.0.) go to 270
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a2(ij,j),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = ij
|
||||
do 255 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),z(j1))
|
||||
j1 = j1+n
|
||||
255 continue
|
||||
if(j.eq.kk) go to 280
|
||||
j1 = j+1
|
||||
c transformations to left hand side.
|
||||
do 260 i=j1,kk
|
||||
call fprota(cos,sin,h2(i),a2(ij,i))
|
||||
260 continue
|
||||
270 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
280 do 282 j2=1,idim
|
||||
fp = fp+xi(j2)**2
|
||||
282 continue
|
||||
go to 290
|
||||
c rotation of the new row of the observation matrix into
|
||||
c triangle in case the b-splines nj,k+1(u),j=n7+1,...n-k-1 are all zero
|
||||
c at ui.
|
||||
285 j = l5
|
||||
do 140 i=1,kk1
|
||||
j = j+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 140
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a1(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = j
|
||||
do 125 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),z(j1))
|
||||
j1 = j1+n
|
||||
125 continue
|
||||
if(i.eq.kk1) go to 150
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
c transformations to left hand side.
|
||||
do 130 i1=i3,kk1
|
||||
i2 = i2+1
|
||||
call fprota(cos,sin,h(i1),a1(j,i2))
|
||||
130 continue
|
||||
140 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
150 do 155 j2=1,idim
|
||||
fp = fp+xi(j2)**2
|
||||
155 continue
|
||||
290 continue
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = fpold
|
||||
nrdata(n) = nplus
|
||||
c backward substitution to obtain the b-spline coefficients .
|
||||
j1 = 1
|
||||
do 292 j2=1,idim
|
||||
call fpbacp(a1,a2,z(j1),n7,kk,c(j1),kk1,nest)
|
||||
j1 = j1+n
|
||||
292 continue
|
||||
c calculate from condition (**) the remaining coefficients.
|
||||
do 297 i=1,k
|
||||
j1 = i
|
||||
do 295 j=1,idim
|
||||
j2 = j1+n7
|
||||
c(j2) = c(j1)
|
||||
j1 = j1+n
|
||||
295 continue
|
||||
297 continue
|
||||
if(iopt.lt.0) go to 660
|
||||
c test whether the approximation sinf(u) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 660
|
||||
c if f(p=inf) < s accept the choice of knots.
|
||||
if(fpms.lt.0.) go to 350
|
||||
c if n=nmax, sinf(u) is an interpolating curve.
|
||||
if(n.eq.nmax) go to 630
|
||||
c increase the number of knots.
|
||||
c if n=nest we cannot increase the number of knots because of the
|
||||
c storage capacity limitation.
|
||||
if(n.eq.nest) go to 620
|
||||
c determine the number of knots nplus we are going to add.
|
||||
npl1 = nplus*2
|
||||
rn = nplus
|
||||
if(fpold-fp.gt.acc) npl1 = rn*fpms/(fpold-fp)
|
||||
nplus = min0(nplus*2,max0(npl1,nplus/2,1))
|
||||
fpold = fp
|
||||
c compute the sum of squared residuals for each knot interval
|
||||
c t(j+k) <= ui <= t(j+k+1) and store it in fpint(j),j=1,2,...nrint.
|
||||
fpart = 0.
|
||||
i = 1
|
||||
l = k1
|
||||
jj = 0
|
||||
do 320 it=1,m1
|
||||
if(u(it).lt.t(l)) go to 300
|
||||
new = 1
|
||||
l = l+1
|
||||
300 term = 0.
|
||||
l0 = l-k2
|
||||
do 310 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 305 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
305 continue
|
||||
jj = jj+1
|
||||
term = term+(w(it)*(fac-x(jj)))**2
|
||||
l0 = l0+n
|
||||
310 continue
|
||||
fpart = fpart+term
|
||||
if(new.eq.0) go to 320
|
||||
if(l.gt.k2) go to 315
|
||||
fpint(nrint) = term
|
||||
new = 0
|
||||
go to 320
|
||||
315 store = term*half
|
||||
fpint(i) = fpart-store
|
||||
i = i+1
|
||||
fpart = store
|
||||
new = 0
|
||||
320 continue
|
||||
fpint(nrint) = fpint(nrint)+fpart
|
||||
do 330 l=1,nplus
|
||||
c add a new knot
|
||||
call fpknot(u,m,t,n,fpint,nrdata,nrint,nest,1)
|
||||
c if n=nmax we locate the knots as for interpolation
|
||||
if(n.eq.nmax) go to 5
|
||||
c test whether we cannot further increase the number of knots.
|
||||
if(n.eq.nest) go to 340
|
||||
330 continue
|
||||
c restart the computations with the new set of knots.
|
||||
340 continue
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing closed curve sp(u). c
|
||||
c ********************************************************** c
|
||||
c we have determined the number of knots and their position. c
|
||||
c we now compute the b-spline coefficients of the smoothing curve c
|
||||
c sp(u). the observation matrix a is extended by the rows of matrix c
|
||||
c b expressing that the kth derivative discontinuities of sp(u) at c
|
||||
c the interior knots t(k+2),...t(n-k-1) must be zero. the corres- c
|
||||
c ponding weights of these additional rows are set to 1/p. c
|
||||
c iteratively we then have to determine the value of p such that f(p),c
|
||||
c the sum of squared residuals be = s. we already know that the least-c
|
||||
c squares polynomial curve corresponds to p=0, and that the least- c
|
||||
c squares periodic spline curve corresponds to p=infinity. the c
|
||||
c iteration process which is proposed here, makes use of rational c
|
||||
c interpolation. since f(p) is a convex and strictly decreasing c
|
||||
c function of p, it can be approximated by a rational function c
|
||||
c r(p) = (u*p+v)/(p+w). three values of p(p1,p2,p3) with correspond- c
|
||||
c ing values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s) are used c
|
||||
c to calculate the new value of p such that r(p)=s. convergence is c
|
||||
c guaranteed by taking f1>0 and f3<0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c evaluate the discontinuity jump of the kth derivative of the
|
||||
c b-splines at the knots t(l),l=k+2,...n-k-1 and store in b.
|
||||
350 call fpdisc(t,n,k2,b,nest)
|
||||
c initial value for p.
|
||||
p1 = 0.
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
n11 = n10-1
|
||||
n8 = n7-1
|
||||
p = 0.
|
||||
l = n7
|
||||
do 352 i=1,k
|
||||
j = k+1-i
|
||||
p = p+a2(l,j)
|
||||
l = l-1
|
||||
if(l.eq.0) go to 356
|
||||
352 continue
|
||||
do 354 i=1,n10
|
||||
p = p+a1(i,1)
|
||||
354 continue
|
||||
356 rn = n7
|
||||
p = rn/p
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
c iteration process to find the root of f(p) = s.
|
||||
do 595 iter=1,maxit
|
||||
c form the matrix g as the matrix a extended by the rows of matrix b.
|
||||
c the rows of matrix b with weight 1/p are rotated into
|
||||
c the triangularised observation matrix a.
|
||||
c after triangularisation our n7 x n7 matrix g takes the form
|
||||
c ! g1 ' !
|
||||
c g = ! ' g2 !
|
||||
c ! 0 ' !
|
||||
c with g2 a n7 x (k+1) matrix and g1 a n11 x n11 upper triangular
|
||||
c matrix of bandwidth k+2. ( n11 = n7-k-1)
|
||||
pinv = one/p
|
||||
c store matrix a into g
|
||||
do 358 i=1,nc
|
||||
c(i) = z(i)
|
||||
358 continue
|
||||
do 360 i=1,n7
|
||||
g1(i,k1) = a1(i,k1)
|
||||
g1(i,k2) = 0.
|
||||
g2(i,1) = 0.
|
||||
do 360 j=1,k
|
||||
g1(i,j) = a1(i,j)
|
||||
g2(i,j+1) = a2(i,j)
|
||||
360 continue
|
||||
l = n10
|
||||
do 370 j=1,k1
|
||||
if(l.le.0) go to 375
|
||||
g2(l,1) = a1(l,j)
|
||||
l = l-1
|
||||
370 continue
|
||||
375 do 540 it=1,n8
|
||||
c fetch a new row of matrix b and store it in the arrays h1 (the part
|
||||
c with respect to g1) and h2 (the part with respect to g2).
|
||||
do 380 j=1,idim
|
||||
xi(j) = 0.
|
||||
380 continue
|
||||
do 385 i=1,k1
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
385 continue
|
||||
h1(k2) = 0.
|
||||
if(it.gt.n11) go to 420
|
||||
l = it
|
||||
l0 = it
|
||||
do 390 j=1,k2
|
||||
if(l0.eq.n10) go to 400
|
||||
h1(j) = b(it,j)*pinv
|
||||
l0 = l0+1
|
||||
390 continue
|
||||
go to 470
|
||||
400 l0 = 1
|
||||
do 410 l1=j,k2
|
||||
h2(l0) = b(it,l1)*pinv
|
||||
l0 = l0+1
|
||||
410 continue
|
||||
go to 470
|
||||
420 l = 1
|
||||
i = it-n10
|
||||
do 460 j=1,k2
|
||||
i = i+1
|
||||
l0 = i
|
||||
430 l1 = l0-k1
|
||||
if(l1.le.0) go to 450
|
||||
if(l1.le.n11) go to 440
|
||||
l0 = l1-n11
|
||||
go to 430
|
||||
440 h1(l1) = b(it,j)*pinv
|
||||
go to 460
|
||||
450 h2(l0) = h2(l0)+b(it,j)*pinv
|
||||
460 continue
|
||||
if(n11.le.0) go to 510
|
||||
c rotate this row into triangle by givens transformations
|
||||
c rotation with the rows l,l+1,...n11.
|
||||
470 do 500 j=l,n11
|
||||
piv = h1(1)
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,g1(j,1),cos,sin)
|
||||
c transformation to right hand side.
|
||||
j1 = j
|
||||
do 475 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),c(j1))
|
||||
j1 = j1+n
|
||||
475 continue
|
||||
c transformation to the left hand side with respect to g2.
|
||||
do 480 i=1,k1
|
||||
call fprota(cos,sin,h2(i),g2(j,i))
|
||||
480 continue
|
||||
if(j.eq.n11) go to 510
|
||||
i2 = min0(n11-j,k1)
|
||||
c transformation to the left hand side with respect to g1.
|
||||
do 490 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h1(i1),g1(j,i1))
|
||||
h1(i) = h1(i1)
|
||||
490 continue
|
||||
h1(i1) = 0.
|
||||
500 continue
|
||||
c rotation with the rows n11+1,...n7
|
||||
510 do 530 j=1,k1
|
||||
ij = n11+j
|
||||
if(ij.le.0) go to 530
|
||||
piv = h2(j)
|
||||
c calculate the parameters of the givens transformation
|
||||
call fpgivs(piv,g2(ij,j),cos,sin)
|
||||
c transformation to the right hand side.
|
||||
j1 = ij
|
||||
do 515 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),c(j1))
|
||||
j1 = j1+n
|
||||
515 continue
|
||||
if(j.eq.k1) go to 540
|
||||
j1 = j+1
|
||||
c transformation to the left hand side.
|
||||
do 520 i=j1,k1
|
||||
call fprota(cos,sin,h2(i),g2(ij,i))
|
||||
520 continue
|
||||
530 continue
|
||||
540 continue
|
||||
c backward substitution to obtain the b-spline coefficients
|
||||
j1 = 1
|
||||
do 542 j2=1,idim
|
||||
call fpbacp(g1,g2,c(j1),n7,k1,c(j1),k2,nest)
|
||||
j1 = j1+n
|
||||
542 continue
|
||||
c calculate from condition (**) the remaining b-spline coefficients.
|
||||
do 547 i=1,k
|
||||
j1 = i
|
||||
do 545 j=1,idim
|
||||
j2 = j1+n7
|
||||
c(j2) = c(j1)
|
||||
j1 = j1+n
|
||||
545 continue
|
||||
547 continue
|
||||
c computation of f(p).
|
||||
fp = 0.
|
||||
l = k1
|
||||
jj = 0
|
||||
do 570 it=1,m1
|
||||
if(u(it).lt.t(l)) go to 550
|
||||
l = l+1
|
||||
550 l0 = l-k2
|
||||
term = 0.
|
||||
do 565 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 560 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
560 continue
|
||||
jj = jj+1
|
||||
term = term+(fac-x(jj))**2
|
||||
l0 = l0+n
|
||||
565 continue
|
||||
fp = fp+term*w(it)**2
|
||||
570 continue
|
||||
c test whether the approximation sp(u) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 660
|
||||
c test whether the maximal number of iterations is reached.
|
||||
if(iter.eq.maxit) go to 600
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 580
|
||||
if((f2-f3) .gt. acc) go to 575
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p = p1*con9 +p2*con1
|
||||
go to 595
|
||||
575 if(f2.lt.0.) ich3 = 1
|
||||
580 if(ich1.ne.0) go to 590
|
||||
if((f1-f2) .gt. acc) go to 585
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 595
|
||||
if(p.ge.p3) p = p2*con1 +p3*con9
|
||||
go to 595
|
||||
585 if(f2.gt.0.) ich1 = 1
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
590 if(f2.ge.f1 .or. f2.le.f3) go to 610
|
||||
c find the new value for p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
595 continue
|
||||
c error codes and messages.
|
||||
600 ier = 3
|
||||
go to 660
|
||||
610 ier = 2
|
||||
go to 660
|
||||
620 ier = 1
|
||||
go to 660
|
||||
630 ier = -1
|
||||
go to 660
|
||||
640 ier = -2
|
||||
c the point (z(1),z(2),...,z(idim)) is a solution of our problem.
|
||||
c a constant function is a spline of degree k with all b-spline
|
||||
c coefficients equal to that constant.
|
||||
do 650 i=1,k1
|
||||
rn = k1-i
|
||||
t(i) = u(1)-rn*per
|
||||
j = i+k1
|
||||
rn = i-1
|
||||
t(j) = u(m)+rn*per
|
||||
650 continue
|
||||
n = nmin
|
||||
j1 = 0
|
||||
do 658 j=1,idim
|
||||
fac = z(j)
|
||||
j2 = j1
|
||||
do 654 i=1,k1
|
||||
j2 = j2+1
|
||||
c(j2) = fac
|
||||
654 continue
|
||||
j1 = j1+n
|
||||
658 continue
|
||||
fp = fp0
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = 0.
|
||||
nrdata(n) = 0
|
||||
660 return
|
||||
end
|
||||
169
mcc/bsplines/fpcoco.f
Normal file
169
mcc/bsplines/fpcoco.f
Normal file
@@ -0,0 +1,169 @@
|
||||
recursive subroutine fpcoco(iopt,m,x,y,w,v,s,nest,maxtr,maxbin,
|
||||
* n,t,c,sq,sx,bind,e,wrk,lwrk,iwrk,kwrk,ier)
|
||||
implicit none
|
||||
c ..scalar arguments..
|
||||
real*8 s,sq
|
||||
integer iopt,m,nest,maxtr,maxbin,n,lwrk,kwrk,ier
|
||||
c ..array arguments..
|
||||
integer iwrk(kwrk)
|
||||
real*8 x(m),y(m),w(m),v(m),t(nest),c(nest),sx(m),e(nest),wrk(lwrk)
|
||||
*
|
||||
logical bind(nest)
|
||||
c ..local scalars..
|
||||
integer i,ia,ib,ic,iq,iu,iz,izz,i1,j,k,l,l1,m1,nmax,nr,n4,n6,n8,
|
||||
* ji,jib,jjb,jl,jr,ju,mb,nm
|
||||
real*8 sql,sqmax,term,tj,xi,half
|
||||
c ..subroutine references..
|
||||
c fpcosp,fpbspl,fpadno,fpdeno,fpseno,fpfrno
|
||||
c ..
|
||||
c set constant
|
||||
half = 0.5e0
|
||||
c determine the maximal admissible number of knots.
|
||||
nmax = m+4
|
||||
c the initial choice of knots depends on the value of iopt.
|
||||
c if iopt=0 the program starts with the minimal number of knots
|
||||
c so that can be guarantied that the concavity/convexity constraints
|
||||
c will be satisfied.
|
||||
c if iopt = 1 the program will continue from the point on where she
|
||||
c left at the foregoing call.
|
||||
if(iopt.gt.0) go to 80
|
||||
c find the minimal number of knots.
|
||||
c a knot is located at the data point x(i), i=2,3,...m-1 if
|
||||
c 1) v(i) ^= 0 and
|
||||
c 2) v(i)*v(i-1) <= 0 or v(i)*v(i+1) <= 0.
|
||||
m1 = m-1
|
||||
n = 4
|
||||
do 20 i=2,m1
|
||||
if(v(i).eq.0. .or. (v(i)*v(i-1).gt.0. .and.
|
||||
* v(i)*v(i+1).gt.0.)) go to 20
|
||||
n = n+1
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(n+4.gt.nest) go to 200
|
||||
t(n) = x(i)
|
||||
20 continue
|
||||
c find the position of the knots t(1),...t(4) and t(n-3),...t(n) which
|
||||
c are needed for the b-spline representation of s(x).
|
||||
do 30 i=1,4
|
||||
t(i) = x(1)
|
||||
n = n+1
|
||||
t(n) = x(m)
|
||||
30 continue
|
||||
c test whether the minimum number of knots exceeds the maximum number.
|
||||
if(n.gt.nmax) go to 210
|
||||
c main loop for the different sets of knots.
|
||||
c find corresponding values e(j) to the knots t(j+3),j=1,2,...n-6
|
||||
c e(j) will take the value -1,1, or 0 according to the requirement
|
||||
c that s(x) must be locally convex or concave at t(j+3) or that the
|
||||
c sign of s''(x) is unrestricted at that point.
|
||||
40 i= 1
|
||||
xi = x(1)
|
||||
j = 4
|
||||
tj = t(4)
|
||||
n6 = n-6
|
||||
do 70 l=1,n6
|
||||
50 if(xi.eq.tj) go to 60
|
||||
i = i+1
|
||||
xi = x(i)
|
||||
go to 50
|
||||
60 e(l) = v(i)
|
||||
j = j+1
|
||||
tj = t(j)
|
||||
70 continue
|
||||
c we partition the working space
|
||||
nm = n+maxbin
|
||||
mb = maxbin+1
|
||||
ia = 1
|
||||
ib = ia+4*n
|
||||
ic = ib+nm*maxbin
|
||||
iz = ic+n
|
||||
izz = iz+n
|
||||
iu = izz+n
|
||||
iq = iu+maxbin
|
||||
ji = 1
|
||||
ju = ji+maxtr
|
||||
jl = ju+maxtr
|
||||
jr = jl+maxtr
|
||||
jjb = jr+maxtr
|
||||
jib = jjb+mb
|
||||
c given the set of knots t(j),j=1,2,...n, find the least-squares cubic
|
||||
c spline which satisfies the imposed concavity/convexity constraints.
|
||||
call fpcosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,bind,nm,mb,wrk(ia),
|
||||
*
|
||||
* wrk(ib),wrk(ic),wrk(iz),wrk(izz),wrk(iu),wrk(iq),iwrk(ji),
|
||||
* iwrk(ju),iwrk(jl),iwrk(jr),iwrk(jjb),iwrk(jib),ier)
|
||||
c if sq <= s or in case of abnormal exit from fpcosp, control is
|
||||
c repassed to the driver program.
|
||||
if(sq.le.s .or. ier.gt.0) go to 300
|
||||
c calculate for each knot interval t(l-1) <= xi <= t(l) the
|
||||
c sum((wi*(yi-s(xi)))**2).
|
||||
c find the interval t(k-1) <= x <= t(k) for which this sum is maximal
|
||||
c on the condition that this interval contains at least one interior
|
||||
c data point x(nr) and that s(x) is not given there by a straight line.
|
||||
80 sqmax = 0.
|
||||
sql = 0.
|
||||
l = 5
|
||||
nr = 0
|
||||
i1 = 1
|
||||
n4 = n-4
|
||||
do 110 i=1,m
|
||||
term = (w(i)*(sx(i)-y(i)))**2
|
||||
if(x(i).lt.t(l) .or. l.gt.n4) go to 100
|
||||
term = term*half
|
||||
sql = sql+term
|
||||
if(i-i1.le.1 .or. (bind(l-4).and.bind(l-3))) go to 90
|
||||
if(sql.le.sqmax) go to 90
|
||||
k = l
|
||||
sqmax = sql
|
||||
nr = i1+(i-i1)/2
|
||||
90 l = l+1
|
||||
i1 = i
|
||||
sql = 0.
|
||||
100 sql = sql+term
|
||||
110 continue
|
||||
if(m-i1.le.1 .or. (bind(l-4).and.bind(l-3))) go to 120
|
||||
if(sql.le.sqmax) go to 120
|
||||
k = l
|
||||
nr = i1+(m-i1)/2
|
||||
c if no such interval is found, control is repassed to the driver
|
||||
c program (ier = -1).
|
||||
120 if(nr.eq.0) go to 190
|
||||
c if s(x) is given by the same straight line in two succeeding knot
|
||||
c intervals t(l-1) <= x <= t(l) and t(l) <= x <= t(l+1),delete t(l)
|
||||
n8 = n-8
|
||||
l1 = 0
|
||||
if(n8.le.0) go to 150
|
||||
do 140 i=1,n8
|
||||
if(.not. (bind(i).and.bind(i+1).and.bind(i+2))) go to 140
|
||||
l = i+4-l1
|
||||
if(k.gt.l) k = k-1
|
||||
n = n-1
|
||||
l1 = l1+1
|
||||
do 130 j=l,n
|
||||
t(j) = t(j+1)
|
||||
130 continue
|
||||
140 continue
|
||||
c test whether we cannot further increase the number of knots.
|
||||
150 if(n.eq.nmax) go to 180
|
||||
if(n.eq.nest) go to 170
|
||||
c locate an additional knot at the point x(nr).
|
||||
j = n
|
||||
do 160 i=k,n
|
||||
t(j+1) = t(j)
|
||||
j = j-1
|
||||
160 continue
|
||||
t(k) = x(nr)
|
||||
n = n+1
|
||||
c restart the computations with the new set of knots.
|
||||
go to 40
|
||||
c error codes and messages.
|
||||
170 ier = -3
|
||||
go to 300
|
||||
180 ier = -2
|
||||
go to 300
|
||||
190 ier = -1
|
||||
go to 300
|
||||
200 ier = 4
|
||||
go to 300
|
||||
210 ier = 5
|
||||
300 return
|
||||
end
|
||||
443
mcc/bsplines/fpcons.f
Normal file
443
mcc/bsplines/fpcons.f
Normal file
@@ -0,0 +1,443 @@
|
||||
recursive subroutine fpcons(iopt,idim,m,u,mx,x,w,ib,ie,k,s,nest,
|
||||
* tol,maxit,k1,k2,n,t,nc,c,fp,fpint,z,a,b,g,q,nrdata,ier)
|
||||
ccc implicit none c XXX: mmnin/nmin variables on line 61
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,tol,fp
|
||||
integer iopt,idim,m,mx,ib,ie,k,nest,maxit,k1,k2,n,nc,ier
|
||||
c ..array arguments..
|
||||
real*8 u(m),x(mx),w(m),t(nest),c(nc),fpint(nest),
|
||||
* z(nc),a(nest,k1),b(nest,k2),g(nest,k2),q(m,k1)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 acc,con1,con4,con9,cos,fac,fpart,fpms,fpold,fp0,f1,f2,f3,
|
||||
* half,one,p,pinv,piv,p1,p2,p3,rn,sin,store,term,ui,wi
|
||||
integer i,ich1,ich3,it,iter,i1,i2,i3,j,jb,je,jj,j1,j2,j3,kbe,
|
||||
* l,li,lj,l0,mb,me,mm,new,nk1,nmax,nmin,nn,nplus,npl1,nrint,n8
|
||||
c ..local arrays..
|
||||
real*8 h(7),xi(10)
|
||||
c ..function references
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpbacp,fpbspl,fpgivs,fpdisc,fpknot,fprota
|
||||
c ..
|
||||
c set constants
|
||||
one = 0.1e+01
|
||||
con1 = 0.1e0
|
||||
con9 = 0.9e0
|
||||
con4 = 0.4e-01
|
||||
half = 0.5e0
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position c
|
||||
c ************************************************************** c
|
||||
c given a set of knots we compute the least-squares curve sinf(u), c
|
||||
c and the corresponding sum of squared residuals fp=f(p=inf). c
|
||||
c if iopt=-1 sinf(u) is the requested curve. c
|
||||
c if iopt=0 or iopt=1 we check whether we can accept the knots: c
|
||||
c if fp <=s we will continue with the current set of knots. c
|
||||
c if fp > s we will increase the number of knots and compute the c
|
||||
c corresponding least-squares curve until finally fp<=s. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmax = m+k+1-max(0,ib-1)-max(0,ie-1) c
|
||||
c if s > 0 and c
|
||||
c iopt=0 we first compute the least-squares polynomial curve of c
|
||||
c degree k; n = nmin = 2*k+2 c
|
||||
c iopt=1 we start with the set of knots found at the last c
|
||||
c call of the routine, except for the case that s > fp0; then c
|
||||
c we compute directly the polynomial curve of degree k. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c determine nmin, the number of knots for polynomial approximation.
|
||||
nmin = 2*k1
|
||||
c find which data points are to be considered.
|
||||
mb = 2
|
||||
jb = ib
|
||||
if(ib.gt.0) go to 10
|
||||
mb = 1
|
||||
jb = 1
|
||||
10 me = m-1
|
||||
je = ie
|
||||
if(ie.gt.0) go to 20
|
||||
me = m
|
||||
je = 1
|
||||
20 if(iopt.lt.0) go to 60
|
||||
c calculation of acc, the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c determine nmax, the number of knots for spline interpolation.
|
||||
kbe = k1-jb-je
|
||||
mmin = kbe+2
|
||||
mm = m-mmin
|
||||
nmax = nmin+mm
|
||||
if(s.gt.0.) go to 40
|
||||
c if s=0, s(u) is an interpolating curve.
|
||||
c test whether the required storage space exceeds the available one.
|
||||
n = nmax
|
||||
if(nmax.gt.nest) go to 420
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
if(mm.eq.0) go to 60
|
||||
25 i = k2
|
||||
j = 3-jb+k/2
|
||||
do 30 l=1,mm
|
||||
t(i) = u(j)
|
||||
i = i+1
|
||||
j = j+1
|
||||
30 continue
|
||||
go to 60
|
||||
c if s>0 our initial choice of knots depends on the value of iopt.
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c polynomial curve which is a spline curve without interior knots.
|
||||
c if iopt=1 and fp0>s we start computing the least squares spline curve
|
||||
c according to the set of knots found at the last call of the routine.
|
||||
40 if(iopt.eq.0) go to 50
|
||||
if(n.eq.nmin) go to 50
|
||||
fp0 = fpint(n)
|
||||
fpold = fpint(n-1)
|
||||
nplus = nrdata(n)
|
||||
if(fp0.gt.s) go to 60
|
||||
50 n = nmin
|
||||
fpold = 0.
|
||||
nplus = 0
|
||||
nrdata(1) = m-2
|
||||
c main loop for the different sets of knots. m is a save upper bound
|
||||
c for the number of trials.
|
||||
60 do 200 iter = 1,m
|
||||
if(n.eq.nmin) ier = -2
|
||||
c find nrint, tne number of knot intervals.
|
||||
nrint = n-nmin+1
|
||||
c find the position of the additional knots which are needed for
|
||||
c the b-spline representation of s(u).
|
||||
nk1 = n-k1
|
||||
i = n
|
||||
do 70 j=1,k1
|
||||
t(j) = u(1)
|
||||
t(i) = u(m)
|
||||
i = i-1
|
||||
70 continue
|
||||
c compute the b-spline coefficients of the least-squares spline curve
|
||||
c sinf(u). the observation matrix a is built up row by row and
|
||||
c reduced to upper triangular form by givens transformations.
|
||||
c at the same time fp=f(p=inf) is computed.
|
||||
fp = 0.
|
||||
c nn denotes the dimension of the splines
|
||||
nn = nk1-ib-ie
|
||||
c initialize the b-spline coefficients and the observation matrix a.
|
||||
do 75 i=1,nc
|
||||
z(i) = 0.
|
||||
c(i) = 0.
|
||||
75 continue
|
||||
if(me.lt.mb) go to 134
|
||||
if(nn.eq.0) go to 82
|
||||
do 80 i=1,nn
|
||||
do 80 j=1,k1
|
||||
a(i,j) = 0.
|
||||
80 continue
|
||||
82 l = k1
|
||||
jj = (mb-1)*idim
|
||||
do 130 it=mb,me
|
||||
c fetch the current data point u(it),x(it).
|
||||
ui = u(it)
|
||||
wi = w(it)
|
||||
do 84 j=1,idim
|
||||
jj = jj+1
|
||||
xi(j) = x(jj)*wi
|
||||
84 continue
|
||||
c search for knot interval t(l) <= ui < t(l+1).
|
||||
86 if(ui.lt.t(l+1) .or. l.eq.nk1) go to 90
|
||||
l = l+1
|
||||
go to 86
|
||||
c evaluate the (k+1) non-zero b-splines at ui and store them in q.
|
||||
90 call fpbspl(t,n,k,ui,l,h)
|
||||
do 92 i=1,k1
|
||||
q(it,i) = h(i)
|
||||
h(i) = h(i)*wi
|
||||
92 continue
|
||||
c take into account that certain b-spline coefficients must be zero.
|
||||
lj = k1
|
||||
j = nk1-l-ie
|
||||
if(j.ge.0) go to 94
|
||||
lj = lj+j
|
||||
94 li = 1
|
||||
j = l-k1-ib
|
||||
if(j.ge.0) go to 96
|
||||
li = li-j
|
||||
j = 0
|
||||
96 if(li.gt.lj) go to 120
|
||||
c rotate the new row of the observation matrix into triangle.
|
||||
do 110 i=li,lj
|
||||
j = j+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 110
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = j
|
||||
do 98 j2 =1,idim
|
||||
call fprota(cos,sin,xi(j2),z(j1))
|
||||
j1 = j1+n
|
||||
98 continue
|
||||
if(i.eq.lj) go to 120
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 100 i1 = i3,lj
|
||||
i2 = i2+1
|
||||
c transformations to left hand side.
|
||||
call fprota(cos,sin,h(i1),a(j,i2))
|
||||
100 continue
|
||||
110 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
120 do 125 j2=1,idim
|
||||
fp = fp+xi(j2)**2
|
||||
125 continue
|
||||
130 continue
|
||||
if(ier.eq.(-2)) fp0 = fp
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = fpold
|
||||
nrdata(n) = nplus
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
if(nn.eq.0) go to 134
|
||||
j1 = 1
|
||||
do 132 j2=1,idim
|
||||
j3 = j1+ib
|
||||
call fpback(a,z(j1),nn,k1,c(j3),nest)
|
||||
j1 = j1+n
|
||||
132 continue
|
||||
c test whether the approximation sinf(u) is an acceptable solution.
|
||||
134 if(iopt.lt.0) go to 440
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c if f(p=inf) < s accept the choice of knots.
|
||||
if(fpms.lt.0.) go to 250
|
||||
c if n = nmax, sinf(u) is an interpolating spline curve.
|
||||
if(n.eq.nmax) go to 430
|
||||
c increase the number of knots.
|
||||
c if n=nest we cannot increase the number of knots because of
|
||||
c the storage capacity limitation.
|
||||
if(n.eq.nest) go to 420
|
||||
c determine the number of knots nplus we are going to add.
|
||||
if(ier.eq.0) go to 140
|
||||
nplus = 1
|
||||
ier = 0
|
||||
go to 150
|
||||
140 npl1 = nplus*2
|
||||
rn = nplus
|
||||
if(fpold-fp.gt.acc) npl1 = rn*fpms/(fpold-fp)
|
||||
nplus = min0(nplus*2,max0(npl1,nplus/2,1))
|
||||
150 fpold = fp
|
||||
c compute the sum of squared residuals for each knot interval
|
||||
c t(j+k) <= u(i) <= t(j+k+1) and store it in fpint(j),j=1,2,...nrint.
|
||||
fpart = 0.
|
||||
i = 1
|
||||
l = k2
|
||||
new = 0
|
||||
jj = (mb-1)*idim
|
||||
do 180 it=mb,me
|
||||
if(u(it).lt.t(l) .or. l.gt.nk1) go to 160
|
||||
new = 1
|
||||
l = l+1
|
||||
160 term = 0.
|
||||
l0 = l-k2
|
||||
do 175 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 170 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
170 continue
|
||||
jj = jj+1
|
||||
term = term+(w(it)*(fac-x(jj)))**2
|
||||
l0 = l0+n
|
||||
175 continue
|
||||
fpart = fpart+term
|
||||
if(new.eq.0) go to 180
|
||||
store = term*half
|
||||
fpint(i) = fpart-store
|
||||
i = i+1
|
||||
fpart = store
|
||||
new = 0
|
||||
180 continue
|
||||
fpint(nrint) = fpart
|
||||
do 190 l=1,nplus
|
||||
c add a new knot.
|
||||
call fpknot(u,m,t,n,fpint,nrdata,nrint,nest,1)
|
||||
c if n=nmax we locate the knots as for interpolation
|
||||
if(n.eq.nmax) go to 25
|
||||
c test whether we cannot further increase the number of knots.
|
||||
if(n.eq.nest) go to 200
|
||||
190 continue
|
||||
c restart the computations with the new set of knots.
|
||||
200 continue
|
||||
c test whether the least-squares kth degree polynomial curve is a
|
||||
c solution of our approximation problem.
|
||||
250 if(ier.eq.(-2)) go to 440
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing spline curve sp(u). c
|
||||
c ********************************************************** c
|
||||
c we have determined the number of knots and their position. c
|
||||
c we now compute the b-spline coefficients of the smoothing curve c
|
||||
c sp(u). the observation matrix a is extended by the rows of matrix c
|
||||
c b expressing that the kth derivative discontinuities of sp(u) at c
|
||||
c the interior knots t(k+2),...t(n-k-1) must be zero. the corres- c
|
||||
c ponding weights of these additional rows are set to 1/p. c
|
||||
c iteratively we then have to determine the value of p such that f(p),c
|
||||
c the sum of squared residuals be = s. we already know that the least c
|
||||
c squares kth degree polynomial curve corresponds to p=0, and that c
|
||||
c the least-squares spline curve corresponds to p=infinity. the c
|
||||
c iteration process which is proposed here, makes use of rational c
|
||||
c interpolation. since f(p) is a convex and strictly decreasing c
|
||||
c function of p, it can be approximated by a rational function c
|
||||
c r(p) = (u*p+v)/(p+w). three values of p(p1,p2,p3) with correspond- c
|
||||
c ing values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s) are used c
|
||||
c to calculate the new value of p such that r(p)=s. convergence is c
|
||||
c guaranteed by taking f1>0 and f3<0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c evaluate the discontinuity jump of the kth derivative of the
|
||||
c b-splines at the knots t(l),l=k+2,...n-k-1 and store in b.
|
||||
call fpdisc(t,n,k2,b,nest)
|
||||
c initial value for p.
|
||||
p1 = 0.
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
p = 0.
|
||||
do 252 i=1,nn
|
||||
p = p+a(i,1)
|
||||
252 continue
|
||||
rn = nn
|
||||
p = rn/p
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
n8 = n-nmin
|
||||
c iteration process to find the root of f(p) = s.
|
||||
do 360 iter=1,maxit
|
||||
c the rows of matrix b with weight 1/p are rotated into the
|
||||
c triangularised observation matrix a which is stored in g.
|
||||
pinv = one/p
|
||||
do 255 i=1,nc
|
||||
c(i) = z(i)
|
||||
255 continue
|
||||
do 260 i=1,nn
|
||||
g(i,k2) = 0.
|
||||
do 260 j=1,k1
|
||||
g(i,j) = a(i,j)
|
||||
260 continue
|
||||
do 300 it=1,n8
|
||||
c the row of matrix b is rotated into triangle by givens transformation
|
||||
do 264 i=1,k2
|
||||
h(i) = b(it,i)*pinv
|
||||
264 continue
|
||||
do 268 j=1,idim
|
||||
xi(j) = 0.
|
||||
268 continue
|
||||
c take into account that certain b-spline coefficients must be zero.
|
||||
if(it.gt.ib) go to 274
|
||||
j1 = ib-it+2
|
||||
j2 = 1
|
||||
do 270 i=j1,k2
|
||||
h(j2) = h(i)
|
||||
j2 = j2+1
|
||||
270 continue
|
||||
do 272 i=j2,k2
|
||||
h(i) = 0.
|
||||
272 continue
|
||||
274 jj = max0(1,it-ib)
|
||||
do 290 j=jj,nn
|
||||
piv = h(1)
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,g(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = j
|
||||
do 277 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),c(j1))
|
||||
j1 = j1+n
|
||||
277 continue
|
||||
if(j.eq.nn) go to 300
|
||||
i2 = min0(nn-j,k1)
|
||||
do 280 i=1,i2
|
||||
c transformations to left hand side.
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h(i1),g(j,i1))
|
||||
h(i) = h(i1)
|
||||
280 continue
|
||||
h(i2+1) = 0.
|
||||
290 continue
|
||||
300 continue
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
j1 = 1
|
||||
do 308 j2=1,idim
|
||||
j3 = j1+ib
|
||||
call fpback(g,c(j1),nn,k2,c(j3),nest)
|
||||
if(ib.eq.0) go to 306
|
||||
j3 = j1
|
||||
do 304 i=1,ib
|
||||
c(j3) = 0.
|
||||
j3 = j3+1
|
||||
304 continue
|
||||
306 j1 =j1+n
|
||||
308 continue
|
||||
c computation of f(p).
|
||||
fp = 0.
|
||||
l = k2
|
||||
jj = (mb-1)*idim
|
||||
do 330 it=mb,me
|
||||
if(u(it).lt.t(l) .or. l.gt.nk1) go to 310
|
||||
l = l+1
|
||||
310 l0 = l-k2
|
||||
term = 0.
|
||||
do 325 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 320 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
320 continue
|
||||
jj = jj+1
|
||||
term = term+(fac-x(jj))**2
|
||||
l0 = l0+n
|
||||
325 continue
|
||||
fp = fp+term*w(it)**2
|
||||
330 continue
|
||||
c test whether the approximation sp(u) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c test whether the maximal number of iterations is reached.
|
||||
if(iter.eq.maxit) go to 400
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 340
|
||||
if((f2-f3).gt.acc) go to 335
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p=p1*con9 + p2*con1
|
||||
go to 360
|
||||
335 if(f2.lt.0.) ich3=1
|
||||
340 if(ich1.ne.0) go to 350
|
||||
if((f1-f2).gt.acc) go to 345
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 360
|
||||
if(p.ge.p3) p = p2*con1 + p3*con9
|
||||
go to 360
|
||||
345 if(f2.gt.0.) ich1=1
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
350 if(f2.ge.f1 .or. f2.le.f3) go to 410
|
||||
c find the new value for p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
360 continue
|
||||
c error codes and messages.
|
||||
400 ier = 3
|
||||
go to 440
|
||||
410 ier = 2
|
||||
go to 440
|
||||
420 ier = 1
|
||||
go to 440
|
||||
430 ier = -1
|
||||
440 return
|
||||
end
|
||||
363
mcc/bsplines/fpcosp.f
Normal file
363
mcc/bsplines/fpcosp.f
Normal file
@@ -0,0 +1,363 @@
|
||||
recursive subroutine fpcosp(m,x,y,w,n,t,e,maxtr,maxbin,c,sq,sx,
|
||||
* bind,nm,mb,a,
|
||||
* b,const,z,zz,u,q,info,up,left,right,jbind,ibind,ier)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 sq
|
||||
integer m,n,maxtr,maxbin,nm,mb,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),t(n),e(n),c(n),sx(m),a(n,4),b(nm,maxbin),
|
||||
* const(n),z(n),zz(n),u(maxbin),q(m,4)
|
||||
integer info(maxtr),up(maxtr),left(maxtr),right(maxtr),jbind(mb),
|
||||
* ibind(mb)
|
||||
logical bind(n)
|
||||
c ..local scalars..
|
||||
integer count,i,i1,j,j1,j2,j3,k,kdim,k1,k2,k3,k4,k5,k6,
|
||||
* l,lp1,l1,l2,l3,merk,nbind,number,n1,n4,n6
|
||||
real*8 f,wi,xi
|
||||
c ..local array..
|
||||
real*8 h(4)
|
||||
c ..subroutine references..
|
||||
c fpbspl,fpadno,fpdeno,fpfrno,fpseno
|
||||
c ..
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c if we use the b-spline representation of s(x) our approximation c
|
||||
c problem results in a quadratic programming problem: c
|
||||
c find the b-spline coefficients c(j),j=1,2,...n-4 such that c
|
||||
c (1) sumi((wi*(yi-sumj(cj*nj(xi))))**2),i=1,2,...m is minimal c
|
||||
c (2) sumj(cj*n''j(t(l+3)))*e(l) <= 0, l=1,2,...n-6. c
|
||||
c to solve this problem we use the theil-van de panne procedure. c
|
||||
c if the inequality constraints (2) are numbered from 1 to n-6, c
|
||||
c this algorithm finds a subset of constraints ibind(1)..ibind(nbind) c
|
||||
c such that the solution of the minimization problem (1) with these c
|
||||
c constraints in equality form, satisfies all constraints. such a c
|
||||
c feasible solution is optimal if the lagrange parameters associated c
|
||||
c with that problem with equality constraints, are all positive. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c determine n6, the number of inequality constraints.
|
||||
n6 = n-6
|
||||
c fix the parameters which determine these constraints.
|
||||
do 10 i=1,n6
|
||||
const(i) = e(i)*(t(i+4)-t(i+1))/(t(i+5)-t(i+2))
|
||||
10 continue
|
||||
c initialize the triply linked tree which is used to find the subset
|
||||
c of constraints ibind(1),...ibind(nbind).
|
||||
count = 1
|
||||
info(1) = 0
|
||||
left(1) = 0
|
||||
right(1) = 0
|
||||
up(1) = 1
|
||||
merk = 1
|
||||
c set up the normal equations n'nc=n'y where n denotes the m x (n-4)
|
||||
c observation matrix with elements ni,j = wi*nj(xi) and y is the
|
||||
c column vector with elements yi*wi.
|
||||
c from the properties of the b-splines nj(x),j=1,2,...n-4, it follows
|
||||
c that n'n is a (n-4) x (n-4) positive definite bandmatrix of
|
||||
c bandwidth 7. the matrices n'n and n'y are built up in a and z.
|
||||
n4 = n-4
|
||||
c initialization
|
||||
do 20 i=1,n4
|
||||
z(i) = 0.
|
||||
do 20 j=1,4
|
||||
a(i,j) = 0.
|
||||
20 continue
|
||||
l = 4
|
||||
lp1 = l+1
|
||||
do 70 i=1,m
|
||||
c fetch the current row of the observation matrix.
|
||||
xi = x(i)
|
||||
wi = w(i)**2
|
||||
c search for knot interval t(l) <= xi < t(l+1)
|
||||
30 if(xi.lt.t(lp1) .or. l.eq.n4) go to 40
|
||||
l = lp1
|
||||
lp1 = l+1
|
||||
go to 30
|
||||
c evaluate the four non-zero cubic b-splines nj(xi),j=l-3,...l.
|
||||
40 call fpbspl(t,n,3,xi,l,h)
|
||||
c store in q these values h(1),h(2),...h(4).
|
||||
do 50 j=1,4
|
||||
q(i,j) = h(j)
|
||||
50 continue
|
||||
c add the contribution of the current row of the observation matrix
|
||||
c n to the normal equations.
|
||||
l3 = l-3
|
||||
k1 = 0
|
||||
do 60 j1 = l3,l
|
||||
k1 = k1+1
|
||||
f = h(k1)
|
||||
z(j1) = z(j1)+f*wi*y(i)
|
||||
k2 = k1
|
||||
j2 = 4
|
||||
do 60 j3 = j1,l
|
||||
a(j3,j2) = a(j3,j2)+f*wi*h(k2)
|
||||
k2 = k2+1
|
||||
j2 = j2-1
|
||||
60 continue
|
||||
70 continue
|
||||
c since n'n is a symmetric matrix it can be factorized as
|
||||
c (3) n'n = (r1)'(d1)(r1)
|
||||
c with d1 a diagonal matrix and r1 an (n-4) x (n-4) unit upper
|
||||
c triangular matrix of bandwidth 4. the matrices r1 and d1 are built
|
||||
c up in a. at the same time we solve the systems of equations
|
||||
c (4) (r1)'(z2) = n'y
|
||||
c (5) (d1) (z1) = (z2)
|
||||
c the vectors z2 and z1 are kept in zz and z.
|
||||
do 140 i=1,n4
|
||||
k1 = 1
|
||||
if(i.lt.4) k1 = 5-i
|
||||
k2 = i-4+k1
|
||||
k3 = k2
|
||||
do 100 j=k1,4
|
||||
k4 = j-1
|
||||
k5 = 4-j+k1
|
||||
f = a(i,j)
|
||||
if(k1.gt.k4) go to 90
|
||||
k6 = k2
|
||||
do 80 k=k1,k4
|
||||
f = f-a(i,k)*a(k3,k5)*a(k6,4)
|
||||
k5 = k5+1
|
||||
k6 = k6+1
|
||||
80 continue
|
||||
90 if(j.eq.4) go to 110
|
||||
a(i,j) = f/a(k3,4)
|
||||
k3 = k3+1
|
||||
100 continue
|
||||
110 a(i,4) = f
|
||||
f = z(i)
|
||||
if(i.eq.1) go to 130
|
||||
k4 = i
|
||||
do 120 j=k1,3
|
||||
k = k1+3-j
|
||||
k4 = k4-1
|
||||
f = f-a(i,k)*z(k4)*a(k4,4)
|
||||
120 continue
|
||||
130 z(i) = f/a(i,4)
|
||||
zz(i) = f
|
||||
140 continue
|
||||
c start computing the least-squares cubic spline without taking account
|
||||
c of any constraint.
|
||||
nbind = 0
|
||||
n1 = 1
|
||||
ibind(1) = 0
|
||||
c main loop for the least-squares problems with different subsets of
|
||||
c the constraints (2) in equality form. the resulting b-spline coeff.
|
||||
c c and lagrange parameters u are the solution of the system
|
||||
c ! n'n b' ! ! c ! ! n'y !
|
||||
c (6) ! ! ! ! = ! !
|
||||
c ! b 0 ! ! u ! ! 0 !
|
||||
c z1 is stored into array c.
|
||||
150 do 160 i=1,n4
|
||||
c(i) = z(i)
|
||||
160 continue
|
||||
c if there are no equality constraints, compute the coeff. c directly.
|
||||
if(nbind.eq.0) go to 370
|
||||
c initialization
|
||||
kdim = n4+nbind
|
||||
do 170 i=1,nbind
|
||||
do 170 j=1,kdim
|
||||
b(j,i) = 0.
|
||||
170 continue
|
||||
c matrix b is built up,expressing that the constraints nrs ibind(1),...
|
||||
c ibind(nbind) must be satisfied in equality form.
|
||||
do 180 i=1,nbind
|
||||
l = ibind(i)
|
||||
b(l,i) = e(l)
|
||||
b(l+1,i) = -(e(l)+const(l))
|
||||
b(l+2,i) = const(l)
|
||||
180 continue
|
||||
c find the matrix (b1) as the solution of the system of equations
|
||||
c (7) (r1)'(d1)(b1) = b'
|
||||
c (b1) is built up in the upper part of the array b(rows 1,...n-4).
|
||||
do 220 k1=1,nbind
|
||||
l = ibind(k1)
|
||||
do 210 i=l,n4
|
||||
f = b(i,k1)
|
||||
if(i.eq.1) go to 200
|
||||
k2 = 3
|
||||
if(i.lt.4) k2 = i-1
|
||||
do 190 k3=1,k2
|
||||
l1 = i-k3
|
||||
l2 = 4-k3
|
||||
f = f-b(l1,k1)*a(i,l2)*a(l1,4)
|
||||
190 continue
|
||||
200 b(i,k1) = f/a(i,4)
|
||||
210 continue
|
||||
220 continue
|
||||
c factorization of the symmetric matrix -(b1)'(d1)(b1)
|
||||
c (8) -(b1)'(d1)(b1) = (r2)'(d2)(r2)
|
||||
c with (d2) a diagonal matrix and (r2) an nbind x nbind unit upper
|
||||
c triangular matrix. the matrices r2 and d2 are built up in the lower
|
||||
c part of the array b (rows n-3,n-2,...n-4+nbind).
|
||||
do 270 i=1,nbind
|
||||
i1 = i-1
|
||||
do 260 j=i,nbind
|
||||
f = 0.
|
||||
do 230 k=1,n4
|
||||
f = f+b(k,i)*b(k,j)*a(k,4)
|
||||
230 continue
|
||||
k1 = n4+1
|
||||
if(i1.eq.0) go to 250
|
||||
do 240 k=1,i1
|
||||
f = f+b(k1,i)*b(k1,j)*b(k1,k)
|
||||
k1 = k1+1
|
||||
240 continue
|
||||
250 b(k1,j) = -f
|
||||
if(j.eq.i) go to 260
|
||||
b(k1,j) = b(k1,j)/b(k1,i)
|
||||
260 continue
|
||||
270 continue
|
||||
c according to (3),(7) and (8) the system of equations (6) becomes
|
||||
c ! (r1)' 0 ! ! (d1) 0 ! ! (r1) (b1) ! ! c ! ! n'y !
|
||||
c (9) ! ! ! ! ! ! ! ! = ! !
|
||||
c ! (b1)' (r2)'! ! 0 (d2) ! ! 0 (r2) ! ! u ! ! 0 !
|
||||
c backward substitution to obtain the b-spline coefficients c(j),j=1,..
|
||||
c n-4 and the lagrange parameters u(j),j=1,2,...nbind.
|
||||
c first step of the backward substitution: solve the system
|
||||
c ! (r1)'(d1) 0 ! ! (c1) ! ! n'y !
|
||||
c (10) ! ! ! ! = ! !
|
||||
c ! (b1)'(d1) (r2)'(d2) ! ! (u1) ! ! 0 !
|
||||
c from (4) and (5) we know that this is equivalent to
|
||||
c (11) (c1) = (z1)
|
||||
c (12) (r2)'(d2)(u1) = -(b1)'(z2)
|
||||
do 310 i=1,nbind
|
||||
f = 0.
|
||||
do 280 j=1,n4
|
||||
f = f+b(j,i)*zz(j)
|
||||
280 continue
|
||||
i1 = i-1
|
||||
k1 = n4+1
|
||||
if(i1.eq.0) go to 300
|
||||
do 290 j=1,i1
|
||||
f = f+u(j)*b(k1,i)*b(k1,j)
|
||||
k1 = k1+1
|
||||
290 continue
|
||||
300 u(i) = -f/b(k1,i)
|
||||
310 continue
|
||||
c second step of the backward substitution: solve the system
|
||||
c ! (r1) (b1) ! ! c ! ! c1 !
|
||||
c (13) ! ! ! ! = ! !
|
||||
c ! 0 (r2) ! ! u ! ! u1 !
|
||||
k1 = nbind
|
||||
k2 = kdim
|
||||
c find the lagrange parameters u.
|
||||
do 340 i=1,nbind
|
||||
f = u(k1)
|
||||
if(i.eq.1) go to 330
|
||||
k3 = k1+1
|
||||
do 320 j=k3,nbind
|
||||
f = f-u(j)*b(k2,j)
|
||||
320 continue
|
||||
330 u(k1) = f
|
||||
k1 = k1-1
|
||||
k2 = k2-1
|
||||
340 continue
|
||||
c find the b-spline coefficients c.
|
||||
do 360 i=1,n4
|
||||
f = c(i)
|
||||
do 350 j=1,nbind
|
||||
f = f-u(j)*b(i,j)
|
||||
350 continue
|
||||
c(i) = f
|
||||
360 continue
|
||||
370 k1 = n4
|
||||
do 390 i=2,n4
|
||||
k1 = k1-1
|
||||
f = c(k1)
|
||||
k2 = 1
|
||||
if(i.lt.5) k2 = 5-i
|
||||
k3 = k1
|
||||
l = 3
|
||||
do 380 j=k2,3
|
||||
k3 = k3+1
|
||||
f = f-a(k3,l)*c(k3)
|
||||
l = l-1
|
||||
380 continue
|
||||
c(k1) = f
|
||||
390 continue
|
||||
c test whether the solution of the least-squares problem with the
|
||||
c constraints ibind(1),...ibind(nbind) in equality form, satisfies
|
||||
c all of the constraints (2).
|
||||
k = 1
|
||||
c number counts the number of violated inequality constraints.
|
||||
number = 0
|
||||
do 440 j=1,n6
|
||||
l = ibind(k)
|
||||
k = k+1
|
||||
if(j.eq.l) go to 440
|
||||
k = k-1
|
||||
c test whether constraint j is satisfied
|
||||
f = e(j)*(c(j)-c(j+1))+const(j)*(c(j+2)-c(j+1))
|
||||
if(f.le.0.) go to 440
|
||||
c if constraint j is not satisfied, add a branch of length nbind+1
|
||||
c to the tree. the nodes of this branch contain in their information
|
||||
c field the number of the constraints ibind(1),...ibind(nbind) and j,
|
||||
c arranged in increasing order.
|
||||
number = number+1
|
||||
k1 = k-1
|
||||
if(k1.eq.0) go to 410
|
||||
do 400 i=1,k1
|
||||
jbind(i) = ibind(i)
|
||||
400 continue
|
||||
410 jbind(k) = j
|
||||
if(l.eq.0) go to 430
|
||||
do 420 i=k,nbind
|
||||
jbind(i+1) = ibind(i)
|
||||
420 continue
|
||||
430 call fpadno(maxtr,up,left,right,info,count,merk,jbind,n1,ier)
|
||||
c test whether the storage space which is required for the tree,exceeds
|
||||
c the available storage space.
|
||||
if(ier.ne.0) go to 560
|
||||
440 continue
|
||||
c test whether the solution of the least-squares problem with equality
|
||||
c constraints is a feasible solution.
|
||||
if(number.eq.0) go to 470
|
||||
c test whether there are still cases with nbind constraints in
|
||||
c equality form to be considered.
|
||||
450 if(merk.gt.1) go to 460
|
||||
nbind = n1
|
||||
c test whether the number of knots where s''(x)=0 exceeds maxbin.
|
||||
if(nbind.gt.maxbin) go to 550
|
||||
n1 = n1+1
|
||||
ibind(n1) = 0
|
||||
c search which cases with nbind constraints in equality form
|
||||
c are going to be considered.
|
||||
call fpdeno(maxtr,up,left,right,nbind,merk)
|
||||
c test whether the quadratic programming problem has a solution.
|
||||
if(merk.eq.1) go to 570
|
||||
c find a new case with nbind constraints in equality form.
|
||||
460 call fpseno(maxtr,up,left,right,info,merk,ibind,nbind)
|
||||
go to 150
|
||||
c test whether the feasible solution is optimal.
|
||||
470 ier = 0
|
||||
do 480 i=1,n6
|
||||
bind(i) = .false.
|
||||
480 continue
|
||||
if(nbind.eq.0) go to 500
|
||||
do 490 i=1,nbind
|
||||
if(u(i).le.0.) go to 450
|
||||
j = ibind(i)
|
||||
bind(j) = .true.
|
||||
490 continue
|
||||
c evaluate s(x) at the data points x(i) and calculate the weighted
|
||||
c sum of squared residual right hand sides sq.
|
||||
500 sq = 0.
|
||||
l = 4
|
||||
lp1 = 5
|
||||
do 530 i=1,m
|
||||
510 if(x(i).lt.t(lp1) .or. l.eq.n4) go to 520
|
||||
l = lp1
|
||||
lp1 = l+1
|
||||
go to 510
|
||||
520 sx(i) = c(l-3)*q(i,1)+c(l-2)*q(i,2)+c(l-1)*q(i,3)+c(l)*q(i,4)
|
||||
sq = sq+(w(i)*(y(i)-sx(i)))**2
|
||||
530 continue
|
||||
go to 600
|
||||
c error codes and messages.
|
||||
550 ier = 1
|
||||
go to 600
|
||||
560 ier = 2
|
||||
go to 600
|
||||
570 ier = 3
|
||||
600 return
|
||||
end
|
||||
57
mcc/bsplines/fpcsin.f
Normal file
57
mcc/bsplines/fpcsin.f
Normal file
@@ -0,0 +1,57 @@
|
||||
recursive subroutine fpcsin(a,b,par,sia,coa,sib,cob,ress,resc)
|
||||
implicit none
|
||||
c fpcsin calculates the integrals ress=integral((b-x)**3*sin(par*x))
|
||||
c and resc=integral((b-x)**3*cos(par*x)) over the interval (a,b),
|
||||
c given sia=sin(par*a),coa=cos(par*a),sib=sin(par*b) and cob=cos(par*b)
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 a,b,par,sia,coa,sib,cob,ress,resc
|
||||
c ..local scalars..
|
||||
integer i,j
|
||||
real*8 ab,ab4,ai,alfa,beta,b2,b4,eps,fac,f1,f2,one,quart,six,
|
||||
* three,two
|
||||
c ..function references..
|
||||
real*8 abs
|
||||
c ..
|
||||
one = 0.1e+01
|
||||
two = 0.2e+01
|
||||
three = 0.3e+01
|
||||
six = 0.6e+01
|
||||
quart = 0.25e+0
|
||||
eps = 0.1e-09
|
||||
ab = b-a
|
||||
ab4 = ab**4
|
||||
alfa = ab*par
|
||||
c the way of calculating the integrals ress and resc depends on
|
||||
c the value of alfa = (b-a)*par.
|
||||
if(abs(alfa).le.one) go to 100
|
||||
c integration by parts.
|
||||
beta = one/alfa
|
||||
b2 = beta**2
|
||||
b4 = six*b2**2
|
||||
f1 = three*b2*(one-two*b2)
|
||||
f2 = beta*(one-six*b2)
|
||||
ress = ab4*(coa*f2+sia*f1+sib*b4)
|
||||
resc = ab4*(coa*f1-sia*f2+cob*b4)
|
||||
go to 400
|
||||
c ress and resc are found by evaluating a series expansion.
|
||||
100 fac = quart
|
||||
f1 = fac
|
||||
f2 = 0.
|
||||
i = 4
|
||||
do 200 j=1,5
|
||||
i = i+1
|
||||
ai = i
|
||||
fac = fac*alfa/ai
|
||||
f2 = f2+fac
|
||||
if(abs(fac).le.eps) go to 300
|
||||
i = i+1
|
||||
ai = i
|
||||
fac = -fac*alfa/ai
|
||||
f1 = f1+fac
|
||||
if(abs(fac).le.eps) go to 300
|
||||
200 continue
|
||||
300 ress = ab4*(coa*f2+sia*f1)
|
||||
resc = ab4*(coa*f1-sia*f2)
|
||||
400 return
|
||||
end
|
||||
360
mcc/bsplines/fpcurf.f
Normal file
360
mcc/bsplines/fpcurf.f
Normal file
@@ -0,0 +1,360 @@
|
||||
recursive subroutine fpcurf(iopt,x,y,w,m,xb,xe,k,s,nest,tol,
|
||||
* maxit,k1,k2,n,t,c,fp,fpint,z,a,b,g,q,nrdata,ier)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 xb,xe,s,tol,fp
|
||||
integer iopt,m,k,nest,maxit,k1,k2,n,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),t(nest),c(nest),fpint(nest),
|
||||
* z(nest),a(nest,k1),b(nest,k2),g(nest,k2),q(m,k1)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 acc,con1,con4,con9,cos,half,fpart,fpms,fpold,fp0,f1,f2,f3,
|
||||
* one,p,pinv,piv,p1,p2,p3,rn,sin,store,term,wi,xi,yi
|
||||
integer i,ich1,ich3,it,iter,i1,i2,i3,j,k3,l,l0,
|
||||
* mk1,new,nk1,nmax,nmin,nplus,npl1,nrint,n8
|
||||
c ..local arrays..
|
||||
real*8 h(7)
|
||||
c ..function references
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpgivs,fpdisc,fpknot,fprota
|
||||
c ..
|
||||
c set constants
|
||||
one = 0.1d+01
|
||||
con1 = 0.1d0
|
||||
con9 = 0.9d0
|
||||
con4 = 0.4d-01
|
||||
half = 0.5d0
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position c
|
||||
c ************************************************************** c
|
||||
c given a set of knots we compute the least-squares spline sinf(x), c
|
||||
c and the corresponding sum of squared residuals fp=f(p=inf). c
|
||||
c if iopt=-1 sinf(x) is the requested approximation. c
|
||||
c if iopt=0 or iopt=1 we check whether we can accept the knots: c
|
||||
c if fp <=s we will continue with the current set of knots. c
|
||||
c if fp > s we will increase the number of knots and compute the c
|
||||
c corresponding least-squares spline until finally fp<=s. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmax = m+k+1. c
|
||||
c if s > 0 and c
|
||||
c iopt=0 we first compute the least-squares polynomial of c
|
||||
c degree k; n = nmin = 2*k+2 c
|
||||
c iopt=1 we start with the set of knots found at the last c
|
||||
c call of the routine, except for the case that s > fp0; then c
|
||||
c we compute directly the least-squares polynomial of degree k. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c determine nmin, the number of knots for polynomial approximation.
|
||||
nmin = 2*k1
|
||||
if(iopt.lt.0) go to 60
|
||||
c calculation of acc, the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c determine nmax, the number of knots for spline interpolation.
|
||||
nmax = m+k1
|
||||
if(s.gt.0.0d0) go to 45
|
||||
c if s=0, s(x) is an interpolating spline.
|
||||
c test whether the required storage space exceeds the available one.
|
||||
n = nmax
|
||||
if(nmax.gt.nest) go to 420
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
10 mk1 = m-k1
|
||||
if(mk1.eq.0) go to 60
|
||||
k3 = k/2
|
||||
i = k2
|
||||
j = k3+2
|
||||
if(k3*2.eq.k) go to 30
|
||||
do 20 l=1,mk1
|
||||
t(i) = x(j)
|
||||
i = i+1
|
||||
j = j+1
|
||||
20 continue
|
||||
go to 60
|
||||
30 do 40 l=1,mk1
|
||||
t(i) = (x(j)+x(j-1))*half
|
||||
i = i+1
|
||||
j = j+1
|
||||
40 continue
|
||||
go to 60
|
||||
c if s>0 our initial choice of knots depends on the value of iopt.
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c polynomial of degree k which is a spline without interior knots.
|
||||
c if iopt=1 and fp0>s we start computing the least squares spline
|
||||
c according to the set of knots found at the last call of the routine.
|
||||
45 if(iopt.eq.0) go to 50
|
||||
if(n.eq.nmin) go to 50
|
||||
fp0 = fpint(n)
|
||||
fpold = fpint(n-1)
|
||||
nplus = nrdata(n)
|
||||
if(fp0.gt.s) go to 60
|
||||
50 n = nmin
|
||||
fpold = 0.0d0
|
||||
nplus = 0
|
||||
nrdata(1) = m-2
|
||||
c main loop for the different sets of knots. m is a save upper bound
|
||||
c for the number of trials.
|
||||
60 do 200 iter = 1,m
|
||||
if(n.eq.nmin) ier = -2
|
||||
c find nrint, tne number of knot intervals.
|
||||
nrint = n-nmin+1
|
||||
c find the position of the additional knots which are needed for
|
||||
c the b-spline representation of s(x).
|
||||
nk1 = n-k1
|
||||
i = n
|
||||
do 70 j=1,k1
|
||||
t(j) = xb
|
||||
t(i) = xe
|
||||
i = i-1
|
||||
70 continue
|
||||
c compute the b-spline coefficients of the least-squares spline
|
||||
c sinf(x). the observation matrix a is built up row by row and
|
||||
c reduced to upper triangular form by givens transformations.
|
||||
c at the same time fp=f(p=inf) is computed.
|
||||
fp = 0.0d0
|
||||
c initialize the observation matrix a.
|
||||
do 80 i=1,nk1
|
||||
z(i) = 0.0d0
|
||||
do 80 j=1,k1
|
||||
a(i,j) = 0.0d0
|
||||
80 continue
|
||||
l = k1
|
||||
do 130 it=1,m
|
||||
c fetch the current data point x(it),y(it).
|
||||
xi = x(it)
|
||||
wi = w(it)
|
||||
yi = y(it)*wi
|
||||
c search for knot interval t(l) <= xi < t(l+1).
|
||||
85 if(xi.lt.t(l+1) .or. l.eq.nk1) go to 90
|
||||
l = l+1
|
||||
go to 85
|
||||
c evaluate the (k+1) non-zero b-splines at xi and store them in q.
|
||||
90 call fpbspl(t,n,k,xi,l,h)
|
||||
do 95 i=1,k1
|
||||
q(it,i) = h(i)
|
||||
h(i) = h(i)*wi
|
||||
95 continue
|
||||
c rotate the new row of the observation matrix into triangle.
|
||||
j = l-k1
|
||||
do 110 i=1,k1
|
||||
j = j+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.0d0) go to 110
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
call fprota(cos,sin,yi,z(j))
|
||||
if(i.eq.k1) go to 120
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 100 i1 = i3,k1
|
||||
i2 = i2+1
|
||||
c transformations to left hand side.
|
||||
call fprota(cos,sin,h(i1),a(j,i2))
|
||||
100 continue
|
||||
110 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
120 fp = fp+yi*yi
|
||||
130 continue
|
||||
if(ier.eq.(-2)) fp0 = fp
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = fpold
|
||||
nrdata(n) = nplus
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
call fpback(a,z,nk1,k1,c,nest)
|
||||
c test whether the approximation sinf(x) is an acceptable solution.
|
||||
if(iopt.lt.0) go to 440
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c if f(p=inf) < s accept the choice of knots.
|
||||
if(fpms.lt.0.0d0) go to 250
|
||||
c if n = nmax, sinf(x) is an interpolating spline.
|
||||
if(n.eq.nmax) go to 430
|
||||
c increase the number of knots.
|
||||
c if n=nest we cannot increase the number of knots because of
|
||||
c the storage capacity limitation.
|
||||
if(n.eq.nest) go to 420
|
||||
c determine the number of knots nplus we are going to add.
|
||||
if(ier.eq.0) go to 140
|
||||
nplus = 1
|
||||
ier = 0
|
||||
go to 150
|
||||
140 npl1 = nplus*2
|
||||
rn = nplus
|
||||
if(fpold-fp.gt.acc) npl1 = rn*fpms/(fpold-fp)
|
||||
nplus = min0(nplus*2,max0(npl1,nplus/2,1))
|
||||
150 fpold = fp
|
||||
c compute the sum((w(i)*(y(i)-s(x(i))))**2) for each knot interval
|
||||
c t(j+k) <= x(i) <= t(j+k+1) and store it in fpint(j),j=1,2,...nrint.
|
||||
fpart = 0.0d0
|
||||
i = 1
|
||||
l = k2
|
||||
new = 0
|
||||
do 180 it=1,m
|
||||
if(x(it).lt.t(l) .or. l.gt.nk1) go to 160
|
||||
new = 1
|
||||
l = l+1
|
||||
160 term = 0.0d0
|
||||
l0 = l-k2
|
||||
do 170 j=1,k1
|
||||
l0 = l0+1
|
||||
term = term+c(l0)*q(it,j)
|
||||
170 continue
|
||||
term = (w(it)*(term-y(it)))**2
|
||||
fpart = fpart+term
|
||||
if(new.eq.0) go to 180
|
||||
store = term*half
|
||||
fpint(i) = fpart-store
|
||||
i = i+1
|
||||
fpart = store
|
||||
new = 0
|
||||
180 continue
|
||||
fpint(nrint) = fpart
|
||||
do 190 l=1,nplus
|
||||
c add a new knot.
|
||||
call fpknot(x,m,t,n,fpint,nrdata,nrint,nest,1)
|
||||
c if n=nmax we locate the knots as for interpolation.
|
||||
if(n.eq.nmax) go to 10
|
||||
c test whether we cannot further increase the number of knots.
|
||||
if(n.eq.nest) go to 200
|
||||
190 continue
|
||||
c restart the computations with the new set of knots.
|
||||
200 continue
|
||||
c test whether the least-squares kth degree polynomial is a solution
|
||||
c of our approximation problem.
|
||||
250 if(ier.eq.(-2)) go to 440
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing spline sp(x). c
|
||||
c *************************************************** c
|
||||
c we have determined the number of knots and their position. c
|
||||
c we now compute the b-spline coefficients of the smoothing spline c
|
||||
c sp(x). the observation matrix a is extended by the rows of matrix c
|
||||
c b expressing that the kth derivative discontinuities of sp(x) at c
|
||||
c the interior knots t(k+2),...t(n-k-1) must be zero. the corres- c
|
||||
c ponding weights of these additional rows are set to 1/p. c
|
||||
c iteratively we then have to determine the value of p such that c
|
||||
c f(p)=sum((w(i)*(y(i)-sp(x(i))))**2) be = s. we already know that c
|
||||
c the least-squares kth degree polynomial corresponds to p=0, and c
|
||||
c that the least-squares spline corresponds to p=infinity. the c
|
||||
c iteration process which is proposed here, makes use of rational c
|
||||
c interpolation. since f(p) is a convex and strictly decreasing c
|
||||
c function of p, it can be approximated by a rational function c
|
||||
c r(p) = (u*p+v)/(p+w). three values of p(p1,p2,p3) with correspond- c
|
||||
c ing values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s) are used c
|
||||
c to calculate the new value of p such that r(p)=s. convergence is c
|
||||
c guaranteed by taking f1>0 and f3<0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c evaluate the discontinuity jump of the kth derivative of the
|
||||
c b-splines at the knots t(l),l=k+2,...n-k-1 and store in b.
|
||||
call fpdisc(t,n,k2,b,nest)
|
||||
c initial value for p.
|
||||
p1 = 0.0d0
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
p = 0.
|
||||
do 255 i=1,nk1
|
||||
p = p+a(i,1)
|
||||
255 continue
|
||||
rn = nk1
|
||||
p = rn/p
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
n8 = n-nmin
|
||||
c iteration process to find the root of f(p) = s.
|
||||
do 360 iter=1,maxit
|
||||
c the rows of matrix b with weight 1/p are rotated into the
|
||||
c triangularised observation matrix a which is stored in g.
|
||||
pinv = one/p
|
||||
do 260 i=1,nk1
|
||||
c(i) = z(i)
|
||||
g(i,k2) = 0.0d0
|
||||
do 260 j=1,k1
|
||||
g(i,j) = a(i,j)
|
||||
260 continue
|
||||
do 300 it=1,n8
|
||||
c the row of matrix b is rotated into triangle by givens transformation
|
||||
do 270 i=1,k2
|
||||
h(i) = b(it,i)*pinv
|
||||
270 continue
|
||||
yi = 0.0d0
|
||||
do 290 j=it,nk1
|
||||
piv = h(1)
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,g(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
call fprota(cos,sin,yi,c(j))
|
||||
if(j.eq.nk1) go to 300
|
||||
i2 = k1
|
||||
if(j.gt.n8) i2 = nk1-j
|
||||
do 280 i=1,i2
|
||||
c transformations to left hand side.
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h(i1),g(j,i1))
|
||||
h(i) = h(i1)
|
||||
280 continue
|
||||
h(i2+1) = 0.0d0
|
||||
290 continue
|
||||
300 continue
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
call fpback(g,c,nk1,k2,c,nest)
|
||||
c computation of f(p).
|
||||
fp = 0.0d0
|
||||
l = k2
|
||||
do 330 it=1,m
|
||||
if(x(it).lt.t(l) .or. l.gt.nk1) go to 310
|
||||
l = l+1
|
||||
310 l0 = l-k2
|
||||
term = 0.0d0
|
||||
do 320 j=1,k1
|
||||
l0 = l0+1
|
||||
term = term+c(l0)*q(it,j)
|
||||
320 continue
|
||||
fp = fp+(w(it)*(term-y(it)))**2
|
||||
330 continue
|
||||
c test whether the approximation sp(x) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c test whether the maximal number of iterations is reached.
|
||||
if(iter.eq.maxit) go to 400
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 340
|
||||
if((f2-f3).gt.acc) go to 335
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p=p1*con9 + p2*con1
|
||||
go to 360
|
||||
335 if(f2.lt.0.0d0) ich3=1
|
||||
340 if(ich1.ne.0) go to 350
|
||||
if((f1-f2).gt.acc) go to 345
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 360
|
||||
if(p.ge.p3) p = p2*con1 + p3*con9
|
||||
go to 360
|
||||
345 if(f2.gt.0.0d0) ich1=1
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
350 if(f2.ge.f1 .or. f2.le.f3) go to 410
|
||||
c find the new value for p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
360 continue
|
||||
c error codes and messages.
|
||||
400 ier = 3
|
||||
go to 440
|
||||
410 ier = 2
|
||||
go to 440
|
||||
420 ier = 1
|
||||
go to 440
|
||||
430 ier = -1
|
||||
440 return
|
||||
end
|
||||
95
mcc/bsplines/fpcuro.f
Normal file
95
mcc/bsplines/fpcuro.f
Normal file
@@ -0,0 +1,95 @@
|
||||
recursive subroutine fpcuro(a,b,c,d,x,n)
|
||||
implicit none
|
||||
c subroutine fpcuro finds the real zeros of a cubic polynomial
|
||||
c p(x) = a*x**3+b*x**2+c*x+d.
|
||||
c
|
||||
c calling sequence:
|
||||
c call fpcuro(a,b,c,d,x,n)
|
||||
c
|
||||
c input parameters:
|
||||
c a,b,c,d: real values, containing the coefficients of p(x).
|
||||
c
|
||||
c output parameters:
|
||||
c x : real array,length 3, which contains the real zeros of p(x)
|
||||
c n : integer, giving the number of real zeros of p(x).
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 a,b,c,d
|
||||
integer n
|
||||
c ..array argument..
|
||||
real*8 x(3)
|
||||
c ..local scalars..
|
||||
integer i
|
||||
real*8 a1,b1,c1,df,disc,d1,e3,f,four,half,ovfl,pi3,p3,q,r,
|
||||
* step,tent,three,two,u,u1,u2,y
|
||||
c ..function references..
|
||||
real*8 abs,max,datan,atan2,cos,sign,sqrt
|
||||
c set constants
|
||||
two = 0.2d+01
|
||||
three = 0.3d+01
|
||||
four = 0.4d+01
|
||||
ovfl =0.1d+05
|
||||
half = 0.5d+0
|
||||
tent = 0.1d+0
|
||||
e3 = tent/0.3d0
|
||||
pi3 = datan(0.1d+01)/0.75d0
|
||||
a1 = abs(a)
|
||||
b1 = abs(b)
|
||||
c1 = abs(c)
|
||||
d1 = abs(d)
|
||||
c test whether p(x) is a third degree polynomial.
|
||||
if(max(b1,c1,d1).lt.a1*ovfl) go to 300
|
||||
c test whether p(x) is a second degree polynomial.
|
||||
if(max(c1,d1).lt.b1*ovfl) go to 200
|
||||
c test whether p(x) is a first degree polynomial.
|
||||
if(d1.lt.c1*ovfl) go to 100
|
||||
c p(x) is a constant function.
|
||||
n = 0
|
||||
go to 800
|
||||
c p(x) is a first degree polynomial.
|
||||
100 n = 1
|
||||
x(1) = -d/c
|
||||
go to 500
|
||||
c p(x) is a second degree polynomial.
|
||||
200 disc = c*c-four*b*d
|
||||
n = 0
|
||||
if(disc.lt.0.) go to 800
|
||||
n = 2
|
||||
u = sqrt(disc)
|
||||
b1 = b+b
|
||||
x(1) = (-c+u)/b1
|
||||
x(2) = (-c-u)/b1
|
||||
go to 500
|
||||
c p(x) is a third degree polynomial.
|
||||
300 b1 = b/a*e3
|
||||
c1 = c/a
|
||||
d1 = d/a
|
||||
q = c1*e3-b1*b1
|
||||
r = b1*b1*b1+(d1-b1*c1)*half
|
||||
disc = q*q*q+r*r
|
||||
if(disc.gt.0.) go to 400
|
||||
u = sqrt(abs(q))
|
||||
if(r.lt.0.) u = -u
|
||||
p3 = atan2(sqrt(-disc),abs(r))*e3
|
||||
u2 = u+u
|
||||
n = 3
|
||||
x(1) = -u2*cos(p3)-b1
|
||||
x(2) = u2*cos(pi3-p3)-b1
|
||||
x(3) = u2*cos(pi3+p3)-b1
|
||||
go to 500
|
||||
400 u = sqrt(disc)
|
||||
u1 = -r+u
|
||||
u2 = -r-u
|
||||
n = 1
|
||||
x(1) = sign(abs(u1)**e3,u1)+sign(abs(u2)**e3,u2)-b1
|
||||
c apply a newton iteration to improve the accuracy of the roots.
|
||||
500 do 700 i=1,n
|
||||
y = x(i)
|
||||
f = ((a*y+b)*y+c)*y+d
|
||||
df = (three*a*y+two*b)*y+c
|
||||
step = 0.
|
||||
if(abs(f).lt.abs(df)*tent) step = f/df
|
||||
x(i) = y-step
|
||||
700 continue
|
||||
800 return
|
||||
end
|
||||
54
mcc/bsplines/fpcyt1.f
Normal file
54
mcc/bsplines/fpcyt1.f
Normal file
@@ -0,0 +1,54 @@
|
||||
recursive subroutine fpcyt1(a,n,nn)
|
||||
implicit none
|
||||
c (l u)-decomposition of a cyclic tridiagonal matrix with the non-zero
|
||||
c elements stored as follows
|
||||
c
|
||||
c | a(1,2) a(1,3) a(1,1) |
|
||||
c | a(2,1) a(2,2) a(2,3) |
|
||||
c | a(3,1) a(3,2) a(3,3) |
|
||||
c | ............... |
|
||||
c | a(n-1,1) a(n-1,2) a(n-1,3) |
|
||||
c | a(n,3) a(n,1) a(n,2) |
|
||||
c
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer n,nn
|
||||
c ..array arguments..
|
||||
real*8 a(nn,6)
|
||||
c ..local scalars..
|
||||
real*8 aa,beta,gamma,sum,teta,v,one
|
||||
integer i,n1,n2
|
||||
c ..
|
||||
c set constant
|
||||
one = 1
|
||||
n2 = n-2
|
||||
beta = one/a(1,2)
|
||||
gamma = a(n,3)
|
||||
teta = a(1,1)*beta
|
||||
a(1,4) = beta
|
||||
a(1,5) = gamma
|
||||
a(1,6) = teta
|
||||
sum = gamma*teta
|
||||
do 10 i=2,n2
|
||||
v = a(i-1,3)*beta
|
||||
aa = a(i,1)
|
||||
beta = one/(a(i,2)-aa*v)
|
||||
gamma = -gamma*v
|
||||
teta = -teta*aa*beta
|
||||
a(i,4) = beta
|
||||
a(i,5) = gamma
|
||||
a(i,6) = teta
|
||||
sum = sum+gamma*teta
|
||||
10 continue
|
||||
n1 = n-1
|
||||
v = a(n2,3)*beta
|
||||
aa = a(n1,1)
|
||||
beta = one/(a(n1,2)-aa*v)
|
||||
gamma = a(n,1)-gamma*v
|
||||
teta = (a(n1,3)-teta*aa)*beta
|
||||
a(n1,4) = beta
|
||||
a(n1,5) = gamma
|
||||
a(n1,6) = teta
|
||||
a(n,4) = one/(a(n,2)-(sum+gamma*teta))
|
||||
return
|
||||
end
|
||||
33
mcc/bsplines/fpcyt2.f
Normal file
33
mcc/bsplines/fpcyt2.f
Normal file
@@ -0,0 +1,33 @@
|
||||
recursive subroutine fpcyt2(a,n,b,c,nn)
|
||||
implicit none
|
||||
c subroutine fpcyt2 solves a linear n x n system
|
||||
c a * c = b
|
||||
c where matrix a is a cyclic tridiagonal matrix, decomposed
|
||||
c using subroutine fpsyt1.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer n,nn
|
||||
c ..array arguments..
|
||||
real*8 a(nn,6),b(n),c(n)
|
||||
c ..local scalars..
|
||||
real*8 cc,sum
|
||||
integer i,j,j1,n1
|
||||
c ..
|
||||
c(1) = b(1)*a(1,4)
|
||||
sum = c(1)*a(1,5)
|
||||
n1 = n-1
|
||||
do 10 i=2,n1
|
||||
c(i) = (b(i)-a(i,1)*c(i-1))*a(i,4)
|
||||
sum = sum+c(i)*a(i,5)
|
||||
10 continue
|
||||
cc = (b(n)-sum)*a(n,4)
|
||||
c(n) = cc
|
||||
c(n1) = c(n1)-cc*a(n1,6)
|
||||
j = n1
|
||||
do 20 i=3,n
|
||||
j1 = j-1
|
||||
c(j1) = c(j1)-c(j)*a(j1,3)*a(j1,4)-cc*a(j1,6)
|
||||
j = j1
|
||||
20 continue
|
||||
return
|
||||
end
|
||||
56
mcc/bsplines/fpdeno.f
Normal file
56
mcc/bsplines/fpdeno.f
Normal file
@@ -0,0 +1,56 @@
|
||||
recursive subroutine fpdeno(maxtr,up,left,right,nbind,merk)
|
||||
implicit none
|
||||
c subroutine fpdeno frees the nodes of all branches of a triply linked
|
||||
c tree with length < nbind by putting to zero their up field.
|
||||
c on exit the parameter merk points to the terminal node of the
|
||||
c most left branch of length nbind or takes the value 1 if there
|
||||
c is no such branch.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer maxtr,nbind,merk
|
||||
c ..array arguments..
|
||||
integer up(maxtr),left(maxtr),right(maxtr)
|
||||
c ..local scalars ..
|
||||
integer i,j,k,l,niveau,point
|
||||
c ..
|
||||
i = 1
|
||||
niveau = 0
|
||||
10 point = i
|
||||
i = left(point)
|
||||
if(i.eq.0) go to 20
|
||||
niveau = niveau+1
|
||||
go to 10
|
||||
20 if(niveau.eq.nbind) go to 70
|
||||
30 i = right(point)
|
||||
j = up(point)
|
||||
up(point) = 0
|
||||
k = left(j)
|
||||
if(point.ne.k) go to 50
|
||||
if(i.ne.0) go to 40
|
||||
niveau = niveau-1
|
||||
if(niveau.eq.0) go to 80
|
||||
point = j
|
||||
go to 30
|
||||
40 left(j) = i
|
||||
go to 10
|
||||
50 l = right(k)
|
||||
if(point.eq.l) go to 60
|
||||
k = l
|
||||
go to 50
|
||||
60 right(k) = i
|
||||
point = k
|
||||
70 i = right(point)
|
||||
if(i.ne.0) go to 10
|
||||
i = up(point)
|
||||
niveau = niveau-1
|
||||
if(niveau.eq.0) go to 80
|
||||
point = i
|
||||
go to 70
|
||||
80 k = 1
|
||||
l = left(k)
|
||||
if(up(l).eq.0) return
|
||||
90 merk = k
|
||||
k = left(k)
|
||||
if(k.ne.0) go to 90
|
||||
return
|
||||
end
|
||||
44
mcc/bsplines/fpdisc.f
Normal file
44
mcc/bsplines/fpdisc.f
Normal file
@@ -0,0 +1,44 @@
|
||||
recursive subroutine fpdisc(t,n,k2,b,nest)
|
||||
implicit none
|
||||
c subroutine fpdisc calculates the discontinuity jumps of the kth
|
||||
c derivative of the b-splines of degree k at the knots t(k+2)..t(n-k-1)
|
||||
c ..scalar arguments..
|
||||
integer n,k2,nest
|
||||
c ..array arguments..
|
||||
real*8 t(n),b(nest,k2)
|
||||
c ..local scalars..
|
||||
real*8 an,fac,prod
|
||||
integer i,ik,j,jk,k,k1,l,lj,lk,lmk,lp,nk1,nrint
|
||||
c ..local array..
|
||||
real*8 h(12)
|
||||
c ..
|
||||
k1 = k2-1
|
||||
k = k1-1
|
||||
nk1 = n-k1
|
||||
nrint = nk1-k
|
||||
an = nrint
|
||||
fac = an/(t(nk1+1)-t(k1))
|
||||
do 40 l=k2,nk1
|
||||
lmk = l-k1
|
||||
do 10 j=1,k1
|
||||
ik = j+k1
|
||||
lj = l+j
|
||||
lk = lj-k2
|
||||
h(j) = t(l)-t(lk)
|
||||
h(ik) = t(l)-t(lj)
|
||||
10 continue
|
||||
lp = lmk
|
||||
do 30 j=1,k2
|
||||
jk = j
|
||||
prod = h(j)
|
||||
do 20 i=1,k
|
||||
jk = jk+1
|
||||
prod = prod*h(jk)*fac
|
||||
20 continue
|
||||
lk = lp+k1
|
||||
b(lmk,j) = (t(lk)-t(lp))/prod
|
||||
lp = lp+1
|
||||
30 continue
|
||||
40 continue
|
||||
return
|
||||
end
|
||||
70
mcc/bsplines/fpfrno.f
Normal file
70
mcc/bsplines/fpfrno.f
Normal file
@@ -0,0 +1,70 @@
|
||||
recursive subroutine fpfrno(maxtr,up,left,right,info,point,
|
||||
* merk,n1,count,ier)
|
||||
implicit none
|
||||
c subroutine fpfrno collects the free nodes (up field zero) of the
|
||||
c triply linked tree the information of which is kept in the arrays
|
||||
c up,left,right and info. the maximal length of the branches of the
|
||||
c tree is given by n1. if no free nodes are found, the error flag
|
||||
c ier is set to 1.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer maxtr,point,merk,n1,count,ier
|
||||
c ..array arguments..
|
||||
integer up(maxtr),left(maxtr),right(maxtr),info(maxtr)
|
||||
c ..local scalars
|
||||
integer i,j,k,l,n,niveau
|
||||
c ..
|
||||
ier = 1
|
||||
if(n1.eq.2) go to 140
|
||||
niveau = 1
|
||||
count = 2
|
||||
10 j = 0
|
||||
i = 1
|
||||
20 if(j.eq.niveau) go to 30
|
||||
k = 0
|
||||
l = left(i)
|
||||
if(l.eq.0) go to 110
|
||||
i = l
|
||||
j = j+1
|
||||
go to 20
|
||||
30 if (i.lt.count) go to 110
|
||||
if (i.eq.count) go to 100
|
||||
go to 40
|
||||
40 if(up(count).eq.0) go to 50
|
||||
count = count+1
|
||||
go to 30
|
||||
50 up(count) = up(i)
|
||||
left(count) = left(i)
|
||||
right(count) = right(i)
|
||||
info(count) = info(i)
|
||||
if(merk.eq.i) merk = count
|
||||
if(point.eq.i) point = count
|
||||
if(k.eq.0) go to 60
|
||||
right(k) = count
|
||||
go to 70
|
||||
60 n = up(i)
|
||||
left(n) = count
|
||||
70 l = left(i)
|
||||
80 if(l.eq.0) go to 90
|
||||
up(l) = count
|
||||
l = right(l)
|
||||
go to 80
|
||||
90 up(i) = 0
|
||||
i = count
|
||||
100 count = count+1
|
||||
110 l = right(i)
|
||||
k = i
|
||||
if(l.eq.0) go to 120
|
||||
i = l
|
||||
go to 20
|
||||
120 l = up(i)
|
||||
j = j-1
|
||||
if(j.eq.0) go to 130
|
||||
i = l
|
||||
go to 110
|
||||
130 niveau = niveau+1
|
||||
if(niveau.le.n1) go to 10
|
||||
if(count.gt.maxtr) go to 140
|
||||
ier = 0
|
||||
140 return
|
||||
end
|
||||
21
mcc/bsplines/fpgivs.f
Normal file
21
mcc/bsplines/fpgivs.f
Normal file
@@ -0,0 +1,21 @@
|
||||
recursive subroutine fpgivs(piv,ww,cos,sin)
|
||||
implicit none
|
||||
c subroutine fpgivs calculates the parameters of a givens
|
||||
c transformation .
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 piv,ww,cos,sin
|
||||
c ..local scalars..
|
||||
real*8 dd,one,store
|
||||
c ..function references..
|
||||
real*8 abs,sqrt
|
||||
c ..
|
||||
one = 0.1e+01
|
||||
store = abs(piv)
|
||||
if(store.ge.ww) dd = store*sqrt(one+(ww/piv)**2)
|
||||
if(store.lt.ww) dd = ww*sqrt(one+(piv/ww)**2)
|
||||
cos = ww/dd
|
||||
sin = piv/dd
|
||||
ww = dd
|
||||
return
|
||||
end
|
||||
601
mcc/bsplines/fpgrdi.f
Normal file
601
mcc/bsplines/fpgrdi.f
Normal file
@@ -0,0 +1,601 @@
|
||||
recursive subroutine fpgrdi(ifsu,ifsv,ifbu,ifbv,iback,u,mu,v,
|
||||
* mv,z,mz,dz,iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,
|
||||
* mvnu,spu,spv,right,q,au,av1,av2,bu,bv,aa,bb,cc,cosi,nru,nrv)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 p,sq,fp
|
||||
integer ifsu,ifsv,ifbu,ifbv,iback,mu,mv,mz,iop0,iop1,nu,nv,nc,
|
||||
* mm,mvnu
|
||||
c ..array arguments..
|
||||
real*8 u(mu),v(mv),z(mz),dz(3),tu(nu),tv(nv),c(nc),fpu(nu),fpv(nv)
|
||||
*,
|
||||
* spu(mu,4),spv(mv,4),right(mm),q(mvnu),au(nu,5),av1(nv,6),
|
||||
* av2(nv,4),aa(2,mv),bb(2,nv),cc(nv),cosi(2,nv),bu(nu,5),bv(nv,5)
|
||||
integer nru(mu),nrv(mv)
|
||||
c ..local scalars..
|
||||
real*8 arg,co,dz1,dz2,dz3,fac,fac0,pinv,piv,si,term,one,three,half
|
||||
*
|
||||
integer i,ic,ii,ij,ik,iq,irot,it,iz,i0,i1,i2,i3,j,jj,jk,jper,
|
||||
* j0,j1,k,k1,k2,l,l0,l1,l2,mvv,ncof,nrold,nroldu,nroldv,number,
|
||||
* numu,numu1,numv,numv1,nuu,nu4,nu7,nu8,nu9,nv11,nv4,nv7,nv8,n1
|
||||
c ..local arrays..
|
||||
real*8 h(5),h1(5),h2(4)
|
||||
c ..function references..
|
||||
integer min0
|
||||
real*8 cos,sin
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpgivs,fpcyt1,fpcyt2,fpdisc,fpbacp,fprota
|
||||
c ..
|
||||
c let
|
||||
c | (spu) | | (spv) |
|
||||
c (au) = | ---------- | (av) = | ---------- |
|
||||
c | (1/p) (bu) | | (1/p) (bv) |
|
||||
c
|
||||
c | z ' 0 |
|
||||
c q = | ------ |
|
||||
c | 0 ' 0 |
|
||||
c
|
||||
c with c : the (nu-4) x (nv-4) matrix which contains the b-spline
|
||||
c coefficients.
|
||||
c z : the mu x mv matrix which contains the function values.
|
||||
c spu,spv: the mu x (nu-4), resp. mv x (nv-4) observation matrices
|
||||
c according to the least-squares problems in the u-,resp.
|
||||
c v-direction.
|
||||
c bu,bv : the (nu-7) x (nu-4),resp. (nv-7) x (nv-4) matrices
|
||||
c containing the discontinuity jumps of the derivatives
|
||||
c of the b-splines in the u-,resp.v-variable at the knots
|
||||
c the b-spline coefficients of the smoothing spline are then calculated
|
||||
c as the least-squares solution of the following over-determined linear
|
||||
c system of equations
|
||||
c
|
||||
c (1) (av) c (au)' = q
|
||||
c
|
||||
c subject to the constraints
|
||||
c
|
||||
c (2) c(i,nv-3+j) = c(i,j), j=1,2,3 ; i=1,2,...,nu-4
|
||||
c
|
||||
c (3) if iop0 = 0 c(1,j) = dz(1)
|
||||
c iop0 = 1 c(1,j) = dz(1)
|
||||
c c(2,j) = dz(1)+(dz(2)*cosi(1,j)+dz(3)*cosi(2,j))*
|
||||
c tu(5)/3. = cc(j) , j=1,2,...nv-4
|
||||
c
|
||||
c (4) if iop1 = 1 c(nu-4,j) = 0, j=1,2,...,nv-4.
|
||||
c
|
||||
c set constants
|
||||
one = 1
|
||||
three = 3
|
||||
half = 0.5
|
||||
c initialization
|
||||
nu4 = nu-4
|
||||
nu7 = nu-7
|
||||
nu8 = nu-8
|
||||
nu9 = nu-9
|
||||
nv4 = nv-4
|
||||
nv7 = nv-7
|
||||
nv8 = nv-8
|
||||
nv11 = nv-11
|
||||
nuu = nu4-iop0-iop1-1
|
||||
if(p.gt.0.) pinv = one/p
|
||||
c it depends on the value of the flags ifsu,ifsv,ifbu,ifbv and iop0 and
|
||||
c on the value of p whether the matrices (spu), (spv), (bu), (bv) and
|
||||
c (cosi) still must be determined.
|
||||
if(ifsu.ne.0) go to 30
|
||||
c calculate the non-zero elements of the matrix (spu) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the u-direction.
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 25 it=1,mu
|
||||
arg = u(it)
|
||||
10 if(arg.lt.tu(l1) .or. l.eq.nu4) go to 15
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 10
|
||||
15 call fpbspl(tu,nu,3,arg,l,h)
|
||||
do 20 i=1,4
|
||||
spu(it,i) = h(i)
|
||||
20 continue
|
||||
nru(it) = number
|
||||
25 continue
|
||||
ifsu = 1
|
||||
c calculate the non-zero elements of the matrix (spv) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the v-direction.
|
||||
30 if(ifsv.ne.0) go to 85
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 50 it=1,mv
|
||||
arg = v(it)
|
||||
35 if(arg.lt.tv(l1) .or. l.eq.nv4) go to 40
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 35
|
||||
40 call fpbspl(tv,nv,3,arg,l,h)
|
||||
do 45 i=1,4
|
||||
spv(it,i) = h(i)
|
||||
45 continue
|
||||
nrv(it) = number
|
||||
50 continue
|
||||
ifsv = 1
|
||||
if(iop0.eq.0) go to 85
|
||||
c calculate the coefficients of the interpolating splines for cos(v)
|
||||
c and sin(v).
|
||||
do 55 i=1,nv4
|
||||
cosi(1,i) = 0.
|
||||
cosi(2,i) = 0.
|
||||
55 continue
|
||||
if(nv7.lt.4) go to 85
|
||||
do 65 i=1,nv7
|
||||
l = i+3
|
||||
arg = tv(l)
|
||||
call fpbspl(tv,nv,3,arg,l,h)
|
||||
do 60 j=1,3
|
||||
av1(i,j) = h(j)
|
||||
60 continue
|
||||
cosi(1,i) = cos(arg)
|
||||
cosi(2,i) = sin(arg)
|
||||
65 continue
|
||||
call fpcyt1(av1,nv7,nv)
|
||||
do 80 j=1,2
|
||||
do 70 i=1,nv7
|
||||
right(i) = cosi(j,i)
|
||||
70 continue
|
||||
call fpcyt2(av1,nv7,right,right,nv)
|
||||
do 75 i=1,nv7
|
||||
cosi(j,i+1) = right(i)
|
||||
75 continue
|
||||
cosi(j,1) = cosi(j,nv7+1)
|
||||
cosi(j,nv7+2) = cosi(j,2)
|
||||
cosi(j,nv4) = cosi(j,3)
|
||||
80 continue
|
||||
85 if(p.le.0.) go to 150
|
||||
c calculate the non-zero elements of the matrix (bu).
|
||||
if(ifbu.ne.0 .or. nu8.eq.0) go to 90
|
||||
call fpdisc(tu,nu,5,bu,nu)
|
||||
ifbu = 1
|
||||
c calculate the non-zero elements of the matrix (bv).
|
||||
90 if(ifbv.ne.0 .or. nv8.eq.0) go to 150
|
||||
call fpdisc(tv,nv,5,bv,nv)
|
||||
ifbv = 1
|
||||
c substituting (2),(3) and (4) into (1), we obtain the overdetermined
|
||||
c system
|
||||
c (5) (avv) (cr) (auu)' = (qq)
|
||||
c from which the nuu*nv7 remaining coefficients
|
||||
c c(i,j) , i=2+iop0,3+iop0,...,nu-4-iop1 ; j=1,2,...,nv-7 ,
|
||||
c the elements of (cr), are then determined in the least-squares sense.
|
||||
c simultaneously, we compute the resulting sum of squared residuals sq.
|
||||
150 dz1 = dz(1)
|
||||
do 155 i=1,mv
|
||||
aa(1,i) = dz1
|
||||
155 continue
|
||||
if(nv8.eq.0 .or. p.le.0.) go to 165
|
||||
do 160 i=1,nv8
|
||||
bb(1,i) = 0.
|
||||
160 continue
|
||||
165 mvv = mv
|
||||
if(iop0.eq.0) go to 220
|
||||
fac = tu(5)/three
|
||||
dz2 = dz(2)*fac
|
||||
dz3 = dz(3)*fac
|
||||
do 170 i=1,nv4
|
||||
cc(i) = dz1+dz2*cosi(1,i)+dz3*cosi(2,i)
|
||||
170 continue
|
||||
do 190 i=1,mv
|
||||
number = nrv(i)
|
||||
fac = 0.
|
||||
do 180 j=1,4
|
||||
number = number+1
|
||||
fac = fac+cc(number)*spv(i,j)
|
||||
180 continue
|
||||
aa(2,i) = fac
|
||||
190 continue
|
||||
if(nv8.eq.0 .or. p.le.0.) go to 220
|
||||
do 210 i=1,nv8
|
||||
number = i
|
||||
fac = 0.
|
||||
do 200 j=1,5
|
||||
fac = fac+cc(number)*bv(i,j)
|
||||
number = number+1
|
||||
200 continue
|
||||
bb(2,i) = fac*pinv
|
||||
210 continue
|
||||
mvv = mvv+nv8
|
||||
c we first determine the matrices (auu) and (qq). then we reduce the
|
||||
c matrix (auu) to upper triangular form (ru) using givens rotations.
|
||||
c we apply the same transformations to the rows of matrix qq to obtain
|
||||
c the (mv+nv8) x nuu matrix g.
|
||||
c we store matrix (ru) into au and g into q.
|
||||
220 l = mvv*nuu
|
||||
c initialization.
|
||||
sq = 0.
|
||||
do 230 i=1,l
|
||||
q(i) = 0.
|
||||
230 continue
|
||||
do 240 i=1,nuu
|
||||
do 240 j=1,5
|
||||
au(i,j) = 0.
|
||||
240 continue
|
||||
l = 0
|
||||
nrold = 0
|
||||
n1 = nrold+1
|
||||
do 420 it=1,mu
|
||||
number = nru(it)
|
||||
c find the appropriate column of q.
|
||||
250 do 260 j=1,mvv
|
||||
right(j) = 0.
|
||||
260 continue
|
||||
if(nrold.eq.number) go to 280
|
||||
if(p.le.0.) go to 410
|
||||
c fetch a new row of matrix (bu).
|
||||
do 270 j=1,5
|
||||
h(j) = bu(n1,j)*pinv
|
||||
270 continue
|
||||
i0 = 1
|
||||
i1 = 5
|
||||
go to 310
|
||||
c fetch a new row of matrix (spu).
|
||||
280 do 290 j=1,4
|
||||
h(j) = spu(it,j)
|
||||
290 continue
|
||||
c find the appropriate column of q.
|
||||
do 300 j=1,mv
|
||||
l = l+1
|
||||
right(j) = z(l)
|
||||
300 continue
|
||||
i0 = 1
|
||||
i1 = 4
|
||||
310 if(nu7-number .eq. iop1) i1 = i1-1
|
||||
j0 = n1
|
||||
c take into account that we eliminate the constraints (3)
|
||||
320 if(j0-1.gt.iop0) go to 360
|
||||
fac0 = h(i0)
|
||||
do 330 j=1,mv
|
||||
right(j) = right(j)-fac0*aa(j0,j)
|
||||
330 continue
|
||||
if(mv.eq.mvv) go to 350
|
||||
j = mv
|
||||
do 340 jj=1,nv8
|
||||
j = j+1
|
||||
right(j) = right(j)-fac0*bb(j0,jj)
|
||||
340 continue
|
||||
350 j0 = j0+1
|
||||
i0 = i0+1
|
||||
go to 320
|
||||
360 irot = nrold-iop0-1
|
||||
if(irot.lt.0) irot = 0
|
||||
c rotate the new row of matrix (auu) into triangle.
|
||||
do 390 i=i0,i1
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 390
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,au(irot,1),co,si)
|
||||
c apply that transformation to the rows of matrix (qq).
|
||||
iq = (irot-1)*mvv
|
||||
do 370 j=1,mvv
|
||||
iq = iq+1
|
||||
call fprota(co,si,right(j),q(iq))
|
||||
370 continue
|
||||
c apply that transformation to the columns of (auu).
|
||||
if(i.eq.i1) go to 390
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 380 j=i3,i1
|
||||
i2 = i2+1
|
||||
call fprota(co,si,h(j),au(irot,i2))
|
||||
380 continue
|
||||
390 continue
|
||||
c we update the sum of squared residuals
|
||||
do 395 j=1,mvv
|
||||
sq = sq+right(j)**2
|
||||
395 continue
|
||||
if(nrold.eq.number) go to 420
|
||||
410 nrold = n1
|
||||
n1 = n1+1
|
||||
go to 250
|
||||
420 continue
|
||||
c we determine the matrix (avv) and then we reduce her to
|
||||
c upper triangular form (rv) using givens rotations.
|
||||
c we apply the same transformations to the columns of matrix
|
||||
c g to obtain the (nv-7) x (nu-5-iop0-iop1) matrix h.
|
||||
c we store matrix (rv) into av1 and av2, h into c.
|
||||
c the nv7 x nv7 upper triangular matrix (rv) has the form
|
||||
c | av1 ' |
|
||||
c (rv) = | ' av2 |
|
||||
c | 0 ' |
|
||||
c with (av2) a nv7 x 4 matrix and (av1) a nv11 x nv11 upper
|
||||
c triangular matrix of bandwidth 5.
|
||||
ncof = nuu*nv7
|
||||
c initialization.
|
||||
do 430 i=1,ncof
|
||||
c(i) = 0.
|
||||
430 continue
|
||||
do 440 i=1,nv4
|
||||
av1(i,5) = 0.
|
||||
do 440 j=1,4
|
||||
av1(i,j) = 0.
|
||||
av2(i,j) = 0.
|
||||
440 continue
|
||||
jper = 0
|
||||
nrold = 0
|
||||
do 770 it=1,mv
|
||||
number = nrv(it)
|
||||
450 if(nrold.eq.number) go to 480
|
||||
if(p.le.0.) go to 760
|
||||
c fetch a new row of matrix (bv).
|
||||
n1 = nrold+1
|
||||
do 460 j=1,5
|
||||
h(j) = bv(n1,j)*pinv
|
||||
460 continue
|
||||
c find the appropriate row of g.
|
||||
do 465 j=1,nuu
|
||||
right(j) = 0.
|
||||
465 continue
|
||||
if(mv.eq.mvv) go to 510
|
||||
l = mv+n1
|
||||
do 470 j=1,nuu
|
||||
right(j) = q(l)
|
||||
l = l+mvv
|
||||
470 continue
|
||||
go to 510
|
||||
c fetch a new row of matrix (spv)
|
||||
480 h(5) = 0.
|
||||
do 490 j=1,4
|
||||
h(j) = spv(it,j)
|
||||
490 continue
|
||||
c find the appropriate row of g.
|
||||
l = it
|
||||
do 500 j=1,nuu
|
||||
right(j) = q(l)
|
||||
l = l+mvv
|
||||
500 continue
|
||||
c test whether there are non-zero values in the new row of (avv)
|
||||
c corresponding to the b-splines n(j,v),j=nv7+1,...,nv4.
|
||||
510 if(nrold.lt.nv11) go to 710
|
||||
if(jper.ne.0) go to 550
|
||||
c initialize the matrix (av2).
|
||||
jk = nv11+1
|
||||
do 540 i=1,4
|
||||
ik = jk
|
||||
do 520 j=1,5
|
||||
if(ik.le.0) go to 530
|
||||
av2(ik,i) = av1(ik,j)
|
||||
ik = ik-1
|
||||
520 continue
|
||||
530 jk = jk+1
|
||||
540 continue
|
||||
jper = 1
|
||||
c if one of the non-zero elements of the new row corresponds to one of
|
||||
c the b-splines n(j;v),j=nv7+1,...,nv4, we take account of condition
|
||||
c (2) for setting up this row of (avv). the row is stored in h1( the
|
||||
c part with respect to av1) and h2 (the part with respect to av2).
|
||||
550 do 560 i=1,4
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
560 continue
|
||||
h1(5) = 0.
|
||||
j = nrold-nv11
|
||||
do 600 i=1,5
|
||||
j = j+1
|
||||
l0 = j
|
||||
570 l1 = l0-4
|
||||
if(l1.le.0) go to 590
|
||||
if(l1.le.nv11) go to 580
|
||||
l0 = l1-nv11
|
||||
go to 570
|
||||
580 h1(l1) = h(i)
|
||||
go to 600
|
||||
590 h2(l0) = h2(l0) + h(i)
|
||||
600 continue
|
||||
c rotate the new row of (avv) into triangle.
|
||||
if(nv11.le.0) go to 670
|
||||
c rotations with the rows 1,2,...,nv11 of (avv).
|
||||
do 660 j=1,nv11
|
||||
piv = h1(1)
|
||||
i2 = min0(nv11-j,4)
|
||||
if(piv.eq.0.) go to 640
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av1(j,1),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = j
|
||||
do 610 i=1,nuu
|
||||
call fprota(co,si,right(i),c(ic))
|
||||
ic = ic+nv7
|
||||
610 continue
|
||||
c apply that transformation to the rows of (avv) with respect to av2.
|
||||
do 620 i=1,4
|
||||
call fprota(co,si,h2(i),av2(j,i))
|
||||
620 continue
|
||||
c apply that transformation to the rows of (avv) with respect to av1.
|
||||
if(i2.eq.0) go to 670
|
||||
do 630 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(co,si,h1(i1),av1(j,i1))
|
||||
630 continue
|
||||
640 do 650 i=1,i2
|
||||
h1(i) = h1(i+1)
|
||||
650 continue
|
||||
h1(i2+1) = 0.
|
||||
660 continue
|
||||
c rotations with the rows nv11+1,...,nv7 of avv.
|
||||
670 do 700 j=1,4
|
||||
ij = nv11+j
|
||||
if(ij.le.0) go to 700
|
||||
piv = h2(j)
|
||||
if(piv.eq.0.) go to 700
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av2(ij,j),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = ij
|
||||
do 680 i=1,nuu
|
||||
call fprota(co,si,right(i),c(ic))
|
||||
ic = ic+nv7
|
||||
680 continue
|
||||
if(j.eq.4) go to 700
|
||||
c apply that transformation to the rows of (avv) with respect to av2.
|
||||
j1 = j+1
|
||||
do 690 i=j1,4
|
||||
call fprota(co,si,h2(i),av2(ij,i))
|
||||
690 continue
|
||||
700 continue
|
||||
c we update the sum of squared residuals
|
||||
do 705 i=1,nuu
|
||||
sq = sq+right(i)**2
|
||||
705 continue
|
||||
go to 750
|
||||
c rotation into triangle of the new row of (avv), in case the elements
|
||||
c corresponding to the b-splines n(j;v),j=nv7+1,...,nv4 are all zero.
|
||||
710 irot =nrold
|
||||
do 740 i=1,5
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 740
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av1(irot,1),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = irot
|
||||
do 720 j=1,nuu
|
||||
call fprota(co,si,right(j),c(ic))
|
||||
ic = ic+nv7
|
||||
720 continue
|
||||
c apply that transformation to the rows of (avv).
|
||||
if(i.eq.5) go to 740
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 730 j=i3,5
|
||||
i2 = i2+1
|
||||
call fprota(co,si,h(j),av1(irot,i2))
|
||||
730 continue
|
||||
740 continue
|
||||
c we update the sum of squared residuals
|
||||
do 745 i=1,nuu
|
||||
sq = sq+right(i)**2
|
||||
745 continue
|
||||
750 if(nrold.eq.number) go to 770
|
||||
760 nrold = nrold+1
|
||||
go to 450
|
||||
770 continue
|
||||
c test whether the b-spline coefficients must be determined.
|
||||
if(iback.ne.0) return
|
||||
c backward substitution to obtain the b-spline coefficients as the
|
||||
c solution of the linear system (rv) (cr) (ru)' = h.
|
||||
c first step: solve the system (rv) (c1) = h.
|
||||
k = 1
|
||||
do 780 i=1,nuu
|
||||
call fpbacp(av1,av2,c(k),nv7,4,c(k),5,nv)
|
||||
k = k+nv7
|
||||
780 continue
|
||||
c second step: solve the system (cr) (ru)' = (c1).
|
||||
k = 0
|
||||
do 800 j=1,nv7
|
||||
k = k+1
|
||||
l = k
|
||||
do 790 i=1,nuu
|
||||
right(i) = c(l)
|
||||
l = l+nv7
|
||||
790 continue
|
||||
call fpback(au,right,nuu,5,right,nu)
|
||||
l = k
|
||||
do 795 i=1,nuu
|
||||
c(l) = right(i)
|
||||
l = l+nv7
|
||||
795 continue
|
||||
800 continue
|
||||
c calculate from the conditions (2)-(3)-(4), the remaining b-spline
|
||||
c coefficients.
|
||||
ncof = nu4*nv4
|
||||
i = nv4
|
||||
j = 0
|
||||
do 805 l=1,nv4
|
||||
q(l) = dz1
|
||||
805 continue
|
||||
if(iop0.eq.0) go to 815
|
||||
do 810 l=1,nv4
|
||||
i = i+1
|
||||
q(i) = cc(l)
|
||||
810 continue
|
||||
815 if(nuu.eq.0) go to 850
|
||||
do 840 l=1,nuu
|
||||
ii = i
|
||||
do 820 k=1,nv7
|
||||
i = i+1
|
||||
j = j+1
|
||||
q(i) = c(j)
|
||||
820 continue
|
||||
do 830 k=1,3
|
||||
ii = ii+1
|
||||
i = i+1
|
||||
q(i) = q(ii)
|
||||
830 continue
|
||||
840 continue
|
||||
850 if(iop1.eq.0) go to 870
|
||||
do 860 l=1,nv4
|
||||
i = i+1
|
||||
q(i) = 0.
|
||||
860 continue
|
||||
870 do 880 i=1,ncof
|
||||
c(i) = q(i)
|
||||
880 continue
|
||||
c calculate the quantities
|
||||
c res(i,j) = (z(i,j) - s(u(i),v(j)))**2 , i=1,2,..,mu;j=1,2,..,mv
|
||||
c fp = sumi=1,mu(sumj=1,mv(res(i,j)))
|
||||
c fpu(r) = sum''i(sumj=1,mv(res(i,j))) , r=1,2,...,nu-7
|
||||
c tu(r+3) <= u(i) <= tu(r+4)
|
||||
c fpv(r) = sumi=1,mu(sum''j(res(i,j))) , r=1,2,...,nv-7
|
||||
c tv(r+3) <= v(j) <= tv(r+4)
|
||||
fp = 0.
|
||||
do 890 i=1,nu
|
||||
fpu(i) = 0.
|
||||
890 continue
|
||||
do 900 i=1,nv
|
||||
fpv(i) = 0.
|
||||
900 continue
|
||||
iz = 0
|
||||
nroldu = 0
|
||||
c main loop for the different grid points.
|
||||
do 950 i1=1,mu
|
||||
numu = nru(i1)
|
||||
numu1 = numu+1
|
||||
nroldv = 0
|
||||
do 940 i2=1,mv
|
||||
numv = nrv(i2)
|
||||
numv1 = numv+1
|
||||
iz = iz+1
|
||||
c evaluate s(u,v) at the current grid point by making the sum of the
|
||||
c cross products of the non-zero b-splines at (u,v), multiplied with
|
||||
c the appropriate b-spline coefficients.
|
||||
term = 0.
|
||||
k1 = numu*nv4+numv
|
||||
do 920 l1=1,4
|
||||
k2 = k1
|
||||
fac = spu(i1,l1)
|
||||
do 910 l2=1,4
|
||||
k2 = k2+1
|
||||
term = term+fac*spv(i2,l2)*c(k2)
|
||||
910 continue
|
||||
k1 = k1+nv4
|
||||
920 continue
|
||||
c calculate the squared residual at the current grid point.
|
||||
term = (z(iz)-term)**2
|
||||
c adjust the different parameters.
|
||||
fp = fp+term
|
||||
fpu(numu1) = fpu(numu1)+term
|
||||
fpv(numv1) = fpv(numv1)+term
|
||||
fac = term*half
|
||||
if(numv.eq.nroldv) go to 930
|
||||
fpv(numv1) = fpv(numv1)-fac
|
||||
fpv(numv) = fpv(numv)+fac
|
||||
930 nroldv = numv
|
||||
if(numu.eq.nroldu) go to 940
|
||||
fpu(numu1) = fpu(numu1)-fac
|
||||
fpu(numu) = fpu(numu)+fac
|
||||
940 continue
|
||||
nroldu = numu
|
||||
950 continue
|
||||
return
|
||||
end
|
||||
314
mcc/bsplines/fpgrpa.f
Normal file
314
mcc/bsplines/fpgrpa.f
Normal file
@@ -0,0 +1,314 @@
|
||||
recursive subroutine fpgrpa(ifsu,ifsv,ifbu,ifbv,idim,ipar,u,mu,
|
||||
* v,mv,z,mz,tu,nu,tv,nv,p,c,nc,fp,fpu,fpv,mm,mvnu,spu,spv,
|
||||
* right,q,au,au1,av,av1,bu,bv,nru,nrv)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 p,fp
|
||||
integer ifsu,ifsv,ifbu,ifbv,idim,mu,mv,mz,nu,nv,nc,mm,mvnu
|
||||
c ..array arguments..
|
||||
real*8 u(mu),v(mv),z(mz*idim),tu(nu),tv(nv),c(nc*idim),fpu(nu),
|
||||
* fpv(nv),spu(mu,4),spv(mv,4),right(mm*idim),q(mvnu),au(nu,5),
|
||||
* au1(nu,4),av(nv,5),av1(nv,4),bu(nu,5),bv(nv,5)
|
||||
integer ipar(2),nru(mu),nrv(mv)
|
||||
c ..local scalars..
|
||||
real*8 arg,fac,term,one,half,value
|
||||
integer i,id,ii,it,iz,i1,i2,j,jz,k,k1,k2,l,l1,l2,mvv,k0,muu,
|
||||
* ncof,nroldu,nroldv,number,nmd,numu,numu1,numv,numv1,nuu,nvv,
|
||||
* nu4,nu7,nu8,nv4,nv7,nv8, n33
|
||||
c ..local arrays..
|
||||
real*8 h(5)
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpdisc,fpbacp,fptrnp,fptrpe
|
||||
c ..
|
||||
c let
|
||||
c | (spu) | | (spv) |
|
||||
c (au) = | ---------- | (av) = | ---------- |
|
||||
c | (1/p) (bu) | | (1/p) (bv) |
|
||||
c
|
||||
c | z ' 0 |
|
||||
c q = | ------ |
|
||||
c | 0 ' 0 |
|
||||
c
|
||||
c with c : the (nu-4) x (nv-4) matrix which contains the b-spline
|
||||
c coefficients.
|
||||
c z : the mu x mv matrix which contains the function values.
|
||||
c spu,spv: the mu x (nu-4), resp. mv x (nv-4) observation matrices
|
||||
c according to the least-squares problems in the u-,resp.
|
||||
c v-direction.
|
||||
c bu,bv : the (nu-7) x (nu-4),resp. (nv-7) x (nv-4) matrices
|
||||
c containing the discontinuity jumps of the derivatives
|
||||
c of the b-splines in the u-,resp.v-variable at the knots
|
||||
c the b-spline coefficients of the smoothing spline are then calculated
|
||||
c as the least-squares solution of the following over-determined linear
|
||||
c system of equations
|
||||
c
|
||||
c (1) (av) c (au)' = q
|
||||
c
|
||||
c subject to the constraints
|
||||
c
|
||||
c (2) c(nu-3+i,j) = c(i,j), i=1,2,3 ; j=1,2,...,nv-4
|
||||
c if(ipar(1).ne.0)
|
||||
c
|
||||
c (3) c(i,nv-3+j) = c(i,j), j=1,2,3 ; i=1,2,...,nu-4
|
||||
c if(ipar(2).ne.0)
|
||||
c
|
||||
c set constants
|
||||
one = 1
|
||||
half = 0.5
|
||||
c initialization
|
||||
nu4 = nu-4
|
||||
nu7 = nu-7
|
||||
nu8 = nu-8
|
||||
nv4 = nv-4
|
||||
nv7 = nv-7
|
||||
nv8 = nv-8
|
||||
muu = mu
|
||||
if(ipar(1).ne.0) muu = mu-1
|
||||
mvv = mv
|
||||
if(ipar(2).ne.0) mvv = mv-1
|
||||
c it depends on the value of the flags ifsu,ifsv,ifbu and ibvand
|
||||
c on the value of p whether the matrices (spu), (spv), (bu) and (bv)
|
||||
c still must be determined.
|
||||
if(ifsu.ne.0) go to 50
|
||||
c calculate the non-zero elements of the matrix (spu) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the u-direction.
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 40 it=1,muu
|
||||
arg = u(it)
|
||||
10 if(arg.lt.tu(l1) .or. l.eq.nu4) go to 20
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 10
|
||||
20 call fpbspl(tu,nu,3,arg,l,h)
|
||||
do 30 i=1,4
|
||||
spu(it,i) = h(i)
|
||||
30 continue
|
||||
nru(it) = number
|
||||
40 continue
|
||||
ifsu = 1
|
||||
c calculate the non-zero elements of the matrix (spv) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the v-direction.
|
||||
50 if(ifsv.ne.0) go to 100
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 90 it=1,mvv
|
||||
arg = v(it)
|
||||
60 if(arg.lt.tv(l1) .or. l.eq.nv4) go to 70
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 60
|
||||
70 call fpbspl(tv,nv,3,arg,l,h)
|
||||
do 80 i=1,4
|
||||
spv(it,i) = h(i)
|
||||
80 continue
|
||||
nrv(it) = number
|
||||
90 continue
|
||||
ifsv = 1
|
||||
100 if(p.le.0.) go to 150
|
||||
c calculate the non-zero elements of the matrix (bu).
|
||||
if(ifbu.ne.0 .or. nu8.eq.0) go to 110
|
||||
call fpdisc(tu,nu,5,bu,nu)
|
||||
ifbu = 1
|
||||
c calculate the non-zero elements of the matrix (bv).
|
||||
110 if(ifbv.ne.0 .or. nv8.eq.0) go to 150
|
||||
call fpdisc(tv,nv,5,bv,nv)
|
||||
ifbv = 1
|
||||
c substituting (2) and (3) into (1), we obtain the overdetermined
|
||||
c system
|
||||
c (4) (avv) (cr) (auu)' = (qq)
|
||||
c from which the nuu*nvv remaining coefficients
|
||||
c c(i,j) , i=1,...,nu-4-3*ipar(1) ; j=1,...,nv-4-3*ipar(2) ,
|
||||
c the elements of (cr), are then determined in the least-squares sense.
|
||||
c we first determine the matrices (auu) and (qq). then we reduce the
|
||||
c matrix (auu) to upper triangular form (ru) using givens rotations.
|
||||
c we apply the same transformations to the rows of matrix qq to obtain
|
||||
c the (mv) x nuu matrix g.
|
||||
c we store matrix (ru) into au (and au1 if ipar(1)=1) and g into q.
|
||||
150 if(ipar(1).ne.0) go to 160
|
||||
nuu = nu4
|
||||
call fptrnp(mu,mv,idim,nu,nru,spu,p,bu,z,au,q,right)
|
||||
go to 180
|
||||
160 nuu = nu7
|
||||
call fptrpe(mu,mv,idim,nu,nru,spu,p,bu,z,au,au1,q,right)
|
||||
c we determine the matrix (avv) and then we reduce this matrix to
|
||||
c upper triangular form (rv) using givens rotations.
|
||||
c we apply the same transformations to the columns of matrix
|
||||
c g to obtain the (nvv) x (nuu) matrix h.
|
||||
c we store matrix (rv) into av (and av1 if ipar(2)=1) and h into c.
|
||||
180 if(ipar(2).ne.0) go to 190
|
||||
nvv = nv4
|
||||
call fptrnp(mv,nuu,idim,nv,nrv,spv,p,bv,q,av,c,right)
|
||||
go to 200
|
||||
190 nvv = nv7
|
||||
call fptrpe(mv,nuu,idim,nv,nrv,spv,p,bv,q,av,av1,c,right)
|
||||
c backward substitution to obtain the b-spline coefficients as the
|
||||
c solution of the linear system (rv) (cr) (ru)' = h.
|
||||
c first step: solve the system (rv) (c1) = h.
|
||||
200 ncof = nuu*nvv
|
||||
k = 1
|
||||
if(ipar(2).ne.0) go to 240
|
||||
do 220 ii=1,idim
|
||||
do 220 i=1,nuu
|
||||
call fpback(av,c(k),nvv,5,c(k),nv)
|
||||
k = k+nvv
|
||||
220 continue
|
||||
go to 300
|
||||
240 do 260 ii=1,idim
|
||||
do 260 i=1,nuu
|
||||
call fpbacp(av,av1,c(k),nvv,4,c(k),5,nv)
|
||||
k = k+nvv
|
||||
260 continue
|
||||
c second step: solve the system (cr) (ru)' = (c1).
|
||||
300 if(ipar(1).ne.0) go to 400
|
||||
do 360 ii=1,idim
|
||||
k = (ii-1)*ncof
|
||||
do 360 j=1,nvv
|
||||
k = k+1
|
||||
l = k
|
||||
do 320 i=1,nuu
|
||||
right(i) = c(l)
|
||||
l = l+nvv
|
||||
320 continue
|
||||
call fpback(au,right,nuu,5,right,nu)
|
||||
l = k
|
||||
do 340 i=1,nuu
|
||||
c(l) = right(i)
|
||||
l = l+nvv
|
||||
340 continue
|
||||
360 continue
|
||||
go to 500
|
||||
400 do 460 ii=1,idim
|
||||
k = (ii-1)*ncof
|
||||
do 460 j=1,nvv
|
||||
k = k+1
|
||||
l = k
|
||||
do 420 i=1,nuu
|
||||
right(i) = c(l)
|
||||
l = l+nvv
|
||||
420 continue
|
||||
call fpbacp(au,au1,right,nuu,4,right,5,nu)
|
||||
l = k
|
||||
do 440 i=1,nuu
|
||||
c(l) = right(i)
|
||||
l = l+nvv
|
||||
440 continue
|
||||
460 continue
|
||||
c calculate from the conditions (2)-(3), the remaining b-spline
|
||||
c coefficients.
|
||||
500 if(ipar(2).eq.0) go to 600
|
||||
i = 0
|
||||
j = 0
|
||||
do 560 id=1,idim
|
||||
do 560 l=1,nuu
|
||||
ii = i
|
||||
do 520 k=1,nvv
|
||||
i = i+1
|
||||
j = j+1
|
||||
q(i) = c(j)
|
||||
520 continue
|
||||
do 540 k=1,3
|
||||
ii = ii+1
|
||||
i = i+1
|
||||
q(i) = q(ii)
|
||||
540 continue
|
||||
560 continue
|
||||
ncof = nv4*nuu
|
||||
nmd = ncof*idim
|
||||
do 580 i=1,nmd
|
||||
c(i) = q(i)
|
||||
580 continue
|
||||
600 if(ipar(1).eq.0) go to 700
|
||||
i = 0
|
||||
j = 0
|
||||
n33 = 3*nv4
|
||||
do 660 id=1,idim
|
||||
ii = i
|
||||
do 620 k=1,ncof
|
||||
i = i+1
|
||||
j = j+1
|
||||
q(i) = c(j)
|
||||
620 continue
|
||||
do 640 k=1,n33
|
||||
ii = ii+1
|
||||
i = i+1
|
||||
q(i) = q(ii)
|
||||
640 continue
|
||||
660 continue
|
||||
ncof = nv4*nu4
|
||||
nmd = ncof*idim
|
||||
do 680 i=1,nmd
|
||||
c(i) = q(i)
|
||||
680 continue
|
||||
c calculate the quantities
|
||||
c res(i,j) = (z(i,j) - s(u(i),v(j)))**2 , i=1,2,..,mu;j=1,2,..,mv
|
||||
c fp = sumi=1,mu(sumj=1,mv(res(i,j)))
|
||||
c fpu(r) = sum''i(sumj=1,mv(res(i,j))) , r=1,2,...,nu-7
|
||||
c tu(r+3) <= u(i) <= tu(r+4)
|
||||
c fpv(r) = sumi=1,mu(sum''j(res(i,j))) , r=1,2,...,nv-7
|
||||
c tv(r+3) <= v(j) <= tv(r+4)
|
||||
700 fp = 0.
|
||||
do 720 i=1,nu
|
||||
fpu(i) = 0.
|
||||
720 continue
|
||||
do 740 i=1,nv
|
||||
fpv(i) = 0.
|
||||
740 continue
|
||||
nroldu = 0
|
||||
c main loop for the different grid points.
|
||||
do 860 i1=1,muu
|
||||
numu = nru(i1)
|
||||
numu1 = numu+1
|
||||
nroldv = 0
|
||||
iz = (i1-1)*mv
|
||||
do 840 i2=1,mvv
|
||||
numv = nrv(i2)
|
||||
numv1 = numv+1
|
||||
iz = iz+1
|
||||
c evaluate s(u,v) at the current grid point by making the sum of the
|
||||
c cross products of the non-zero b-splines at (u,v), multiplied with
|
||||
c the appropriate b-spline coefficients.
|
||||
term = 0.
|
||||
k0 = numu*nv4+numv
|
||||
jz = iz
|
||||
do 800 id=1,idim
|
||||
k1 = k0
|
||||
value = 0.
|
||||
do 780 l1=1,4
|
||||
k2 = k1
|
||||
fac = spu(i1,l1)
|
||||
do 760 l2=1,4
|
||||
k2 = k2+1
|
||||
value = value+fac*spv(i2,l2)*c(k2)
|
||||
760 continue
|
||||
k1 = k1+nv4
|
||||
780 continue
|
||||
c calculate the squared residual at the current grid point.
|
||||
term = term+(z(jz)-value)**2
|
||||
jz = jz+mz
|
||||
k0 = k0+ncof
|
||||
800 continue
|
||||
c adjust the different parameters.
|
||||
fp = fp+term
|
||||
fpu(numu1) = fpu(numu1)+term
|
||||
fpv(numv1) = fpv(numv1)+term
|
||||
fac = term*half
|
||||
if(numv.eq.nroldv) go to 820
|
||||
fpv(numv1) = fpv(numv1)-fac
|
||||
fpv(numv) = fpv(numv)+fac
|
||||
820 nroldv = numv
|
||||
if(numu.eq.nroldu) go to 840
|
||||
fpu(numu1) = fpu(numu1)-fac
|
||||
fpu(numu) = fpu(numu)+fac
|
||||
840 continue
|
||||
nroldu = numu
|
||||
860 continue
|
||||
return
|
||||
end
|
||||
329
mcc/bsplines/fpgrre.f
Normal file
329
mcc/bsplines/fpgrre.f
Normal file
@@ -0,0 +1,329 @@
|
||||
recursive subroutine fpgrre(ifsx,ifsy,ifbx,ifby,x,mx,y,my,z,mz,
|
||||
* kx,ky,tx,nx,ty,ny,p,c,nc,fp,fpx,fpy,mm,mynx,kx1,kx2,ky1,ky2,
|
||||
* spx,spy,right,q,ax,ay,bx,by,nrx,nry)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 p,fp
|
||||
integer ifsx,ifsy,ifbx,ifby,mx,my,mz,kx,ky,nx,ny,nc,mm,mynx,
|
||||
* kx1,kx2,ky1,ky2
|
||||
c ..array arguments..
|
||||
real*8 x(mx),y(my),z(mz),tx(nx),ty(ny),c(nc),spx(mx,kx1),spy(my,ky
|
||||
*1)
|
||||
* ,right(mm),q(mynx),ax(nx,kx2),bx(nx,kx2),ay(ny,ky2),by(ny,ky2),
|
||||
* fpx(nx),fpy(ny)
|
||||
integer nrx(mx),nry(my)
|
||||
c ..local scalars..
|
||||
real*8 arg,cos,fac,pinv,piv,sin,term,one,half
|
||||
integer i,ibandx,ibandy,ic,iq,irot,it,iz,i1,i2,i3,j,k,k1,k2,l,
|
||||
* l1,l2,ncof,nk1x,nk1y,nrold,nroldx,nroldy,number,numx,numx1,
|
||||
* numy,numy1,n1
|
||||
c ..local arrays..
|
||||
real*8 h(7)
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpgivs,fpdisc,fprota
|
||||
c ..
|
||||
c the b-spline coefficients of the smoothing spline are calculated as
|
||||
c the least-squares solution of the over-determined linear system of
|
||||
c equations (ay) c (ax)' = q where
|
||||
c
|
||||
c | (spx) | | (spy) |
|
||||
c (ax) = | ---------- | (ay) = | ---------- |
|
||||
c | (1/p) (bx) | | (1/p) (by) |
|
||||
c
|
||||
c | z ' 0 |
|
||||
c q = | ------ |
|
||||
c | 0 ' 0 |
|
||||
c
|
||||
c with c : the (ny-ky-1) x (nx-kx-1) matrix which contains the
|
||||
c b-spline coefficients.
|
||||
c z : the my x mx matrix which contains the function values.
|
||||
c spx,spy: the mx x (nx-kx-1) and my x (ny-ky-1) observation
|
||||
c matrices according to the least-squares problems in
|
||||
c the x- and y-direction.
|
||||
c bx,by : the (nx-2*kx-1) x (nx-kx-1) and (ny-2*ky-1) x (ny-ky-1)
|
||||
c matrices which contain the discontinuity jumps of the
|
||||
c derivatives of the b-splines in the x- and y-direction.
|
||||
one = 1
|
||||
half = 0.5
|
||||
nk1x = nx-kx1
|
||||
nk1y = ny-ky1
|
||||
if(p.gt.0.) pinv = one/p
|
||||
c it depends on the value of the flags ifsx,ifsy,ifbx and ifby and on
|
||||
c the value of p whether the matrices (spx),(spy),(bx) and (by) still
|
||||
c must be determined.
|
||||
if(ifsx.ne.0) go to 50
|
||||
c calculate the non-zero elements of the matrix (spx) which is the
|
||||
c observation matrix according to the least-squares spline approximat-
|
||||
c ion problem in the x-direction.
|
||||
l = kx1
|
||||
l1 = kx2
|
||||
number = 0
|
||||
do 40 it=1,mx
|
||||
arg = x(it)
|
||||
10 if(arg.lt.tx(l1) .or. l.eq.nk1x) go to 20
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 10
|
||||
20 call fpbspl(tx,nx,kx,arg,l,h)
|
||||
do 30 i=1,kx1
|
||||
spx(it,i) = h(i)
|
||||
30 continue
|
||||
nrx(it) = number
|
||||
40 continue
|
||||
ifsx = 1
|
||||
50 if(ifsy.ne.0) go to 100
|
||||
c calculate the non-zero elements of the matrix (spy) which is the
|
||||
c observation matrix according to the least-squares spline approximat-
|
||||
c ion problem in the y-direction.
|
||||
l = ky1
|
||||
l1 = ky2
|
||||
number = 0
|
||||
do 90 it=1,my
|
||||
arg = y(it)
|
||||
60 if(arg.lt.ty(l1) .or. l.eq.nk1y) go to 70
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 60
|
||||
70 call fpbspl(ty,ny,ky,arg,l,h)
|
||||
do 80 i=1,ky1
|
||||
spy(it,i) = h(i)
|
||||
80 continue
|
||||
nry(it) = number
|
||||
90 continue
|
||||
ifsy = 1
|
||||
100 if(p.le.0.) go to 120
|
||||
c calculate the non-zero elements of the matrix (bx).
|
||||
if(ifbx.ne.0 .or. nx.eq.2*kx1) go to 110
|
||||
call fpdisc(tx,nx,kx2,bx,nx)
|
||||
ifbx = 1
|
||||
c calculate the non-zero elements of the matrix (by).
|
||||
110 if(ifby.ne.0 .or. ny.eq.2*ky1) go to 120
|
||||
call fpdisc(ty,ny,ky2,by,ny)
|
||||
ifby = 1
|
||||
c reduce the matrix (ax) to upper triangular form (rx) using givens
|
||||
c rotations. apply the same transformations to the rows of matrix q
|
||||
c to obtain the my x (nx-kx-1) matrix g.
|
||||
c store matrix (rx) into (ax) and g into q.
|
||||
120 l = my*nk1x
|
||||
c initialization.
|
||||
do 130 i=1,l
|
||||
q(i) = 0.
|
||||
130 continue
|
||||
do 140 i=1,nk1x
|
||||
do 140 j=1,kx2
|
||||
ax(i,j) = 0.
|
||||
140 continue
|
||||
l = 0
|
||||
nrold = 0
|
||||
c ibandx denotes the bandwidth of the matrices (ax) and (rx).
|
||||
ibandx = kx1
|
||||
do 270 it=1,mx
|
||||
number = nrx(it)
|
||||
150 if(nrold.eq.number) go to 180
|
||||
if(p.le.0.) go to 260
|
||||
ibandx = kx2
|
||||
c fetch a new row of matrix (bx).
|
||||
n1 = nrold+1
|
||||
do 160 j=1,kx2
|
||||
h(j) = bx(n1,j)*pinv
|
||||
160 continue
|
||||
c find the appropriate column of q.
|
||||
do 170 j=1,my
|
||||
right(j) = 0.
|
||||
170 continue
|
||||
irot = nrold
|
||||
go to 210
|
||||
c fetch a new row of matrix (spx).
|
||||
180 h(ibandx) = 0.
|
||||
do 190 j=1,kx1
|
||||
h(j) = spx(it,j)
|
||||
190 continue
|
||||
c find the appropriate column of q.
|
||||
do 200 j=1,my
|
||||
l = l+1
|
||||
right(j) = z(l)
|
||||
200 continue
|
||||
irot = number
|
||||
c rotate the new row of matrix (ax) into triangle.
|
||||
210 do 240 i=1,ibandx
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 240
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,ax(irot,1),cos,sin)
|
||||
c apply that transformation to the rows of matrix q.
|
||||
iq = (irot-1)*my
|
||||
do 220 j=1,my
|
||||
iq = iq+1
|
||||
call fprota(cos,sin,right(j),q(iq))
|
||||
220 continue
|
||||
c apply that transformation to the columns of (ax).
|
||||
if(i.eq.ibandx) go to 250
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 230 j=i3,ibandx
|
||||
i2 = i2+1
|
||||
call fprota(cos,sin,h(j),ax(irot,i2))
|
||||
230 continue
|
||||
240 continue
|
||||
250 if(nrold.eq.number) go to 270
|
||||
260 nrold = nrold+1
|
||||
go to 150
|
||||
270 continue
|
||||
c reduce the matrix (ay) to upper triangular form (ry) using givens
|
||||
c rotations. apply the same transformations to the columns of matrix g
|
||||
c to obtain the (ny-ky-1) x (nx-kx-1) matrix h.
|
||||
c store matrix (ry) into (ay) and h into c.
|
||||
ncof = nk1x*nk1y
|
||||
c initialization.
|
||||
do 280 i=1,ncof
|
||||
c(i) = 0.
|
||||
280 continue
|
||||
do 290 i=1,nk1y
|
||||
do 290 j=1,ky2
|
||||
ay(i,j) = 0.
|
||||
290 continue
|
||||
nrold = 0
|
||||
c ibandy denotes the bandwidth of the matrices (ay) and (ry).
|
||||
ibandy = ky1
|
||||
do 420 it=1,my
|
||||
number = nry(it)
|
||||
300 if(nrold.eq.number) go to 330
|
||||
if(p.le.0.) go to 410
|
||||
ibandy = ky2
|
||||
c fetch a new row of matrix (by).
|
||||
n1 = nrold+1
|
||||
do 310 j=1,ky2
|
||||
h(j) = by(n1,j)*pinv
|
||||
310 continue
|
||||
c find the appropriate row of g.
|
||||
do 320 j=1,nk1x
|
||||
right(j) = 0.
|
||||
320 continue
|
||||
irot = nrold
|
||||
go to 360
|
||||
c fetch a new row of matrix (spy)
|
||||
330 h(ibandy) = 0.
|
||||
do 340 j=1,ky1
|
||||
h(j) = spy(it,j)
|
||||
340 continue
|
||||
c find the appropriate row of g.
|
||||
l = it
|
||||
do 350 j=1,nk1x
|
||||
right(j) = q(l)
|
||||
l = l+my
|
||||
350 continue
|
||||
irot = number
|
||||
c rotate the new row of matrix (ay) into triangle.
|
||||
360 do 390 i=1,ibandy
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 390
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,ay(irot,1),cos,sin)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = irot
|
||||
do 370 j=1,nk1x
|
||||
call fprota(cos,sin,right(j),c(ic))
|
||||
ic = ic+nk1y
|
||||
370 continue
|
||||
c apply that transformation to the columns of matrix (ay).
|
||||
if(i.eq.ibandy) go to 400
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 380 j=i3,ibandy
|
||||
i2 = i2+1
|
||||
call fprota(cos,sin,h(j),ay(irot,i2))
|
||||
380 continue
|
||||
390 continue
|
||||
400 if(nrold.eq.number) go to 420
|
||||
410 nrold = nrold+1
|
||||
go to 300
|
||||
420 continue
|
||||
c backward substitution to obtain the b-spline coefficients as the
|
||||
c solution of the linear system (ry) c (rx)' = h.
|
||||
c first step: solve the system (ry) (c1) = h.
|
||||
k = 1
|
||||
do 450 i=1,nk1x
|
||||
call fpback(ay,c(k),nk1y,ibandy,c(k),ny)
|
||||
k = k+nk1y
|
||||
450 continue
|
||||
c second step: solve the system c (rx)' = (c1).
|
||||
k = 0
|
||||
do 480 j=1,nk1y
|
||||
k = k+1
|
||||
l = k
|
||||
do 460 i=1,nk1x
|
||||
right(i) = c(l)
|
||||
l = l+nk1y
|
||||
460 continue
|
||||
call fpback(ax,right,nk1x,ibandx,right,nx)
|
||||
l = k
|
||||
do 470 i=1,nk1x
|
||||
c(l) = right(i)
|
||||
l = l+nk1y
|
||||
470 continue
|
||||
480 continue
|
||||
c calculate the quantities
|
||||
c res(i,j) = (z(i,j) - s(x(i),y(j)))**2 , i=1,2,..,mx;j=1,2,..,my
|
||||
c fp = sumi=1,mx(sumj=1,my(res(i,j)))
|
||||
c fpx(r) = sum''i(sumj=1,my(res(i,j))) , r=1,2,...,nx-2*kx-1
|
||||
c tx(r+kx) <= x(i) <= tx(r+kx+1)
|
||||
c fpy(r) = sumi=1,mx(sum''j(res(i,j))) , r=1,2,...,ny-2*ky-1
|
||||
c ty(r+ky) <= y(j) <= ty(r+ky+1)
|
||||
fp = 0.
|
||||
do 490 i=1,nx
|
||||
fpx(i) = 0.
|
||||
490 continue
|
||||
do 500 i=1,ny
|
||||
fpy(i) = 0.
|
||||
500 continue
|
||||
nk1y = ny-ky1
|
||||
iz = 0
|
||||
nroldx = 0
|
||||
c main loop for the different grid points.
|
||||
do 550 i1=1,mx
|
||||
numx = nrx(i1)
|
||||
numx1 = numx+1
|
||||
nroldy = 0
|
||||
do 540 i2=1,my
|
||||
numy = nry(i2)
|
||||
numy1 = numy+1
|
||||
iz = iz+1
|
||||
c evaluate s(x,y) at the current grid point by making the sum of the
|
||||
c cross products of the non-zero b-splines at (x,y), multiplied with
|
||||
c the appropriate b-spline coefficients.
|
||||
term = 0.
|
||||
k1 = numx*nk1y+numy
|
||||
do 520 l1=1,kx1
|
||||
k2 = k1
|
||||
fac = spx(i1,l1)
|
||||
do 510 l2=1,ky1
|
||||
k2 = k2+1
|
||||
term = term+fac*spy(i2,l2)*c(k2)
|
||||
510 continue
|
||||
k1 = k1+nk1y
|
||||
520 continue
|
||||
c calculate the squared residual at the current grid point.
|
||||
term = (z(iz)-term)**2
|
||||
c adjust the different parameters.
|
||||
fp = fp+term
|
||||
fpx(numx1) = fpx(numx1)+term
|
||||
fpy(numy1) = fpy(numy1)+term
|
||||
fac = term*half
|
||||
if(numy.eq.nroldy) go to 530
|
||||
fpy(numy1) = fpy(numy1)-fac
|
||||
fpy(numy) = fpy(numy)+fac
|
||||
530 nroldy = numy
|
||||
if(numx.eq.nroldx) go to 540
|
||||
fpx(numx1) = fpx(numx1)-fac
|
||||
fpx(numx) = fpx(numx)+fac
|
||||
540 continue
|
||||
nroldx = numx
|
||||
550 continue
|
||||
return
|
||||
end
|
||||
|
||||
658
mcc/bsplines/fpgrsp.f
Normal file
658
mcc/bsplines/fpgrsp.f
Normal file
@@ -0,0 +1,658 @@
|
||||
recursive subroutine fpgrsp(ifsu,ifsv,ifbu,ifbv,iback,u,mu,v,
|
||||
* mv,r,mr,dr,iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,
|
||||
* mvnu,spu,spv,right,q,au,av1,av2,bu,bv,a0,a1,b0,b1,c0,c1,
|
||||
* cosi,nru,nrv)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 p,sq,fp
|
||||
integer ifsu,ifsv,ifbu,ifbv,iback,mu,mv,mr,iop0,iop1,nu,nv,nc,
|
||||
* mm,mvnu
|
||||
c ..array arguments..
|
||||
real*8 u(mu),v(mv),r(mr),dr(6),tu(nu),tv(nv),c(nc),fpu(nu),fpv(nv)
|
||||
*,
|
||||
* spu(mu,4),spv(mv,4),right(mm),q(mvnu),au(nu,5),av1(nv,6),c0(nv),
|
||||
* av2(nv,4),a0(2,mv),b0(2,nv),cosi(2,nv),bu(nu,5),bv(nv,5),c1(nv),
|
||||
* a1(2,mv),b1(2,nv)
|
||||
integer nru(mu),nrv(mv)
|
||||
c ..local scalars..
|
||||
real*8 arg,co,dr01,dr02,dr03,dr11,dr12,dr13,fac,fac0,fac1,pinv,piv
|
||||
*,
|
||||
* si,term,one,three,half
|
||||
integer i,ic,ii,ij,ik,iq,irot,it,ir,i0,i1,i2,i3,j,jj,jk,jper,
|
||||
* j0,j1,k,k1,k2,l,l0,l1,l2,mvv,ncof,nrold,nroldu,nroldv,number,
|
||||
* numu,numu1,numv,numv1,nuu,nu4,nu7,nu8,nu9,nv11,nv4,nv7,nv8,n1
|
||||
c ..local arrays..
|
||||
real*8 h(5),h1(5),h2(4)
|
||||
c ..function references..
|
||||
integer min0
|
||||
real*8 cos,sin
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpgivs,fpcyt1,fpcyt2,fpdisc,fpbacp,fprota
|
||||
c ..
|
||||
c let
|
||||
c | (spu) | | (spv) |
|
||||
c (au) = | -------------- | (av) = | -------------- |
|
||||
c | sqrt(1/p) (bu) | | sqrt(1/p) (bv) |
|
||||
c
|
||||
c | r ' 0 |
|
||||
c q = | ------ |
|
||||
c | 0 ' 0 |
|
||||
c
|
||||
c with c : the (nu-4) x (nv-4) matrix which contains the b-spline
|
||||
c coefficients.
|
||||
c r : the mu x mv matrix which contains the function values.
|
||||
c spu,spv: the mu x (nu-4), resp. mv x (nv-4) observation matrices
|
||||
c according to the least-squares problems in the u-,resp.
|
||||
c v-direction.
|
||||
c bu,bv : the (nu-7) x (nu-4),resp. (nv-7) x (nv-4) matrices
|
||||
c containing the discontinuity jumps of the derivatives
|
||||
c of the b-splines in the u-,resp.v-variable at the knots
|
||||
c the b-spline coefficients of the smoothing spline are then calculated
|
||||
c as the least-squares solution of the following over-determined linear
|
||||
c system of equations
|
||||
c
|
||||
c (1) (av) c (au)' = q
|
||||
c
|
||||
c subject to the constraints
|
||||
c
|
||||
c (2) c(i,nv-3+j) = c(i,j), j=1,2,3 ; i=1,2,...,nu-4
|
||||
c
|
||||
c (3) if iop0 = 0 c(1,j) = dr(1)
|
||||
c iop0 = 1 c(1,j) = dr(1)
|
||||
c c(2,j) = dr(1)+(dr(2)*cosi(1,j)+dr(3)*cosi(2,j))*
|
||||
c tu(5)/3. = c0(j) , j=1,2,...nv-4
|
||||
c
|
||||
c (4) if iop1 = 0 c(nu-4,j) = dr(4)
|
||||
c iop1 = 1 c(nu-4,j) = dr(4)
|
||||
c c(nu-5,j) = dr(4)+(dr(5)*cosi(1,j)+dr(6)*cosi(2,j))
|
||||
c *(tu(nu-4)-tu(nu-3))/3. = c1(j)
|
||||
c
|
||||
c set constants
|
||||
one = 1
|
||||
three = 3
|
||||
half = 0.5
|
||||
c initialization
|
||||
nu4 = nu-4
|
||||
nu7 = nu-7
|
||||
nu8 = nu-8
|
||||
nu9 = nu-9
|
||||
nv4 = nv-4
|
||||
nv7 = nv-7
|
||||
nv8 = nv-8
|
||||
nv11 = nv-11
|
||||
nuu = nu4-iop0-iop1-2
|
||||
if(p.gt.0.) pinv = one/p
|
||||
c it depends on the value of the flags ifsu,ifsv,ifbu,ifbv,iop0,iop1
|
||||
c and on the value of p whether the matrices (spu), (spv), (bu), (bv),
|
||||
c (cosi) still must be determined.
|
||||
if(ifsu.ne.0) go to 30
|
||||
c calculate the non-zero elements of the matrix (spu) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the u-direction.
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 25 it=1,mu
|
||||
arg = u(it)
|
||||
10 if(arg.lt.tu(l1) .or. l.eq.nu4) go to 15
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 10
|
||||
15 call fpbspl(tu,nu,3,arg,l,h)
|
||||
do 20 i=1,4
|
||||
spu(it,i) = h(i)
|
||||
20 continue
|
||||
nru(it) = number
|
||||
25 continue
|
||||
ifsu = 1
|
||||
c calculate the non-zero elements of the matrix (spv) which is the ob-
|
||||
c servation matrix according to the least-squares spline approximation
|
||||
c problem in the v-direction.
|
||||
30 if(ifsv.ne.0) go to 85
|
||||
l = 4
|
||||
l1 = 5
|
||||
number = 0
|
||||
do 50 it=1,mv
|
||||
arg = v(it)
|
||||
35 if(arg.lt.tv(l1) .or. l.eq.nv4) go to 40
|
||||
l = l1
|
||||
l1 = l+1
|
||||
number = number+1
|
||||
go to 35
|
||||
40 call fpbspl(tv,nv,3,arg,l,h)
|
||||
do 45 i=1,4
|
||||
spv(it,i) = h(i)
|
||||
45 continue
|
||||
nrv(it) = number
|
||||
50 continue
|
||||
ifsv = 1
|
||||
if(iop0.eq.0 .and. iop1.eq.0) go to 85
|
||||
c calculate the coefficients of the interpolating splines for cos(v)
|
||||
c and sin(v).
|
||||
do 55 i=1,nv4
|
||||
cosi(1,i) = 0.
|
||||
cosi(2,i) = 0.
|
||||
55 continue
|
||||
if(nv7.lt.4) go to 85
|
||||
do 65 i=1,nv7
|
||||
l = i+3
|
||||
arg = tv(l)
|
||||
call fpbspl(tv,nv,3,arg,l,h)
|
||||
do 60 j=1,3
|
||||
av1(i,j) = h(j)
|
||||
60 continue
|
||||
cosi(1,i) = cos(arg)
|
||||
cosi(2,i) = sin(arg)
|
||||
65 continue
|
||||
call fpcyt1(av1,nv7,nv)
|
||||
do 80 j=1,2
|
||||
do 70 i=1,nv7
|
||||
right(i) = cosi(j,i)
|
||||
70 continue
|
||||
call fpcyt2(av1,nv7,right,right,nv)
|
||||
do 75 i=1,nv7
|
||||
cosi(j,i+1) = right(i)
|
||||
75 continue
|
||||
cosi(j,1) = cosi(j,nv7+1)
|
||||
cosi(j,nv7+2) = cosi(j,2)
|
||||
cosi(j,nv4) = cosi(j,3)
|
||||
80 continue
|
||||
85 if(p.le.0.) go to 150
|
||||
c calculate the non-zero elements of the matrix (bu).
|
||||
if(ifbu.ne.0 .or. nu8.eq.0) go to 90
|
||||
call fpdisc(tu,nu,5,bu,nu)
|
||||
ifbu = 1
|
||||
c calculate the non-zero elements of the matrix (bv).
|
||||
90 if(ifbv.ne.0 .or. nv8.eq.0) go to 150
|
||||
call fpdisc(tv,nv,5,bv,nv)
|
||||
ifbv = 1
|
||||
c substituting (2),(3) and (4) into (1), we obtain the overdetermined
|
||||
c system
|
||||
c (5) (avv) (cc) (auu)' = (qq)
|
||||
c from which the nuu*nv7 remaining coefficients
|
||||
c c(i,j) , i=2+iop0,3+iop0,...,nu-5-iop1,j=1,2,...,nv-7.
|
||||
c the elements of (cc), are then determined in the least-squares sense.
|
||||
c simultaneously, we compute the resulting sum of squared residuals sq.
|
||||
150 dr01 = dr(1)
|
||||
dr11 = dr(4)
|
||||
do 155 i=1,mv
|
||||
a0(1,i) = dr01
|
||||
a1(1,i) = dr11
|
||||
155 continue
|
||||
if(nv8.eq.0 .or. p.le.0.) go to 165
|
||||
do 160 i=1,nv8
|
||||
b0(1,i) = 0.
|
||||
b1(1,i) = 0.
|
||||
160 continue
|
||||
165 mvv = mv
|
||||
if(iop0.eq.0) go to 195
|
||||
fac = (tu(5)-tu(4))/three
|
||||
dr02 = dr(2)*fac
|
||||
dr03 = dr(3)*fac
|
||||
do 170 i=1,nv4
|
||||
c0(i) = dr01+dr02*cosi(1,i)+dr03*cosi(2,i)
|
||||
170 continue
|
||||
do 180 i=1,mv
|
||||
number = nrv(i)
|
||||
fac = 0.
|
||||
do 175 j=1,4
|
||||
number = number+1
|
||||
fac = fac+c0(number)*spv(i,j)
|
||||
175 continue
|
||||
a0(2,i) = fac
|
||||
180 continue
|
||||
if(nv8.eq.0 .or. p.le.0.) go to 195
|
||||
do 190 i=1,nv8
|
||||
number = i
|
||||
fac = 0.
|
||||
do 185 j=1,5
|
||||
fac = fac+c0(number)*bv(i,j)
|
||||
number = number+1
|
||||
185 continue
|
||||
b0(2,i) = fac*pinv
|
||||
190 continue
|
||||
mvv = mv+nv8
|
||||
195 if(iop1.eq.0) go to 225
|
||||
fac = (tu(nu4)-tu(nu4+1))/three
|
||||
dr12 = dr(5)*fac
|
||||
dr13 = dr(6)*fac
|
||||
do 200 i=1,nv4
|
||||
c1(i) = dr11+dr12*cosi(1,i)+dr13*cosi(2,i)
|
||||
200 continue
|
||||
do 210 i=1,mv
|
||||
number = nrv(i)
|
||||
fac = 0.
|
||||
do 205 j=1,4
|
||||
number = number+1
|
||||
fac = fac+c1(number)*spv(i,j)
|
||||
205 continue
|
||||
a1(2,i) = fac
|
||||
210 continue
|
||||
if(nv8.eq.0 .or. p.le.0.) go to 225
|
||||
do 220 i=1,nv8
|
||||
number = i
|
||||
fac = 0.
|
||||
do 215 j=1,5
|
||||
fac = fac+c1(number)*bv(i,j)
|
||||
number = number+1
|
||||
215 continue
|
||||
b1(2,i) = fac*pinv
|
||||
220 continue
|
||||
mvv = mv+nv8
|
||||
c we first determine the matrices (auu) and (qq). then we reduce the
|
||||
c matrix (auu) to an unit upper triangular form (ru) using givens
|
||||
c rotations without square roots. we apply the same transformations to
|
||||
c the rows of matrix qq to obtain the mv x nuu matrix g.
|
||||
c we store matrix (ru) into au and g into q.
|
||||
225 l = mvv*nuu
|
||||
c initialization.
|
||||
sq = 0.
|
||||
if(l.eq.0) go to 245
|
||||
do 230 i=1,l
|
||||
q(i) = 0.
|
||||
230 continue
|
||||
do 240 i=1,nuu
|
||||
do 240 j=1,5
|
||||
au(i,j) = 0.
|
||||
240 continue
|
||||
l = 0
|
||||
245 nrold = 0
|
||||
n1 = nrold+1
|
||||
do 420 it=1,mu
|
||||
number = nru(it)
|
||||
c find the appropriate column of q.
|
||||
250 do 260 j=1,mvv
|
||||
right(j) = 0.
|
||||
260 continue
|
||||
if(nrold.eq.number) go to 280
|
||||
if(p.le.0.) go to 410
|
||||
c fetch a new row of matrix (bu).
|
||||
do 270 j=1,5
|
||||
h(j) = bu(n1,j)*pinv
|
||||
270 continue
|
||||
i0 = 1
|
||||
i1 = 5
|
||||
go to 310
|
||||
c fetch a new row of matrix (spu).
|
||||
280 do 290 j=1,4
|
||||
h(j) = spu(it,j)
|
||||
290 continue
|
||||
c find the appropriate column of q.
|
||||
do 300 j=1,mv
|
||||
l = l+1
|
||||
right(j) = r(l)
|
||||
300 continue
|
||||
i0 = 1
|
||||
i1 = 4
|
||||
310 j0 = n1
|
||||
j1 = nu7-number
|
||||
c take into account that we eliminate the constraints (3)
|
||||
315 if(j0-1.gt.iop0) go to 335
|
||||
fac0 = h(i0)
|
||||
do 320 j=1,mv
|
||||
right(j) = right(j)-fac0*a0(j0,j)
|
||||
320 continue
|
||||
if(mv.eq.mvv) go to 330
|
||||
j = mv
|
||||
do 325 jj=1,nv8
|
||||
j = j+1
|
||||
right(j) = right(j)-fac0*b0(j0,jj)
|
||||
325 continue
|
||||
330 j0 = j0+1
|
||||
i0 = i0+1
|
||||
go to 315
|
||||
c take into account that we eliminate the constraints (4)
|
||||
335 if(j1-1.gt.iop1) go to 360
|
||||
fac1 = h(i1)
|
||||
do 340 j=1,mv
|
||||
right(j) = right(j)-fac1*a1(j1,j)
|
||||
340 continue
|
||||
if(mv.eq.mvv) go to 350
|
||||
j = mv
|
||||
do 345 jj=1,nv8
|
||||
j = j+1
|
||||
right(j) = right(j)-fac1*b1(j1,jj)
|
||||
345 continue
|
||||
350 j1 = j1+1
|
||||
i1 = i1-1
|
||||
go to 335
|
||||
360 irot = nrold-iop0-1
|
||||
if(irot.lt.0) irot = 0
|
||||
c rotate the new row of matrix (auu) into triangle.
|
||||
if(i0.gt.i1) go to 390
|
||||
do 385 i=i0,i1
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 385
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,au(irot,1),co,si)
|
||||
c apply that transformation to the rows of matrix (qq).
|
||||
iq = (irot-1)*mvv
|
||||
do 370 j=1,mvv
|
||||
iq = iq+1
|
||||
call fprota(co,si,right(j),q(iq))
|
||||
370 continue
|
||||
c apply that transformation to the columns of (auu).
|
||||
if(i.eq.i1) go to 385
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 380 j=i3,i1
|
||||
i2 = i2+1
|
||||
call fprota(co,si,h(j),au(irot,i2))
|
||||
380 continue
|
||||
385 continue
|
||||
c we update the sum of squared residuals.
|
||||
390 do 395 j=1,mvv
|
||||
sq = sq+right(j)**2
|
||||
395 continue
|
||||
if(nrold.eq.number) go to 420
|
||||
410 nrold = n1
|
||||
n1 = n1+1
|
||||
go to 250
|
||||
420 continue
|
||||
if(nuu.eq.0) go to 800
|
||||
c we determine the matrix (avv) and then we reduce her to an unit
|
||||
c upper triangular form (rv) using givens rotations without square
|
||||
c roots. we apply the same transformations to the columns of matrix
|
||||
c g to obtain the (nv-7) x (nu-6-iop0-iop1) matrix h.
|
||||
c we store matrix (rv) into av1 and av2, h into c.
|
||||
c the nv7 x nv7 triangular unit upper matrix (rv) has the form
|
||||
c | av1 ' |
|
||||
c (rv) = | ' av2 |
|
||||
c | 0 ' |
|
||||
c with (av2) a nv7 x 4 matrix and (av1) a nv11 x nv11 unit upper
|
||||
c triangular matrix of bandwidth 5.
|
||||
ncof = nuu*nv7
|
||||
c initialization.
|
||||
do 430 i=1,ncof
|
||||
c(i) = 0.
|
||||
430 continue
|
||||
do 440 i=1,nv4
|
||||
av1(i,5) = 0.
|
||||
do 440 j=1,4
|
||||
av1(i,j) = 0.
|
||||
av2(i,j) = 0.
|
||||
440 continue
|
||||
jper = 0
|
||||
nrold = 0
|
||||
do 770 it=1,mv
|
||||
number = nrv(it)
|
||||
450 if(nrold.eq.number) go to 480
|
||||
if(p.le.0.) go to 760
|
||||
c fetch a new row of matrix (bv).
|
||||
n1 = nrold+1
|
||||
do 460 j=1,5
|
||||
h(j) = bv(n1,j)*pinv
|
||||
460 continue
|
||||
c find the appropriate row of g.
|
||||
do 465 j=1,nuu
|
||||
right(j) = 0.
|
||||
465 continue
|
||||
if(mv.eq.mvv) go to 510
|
||||
l = mv+n1
|
||||
do 470 j=1,nuu
|
||||
right(j) = q(l)
|
||||
l = l+mvv
|
||||
470 continue
|
||||
go to 510
|
||||
c fetch a new row of matrix (spv)
|
||||
480 h(5) = 0.
|
||||
do 490 j=1,4
|
||||
h(j) = spv(it,j)
|
||||
490 continue
|
||||
c find the appropriate row of g.
|
||||
l = it
|
||||
do 500 j=1,nuu
|
||||
right(j) = q(l)
|
||||
l = l+mvv
|
||||
500 continue
|
||||
c test whether there are non-zero values in the new row of (avv)
|
||||
c corresponding to the b-splines n(j;v),j=nv7+1,...,nv4.
|
||||
510 if(nrold.lt.nv11) go to 710
|
||||
if(jper.ne.0) go to 550
|
||||
c initialize the matrix (av2).
|
||||
jk = nv11+1
|
||||
do 540 i=1,4
|
||||
ik = jk
|
||||
do 520 j=1,5
|
||||
if(ik.le.0) go to 530
|
||||
av2(ik,i) = av1(ik,j)
|
||||
ik = ik-1
|
||||
520 continue
|
||||
530 jk = jk+1
|
||||
540 continue
|
||||
jper = 1
|
||||
c if one of the non-zero elements of the new row corresponds to one of
|
||||
c the b-splines n(j;v),j=nv7+1,...,nv4, we take account of condition
|
||||
c (2) for setting up this row of (avv). the row is stored in h1( the
|
||||
c part with respect to av1) and h2 (the part with respect to av2).
|
||||
550 do 560 i=1,4
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
560 continue
|
||||
h1(5) = 0.
|
||||
j = nrold-nv11
|
||||
do 600 i=1,5
|
||||
j = j+1
|
||||
l0 = j
|
||||
570 l1 = l0-4
|
||||
if(l1.le.0) go to 590
|
||||
if(l1.le.nv11) go to 580
|
||||
l0 = l1-nv11
|
||||
go to 570
|
||||
580 h1(l1) = h(i)
|
||||
go to 600
|
||||
590 h2(l0) = h2(l0) + h(i)
|
||||
600 continue
|
||||
c rotate the new row of (avv) into triangle.
|
||||
if(nv11.le.0) go to 670
|
||||
c rotations with the rows 1,2,...,nv11 of (avv).
|
||||
do 660 j=1,nv11
|
||||
piv = h1(1)
|
||||
i2 = min0(nv11-j,4)
|
||||
if(piv.eq.0.) go to 640
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av1(j,1),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = j
|
||||
do 610 i=1,nuu
|
||||
call fprota(co,si,right(i),c(ic))
|
||||
ic = ic+nv7
|
||||
610 continue
|
||||
c apply that transformation to the rows of (avv) with respect to av2.
|
||||
do 620 i=1,4
|
||||
call fprota(co,si,h2(i),av2(j,i))
|
||||
620 continue
|
||||
c apply that transformation to the rows of (avv) with respect to av1.
|
||||
if(i2.eq.0) go to 670
|
||||
do 630 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(co,si,h1(i1),av1(j,i1))
|
||||
630 continue
|
||||
640 do 650 i=1,i2
|
||||
h1(i) = h1(i+1)
|
||||
650 continue
|
||||
h1(i2+1) = 0.
|
||||
660 continue
|
||||
c rotations with the rows nv11+1,...,nv7 of avv.
|
||||
670 do 700 j=1,4
|
||||
ij = nv11+j
|
||||
if(ij.le.0) go to 700
|
||||
piv = h2(j)
|
||||
if(piv.eq.0.) go to 700
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av2(ij,j),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = ij
|
||||
do 680 i=1,nuu
|
||||
call fprota(co,si,right(i),c(ic))
|
||||
ic = ic+nv7
|
||||
680 continue
|
||||
if(j.eq.4) go to 700
|
||||
c apply that transformation to the rows of (avv) with respect to av2.
|
||||
j1 = j+1
|
||||
do 690 i=j1,4
|
||||
call fprota(co,si,h2(i),av2(ij,i))
|
||||
690 continue
|
||||
700 continue
|
||||
c we update the sum of squared residuals.
|
||||
do 705 i=1,nuu
|
||||
sq = sq+right(i)**2
|
||||
705 continue
|
||||
go to 750
|
||||
c rotation into triangle of the new row of (avv), in case the elements
|
||||
c corresponding to the b-splines n(j;v),j=nv7+1,...,nv4 are all zero.
|
||||
710 irot =nrold
|
||||
do 740 i=1,5
|
||||
irot = irot+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 740
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,av1(irot,1),co,si)
|
||||
c apply that transformation to the columns of matrix g.
|
||||
ic = irot
|
||||
do 720 j=1,nuu
|
||||
call fprota(co,si,right(j),c(ic))
|
||||
ic = ic+nv7
|
||||
720 continue
|
||||
c apply that transformation to the rows of (avv).
|
||||
if(i.eq.5) go to 740
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 730 j=i3,5
|
||||
i2 = i2+1
|
||||
call fprota(co,si,h(j),av1(irot,i2))
|
||||
730 continue
|
||||
740 continue
|
||||
c we update the sum of squared residuals.
|
||||
do 745 i=1,nuu
|
||||
sq = sq+right(i)**2
|
||||
745 continue
|
||||
750 if(nrold.eq.number) go to 770
|
||||
760 nrold = nrold+1
|
||||
go to 450
|
||||
770 continue
|
||||
c test whether the b-spline coefficients must be determined.
|
||||
if(iback.ne.0) return
|
||||
c backward substitution to obtain the b-spline coefficients as the
|
||||
c solution of the linear system (rv) (cr) (ru)' = h.
|
||||
c first step: solve the system (rv) (c1) = h.
|
||||
k = 1
|
||||
do 780 i=1,nuu
|
||||
call fpbacp(av1,av2,c(k),nv7,4,c(k),5,nv)
|
||||
k = k+nv7
|
||||
780 continue
|
||||
c second step: solve the system (cr) (ru)' = (c1).
|
||||
k = 0
|
||||
do 795 j=1,nv7
|
||||
k = k+1
|
||||
l = k
|
||||
do 785 i=1,nuu
|
||||
right(i) = c(l)
|
||||
l = l+nv7
|
||||
785 continue
|
||||
call fpback(au,right,nuu,5,right,nu)
|
||||
l = k
|
||||
do 790 i=1,nuu
|
||||
c(l) = right(i)
|
||||
l = l+nv7
|
||||
790 continue
|
||||
795 continue
|
||||
c calculate from the conditions (2)-(3)-(4), the remaining b-spline
|
||||
c coefficients.
|
||||
800 ncof = nu4*nv4
|
||||
j = ncof
|
||||
do 805 l=1,nv4
|
||||
q(l) = dr01
|
||||
q(j) = dr11
|
||||
j = j-1
|
||||
805 continue
|
||||
i = nv4
|
||||
j = 0
|
||||
if(iop0.eq.0) go to 815
|
||||
do 810 l=1,nv4
|
||||
i = i+1
|
||||
q(i) = c0(l)
|
||||
810 continue
|
||||
815 if(nuu.eq.0) go to 835
|
||||
do 830 l=1,nuu
|
||||
ii = i
|
||||
do 820 k=1,nv7
|
||||
i = i+1
|
||||
j = j+1
|
||||
q(i) = c(j)
|
||||
820 continue
|
||||
do 825 k=1,3
|
||||
ii = ii+1
|
||||
i = i+1
|
||||
q(i) = q(ii)
|
||||
825 continue
|
||||
830 continue
|
||||
835 if(iop1.eq.0) go to 845
|
||||
do 840 l=1,nv4
|
||||
i = i+1
|
||||
q(i) = c1(l)
|
||||
840 continue
|
||||
845 do 850 i=1,ncof
|
||||
c(i) = q(i)
|
||||
850 continue
|
||||
c calculate the quantities
|
||||
c res(i,j) = (r(i,j) - s(u(i),v(j)))**2 , i=1,2,..,mu;j=1,2,..,mv
|
||||
c fp = sumi=1,mu(sumj=1,mv(res(i,j)))
|
||||
c fpu(r) = sum''i(sumj=1,mv(res(i,j))) , r=1,2,...,nu-7
|
||||
c tu(r+3) <= u(i) <= tu(r+4)
|
||||
c fpv(r) = sumi=1,mu(sum''j(res(i,j))) , r=1,2,...,nv-7
|
||||
c tv(r+3) <= v(j) <= tv(r+4)
|
||||
fp = 0.
|
||||
do 890 i=1,nu
|
||||
fpu(i) = 0.
|
||||
890 continue
|
||||
do 900 i=1,nv
|
||||
fpv(i) = 0.
|
||||
900 continue
|
||||
ir = 0
|
||||
nroldu = 0
|
||||
c main loop for the different grid points.
|
||||
do 950 i1=1,mu
|
||||
numu = nru(i1)
|
||||
numu1 = numu+1
|
||||
nroldv = 0
|
||||
do 940 i2=1,mv
|
||||
numv = nrv(i2)
|
||||
numv1 = numv+1
|
||||
ir = ir+1
|
||||
c evaluate s(u,v) at the current grid point by making the sum of the
|
||||
c cross products of the non-zero b-splines at (u,v), multiplied with
|
||||
c the appropriate b-spline coefficients.
|
||||
term = 0.
|
||||
k1 = numu*nv4+numv
|
||||
do 920 l1=1,4
|
||||
k2 = k1
|
||||
fac = spu(i1,l1)
|
||||
do 910 l2=1,4
|
||||
k2 = k2+1
|
||||
term = term+fac*spv(i2,l2)*c(k2)
|
||||
910 continue
|
||||
k1 = k1+nv4
|
||||
920 continue
|
||||
c calculate the squared residual at the current grid point.
|
||||
term = (r(ir)-term)**2
|
||||
c adjust the different parameters.
|
||||
fp = fp+term
|
||||
fpu(numu1) = fpu(numu1)+term
|
||||
fpv(numv1) = fpv(numv1)+term
|
||||
fac = term*half
|
||||
if(numv.eq.nroldv) go to 930
|
||||
fpv(numv1) = fpv(numv1)-fac
|
||||
fpv(numv) = fpv(numv)+fac
|
||||
930 nroldv = numv
|
||||
if(numu.eq.nroldu) go to 940
|
||||
fpu(numu1) = fpu(numu1)-fac
|
||||
fpu(numu) = fpu(numu)+fac
|
||||
940 continue
|
||||
nroldu = numu
|
||||
950 continue
|
||||
return
|
||||
end
|
||||
78
mcc/bsplines/fpinst.f
Normal file
78
mcc/bsplines/fpinst.f
Normal file
@@ -0,0 +1,78 @@
|
||||
recursive subroutine fpinst(iopt,t,n,c,k,x,l,tt,nn,cc,nest)
|
||||
implicit none
|
||||
c given the b-spline representation (knots t(j),j=1,2,...,n, b-spline
|
||||
c coefficients c(j),j=1,2,...,n-k-1) of a spline of degree k, fpinst
|
||||
c calculates the b-spline representation (knots tt(j),j=1,2,...,nn,
|
||||
c b-spline coefficients cc(j),j=1,2,...,nn-k-1) of the same spline if
|
||||
c an additional knot is inserted at the point x situated in the inter-
|
||||
c val t(l)<=x<t(l+1). iopt denotes whether (iopt.ne.0) or not (iopt=0)
|
||||
c the given spline is periodic. in case of a periodic spline at least
|
||||
c one of the following conditions must be fulfilled: l>2*k or l<n-2*k.
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer k,n,l,nn,iopt,nest
|
||||
real*8 x
|
||||
c ..array arguments..
|
||||
real*8 t(nest),c(nest),tt(nest),cc(nest)
|
||||
c ..local scalars..
|
||||
real*8 fac,per,one
|
||||
integer i,i1,j,k1,m,mk,nk,nk1,nl,ll
|
||||
c ..
|
||||
one = 0.1e+01
|
||||
k1 = k+1
|
||||
nk1 = n-k1
|
||||
c the new knots
|
||||
ll = l+1
|
||||
i = n
|
||||
do 10 j=ll,n
|
||||
tt(i+1) = t(i)
|
||||
i = i-1
|
||||
10 continue
|
||||
tt(ll) = x
|
||||
do 20 j=1,l
|
||||
tt(j) = t(j)
|
||||
20 continue
|
||||
c the new b-spline coefficients
|
||||
i = nk1
|
||||
do 30 j=l,nk1
|
||||
cc(i+1) = c(i)
|
||||
i = i-1
|
||||
30 continue
|
||||
i = l
|
||||
do 40 j=1,k
|
||||
m = i+k1
|
||||
fac = (x-tt(i))/(tt(m)-tt(i))
|
||||
i1 = i-1
|
||||
cc(i) = fac*c(i)+(one-fac)*c(i1)
|
||||
i = i1
|
||||
40 continue
|
||||
do 50 j=1,i
|
||||
cc(j) = c(j)
|
||||
50 continue
|
||||
nn = n+1
|
||||
if(iopt.eq.0) return
|
||||
c incorporate the boundary conditions for a periodic spline.
|
||||
nk = nn-k
|
||||
nl = nk-k1
|
||||
per = tt(nk)-tt(k1)
|
||||
i = k1
|
||||
j = nk
|
||||
if(ll.le.nl) go to 70
|
||||
do 60 m=1,k
|
||||
mk = m+nl
|
||||
cc(m) = cc(mk)
|
||||
i = i-1
|
||||
j = j-1
|
||||
tt(i) = tt(j)-per
|
||||
60 continue
|
||||
return
|
||||
70 if(ll.gt.(k1+k)) return
|
||||
do 80 m=1,k
|
||||
mk = m+nl
|
||||
cc(mk) = cc(m)
|
||||
i = i+1
|
||||
j = j+1
|
||||
tt(j) = tt(i)+per
|
||||
80 continue
|
||||
return
|
||||
end
|
||||
131
mcc/bsplines/fpintb.f
Normal file
131
mcc/bsplines/fpintb.f
Normal file
@@ -0,0 +1,131 @@
|
||||
recursive subroutine fpintb(t,n,bint,nk1,x,y)
|
||||
implicit none
|
||||
c subroutine fpintb calculates integrals of the normalized b-splines
|
||||
c nj,k+1(x) of degree k, defined on the set of knots t(j),j=1,2,...n.
|
||||
c it makes use of the formulae of gaffney for the calculation of
|
||||
c indefinite integrals of b-splines.
|
||||
c
|
||||
c calling sequence:
|
||||
c call fpintb(t,n,bint,nk1,x,y)
|
||||
c
|
||||
c input parameters:
|
||||
c t : real array,length n, containing the position of the knots.
|
||||
c n : integer value, giving the number of knots.
|
||||
c nk1 : integer value, giving the number of b-splines of degree k,
|
||||
c defined on the set of knots ,i.e. nk1 = n-k-1.
|
||||
c x,y : real values, containing the end points of the integration
|
||||
c interval.
|
||||
c output parameter:
|
||||
c bint : array,length nk1, containing the integrals of the b-splines.
|
||||
c ..
|
||||
c ..scalars arguments..
|
||||
integer n,nk1
|
||||
real*8 x,y
|
||||
c ..array arguments..
|
||||
real*8 t(n),bint(nk1)
|
||||
c ..local scalars..
|
||||
integer i,ia,ib,it,j,j1,k,k1,l,li,lj,lk,l0,min
|
||||
real*8 a,ak,arg,b,f,one
|
||||
c ..local arrays..
|
||||
real*8 aint(6),h(6),h1(6)
|
||||
c initialization.
|
||||
one = 0.1d+01
|
||||
k1 = n-nk1
|
||||
ak = k1
|
||||
k = k1-1
|
||||
do 10 i=1,nk1
|
||||
bint(i) = 0.0d0
|
||||
10 continue
|
||||
c the integration limits are arranged in increasing order.
|
||||
a = x
|
||||
b = y
|
||||
min = 0
|
||||
if (a.lt.b) go to 30
|
||||
if (a.eq.b) go to 160
|
||||
go to 20
|
||||
20 a = y
|
||||
b = x
|
||||
min = 1
|
||||
30 if(a.lt.t(k1)) a = t(k1)
|
||||
if(b.gt.t(nk1+1)) b = t(nk1+1)
|
||||
if(a.gt.b) go to 160
|
||||
c using the expression of gaffney for the indefinite integral of a
|
||||
c b-spline we find that
|
||||
c bint(j) = (t(j+k+1)-t(j))*(res(j,b)-res(j,a))/(k+1)
|
||||
c where for t(l) <= x < t(l+1)
|
||||
c res(j,x) = 0, j=1,2,...,l-k-1
|
||||
c = 1, j=l+1,l+2,...,nk1
|
||||
c = aint(j+k-l+1), j=l-k,l-k+1,...,l
|
||||
c = sumi((x-t(j+i))*nj+i,k+1-i(x)/(t(j+k+1)-t(j+i)))
|
||||
c i=0,1,...,k
|
||||
l = k1
|
||||
l0 = l+1
|
||||
c set arg = a.
|
||||
arg = a
|
||||
do 90 it=1,2
|
||||
c search for the knot interval t(l) <= arg < t(l+1).
|
||||
40 if(arg.lt.t(l0) .or. l.eq.nk1) go to 50
|
||||
l = l0
|
||||
l0 = l+1
|
||||
go to 40
|
||||
c calculation of aint(j), j=1,2,...,k+1.
|
||||
c initialization.
|
||||
50 do 55 j=1,k1
|
||||
aint(j) = 0.0d0
|
||||
55 continue
|
||||
aint(1) = (arg-t(l))/(t(l+1)-t(l))
|
||||
h1(1) = one
|
||||
do 70 j=1,k
|
||||
c evaluation of the non-zero b-splines of degree j at arg,i.e.
|
||||
c h(i+1) = nl-j+i,j(arg), i=0,1,...,j.
|
||||
h(1) = 0.0d0
|
||||
do 60 i=1,j
|
||||
li = l+i
|
||||
lj = li-j
|
||||
f = h1(i)/(t(li)-t(lj))
|
||||
h(i) = h(i)+f*(t(li)-arg)
|
||||
h(i+1) = f*(arg-t(lj))
|
||||
60 continue
|
||||
c updating of the integrals aint.
|
||||
j1 = j+1
|
||||
do 70 i=1,j1
|
||||
li = l+i
|
||||
lj = li-j1
|
||||
aint(i) = aint(i)+h(i)*(arg-t(lj))/(t(li)-t(lj))
|
||||
h1(i) = h(i)
|
||||
70 continue
|
||||
if(it.eq.2) go to 100
|
||||
c updating of the integrals bint
|
||||
lk = l-k
|
||||
ia = lk
|
||||
do 80 i=1,k1
|
||||
bint(lk) = -aint(i)
|
||||
lk = lk+1
|
||||
80 continue
|
||||
c set arg = b.
|
||||
arg = b
|
||||
90 continue
|
||||
c updating of the integrals bint.
|
||||
100 lk = l-k
|
||||
ib = lk-1
|
||||
do 110 i=1,k1
|
||||
bint(lk) = bint(lk)+aint(i)
|
||||
lk = lk+1
|
||||
110 continue
|
||||
if(ib.lt.ia) go to 130
|
||||
do 120 i=ia,ib
|
||||
bint(i) = bint(i)+one
|
||||
120 continue
|
||||
c the scaling factors are taken into account.
|
||||
130 f = one/ak
|
||||
do 140 i=1,nk1
|
||||
j = i+k1
|
||||
bint(i) = bint(i)*(t(j)-t(i))*f
|
||||
140 continue
|
||||
c the order of the integration limits is taken into account.
|
||||
if(min.eq.0) go to 160
|
||||
do 150 i=1,nk1
|
||||
bint(i) = -bint(i)
|
||||
150 continue
|
||||
160 return
|
||||
end
|
||||
73
mcc/bsplines/fpknot.f
Normal file
73
mcc/bsplines/fpknot.f
Normal file
@@ -0,0 +1,73 @@
|
||||
recursive subroutine fpknot(x,m,t,n,fpint,nrdata,nrint,nest,
|
||||
* istart)
|
||||
implicit none
|
||||
c subroutine fpknot locates an additional knot for a spline of degree
|
||||
c k and adjusts the corresponding parameters,i.e.
|
||||
c t : the position of the knots.
|
||||
c n : the number of knots.
|
||||
c nrint : the number of knotintervals.
|
||||
c fpint : the sum of squares of residual right hand sides
|
||||
c for each knot interval.
|
||||
c nrdata: the number of data points inside each knot interval.
|
||||
c istart indicates that the smallest data point at which the new knot
|
||||
c may be added is x(istart+1)
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer m,n,nrint,nest,istart
|
||||
c ..array arguments..
|
||||
real*8 x(m),t(nest),fpint(nest)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 an,am,fpmax
|
||||
integer ihalf,j,jbegin,jj,jk,jpoint,k,maxbeg,maxpt,
|
||||
* next,nrx,number
|
||||
|
||||
c note: do not initialize on the same line to avoid saving between calls
|
||||
logical iserr
|
||||
iserr = .TRUE.
|
||||
|
||||
k = (n-nrint-1)/2
|
||||
c search for knot interval t(number+k) <= x <= t(number+k+1) where
|
||||
c fpint(number) is maximal on the condition that nrdata(number)
|
||||
c not equals zero.
|
||||
fpmax = 0.
|
||||
jbegin = istart
|
||||
do 20 j=1,nrint
|
||||
jpoint = nrdata(j)
|
||||
if(fpmax.ge.fpint(j) .or. jpoint.eq.0) go to 10
|
||||
iserr = .FALSE.
|
||||
fpmax = fpint(j)
|
||||
number = j
|
||||
maxpt = jpoint
|
||||
maxbeg = jbegin
|
||||
10 jbegin = jbegin+jpoint+1
|
||||
20 continue
|
||||
c error condition detected, go to exit
|
||||
if(iserr) go to 50
|
||||
c let coincide the new knot t(number+k+1) with a data point x(nrx)
|
||||
c inside the old knot interval t(number+k) <= x <= t(number+k+1).
|
||||
ihalf = maxpt/2+1
|
||||
nrx = maxbeg+ihalf
|
||||
next = number+1
|
||||
if(next.gt.nrint) go to 40
|
||||
c adjust the different parameters.
|
||||
do 30 j=next,nrint
|
||||
jj = next+nrint-j
|
||||
fpint(jj+1) = fpint(jj)
|
||||
nrdata(jj+1) = nrdata(jj)
|
||||
jk = jj+k
|
||||
t(jk+1) = t(jk)
|
||||
30 continue
|
||||
40 nrdata(number) = ihalf-1
|
||||
nrdata(next) = maxpt-ihalf
|
||||
am = maxpt
|
||||
an = nrdata(number)
|
||||
fpint(number) = fpmax*an/am
|
||||
an = nrdata(next)
|
||||
fpint(next) = fpmax*an/am
|
||||
jk = next+k
|
||||
t(jk) = x(nrx)
|
||||
50 n = n+1
|
||||
nrint = nrint+1
|
||||
return
|
||||
end
|
||||
182
mcc/bsplines/fpopdi.f
Normal file
182
mcc/bsplines/fpopdi.f
Normal file
@@ -0,0 +1,182 @@
|
||||
recursive subroutine fpopdi(ifsu,ifsv,ifbu,ifbv,u,mu,v,mv,z,
|
||||
* mz,z0,dz,iopt,ider,tu,nu,tv,nv,nuest,nvest,p,step,c,nc,fp,
|
||||
* fpu,fpv,nru,nrv,wrk,lwrk)
|
||||
implicit none
|
||||
c given the set of function values z(i,j) defined on the rectangular
|
||||
c grid (u(i),v(j)),i=1,2,...,mu;j=1,2,...,mv, fpopdi determines a
|
||||
c smooth bicubic spline approximation with given knots tu(i),i=1,..,nu
|
||||
c in the u-direction and tv(j),j=1,2,...,nv in the v-direction. this
|
||||
c spline sp(u,v) will be periodic in the variable v and will satisfy
|
||||
c the following constraints
|
||||
c
|
||||
c s(tu(1),v) = dz(1) , tv(4) <=v<= tv(nv-3)
|
||||
c
|
||||
c and (if iopt(2) = 1)
|
||||
c
|
||||
c d s(tu(1),v)
|
||||
c ------------ = dz(2)*cos(v)+dz(3)*sin(v) , tv(4) <=v<= tv(nv-3)
|
||||
c d u
|
||||
c
|
||||
c and (if iopt(3) = 1)
|
||||
c
|
||||
c s(tu(nu),v) = 0 tv(4) <=v<= tv(nv-3)
|
||||
c
|
||||
c where the parameters dz(i) correspond to the derivative values g(i,j)
|
||||
c as defined in subroutine pogrid.
|
||||
c
|
||||
c the b-spline coefficients of sp(u,v) are determined as the least-
|
||||
c squares solution of an overdetermined linear system which depends
|
||||
c on the value of p and on the values dz(i),i=1,2,3. the correspond-
|
||||
c ing sum of squared residuals sq is a simple quadratic function in
|
||||
c the variables dz(i). these may or may not be provided. the values
|
||||
c dz(i) which are not given will be determined so as to minimize the
|
||||
c resulting sum of squared residuals sq. in that case the user must
|
||||
c provide some initial guess dz(i) and some estimate (dz(i)-step,
|
||||
c dz(i)+step) of the range of possible values for these latter.
|
||||
c
|
||||
c sp(u,v) also depends on the parameter p (p>0) in such a way that
|
||||
c - if p tends to infinity, sp(u,v) becomes the least-squares spline
|
||||
c with given knots, satisfying the constraints.
|
||||
c - if p tends to zero, sp(u,v) becomes the least-squares polynomial,
|
||||
c satisfying the constraints.
|
||||
c - the function f(p)=sumi=1,mu(sumj=1,mv((z(i,j)-sp(u(i),v(j)))**2)
|
||||
c is continuous and strictly decreasing for p>0.
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer ifsu,ifsv,ifbu,ifbv,mu,mv,mz,nu,nv,nuest,nvest,
|
||||
* nc,lwrk
|
||||
real*8 z0,p,step,fp
|
||||
c ..array arguments..
|
||||
integer ider(2),nru(mu),nrv(mv),iopt(3)
|
||||
real*8 u(mu),v(mv),z(mz),dz(3),tu(nu),tv(nv),c(nc),fpu(nu),fpv(nv)
|
||||
*,
|
||||
* wrk(lwrk)
|
||||
c ..local scalars..
|
||||
real*8 res,sq,sqq,step1,step2,three
|
||||
integer i,id0,iop0,iop1,i1,j,l,laa,lau,lav1,lav2,lbb,lbu,lbv,
|
||||
* lcc,lcs,lq,lri,lsu,lsv,l1,l2,mm,mvnu,number
|
||||
c ..local arrays..
|
||||
integer nr(3)
|
||||
real*8 delta(3),dzz(3),sum(3),a(6,6),g(6)
|
||||
c ..function references..
|
||||
integer max0
|
||||
c ..subroutine references..
|
||||
c fpgrdi,fpsysy
|
||||
c ..
|
||||
c set constant
|
||||
three = 3
|
||||
c we partition the working space
|
||||
lsu = 1
|
||||
lsv = lsu+4*mu
|
||||
lri = lsv+4*mv
|
||||
mm = max0(nuest,mv+nvest)
|
||||
lq = lri+mm
|
||||
mvnu = nuest*(mv+nvest-8)
|
||||
lau = lq+mvnu
|
||||
lav1 = lau+5*nuest
|
||||
lav2 = lav1+6*nvest
|
||||
lbu = lav2+4*nvest
|
||||
lbv = lbu+5*nuest
|
||||
laa = lbv+5*nvest
|
||||
lbb = laa+2*mv
|
||||
lcc = lbb+2*nvest
|
||||
lcs = lcc+nvest
|
||||
c we calculate the smoothing spline sp(u,v) according to the input
|
||||
c values dz(i),i=1,2,3.
|
||||
iop0 = iopt(2)
|
||||
iop1 = iopt(3)
|
||||
call fpgrdi(ifsu,ifsv,ifbu,ifbv,0,u,mu,v,mv,z,mz,dz,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(laa),wrk(lbb),
|
||||
* wrk(lcc),wrk(lcs),nru,nrv)
|
||||
id0 = ider(1)
|
||||
if(id0.ne.0) go to 5
|
||||
res = (z0-dz(1))**2
|
||||
fp = fp+res
|
||||
sq = sq+res
|
||||
c in case all derivative values dz(i) are given (step<=0) or in case
|
||||
c we have spline interpolation, we accept this spline as a solution.
|
||||
5 if(step.le.0. .or. sq.le.0.) return
|
||||
dzz(1) = dz(1)
|
||||
dzz(2) = dz(2)
|
||||
dzz(3) = dz(3)
|
||||
c number denotes the number of derivative values dz(i) that still must
|
||||
c be optimized. let us denote these parameters by g(j),j=1,...,number.
|
||||
number = 0
|
||||
if(id0.gt.0) go to 10
|
||||
number = 1
|
||||
nr(1) = 1
|
||||
delta(1) = step
|
||||
10 if(iop0.eq.0) go to 20
|
||||
if(ider(2).ne.0) go to 20
|
||||
step2 = step*three/tu(5)
|
||||
nr(number+1) = 2
|
||||
nr(number+2) = 3
|
||||
delta(number+1) = step2
|
||||
delta(number+2) = step2
|
||||
number = number+2
|
||||
20 if(number.eq.0) return
|
||||
c the sum of squared residuals sq is a quadratic polynomial in the
|
||||
c parameters g(j). we determine the unknown coefficients of this
|
||||
c polymomial by calculating (number+1)*(number+2)/2 different splines
|
||||
c according to specific values for g(j).
|
||||
do 30 i=1,number
|
||||
l = nr(i)
|
||||
step1 = delta(i)
|
||||
dzz(l) = dz(l)+step1
|
||||
call fpgrdi(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,z,mz,dzz,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sum(i),fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(laa),wrk(lbb),
|
||||
* wrk(lcc),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sum(i) = sum(i)+(z0-dzz(1))**2
|
||||
dzz(l) = dz(l)-step1
|
||||
call fpgrdi(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,z,mz,dzz,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sqq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(laa),wrk(lbb),
|
||||
* wrk(lcc),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sqq = sqq+(z0-dzz(1))**2
|
||||
a(i,i) = (sum(i)+sqq-sq-sq)/step1**2
|
||||
if(a(i,i).le.0.) go to 80
|
||||
g(i) = (sqq-sum(i))/(step1+step1)
|
||||
dzz(l) = dz(l)
|
||||
30 continue
|
||||
if(number.eq.1) go to 60
|
||||
do 50 i=2,number
|
||||
l1 = nr(i)
|
||||
step1 = delta(i)
|
||||
dzz(l1) = dz(l1)+step1
|
||||
i1 = i-1
|
||||
do 40 j=1,i1
|
||||
l2 = nr(j)
|
||||
step2 = delta(j)
|
||||
dzz(l2) = dz(l2)+step2
|
||||
call fpgrdi(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,z,mz,dzz,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sqq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(laa),wrk(lbb),
|
||||
* wrk(lcc),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sqq = sqq+(z0-dzz(1))**2
|
||||
a(i,j) = (sq+sqq-sum(i)-sum(j))/(step1*step2)
|
||||
dzz(l2) = dz(l2)
|
||||
40 continue
|
||||
dzz(l1) = dz(l1)
|
||||
50 continue
|
||||
c the optimal values g(j) are found as the solution of the system
|
||||
c d (sq) / d (g(j)) = 0 , j=1,...,number.
|
||||
60 call fpsysy(a,number,g)
|
||||
do 70 i=1,number
|
||||
l = nr(i)
|
||||
dz(l) = dz(l)+g(i)
|
||||
70 continue
|
||||
c we determine the spline sp(u,v) according to the optimal values g(j).
|
||||
80 call fpgrdi(ifsu,ifsv,ifbu,ifbv,0,u,mu,v,mv,z,mz,dz,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(laa),wrk(lbb),
|
||||
* wrk(lcc),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) fp = fp+(z0-dz(1))**2
|
||||
return
|
||||
end
|
||||
212
mcc/bsplines/fpopsp.f
Normal file
212
mcc/bsplines/fpopsp.f
Normal file
@@ -0,0 +1,212 @@
|
||||
recursive subroutine fpopsp(ifsu,ifsv,ifbu,ifbv,u,mu,v,mv,r,
|
||||
* mr,r0,r1,dr,iopt,ider,tu,nu,tv,nv,nuest,nvest,p,step,c,nc,
|
||||
* fp,fpu,fpv,nru,nrv,wrk,lwrk)
|
||||
implicit none
|
||||
c given the set of function values r(i,j) defined on the rectangular
|
||||
c grid (u(i),v(j)),i=1,2,...,mu;j=1,2,...,mv, fpopsp determines a
|
||||
c smooth bicubic spline approximation with given knots tu(i),i=1,..,nu
|
||||
c in the u-direction and tv(j),j=1,2,...,nv in the v-direction. this
|
||||
c spline sp(u,v) will be periodic in the variable v and will satisfy
|
||||
c the following constraints
|
||||
c
|
||||
c s(tu(1),v) = dr(1) , tv(4) <=v<= tv(nv-3)
|
||||
c
|
||||
c s(tu(nu),v) = dr(4) , tv(4) <=v<= tv(nv-3)
|
||||
c
|
||||
c and (if iopt(2) = 1)
|
||||
c
|
||||
c d s(tu(1),v)
|
||||
c ------------ = dr(2)*cos(v)+dr(3)*sin(v) , tv(4) <=v<= tv(nv-3)
|
||||
c d u
|
||||
c
|
||||
c and (if iopt(3) = 1)
|
||||
c
|
||||
c d s(tu(nu),v)
|
||||
c ------------- = dr(5)*cos(v)+dr(6)*sin(v) , tv(4) <=v<= tv(nv-3)
|
||||
c d u
|
||||
c
|
||||
c where the parameters dr(i) correspond to the derivative values at the
|
||||
c poles as defined in subroutine spgrid.
|
||||
c
|
||||
c the b-spline coefficients of sp(u,v) are determined as the least-
|
||||
c squares solution of an overdetermined linear system which depends
|
||||
c on the value of p and on the values dr(i),i=1,...,6. the correspond-
|
||||
c ing sum of squared residuals sq is a simple quadratic function in
|
||||
c the variables dr(i). these may or may not be provided. the values
|
||||
c dr(i) which are not given will be determined so as to minimize the
|
||||
c resulting sum of squared residuals sq. in that case the user must
|
||||
c provide some initial guess dr(i) and some estimate (dr(i)-step,
|
||||
c dr(i)+step) of the range of possible values for these latter.
|
||||
c
|
||||
c sp(u,v) also depends on the parameter p (p>0) in such a way that
|
||||
c - if p tends to infinity, sp(u,v) becomes the least-squares spline
|
||||
c with given knots, satisfying the constraints.
|
||||
c - if p tends to zero, sp(u,v) becomes the least-squares polynomial,
|
||||
c satisfying the constraints.
|
||||
c - the function f(p)=sumi=1,mu(sumj=1,mv((r(i,j)-sp(u(i),v(j)))**2)
|
||||
c is continuous and strictly decreasing for p>0.
|
||||
c
|
||||
c ..scalar arguments..
|
||||
integer ifsu,ifsv,ifbu,ifbv,mu,mv,mr,nu,nv,nuest,nvest,
|
||||
* nc,lwrk
|
||||
real*8 r0,r1,p,fp
|
||||
c ..array arguments..
|
||||
integer ider(4),nru(mu),nrv(mv),iopt(3)
|
||||
real*8 u(mu),v(mv),r(mr),dr(6),tu(nu),tv(nv),c(nc),fpu(nu),fpv(nv)
|
||||
*,
|
||||
* wrk(lwrk),step(2)
|
||||
c ..local scalars..
|
||||
real*8 sq,sqq,sq0,sq1,step1,step2,three
|
||||
integer i,id0,iop0,iop1,i1,j,l,lau,lav1,lav2,la0,la1,lbu,lbv,lb0,
|
||||
* lb1,lc0,lc1,lcs,lq,lri,lsu,lsv,l1,l2,mm,mvnu,number, id1
|
||||
c ..local arrays..
|
||||
integer nr(6)
|
||||
real*8 delta(6),drr(6),sum(6),a(6,6),g(6)
|
||||
c ..function references..
|
||||
integer max0
|
||||
c ..subroutine references..
|
||||
c fpgrsp,fpsysy
|
||||
c ..
|
||||
c set constant
|
||||
three = 3
|
||||
c we partition the working space
|
||||
lsu = 1
|
||||
lsv = lsu+4*mu
|
||||
lri = lsv+4*mv
|
||||
mm = max0(nuest,mv+nvest)
|
||||
lq = lri+mm
|
||||
mvnu = nuest*(mv+nvest-8)
|
||||
lau = lq+mvnu
|
||||
lav1 = lau+5*nuest
|
||||
lav2 = lav1+6*nvest
|
||||
lbu = lav2+4*nvest
|
||||
lbv = lbu+5*nuest
|
||||
la0 = lbv+5*nvest
|
||||
la1 = la0+2*mv
|
||||
lb0 = la1+2*mv
|
||||
lb1 = lb0+2*nvest
|
||||
lc0 = lb1+2*nvest
|
||||
lc1 = lc0+nvest
|
||||
lcs = lc1+nvest
|
||||
c we calculate the smoothing spline sp(u,v) according to the input
|
||||
c values dr(i),i=1,...,6.
|
||||
iop0 = iopt(2)
|
||||
iop1 = iopt(3)
|
||||
id0 = ider(1)
|
||||
id1 = ider(3)
|
||||
call fpgrsp(ifsu,ifsv,ifbu,ifbv,0,u,mu,v,mv,r,mr,dr,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(la0),wrk(la1),wrk(lb0),
|
||||
* wrk(lb1),wrk(lc0),wrk(lc1),wrk(lcs),nru,nrv)
|
||||
sq0 = 0.
|
||||
sq1 = 0.
|
||||
if(id0.eq.0) sq0 = (r0-dr(1))**2
|
||||
if(id1.eq.0) sq1 = (r1-dr(4))**2
|
||||
sq = sq+sq0+sq1
|
||||
c in case all derivative values dr(i) are given (step<=0) or in case
|
||||
c we have spline interpolation, we accept this spline as a solution.
|
||||
if(sq.le.0.) return
|
||||
if(step(1).le.0. .and. step(2).le.0.) return
|
||||
do 10 i=1,6
|
||||
drr(i) = dr(i)
|
||||
10 continue
|
||||
c number denotes the number of derivative values dr(i) that still must
|
||||
c be optimized. let us denote these parameters by g(j),j=1,...,number.
|
||||
number = 0
|
||||
if(id0.gt.0) go to 20
|
||||
number = 1
|
||||
nr(1) = 1
|
||||
delta(1) = step(1)
|
||||
20 if(iop0.eq.0) go to 30
|
||||
if(ider(2).ne.0) go to 30
|
||||
step2 = step(1)*three/(tu(5)-tu(4))
|
||||
nr(number+1) = 2
|
||||
nr(number+2) = 3
|
||||
delta(number+1) = step2
|
||||
delta(number+2) = step2
|
||||
number = number+2
|
||||
30 if(id1.gt.0) go to 40
|
||||
number = number+1
|
||||
nr(number) = 4
|
||||
delta(number) = step(2)
|
||||
40 if(iop1.eq.0) go to 50
|
||||
if(ider(4).ne.0) go to 50
|
||||
step2 = step(2)*three/(tu(nu)-tu(nu-4))
|
||||
nr(number+1) = 5
|
||||
nr(number+2) = 6
|
||||
delta(number+1) = step2
|
||||
delta(number+2) = step2
|
||||
number = number+2
|
||||
50 if(number.eq.0) return
|
||||
c the sum of squared residulas sq is a quadratic polynomial in the
|
||||
c parameters g(j). we determine the unknown coefficients of this
|
||||
c polymomial by calculating (number+1)*(number+2)/2 different splines
|
||||
c according to specific values for g(j).
|
||||
do 60 i=1,number
|
||||
l = nr(i)
|
||||
step1 = delta(i)
|
||||
drr(l) = dr(l)+step1
|
||||
call fpgrsp(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,r,mr,drr,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sum(i),fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(la0),wrk(la1),wrk(lb0),
|
||||
* wrk(lb1),wrk(lc0),wrk(lc1),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sq0 = (r0-drr(1))**2
|
||||
if(id1.eq.0) sq1 = (r1-drr(4))**2
|
||||
sum(i) = sum(i)+sq0+sq1
|
||||
drr(l) = dr(l)-step1
|
||||
call fpgrsp(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,r,mr,drr,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sqq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(la0),wrk(la1),wrk(lb0),
|
||||
* wrk(lb1),wrk(lc0),wrk(lc1),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sq0 = (r0-drr(1))**2
|
||||
if(id1.eq.0) sq1 = (r1-drr(4))**2
|
||||
sqq = sqq+sq0+sq1
|
||||
drr(l) = dr(l)
|
||||
a(i,i) = (sum(i)+sqq-sq-sq)/step1**2
|
||||
if(a(i,i).le.0.) go to 110
|
||||
g(i) = (sqq-sum(i))/(step1+step1)
|
||||
60 continue
|
||||
if(number.eq.1) go to 90
|
||||
do 80 i=2,number
|
||||
l1 = nr(i)
|
||||
step1 = delta(i)
|
||||
drr(l1) = dr(l1)+step1
|
||||
i1 = i-1
|
||||
do 70 j=1,i1
|
||||
l2 = nr(j)
|
||||
step2 = delta(j)
|
||||
drr(l2) = dr(l2)+step2
|
||||
call fpgrsp(ifsu,ifsv,ifbu,ifbv,1,u,mu,v,mv,r,mr,drr,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sqq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(la0),wrk(la1),wrk(lb0),
|
||||
* wrk(lb1),wrk(lc0),wrk(lc1),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sq0 = (r0-drr(1))**2
|
||||
if(id1.eq.0) sq1 = (r1-drr(4))**2
|
||||
sqq = sqq+sq0+sq1
|
||||
a(i,j) = (sq+sqq-sum(i)-sum(j))/(step1*step2)
|
||||
drr(l2) = dr(l2)
|
||||
70 continue
|
||||
drr(l1) = dr(l1)
|
||||
80 continue
|
||||
c the optimal values g(j) are found as the solution of the system
|
||||
c d (sq) / d (g(j)) = 0 , j=1,...,number.
|
||||
90 call fpsysy(a,number,g)
|
||||
do 100 i=1,number
|
||||
l = nr(i)
|
||||
dr(l) = dr(l)+g(i)
|
||||
100 continue
|
||||
c we determine the spline sp(u,v) according to the optimal values g(j).
|
||||
110 call fpgrsp(ifsu,ifsv,ifbu,ifbv,0,u,mu,v,mv,r,mr,dr,
|
||||
* iop0,iop1,tu,nu,tv,nv,p,c,nc,sq,fp,fpu,fpv,mm,mvnu,
|
||||
* wrk(lsu),wrk(lsv),wrk(lri),wrk(lq),wrk(lau),wrk(lav1),
|
||||
* wrk(lav2),wrk(lbu),wrk(lbv),wrk(la0),wrk(la1),wrk(lb0),
|
||||
* wrk(lb1),wrk(lc0),wrk(lc1),wrk(lcs),nru,nrv)
|
||||
if(id0.eq.0) sq0 = (r0-dr(1))**2
|
||||
if(id1.eq.0) sq1 = (r1-dr(4))**2
|
||||
sq = sq+sq0+sq1
|
||||
return
|
||||
end
|
||||
48
mcc/bsplines/fporde.f
Normal file
48
mcc/bsplines/fporde.f
Normal file
@@ -0,0 +1,48 @@
|
||||
recursive subroutine fporde(x,y,m,kx,ky,tx,nx,ty,ny,nummer,
|
||||
* index,nreg)
|
||||
c subroutine fporde sorts the data points (x(i),y(i)),i=1,2,...,m
|
||||
c according to the panel tx(l)<=x<tx(l+1),ty(k)<=y<ty(k+1), they belong
|
||||
c to. for each panel a stack is constructed containing the numbers
|
||||
c of data points lying inside; index(j),j=1,2,...,nreg points to the
|
||||
c first data point in the jth panel while nummer(i),i=1,2,...,m gives
|
||||
c the number of the next data point in the panel.
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer m,kx,ky,nx,ny,nreg
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),tx(nx),ty(ny)
|
||||
integer nummer(m),index(nreg)
|
||||
c ..local scalars..
|
||||
real*8 xi,yi
|
||||
integer i,im,k,kx1,ky1,k1,l,l1,nk1x,nk1y,num,nyy
|
||||
c ..
|
||||
kx1 = kx+1
|
||||
ky1 = ky+1
|
||||
nk1x = nx-kx1
|
||||
nk1y = ny-ky1
|
||||
nyy = nk1y-ky
|
||||
do 10 i=1,nreg
|
||||
index(i) = 0
|
||||
10 continue
|
||||
do 60 im=1,m
|
||||
xi = x(im)
|
||||
yi = y(im)
|
||||
l = kx1
|
||||
l1 = l+1
|
||||
20 if(xi.lt.tx(l1) .or. l.eq.nk1x) go to 30
|
||||
l = l1
|
||||
l1 = l+1
|
||||
go to 20
|
||||
30 k = ky1
|
||||
k1 = k+1
|
||||
40 if(yi.lt.ty(k1) .or. k.eq.nk1y) go to 50
|
||||
k = k1
|
||||
k1 = k+1
|
||||
go to 40
|
||||
50 num = (l-kx1)*nyy+k-ky
|
||||
nummer(im) = index(num)
|
||||
index(num) = im
|
||||
60 continue
|
||||
return
|
||||
end
|
||||
|
||||
402
mcc/bsplines/fppara.f
Normal file
402
mcc/bsplines/fppara.f
Normal file
@@ -0,0 +1,402 @@
|
||||
subroutine fppara(iopt,idim,m,u,mx,x,w,ub,ue,k,s,nest,tol,maxit,
|
||||
* k1,k2,n,t,nc,c,fp,fpint,z,a,b,g,q,nrdata,ier)
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 ub,ue,s,tol,fp
|
||||
integer iopt,idim,m,mx,k,nest,maxit,k1,k2,n,nc,ier
|
||||
c ..array arguments..
|
||||
real*8 u(m),x(mx),w(m),t(nest),c(nc),fpint(nest),
|
||||
* z(nc),a(nest,k1),b(nest,k2),g(nest,k2),q(m,k1)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 acc,con1,con4,con9,cos,fac,fpart,fpms,fpold,fp0,f1,f2,f3,
|
||||
* half,one,p,pinv,piv,p1,p2,p3,rn,sin,store,term,ui,wi
|
||||
integer i,ich1,ich3,it,iter,i1,i2,i3,j,jj,j1,j2,k3,l,l0,
|
||||
* mk1,new,nk1,nmax,nmin,nplus,npl1,nrint,n8
|
||||
c ..local arrays..
|
||||
real*8 h(7),xi(10)
|
||||
c ..function references
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpback,fpbspl,fpgivs,fpdisc,fpknot,fprota
|
||||
c ..
|
||||
c set constants
|
||||
one = 0.1e+01
|
||||
con1 = 0.1e0
|
||||
con9 = 0.9e0
|
||||
con4 = 0.4e-01
|
||||
half = 0.5e0
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position c
|
||||
c ************************************************************** c
|
||||
c given a set of knots we compute the least-squares curve sinf(u), c
|
||||
c and the corresponding sum of squared residuals fp=f(p=inf). c
|
||||
c if iopt=-1 sinf(u) is the requested curve. c
|
||||
c if iopt=0 or iopt=1 we check whether we can accept the knots: c
|
||||
c if fp <=s we will continue with the current set of knots. c
|
||||
c if fp > s we will increase the number of knots and compute the c
|
||||
c corresponding least-squares curve until finally fp<=s. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmax = m+k+1. c
|
||||
c if s > 0 and c
|
||||
c iopt=0 we first compute the least-squares polynomial curve of c
|
||||
c degree k; n = nmin = 2*k+2 c
|
||||
c iopt=1 we start with the set of knots found at the last c
|
||||
c call of the routine, except for the case that s > fp0; then c
|
||||
c we compute directly the polynomial curve of degree k. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c determine nmin, the number of knots for polynomial approximation.
|
||||
nmin = 2*k1
|
||||
if(iopt.lt.0) go to 60
|
||||
c calculation of acc, the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c determine nmax, the number of knots for spline interpolation.
|
||||
nmax = m+k1
|
||||
if(s.gt.0.) go to 45
|
||||
c if s=0, s(u) is an interpolating curve.
|
||||
c test whether the required storage space exceeds the available one.
|
||||
n = nmax
|
||||
if(nmax.gt.nest) go to 420
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
10 mk1 = m-k1
|
||||
if(mk1.eq.0) go to 60
|
||||
k3 = k/2
|
||||
i = k2
|
||||
j = k3+2
|
||||
if(k3*2.eq.k) go to 30
|
||||
do 20 l=1,mk1
|
||||
t(i) = u(j)
|
||||
i = i+1
|
||||
j = j+1
|
||||
20 continue
|
||||
go to 60
|
||||
30 do 40 l=1,mk1
|
||||
t(i) = (u(j)+u(j-1))*half
|
||||
i = i+1
|
||||
j = j+1
|
||||
40 continue
|
||||
go to 60
|
||||
c if s>0 our initial choice of knots depends on the value of iopt.
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c polynomial curve which is a spline curve without interior knots.
|
||||
c if iopt=1 and fp0>s we start computing the least squares spline curve
|
||||
c according to the set of knots found at the last call of the routine.
|
||||
45 if(iopt.eq.0) go to 50
|
||||
if(n.eq.nmin) go to 50
|
||||
fp0 = fpint(n)
|
||||
fpold = fpint(n-1)
|
||||
nplus = nrdata(n)
|
||||
if(fp0.gt.s) go to 60
|
||||
50 n = nmin
|
||||
fpold = 0.
|
||||
nplus = 0
|
||||
nrdata(1) = m-2
|
||||
c main loop for the different sets of knots. m is a save upper bound
|
||||
c for the number of trials.
|
||||
60 do 200 iter = 1,m
|
||||
if(n.eq.nmin) ier = -2
|
||||
c find nrint, tne number of knot intervals.
|
||||
nrint = n-nmin+1
|
||||
c find the position of the additional knots which are needed for
|
||||
c the b-spline representation of s(u).
|
||||
nk1 = n-k1
|
||||
i = n
|
||||
do 70 j=1,k1
|
||||
t(j) = ub
|
||||
t(i) = ue
|
||||
i = i-1
|
||||
70 continue
|
||||
c compute the b-spline coefficients of the least-squares spline curve
|
||||
c sinf(u). the observation matrix a is built up row by row and
|
||||
c reduced to upper triangular form by givens transformations.
|
||||
c at the same time fp=f(p=inf) is computed.
|
||||
fp = 0.
|
||||
c initialize the b-spline coefficients and the observation matrix a.
|
||||
do 75 i=1,nc
|
||||
z(i) = 0.
|
||||
75 continue
|
||||
do 80 i=1,nk1
|
||||
do 80 j=1,k1
|
||||
a(i,j) = 0.
|
||||
80 continue
|
||||
l = k1
|
||||
jj = 0
|
||||
do 130 it=1,m
|
||||
c fetch the current data point u(it),x(it).
|
||||
ui = u(it)
|
||||
wi = w(it)
|
||||
do 83 j=1,idim
|
||||
jj = jj+1
|
||||
xi(j) = x(jj)*wi
|
||||
83 continue
|
||||
c search for knot interval t(l) <= ui < t(l+1).
|
||||
85 if(ui.lt.t(l+1) .or. l.eq.nk1) go to 90
|
||||
l = l+1
|
||||
go to 85
|
||||
c evaluate the (k+1) non-zero b-splines at ui and store them in q.
|
||||
90 call fpbspl(t,n,k,ui,l,h)
|
||||
do 95 i=1,k1
|
||||
q(it,i) = h(i)
|
||||
h(i) = h(i)*wi
|
||||
95 continue
|
||||
c rotate the new row of the observation matrix into triangle.
|
||||
j = l-k1
|
||||
do 110 i=1,k1
|
||||
j = j+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 110
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = j
|
||||
do 97 j2 =1,idim
|
||||
call fprota(cos,sin,xi(j2),z(j1))
|
||||
j1 = j1+n
|
||||
97 continue
|
||||
if(i.eq.k1) go to 120
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
do 100 i1 = i3,k1
|
||||
i2 = i2+1
|
||||
c transformations to left hand side.
|
||||
call fprota(cos,sin,h(i1),a(j,i2))
|
||||
100 continue
|
||||
110 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
120 do 125 j2=1,idim
|
||||
fp = fp+xi(j2)**2
|
||||
125 continue
|
||||
130 continue
|
||||
if(ier.eq.(-2)) fp0 = fp
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = fpold
|
||||
nrdata(n) = nplus
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
j1 = 1
|
||||
do 135 j2=1,idim
|
||||
call fpback(a,z(j1),nk1,k1,c(j1),nest)
|
||||
j1 = j1+n
|
||||
135 continue
|
||||
c test whether the approximation sinf(u) is an acceptable solution.
|
||||
if(iopt.lt.0) go to 440
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c if f(p=inf) < s accept the choice of knots.
|
||||
if(fpms.lt.0.) go to 250
|
||||
c if n = nmax, sinf(u) is an interpolating spline curve.
|
||||
if(n.eq.nmax) go to 430
|
||||
c increase the number of knots.
|
||||
c if n=nest we cannot increase the number of knots because of
|
||||
c the storage capacity limitation.
|
||||
if(n.eq.nest) go to 420
|
||||
c determine the number of knots nplus we are going to add.
|
||||
if(ier.eq.0) go to 140
|
||||
nplus = 1
|
||||
ier = 0
|
||||
go to 150
|
||||
140 npl1 = nplus*2
|
||||
rn = nplus
|
||||
if(fpold-fp.gt.acc) npl1 = rn*fpms/(fpold-fp)
|
||||
nplus = min0(nplus*2,max0(npl1,nplus/2,1))
|
||||
150 fpold = fp
|
||||
c compute the sum of squared residuals for each knot interval
|
||||
c t(j+k) <= u(i) <= t(j+k+1) and store it in fpint(j),j=1,2,...nrint.
|
||||
fpart = 0.
|
||||
i = 1
|
||||
l = k2
|
||||
new = 0
|
||||
jj = 0
|
||||
do 180 it=1,m
|
||||
if(u(it).lt.t(l) .or. l.gt.nk1) go to 160
|
||||
new = 1
|
||||
l = l+1
|
||||
160 term = 0.
|
||||
l0 = l-k2
|
||||
do 175 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 170 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
170 continue
|
||||
jj = jj+1
|
||||
term = term+(w(it)*(fac-x(jj)))**2
|
||||
l0 = l0+n
|
||||
175 continue
|
||||
fpart = fpart+term
|
||||
if(new.eq.0) go to 180
|
||||
store = term*half
|
||||
fpint(i) = fpart-store
|
||||
i = i+1
|
||||
fpart = store
|
||||
new = 0
|
||||
180 continue
|
||||
fpint(nrint) = fpart
|
||||
do 190 l=1,nplus
|
||||
c add a new knot.
|
||||
call fpknot(u,m,t,n,fpint,nrdata,nrint,nest,1)
|
||||
c if n=nmax we locate the knots as for interpolation
|
||||
if(n.eq.nmax) go to 10
|
||||
c test whether we cannot further increase the number of knots.
|
||||
if(n.eq.nest) go to 200
|
||||
190 continue
|
||||
c restart the computations with the new set of knots.
|
||||
200 continue
|
||||
c test whether the least-squares kth degree polynomial curve is a
|
||||
c solution of our approximation problem.
|
||||
250 if(ier.eq.(-2)) go to 440
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing spline curve sp(u). c
|
||||
c ********************************************************** c
|
||||
c we have determined the number of knots and their position. c
|
||||
c we now compute the b-spline coefficients of the smoothing curve c
|
||||
c sp(u). the observation matrix a is extended by the rows of matrix c
|
||||
c b expressing that the kth derivative discontinuities of sp(u) at c
|
||||
c the interior knots t(k+2),...t(n-k-1) must be zero. the corres- c
|
||||
c ponding weights of these additional rows are set to 1/p. c
|
||||
c iteratively we then have to determine the value of p such that f(p),c
|
||||
c the sum of squared residuals be = s. we already know that the least c
|
||||
c squares kth degree polynomial curve corresponds to p=0, and that c
|
||||
c the least-squares spline curve corresponds to p=infinity. the c
|
||||
c iteration process which is proposed here, makes use of rational c
|
||||
c interpolation. since f(p) is a convex and strictly decreasing c
|
||||
c function of p, it can be approximated by a rational function c
|
||||
c r(p) = (u*p+v)/(p+w). three values of p(p1,p2,p3) with correspond- c
|
||||
c ing values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s) are used c
|
||||
c to calculate the new value of p such that r(p)=s. convergence is c
|
||||
c guaranteed by taking f1>0 and f3<0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c evaluate the discontinuity jump of the kth derivative of the
|
||||
c b-splines at the knots t(l),l=k+2,...n-k-1 and store in b.
|
||||
call fpdisc(t,n,k2,b,nest)
|
||||
c initial value for p.
|
||||
p1 = 0.
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
p = 0.
|
||||
do 252 i=1,nk1
|
||||
p = p+a(i,1)
|
||||
252 continue
|
||||
rn = nk1
|
||||
p = rn/p
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
n8 = n-nmin
|
||||
c iteration process to find the root of f(p) = s.
|
||||
do 360 iter=1,maxit
|
||||
c the rows of matrix b with weight 1/p are rotated into the
|
||||
c triangularised observation matrix a which is stored in g.
|
||||
pinv = one/p
|
||||
do 255 i=1,nc
|
||||
c(i) = z(i)
|
||||
255 continue
|
||||
do 260 i=1,nk1
|
||||
g(i,k2) = 0.
|
||||
do 260 j=1,k1
|
||||
g(i,j) = a(i,j)
|
||||
260 continue
|
||||
do 300 it=1,n8
|
||||
c the row of matrix b is rotated into triangle by givens transformation
|
||||
do 270 i=1,k2
|
||||
h(i) = b(it,i)*pinv
|
||||
270 continue
|
||||
do 275 j=1,idim
|
||||
xi(j) = 0.
|
||||
275 continue
|
||||
do 290 j=it,nk1
|
||||
piv = h(1)
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,g(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
j1 = j
|
||||
do 277 j2=1,idim
|
||||
call fprota(cos,sin,xi(j2),c(j1))
|
||||
j1 = j1+n
|
||||
277 continue
|
||||
if(j.eq.nk1) go to 300
|
||||
i2 = k1
|
||||
if(j.gt.n8) i2 = nk1-j
|
||||
do 280 i=1,i2
|
||||
c transformations to left hand side.
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h(i1),g(j,i1))
|
||||
h(i) = h(i1)
|
||||
280 continue
|
||||
h(i2+1) = 0.
|
||||
290 continue
|
||||
300 continue
|
||||
c backward substitution to obtain the b-spline coefficients.
|
||||
j1 = 1
|
||||
do 305 j2=1,idim
|
||||
call fpback(g,c(j1),nk1,k2,c(j1),nest)
|
||||
j1 =j1+n
|
||||
305 continue
|
||||
c computation of f(p).
|
||||
fp = 0.
|
||||
l = k2
|
||||
jj = 0
|
||||
do 330 it=1,m
|
||||
if(u(it).lt.t(l) .or. l.gt.nk1) go to 310
|
||||
l = l+1
|
||||
310 l0 = l-k2
|
||||
term = 0.
|
||||
do 325 j2=1,idim
|
||||
fac = 0.
|
||||
j1 = l0
|
||||
do 320 j=1,k1
|
||||
j1 = j1+1
|
||||
fac = fac+c(j1)*q(it,j)
|
||||
320 continue
|
||||
jj = jj+1
|
||||
term = term+(fac-x(jj))**2
|
||||
l0 = l0+n
|
||||
325 continue
|
||||
fp = fp+term*w(it)**2
|
||||
330 continue
|
||||
c test whether the approximation sp(u) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c test whether the maximal number of iterations is reached.
|
||||
if(iter.eq.maxit) go to 400
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 340
|
||||
if((f2-f3).gt.acc) go to 335
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p=p1*con9 + p2*con1
|
||||
go to 360
|
||||
335 if(f2.lt.0.) ich3=1
|
||||
340 if(ich1.ne.0) go to 350
|
||||
if((f1-f2).gt.acc) go to 345
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 360
|
||||
if(p.ge.p3) p = p2*con1 + p3*con9
|
||||
go to 360
|
||||
345 if(f2.gt.0.) ich1=1
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
350 if(f2.ge.f1 .or. f2.le.f3) go to 410
|
||||
c find the new value for p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
360 continue
|
||||
c error codes and messages.
|
||||
400 ier = 3
|
||||
go to 440
|
||||
410 ier = 2
|
||||
go to 440
|
||||
420 ier = 1
|
||||
go to 440
|
||||
430 ier = -1
|
||||
440 return
|
||||
end
|
||||
393
mcc/bsplines/fppasu.f
Normal file
393
mcc/bsplines/fppasu.f
Normal file
@@ -0,0 +1,393 @@
|
||||
subroutine fppasu(iopt,ipar,idim,u,mu,v,mv,z,mz,s,nuest,nvest,
|
||||
* tol,maxit,nc,nu,tu,nv,tv,c,fp,fp0,fpold,reducu,reducv,fpintu,
|
||||
* fpintv,lastdi,nplusu,nplusv,nru,nrv,nrdatu,nrdatv,wrk,lwrk,ier)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,tol,fp,fp0,fpold,reducu,reducv
|
||||
integer iopt,idim,mu,mv,mz,nuest,nvest,maxit,nc,nu,nv,lastdi,
|
||||
* nplusu,nplusv,lwrk,ier
|
||||
c ..array arguments..
|
||||
real*8 u(mu),v(mv),z(mz*idim),tu(nuest),tv(nvest),c(nc*idim),
|
||||
* fpintu(nuest),fpintv(nvest),wrk(lwrk)
|
||||
integer ipar(2),nrdatu(nuest),nrdatv(nvest),nru(mu),nrv(mv)
|
||||
c ..local scalars
|
||||
real*8 acc,fpms,f1,f2,f3,p,p1,p2,p3,rn,one,con1,con9,con4,
|
||||
* peru,perv,ub,ue,vb,ve
|
||||
integer i,ich1,ich3,ifbu,ifbv,ifsu,ifsv,iter,j,lau1,lav1,laa,
|
||||
* l,lau,lav,lbu,lbv,lq,lri,lsu,lsv,l1,l2,l3,l4,mm,mpm,mvnu,ncof,
|
||||
* nk1u,nk1v,nmaxu,nmaxv,nminu,nminv,nplu,nplv,npl1,nrintu,
|
||||
* nrintv,nue,nuk,nve,nuu,nvv
|
||||
c ..function references..
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpgrpa,fpknot
|
||||
c ..
|
||||
c set constants
|
||||
one = 1
|
||||
con1 = 0.1e0
|
||||
con9 = 0.9e0
|
||||
con4 = 0.4e-01
|
||||
c set boundaries of the approximation domain
|
||||
ub = u(1)
|
||||
ue = u(mu)
|
||||
vb = v(1)
|
||||
ve = v(mv)
|
||||
c we partition the working space.
|
||||
lsu = 1
|
||||
lsv = lsu+mu*4
|
||||
lri = lsv+mv*4
|
||||
mm = max0(nuest,mv)
|
||||
lq = lri+mm*idim
|
||||
mvnu = nuest*mv*idim
|
||||
lau = lq+mvnu
|
||||
nuk = nuest*5
|
||||
lbu = lau+nuk
|
||||
lav = lbu+nuk
|
||||
nuk = nvest*5
|
||||
lbv = lav+nuk
|
||||
laa = lbv+nuk
|
||||
lau1 = lau
|
||||
if(ipar(1).eq.0) go to 10
|
||||
peru = ue-ub
|
||||
lau1 = laa
|
||||
laa = laa+4*nuest
|
||||
10 lav1 = lav
|
||||
if(ipar(2).eq.0) go to 20
|
||||
perv = ve-vb
|
||||
lav1 = laa
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position. c
|
||||
c **************************************************************** c
|
||||
c given a set of knots we compute the least-squares spline sinf(u,v), c
|
||||
c and the corresponding sum of squared residuals fp=f(p=inf). c
|
||||
c if iopt=-1 sinf(u,v) is the requested approximation. c
|
||||
c if iopt=0 or iopt=1 we check whether we can accept the knots: c
|
||||
c if fp <=s we will continue with the current set of knots. c
|
||||
c if fp > s we will increase the number of knots and compute the c
|
||||
c corresponding least-squares spline until finally fp<=s. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmaxu = mu+4+2*ipar(1) and nmaxv = mv+4+2*ipar(2) c
|
||||
c if s>0 and c
|
||||
c *iopt=0 we first compute the least-squares polynomial c
|
||||
c nu=nminu=8 and nv=nminv=8 c
|
||||
c *iopt=1 we start with the knots found at the last call of the c
|
||||
c routine, except for the case that s > fp0; then we can compute c
|
||||
c the least-squares polynomial directly. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c determine the number of knots for polynomial approximation.
|
||||
20 nminu = 8
|
||||
nminv = 8
|
||||
if(iopt.lt.0) go to 100
|
||||
c acc denotes the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c find nmaxu and nmaxv which denote the number of knots in u- and v-
|
||||
c direction in case of spline interpolation.
|
||||
nmaxu = mu+4+2*ipar(1)
|
||||
nmaxv = mv+4+2*ipar(2)
|
||||
c find nue and nve which denote the maximum number of knots
|
||||
c allowed in each direction
|
||||
nue = min0(nmaxu,nuest)
|
||||
nve = min0(nmaxv,nvest)
|
||||
if(s.gt.0.) go to 60
|
||||
c if s = 0, s(u,v) is an interpolating spline.
|
||||
nu = nmaxu
|
||||
nv = nmaxv
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(nv.gt.nvest .or. nu.gt.nuest) go to 420
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
c the knots in the u-direction.
|
||||
nuu = nu-8
|
||||
if(nuu.eq.0) go to 40
|
||||
i = 5
|
||||
j = 3-ipar(1)
|
||||
do 30 l=1,nuu
|
||||
tu(i) = u(j)
|
||||
i = i+1
|
||||
j = j+1
|
||||
30 continue
|
||||
c the knots in the v-direction.
|
||||
40 nvv = nv-8
|
||||
if(nvv.eq.0) go to 60
|
||||
i = 5
|
||||
j = 3-ipar(2)
|
||||
do 50 l=1,nvv
|
||||
tv(i) = v(j)
|
||||
i = i+1
|
||||
j = j+1
|
||||
50 continue
|
||||
go to 100
|
||||
c if s > 0 our initial choice of knots depends on the value of iopt.
|
||||
60 if(iopt.eq.0) go to 90
|
||||
if(fp0.le.s) go to 90
|
||||
c if iopt=1 and fp0 > s we start computing the least- squares spline
|
||||
c according to the set of knots found at the last call of the routine.
|
||||
c we determine the number of grid coordinates u(i) inside each knot
|
||||
c interval (tu(l),tu(l+1)).
|
||||
l = 5
|
||||
j = 1
|
||||
nrdatu(1) = 0
|
||||
mpm = mu-1
|
||||
do 70 i=2,mpm
|
||||
nrdatu(j) = nrdatu(j)+1
|
||||
if(u(i).lt.tu(l)) go to 70
|
||||
nrdatu(j) = nrdatu(j)-1
|
||||
l = l+1
|
||||
j = j+1
|
||||
nrdatu(j) = 0
|
||||
70 continue
|
||||
c we determine the number of grid coordinates v(i) inside each knot
|
||||
c interval (tv(l),tv(l+1)).
|
||||
l = 5
|
||||
j = 1
|
||||
nrdatv(1) = 0
|
||||
mpm = mv-1
|
||||
do 80 i=2,mpm
|
||||
nrdatv(j) = nrdatv(j)+1
|
||||
if(v(i).lt.tv(l)) go to 80
|
||||
nrdatv(j) = nrdatv(j)-1
|
||||
l = l+1
|
||||
j = j+1
|
||||
nrdatv(j) = 0
|
||||
80 continue
|
||||
go to 100
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c polynomial (which is a spline without interior knots).
|
||||
90 nu = nminu
|
||||
nv = nminv
|
||||
nrdatu(1) = mu-2
|
||||
nrdatv(1) = mv-2
|
||||
lastdi = 0
|
||||
nplusu = 0
|
||||
nplusv = 0
|
||||
fp0 = 0.
|
||||
fpold = 0.
|
||||
reducu = 0.
|
||||
reducv = 0.
|
||||
100 mpm = mu+mv
|
||||
ifsu = 0
|
||||
ifsv = 0
|
||||
ifbu = 0
|
||||
ifbv = 0
|
||||
p = -one
|
||||
c main loop for the different sets of knots.mpm=mu+mv is a save upper
|
||||
c bound for the number of trials.
|
||||
do 250 iter=1,mpm
|
||||
if(nu.eq.nminu .and. nv.eq.nminv) ier = -2
|
||||
c find nrintu (nrintv) which is the number of knot intervals in the
|
||||
c u-direction (v-direction).
|
||||
nrintu = nu-nminu+1
|
||||
nrintv = nv-nminv+1
|
||||
c find ncof, the number of b-spline coefficients for the current set
|
||||
c of knots.
|
||||
nk1u = nu-4
|
||||
nk1v = nv-4
|
||||
ncof = nk1u*nk1v
|
||||
c find the position of the additional knots which are needed for the
|
||||
c b-spline representation of s(u,v).
|
||||
if(ipar(1).ne.0) go to 110
|
||||
i = nu
|
||||
do 105 j=1,4
|
||||
tu(j) = ub
|
||||
tu(i) = ue
|
||||
i = i-1
|
||||
105 continue
|
||||
go to 120
|
||||
110 l1 = 4
|
||||
l2 = l1
|
||||
l3 = nu-3
|
||||
l4 = l3
|
||||
tu(l2) = ub
|
||||
tu(l3) = ue
|
||||
do 115 j=1,3
|
||||
l1 = l1+1
|
||||
l2 = l2-1
|
||||
l3 = l3+1
|
||||
l4 = l4-1
|
||||
tu(l2) = tu(l4)-peru
|
||||
tu(l3) = tu(l1)+peru
|
||||
115 continue
|
||||
120 if(ipar(2).ne.0) go to 130
|
||||
i = nv
|
||||
do 125 j=1,4
|
||||
tv(j) = vb
|
||||
tv(i) = ve
|
||||
i = i-1
|
||||
125 continue
|
||||
go to 140
|
||||
130 l1 = 4
|
||||
l2 = l1
|
||||
l3 = nv-3
|
||||
l4 = l3
|
||||
tv(l2) = vb
|
||||
tv(l3) = ve
|
||||
do 135 j=1,3
|
||||
l1 = l1+1
|
||||
l2 = l2-1
|
||||
l3 = l3+1
|
||||
l4 = l4-1
|
||||
tv(l2) = tv(l4)-perv
|
||||
tv(l3) = tv(l1)+perv
|
||||
135 continue
|
||||
c find the least-squares spline sinf(u,v) and calculate for each knot
|
||||
c interval tu(j+3)<=u<=tu(j+4) (tv(j+3)<=v<=tv(j+4)) the sum
|
||||
c of squared residuals fpintu(j),j=1,2,...,nu-7 (fpintv(j),j=1,2,...
|
||||
c ,nv-7) for the data points having their absciss (ordinate)-value
|
||||
c belonging to that interval.
|
||||
c fp gives the total sum of squared residuals.
|
||||
140 call fpgrpa(ifsu,ifsv,ifbu,ifbv,idim,ipar,u,mu,v,mv,z,mz,tu,
|
||||
* nu,tv,nv,p,c,nc,fp,fpintu,fpintv,mm,mvnu,wrk(lsu),wrk(lsv),
|
||||
* wrk(lri),wrk(lq),wrk(lau),wrk(lau1),wrk(lav),wrk(lav1),
|
||||
* wrk(lbu),wrk(lbv),nru,nrv)
|
||||
if(ier.eq.(-2)) fp0 = fp
|
||||
c test whether the least-squares spline is an acceptable solution.
|
||||
if(iopt.lt.0) go to 440
|
||||
fpms = fp-s
|
||||
if(abs(fpms) .lt. acc) go to 440
|
||||
c if f(p=inf) < s, we accept the choice of knots.
|
||||
if(fpms.lt.0.) go to 300
|
||||
c if nu=nmaxu and nv=nmaxv, sinf(u,v) is an interpolating spline.
|
||||
if(nu.eq.nmaxu .and. nv.eq.nmaxv) go to 430
|
||||
c increase the number of knots.
|
||||
c if nu=nue and nv=nve we cannot further increase the number of knots
|
||||
c because of the storage capacity limitation.
|
||||
if(nu.eq.nue .and. nv.eq.nve) go to 420
|
||||
ier = 0
|
||||
c adjust the parameter reducu or reducv according to the direction
|
||||
c in which the last added knots were located.
|
||||
if (lastdi.lt.0) go to 150
|
||||
if (lastdi.eq.0) go to 170
|
||||
go to 160
|
||||
150 reducu = fpold-fp
|
||||
go to 170
|
||||
160 reducv = fpold-fp
|
||||
c store the sum of squared residuals for the current set of knots.
|
||||
170 fpold = fp
|
||||
c find nplu, the number of knots we should add in the u-direction.
|
||||
nplu = 1
|
||||
if(nu.eq.nminu) go to 180
|
||||
npl1 = nplusu*2
|
||||
rn = nplusu
|
||||
if(reducu.gt.acc) npl1 = rn*fpms/reducu
|
||||
nplu = min0(nplusu*2,max0(npl1,nplusu/2,1))
|
||||
c find nplv, the number of knots we should add in the v-direction.
|
||||
180 nplv = 1
|
||||
if(nv.eq.nminv) go to 190
|
||||
npl1 = nplusv*2
|
||||
rn = nplusv
|
||||
if(reducv.gt.acc) npl1 = rn*fpms/reducv
|
||||
nplv = min0(nplusv*2,max0(npl1,nplusv/2,1))
|
||||
190 if (nplu.lt.nplv) go to 210
|
||||
if (nplu.eq.nplv) go to 200
|
||||
go to 230
|
||||
200 if(lastdi.lt.0) go to 230
|
||||
210 if(nu.eq.nue) go to 230
|
||||
c addition in the u-direction.
|
||||
lastdi = -1
|
||||
nplusu = nplu
|
||||
ifsu = 0
|
||||
do 220 l=1,nplusu
|
||||
c add a new knot in the u-direction
|
||||
call fpknot(u,mu,tu,nu,fpintu,nrdatu,nrintu,nuest,1)
|
||||
c test whether we cannot further increase the number of knots in the
|
||||
c u-direction.
|
||||
if(nu.eq.nue) go to 250
|
||||
220 continue
|
||||
go to 250
|
||||
230 if(nv.eq.nve) go to 210
|
||||
c addition in the v-direction.
|
||||
lastdi = 1
|
||||
nplusv = nplv
|
||||
ifsv = 0
|
||||
do 240 l=1,nplusv
|
||||
c add a new knot in the v-direction.
|
||||
call fpknot(v,mv,tv,nv,fpintv,nrdatv,nrintv,nvest,1)
|
||||
c test whether we cannot further increase the number of knots in the
|
||||
c v-direction.
|
||||
if(nv.eq.nve) go to 250
|
||||
240 continue
|
||||
c restart the computations with the new set of knots.
|
||||
250 continue
|
||||
c test whether the least-squares polynomial is a solution of our
|
||||
c approximation problem.
|
||||
300 if(ier.eq.(-2)) go to 440
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing spline sp(u,v) c
|
||||
c ***************************************************** c
|
||||
c we have determined the number of knots and their position. we now c
|
||||
c compute the b-spline coefficients of the smoothing spline sp(u,v). c
|
||||
c this smoothing spline varies with the parameter p in such a way thatc
|
||||
c f(p)=suml=1,idim(sumi=1,mu(sumj=1,mv((z(i,j,l)-sp(u(i),v(j),l))**2) c
|
||||
c is a continuous, strictly decreasing function of p. moreover the c
|
||||
c least-squares polynomial corresponds to p=0 and the least-squares c
|
||||
c spline to p=infinity. iteratively we then have to determine the c
|
||||
c positive value of p such that f(p)=s. the process which is proposed c
|
||||
c here makes use of rational interpolation. f(p) is approximated by a c
|
||||
c rational function r(p)=(u*p+v)/(p+w); three values of p (p1,p2,p3) c
|
||||
c with corresponding values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s)c
|
||||
c are used to calculate the new value of p such that r(p)=s. c
|
||||
c convergence is guaranteed by taking f1 > 0 and f3 < 0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c initial value for p.
|
||||
p1 = 0.
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
p = one
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
c iteration process to find the root of f(p)=s.
|
||||
do 350 iter = 1,maxit
|
||||
c find the smoothing spline sp(u,v) and the corresponding sum of
|
||||
c squared residuals fp.
|
||||
call fpgrpa(ifsu,ifsv,ifbu,ifbv,idim,ipar,u,mu,v,mv,z,mz,tu,
|
||||
* nu,tv,nv,p,c,nc,fp,fpintu,fpintv,mm,mvnu,wrk(lsu),wrk(lsv),
|
||||
* wrk(lri),wrk(lq),wrk(lau),wrk(lau1),wrk(lav),wrk(lav1),
|
||||
* wrk(lbu),wrk(lbv),nru,nrv)
|
||||
c test whether the approximation sp(u,v) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 440
|
||||
c test whether the maximum allowable number of iterations has been
|
||||
c reached.
|
||||
if(iter.eq.maxit) go to 400
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 320
|
||||
if((f2-f3).gt.acc) go to 310
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p = p1*con9 + p2*con1
|
||||
go to 350
|
||||
310 if(f2.lt.0.) ich3 = 1
|
||||
320 if(ich1.ne.0) go to 340
|
||||
if((f1-f2).gt.acc) go to 330
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 350
|
||||
if(p.ge.p3) p = p2*con1 + p3*con9
|
||||
go to 350
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
330 if(f2.gt.0.) ich1 = 1
|
||||
340 if(f2.ge.f1 .or. f2.le.f3) go to 410
|
||||
c find the new value of p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
350 continue
|
||||
c error codes and messages.
|
||||
400 ier = 3
|
||||
go to 440
|
||||
410 ier = 2
|
||||
go to 440
|
||||
420 ier = 1
|
||||
go to 440
|
||||
430 ier = -1
|
||||
fp = 0.
|
||||
440 return
|
||||
end
|
||||
617
mcc/bsplines/fpperi.f
Normal file
617
mcc/bsplines/fpperi.f
Normal file
@@ -0,0 +1,617 @@
|
||||
recursive subroutine fpperi(iopt,x,y,w,m,k,s,nest,tol,maxit,
|
||||
* k1,k2,n,t,c,fp,fpint,z,a1,a2,b,g1,g2,q,nrdata,ier)
|
||||
implicit none
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
real*8 s,tol,fp
|
||||
integer iopt,m,k,nest,maxit,k1,k2,n,ier
|
||||
c ..array arguments..
|
||||
real*8 x(m),y(m),w(m),t(nest),c(nest),fpint(nest),z(nest),
|
||||
* a1(nest,k1),a2(nest,k),b(nest,k2),g1(nest,k2),g2(nest,k1),
|
||||
* q(m,k1)
|
||||
integer nrdata(nest)
|
||||
c ..local scalars..
|
||||
real*8 acc,cos,c1,d1,fpart,fpms,fpold,fp0,f1,f2,f3,p,per,pinv,piv,
|
||||
*
|
||||
* p1,p2,p3,sin,store,term,wi,xi,yi,rn,one,con1,con4,con9,half
|
||||
integer i,ich1,ich3,ij,ik,it,iter,i1,i2,i3,j,jk,jper,j1,j2,kk,
|
||||
* kk1,k3,l,l0,l1,l5,mm,m1,new,nk1,nk2,nmax,nmin,nplus,npl1,
|
||||
* nrint,n10,n11,n7,n8
|
||||
c ..local arrays..
|
||||
real*8 h(6),h1(7),h2(6)
|
||||
c ..function references..
|
||||
real*8 abs,fprati
|
||||
integer max0,min0
|
||||
c ..subroutine references..
|
||||
c fpbacp,fpbspl,fpgivs,fpdisc,fpknot,fprota
|
||||
c ..
|
||||
c set constants
|
||||
one = 0.1e+01
|
||||
con1 = 0.1e0
|
||||
con9 = 0.9e0
|
||||
con4 = 0.4e-01
|
||||
half = 0.5e0
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 1: determination of the number of knots and their position c
|
||||
c ************************************************************** c
|
||||
c given a set of knots we compute the least-squares periodic spline c
|
||||
c sinf(x). if the sum f(p=inf) <= s we accept the choice of knots. c
|
||||
c the initial choice of knots depends on the value of s and iopt. c
|
||||
c if s=0 we have spline interpolation; in that case the number of c
|
||||
c knots equals nmax = m+2*k. c
|
||||
c if s > 0 and c
|
||||
c iopt=0 we first compute the least-squares polynomial of c
|
||||
c degree k; n = nmin = 2*k+2. since s(x) must be periodic we c
|
||||
c find that s(x) is a constant function. c
|
||||
c iopt=1 we start with the set of knots found at the last c
|
||||
c call of the routine, except for the case that s > fp0; then c
|
||||
c we compute directly the least-squares periodic polynomial. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
m1 = m-1
|
||||
kk = k
|
||||
kk1 = k1
|
||||
k3 = 3*k+1
|
||||
nmin = 2*k1
|
||||
c determine the length of the period of s(x).
|
||||
per = x(m)-x(1)
|
||||
if(iopt.lt.0) go to 50
|
||||
c calculation of acc, the absolute tolerance for the root of f(p)=s.
|
||||
acc = tol*s
|
||||
c determine nmax, the number of knots for periodic spline interpolation
|
||||
nmax = m+2*k
|
||||
if(s.gt.0. .or. nmax.eq.nmin) go to 30
|
||||
c if s=0, s(x) is an interpolating spline.
|
||||
n = nmax
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(n.gt.nest) go to 620
|
||||
c find the position of the interior knots in case of interpolation.
|
||||
5 if((k/2)*2 .eq. k) go to 20
|
||||
do 10 i=2,m1
|
||||
j = i+k
|
||||
t(j) = x(i)
|
||||
10 continue
|
||||
if(s.gt.0.) go to 50
|
||||
kk = k-1
|
||||
kk1 = k
|
||||
if(kk.gt.0) go to 50
|
||||
t(1) = t(m)-per
|
||||
t(2) = x(1)
|
||||
t(m+1) = x(m)
|
||||
t(m+2) = t(3)+per
|
||||
do 15 i=1,m1
|
||||
c(i) = y(i)
|
||||
15 continue
|
||||
c(m) = c(1)
|
||||
fp = 0.
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = 0.
|
||||
nrdata(n) = 0
|
||||
go to 630
|
||||
20 do 25 i=2,m1
|
||||
j = i+k
|
||||
t(j) = (x(i)+x(i-1))*half
|
||||
25 continue
|
||||
go to 50
|
||||
c if s > 0 our initial choice depends on the value of iopt.
|
||||
c if iopt=0 or iopt=1 and s>=fp0, we start computing the least-squares
|
||||
c periodic polynomial. (i.e. a constant function).
|
||||
c if iopt=1 and fp0>s we start computing the least-squares periodic
|
||||
c spline according the set of knots found at the last call of the
|
||||
c routine.
|
||||
30 if(iopt.eq.0) go to 35
|
||||
if(n.eq.nmin) go to 35
|
||||
fp0 = fpint(n)
|
||||
fpold = fpint(n-1)
|
||||
nplus = nrdata(n)
|
||||
if(fp0.gt.s) go to 50
|
||||
c the case that s(x) is a constant function is treated separetely.
|
||||
c find the least-squares constant c1 and compute fp0 at the same time.
|
||||
35 fp0 = 0.
|
||||
d1 = 0.
|
||||
c1 = 0.
|
||||
do 40 it=1,m1
|
||||
wi = w(it)
|
||||
yi = y(it)*wi
|
||||
call fpgivs(wi,d1,cos,sin)
|
||||
call fprota(cos,sin,yi,c1)
|
||||
fp0 = fp0+yi**2
|
||||
40 continue
|
||||
c1 = c1/d1
|
||||
c test whether that constant function is a solution of our problem.
|
||||
fpms = fp0-s
|
||||
if(fpms.lt.acc .or. nmax.eq.nmin) go to 640
|
||||
fpold = fp0
|
||||
c test whether the required storage space exceeds the available one.
|
||||
if(nmin.ge.nest) go to 620
|
||||
c start computing the least-squares periodic spline with one
|
||||
c interior knot.
|
||||
nplus = 1
|
||||
n = nmin+1
|
||||
mm = (m+1)/2
|
||||
t(k2) = x(mm)
|
||||
nrdata(1) = mm-2
|
||||
nrdata(2) = m1-mm
|
||||
c main loop for the different sets of knots. m is a save upper
|
||||
c bound for the number of trials.
|
||||
50 do 340 iter=1,m
|
||||
c find nrint, the number of knot intervals.
|
||||
nrint = n-nmin+1
|
||||
c find the position of the additional knots which are needed for
|
||||
c the b-spline representation of s(x). if we take
|
||||
c t(k+1) = x(1), t(n-k) = x(m)
|
||||
c t(k+1-j) = t(n-k-j) - per, j=1,2,...k
|
||||
c t(n-k+j) = t(k+1+j) + per, j=1,2,...k
|
||||
c then s(x) is a periodic spline with period per if the b-spline
|
||||
c coefficients satisfy the following conditions
|
||||
c c(n7+j) = c(j), j=1,...k (**) with n7=n-2*k-1.
|
||||
t(k1) = x(1)
|
||||
nk1 = n-k1
|
||||
nk2 = nk1+1
|
||||
t(nk2) = x(m)
|
||||
do 60 j=1,k
|
||||
i1 = nk2+j
|
||||
i2 = nk2-j
|
||||
j1 = k1+j
|
||||
j2 = k1-j
|
||||
t(i1) = t(j1)+per
|
||||
t(j2) = t(i2)-per
|
||||
60 continue
|
||||
c compute the b-spline coefficients c(j),j=1,...n7 of the least-squares
|
||||
c periodic spline sinf(x). the observation matrix a is built up row
|
||||
c by row while taking into account condition (**) and is reduced to
|
||||
c triangular form by givens transformations .
|
||||
c at the same time fp=f(p=inf) is computed.
|
||||
c the n7 x n7 triangularised upper matrix a has the form
|
||||
c ! a1 ' !
|
||||
c a = ! ' a2 !
|
||||
c ! 0 ' !
|
||||
c with a2 a n7 x k matrix and a1 a n10 x n10 upper triangular
|
||||
c matrix of bandwidth k+1 ( n10 = n7-k).
|
||||
c initialization.
|
||||
do 70 i=1,nk1
|
||||
z(i) = 0.
|
||||
do 70 j=1,kk1
|
||||
a1(i,j) = 0.
|
||||
70 continue
|
||||
n7 = nk1-k
|
||||
n10 = n7-kk
|
||||
jper = 0
|
||||
fp = 0.
|
||||
l = k1
|
||||
do 290 it=1,m1
|
||||
c fetch the current data point x(it),y(it)
|
||||
xi = x(it)
|
||||
wi = w(it)
|
||||
yi = y(it)*wi
|
||||
c search for knot interval t(l) <= xi < t(l+1).
|
||||
80 if(xi.lt.t(l+1)) go to 85
|
||||
l = l+1
|
||||
go to 80
|
||||
c evaluate the (k+1) non-zero b-splines at xi and store them in q.
|
||||
85 call fpbspl(t,n,k,xi,l,h)
|
||||
do 90 i=1,k1
|
||||
q(it,i) = h(i)
|
||||
h(i) = h(i)*wi
|
||||
90 continue
|
||||
l5 = l-k1
|
||||
c test whether the b-splines nj,k+1(x),j=1+n7,...nk1 are all zero at xi
|
||||
if(l5.lt.n10) go to 285
|
||||
if(jper.ne.0) go to 160
|
||||
c initialize the matrix a2.
|
||||
do 95 i=1,n7
|
||||
do 95 j=1,kk
|
||||
a2(i,j) = 0.
|
||||
95 continue
|
||||
jk = n10+1
|
||||
do 110 i=1,kk
|
||||
ik = jk
|
||||
do 100 j=1,kk1
|
||||
if(ik.le.0) go to 105
|
||||
a2(ik,i) = a1(ik,j)
|
||||
ik = ik-1
|
||||
100 continue
|
||||
105 jk = jk+1
|
||||
110 continue
|
||||
jper = 1
|
||||
c if one of the b-splines nj,k+1(x),j=n7+1,...nk1 is not zero at xi
|
||||
c we take account of condition (**) for setting up the new row
|
||||
c of the observation matrix a. this row is stored in the arrays h1
|
||||
c (the part with respect to a1) and h2 (the part with
|
||||
c respect to a2).
|
||||
160 do 170 i=1,kk
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
170 continue
|
||||
h1(kk1) = 0.
|
||||
j = l5-n10
|
||||
do 210 i=1,kk1
|
||||
j = j+1
|
||||
l0 = j
|
||||
180 l1 = l0-kk
|
||||
if(l1.le.0) go to 200
|
||||
if(l1.le.n10) go to 190
|
||||
l0 = l1-n10
|
||||
go to 180
|
||||
190 h1(l1) = h(i)
|
||||
go to 210
|
||||
200 h2(l0) = h2(l0)+h(i)
|
||||
210 continue
|
||||
c rotate the new row of the observation matrix into triangle
|
||||
c by givens transformations.
|
||||
if(n10.le.0) go to 250
|
||||
c rotation with the rows 1,2,...n10 of matrix a.
|
||||
do 240 j=1,n10
|
||||
piv = h1(1)
|
||||
if(piv.ne.0.) go to 214
|
||||
do 212 i=1,kk
|
||||
h1(i) = h1(i+1)
|
||||
212 continue
|
||||
h1(kk1) = 0.
|
||||
go to 240
|
||||
c calculate the parameters of the givens transformation.
|
||||
214 call fpgivs(piv,a1(j,1),cos,sin)
|
||||
c transformation to the right hand side.
|
||||
call fprota(cos,sin,yi,z(j))
|
||||
c transformations to the left hand side with respect to a2.
|
||||
do 220 i=1,kk
|
||||
call fprota(cos,sin,h2(i),a2(j,i))
|
||||
220 continue
|
||||
if(j.eq.n10) go to 250
|
||||
i2 = min0(n10-j,kk)
|
||||
c transformations to the left hand side with respect to a1.
|
||||
do 230 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h1(i1),a1(j,i1))
|
||||
h1(i) = h1(i1)
|
||||
230 continue
|
||||
h1(i1) = 0.
|
||||
240 continue
|
||||
c rotation with the rows n10+1,...n7 of matrix a.
|
||||
250 do 270 j=1,kk
|
||||
ij = n10+j
|
||||
if(ij.le.0) go to 270
|
||||
piv = h2(j)
|
||||
if(piv.eq.0.) go to 270
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a2(ij,j),cos,sin)
|
||||
c transformations to right hand side.
|
||||
call fprota(cos,sin,yi,z(ij))
|
||||
if(j.eq.kk) go to 280
|
||||
j1 = j+1
|
||||
c transformations to left hand side.
|
||||
do 260 i=j1,kk
|
||||
call fprota(cos,sin,h2(i),a2(ij,i))
|
||||
260 continue
|
||||
270 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
280 fp = fp+yi**2
|
||||
go to 290
|
||||
c rotation of the new row of the observation matrix into
|
||||
c triangle in case the b-splines nj,k+1(x),j=n7+1,...n-k-1 are all zero
|
||||
c at xi.
|
||||
285 j = l5
|
||||
do 140 i=1,kk1
|
||||
j = j+1
|
||||
piv = h(i)
|
||||
if(piv.eq.0.) go to 140
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,a1(j,1),cos,sin)
|
||||
c transformations to right hand side.
|
||||
call fprota(cos,sin,yi,z(j))
|
||||
if(i.eq.kk1) go to 150
|
||||
i2 = 1
|
||||
i3 = i+1
|
||||
c transformations to left hand side.
|
||||
do 130 i1=i3,kk1
|
||||
i2 = i2+1
|
||||
call fprota(cos,sin,h(i1),a1(j,i2))
|
||||
130 continue
|
||||
140 continue
|
||||
c add contribution of this row to the sum of squares of residual
|
||||
c right hand sides.
|
||||
150 fp = fp+yi**2
|
||||
290 continue
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = fpold
|
||||
nrdata(n) = nplus
|
||||
c backward substitution to obtain the b-spline coefficients c(j),j=1,.n
|
||||
call fpbacp(a1,a2,z,n7,kk,c,kk1,nest)
|
||||
c calculate from condition (**) the coefficients c(j+n7),j=1,2,...k.
|
||||
do 295 i=1,k
|
||||
j = i+n7
|
||||
c(j) = c(i)
|
||||
295 continue
|
||||
if(iopt.lt.0) go to 660
|
||||
c test whether the approximation sinf(x) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 660
|
||||
c if f(p=inf) < s accept the choice of knots.
|
||||
if(fpms.lt.0.) go to 350
|
||||
c if n=nmax, sinf(x) is an interpolating spline.
|
||||
if(n.eq.nmax) go to 630
|
||||
c increase the number of knots.
|
||||
c if n=nest we cannot increase the number of knots because of the
|
||||
c storage capacity limitation.
|
||||
if(n.eq.nest) go to 620
|
||||
c determine the number of knots nplus we are going to add.
|
||||
npl1 = nplus*2
|
||||
rn = nplus
|
||||
if(fpold-fp.gt.acc) npl1 = rn*fpms/(fpold-fp)
|
||||
nplus = min0(nplus*2,max0(npl1,nplus/2,1))
|
||||
fpold = fp
|
||||
c compute the sum(wi*(yi-s(xi))**2) for each knot interval
|
||||
c t(j+k) <= xi <= t(j+k+1) and store it in fpint(j),j=1,2,...nrint.
|
||||
fpart = 0.
|
||||
i = 1
|
||||
l = k1
|
||||
do 320 it=1,m1
|
||||
if(x(it).lt.t(l)) go to 300
|
||||
new = 1
|
||||
l = l+1
|
||||
300 term = 0.
|
||||
l0 = l-k2
|
||||
do 310 j=1,k1
|
||||
l0 = l0+1
|
||||
term = term+c(l0)*q(it,j)
|
||||
310 continue
|
||||
term = (w(it)*(term-y(it)))**2
|
||||
fpart = fpart+term
|
||||
if(new.eq.0) go to 320
|
||||
if(l.gt.k2) go to 315
|
||||
fpint(nrint) = term
|
||||
new = 0
|
||||
go to 320
|
||||
315 store = term*half
|
||||
fpint(i) = fpart-store
|
||||
i = i+1
|
||||
fpart = store
|
||||
new = 0
|
||||
320 continue
|
||||
fpint(nrint) = fpint(nrint)+fpart
|
||||
do 330 l=1,nplus
|
||||
c add a new knot
|
||||
call fpknot(x,m,t,n,fpint,nrdata,nrint,nest,1)
|
||||
c if n=nmax we locate the knots as for interpolation.
|
||||
if(n.eq.nmax) go to 5
|
||||
c test whether we cannot further increase the number of knots.
|
||||
if(n.eq.nest) go to 340
|
||||
330 continue
|
||||
c restart the computations with the new set of knots.
|
||||
340 continue
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c part 2: determination of the smoothing periodic spline sp(x). c
|
||||
c ************************************************************* c
|
||||
c we have determined the number of knots and their position. c
|
||||
c we now compute the b-spline coefficients of the smoothing spline c
|
||||
c sp(x). the observation matrix a is extended by the rows of matrix c
|
||||
c b expressing that the kth derivative discontinuities of sp(x) at c
|
||||
c the interior knots t(k+2),...t(n-k-1) must be zero. the corres- c
|
||||
c ponding weights of these additional rows are set to 1/sqrt(p). c
|
||||
c iteratively we then have to determine the value of p such that c
|
||||
c f(p)=sum(w(i)*(y(i)-sp(x(i)))**2) be = s. we already know that c
|
||||
c the least-squares constant function corresponds to p=0, and that c
|
||||
c the least-squares periodic spline corresponds to p=infinity. the c
|
||||
c iteration process which is proposed here, makes use of rational c
|
||||
c interpolation. since f(p) is a convex and strictly decreasing c
|
||||
c function of p, it can be approximated by a rational function c
|
||||
c r(p) = (u*p+v)/(p+w). three values of p(p1,p2,p3) with correspond- c
|
||||
c ing values of f(p) (f1=f(p1)-s,f2=f(p2)-s,f3=f(p3)-s) are used c
|
||||
c to calculate the new value of p such that r(p)=s. convergence is c
|
||||
c guaranteed by taking f1>0 and f3<0. c
|
||||
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
|
||||
c evaluate the discontinuity jump of the kth derivative of the
|
||||
c b-splines at the knots t(l),l=k+2,...n-k-1 and store in b.
|
||||
350 call fpdisc(t,n,k2,b,nest)
|
||||
c initial value for p.
|
||||
p1 = 0.
|
||||
f1 = fp0-s
|
||||
p3 = -one
|
||||
f3 = fpms
|
||||
n11 = n10-1
|
||||
n8 = n7-1
|
||||
p = 0.
|
||||
l = n7
|
||||
do 352 i=1,k
|
||||
j = k+1-i
|
||||
p = p+a2(l,j)
|
||||
l = l-1
|
||||
if(l.eq.0) go to 356
|
||||
352 continue
|
||||
do 354 i=1,n10
|
||||
p = p+a1(i,1)
|
||||
354 continue
|
||||
356 rn = n7
|
||||
p = rn/p
|
||||
ich1 = 0
|
||||
ich3 = 0
|
||||
c iteration process to find the root of f(p) = s.
|
||||
do 595 iter=1,maxit
|
||||
c form the matrix g as the matrix a extended by the rows of matrix b.
|
||||
c the rows of matrix b with weight 1/p are rotated into
|
||||
c the triangularised observation matrix a.
|
||||
c after triangularisation our n7 x n7 matrix g takes the form
|
||||
c ! g1 ' !
|
||||
c g = ! ' g2 !
|
||||
c ! 0 ' !
|
||||
c with g2 a n7 x (k+1) matrix and g1 a n11 x n11 upper triangular
|
||||
c matrix of bandwidth k+2. ( n11 = n7-k-1)
|
||||
pinv = one/p
|
||||
c store matrix a into g
|
||||
do 360 i=1,n7
|
||||
c(i) = z(i)
|
||||
g1(i,k1) = a1(i,k1)
|
||||
g1(i,k2) = 0.
|
||||
g2(i,1) = 0.
|
||||
do 360 j=1,k
|
||||
g1(i,j) = a1(i,j)
|
||||
g2(i,j+1) = a2(i,j)
|
||||
360 continue
|
||||
l = n10
|
||||
do 370 j=1,k1
|
||||
if(l.le.0) go to 375
|
||||
g2(l,1) = a1(l,j)
|
||||
l = l-1
|
||||
370 continue
|
||||
375 do 540 it=1,n8
|
||||
c fetch a new row of matrix b and store it in the arrays h1 (the part
|
||||
c with respect to g1) and h2 (the part with respect to g2).
|
||||
yi = 0.
|
||||
do 380 i=1,k1
|
||||
h1(i) = 0.
|
||||
h2(i) = 0.
|
||||
380 continue
|
||||
h1(k2) = 0.
|
||||
if(it.gt.n11) go to 420
|
||||
l = it
|
||||
l0 = it
|
||||
do 390 j=1,k2
|
||||
if(l0.eq.n10) go to 400
|
||||
h1(j) = b(it,j)*pinv
|
||||
l0 = l0+1
|
||||
390 continue
|
||||
go to 470
|
||||
400 l0 = 1
|
||||
do 410 l1=j,k2
|
||||
h2(l0) = b(it,l1)*pinv
|
||||
l0 = l0+1
|
||||
410 continue
|
||||
go to 470
|
||||
420 l = 1
|
||||
i = it-n10
|
||||
do 460 j=1,k2
|
||||
i = i+1
|
||||
l0 = i
|
||||
430 l1 = l0-k1
|
||||
if(l1.le.0) go to 450
|
||||
if(l1.le.n11) go to 440
|
||||
l0 = l1-n11
|
||||
go to 430
|
||||
440 h1(l1) = b(it,j)*pinv
|
||||
go to 460
|
||||
450 h2(l0) = h2(l0)+b(it,j)*pinv
|
||||
460 continue
|
||||
if(n11.le.0) go to 510
|
||||
c rotate this row into triangle by givens transformations without
|
||||
c square roots.
|
||||
c rotation with the rows l,l+1,...n11.
|
||||
470 do 500 j=l,n11
|
||||
piv = h1(1)
|
||||
c calculate the parameters of the givens transformation.
|
||||
call fpgivs(piv,g1(j,1),cos,sin)
|
||||
c transformation to right hand side.
|
||||
call fprota(cos,sin,yi,c(j))
|
||||
c transformation to the left hand side with respect to g2.
|
||||
do 480 i=1,k1
|
||||
call fprota(cos,sin,h2(i),g2(j,i))
|
||||
480 continue
|
||||
if(j.eq.n11) go to 510
|
||||
i2 = min0(n11-j,k1)
|
||||
c transformation to the left hand side with respect to g1.
|
||||
do 490 i=1,i2
|
||||
i1 = i+1
|
||||
call fprota(cos,sin,h1(i1),g1(j,i1))
|
||||
h1(i) = h1(i1)
|
||||
490 continue
|
||||
h1(i1) = 0.
|
||||
500 continue
|
||||
c rotation with the rows n11+1,...n7
|
||||
510 do 530 j=1,k1
|
||||
ij = n11+j
|
||||
if(ij.le.0) go to 530
|
||||
piv = h2(j)
|
||||
c calculate the parameters of the givens transformation
|
||||
call fpgivs(piv,g2(ij,j),cos,sin)
|
||||
c transformation to the right hand side.
|
||||
call fprota(cos,sin,yi,c(ij))
|
||||
if(j.eq.k1) go to 540
|
||||
j1 = j+1
|
||||
c transformation to the left hand side.
|
||||
do 520 i=j1,k1
|
||||
call fprota(cos,sin,h2(i),g2(ij,i))
|
||||
520 continue
|
||||
530 continue
|
||||
540 continue
|
||||
c backward substitution to obtain the b-spline coefficients
|
||||
c c(j),j=1,2,...n7 of sp(x).
|
||||
call fpbacp(g1,g2,c,n7,k1,c,k2,nest)
|
||||
c calculate from condition (**) the b-spline coefficients c(n7+j),j=1,.
|
||||
do 545 i=1,k
|
||||
j = i+n7
|
||||
c(j) = c(i)
|
||||
545 continue
|
||||
c computation of f(p).
|
||||
fp = 0.
|
||||
l = k1
|
||||
do 570 it=1,m1
|
||||
if(x(it).lt.t(l)) go to 550
|
||||
l = l+1
|
||||
550 l0 = l-k2
|
||||
term = 0.
|
||||
do 560 j=1,k1
|
||||
l0 = l0+1
|
||||
term = term+c(l0)*q(it,j)
|
||||
560 continue
|
||||
fp = fp+(w(it)*(term-y(it)))**2
|
||||
570 continue
|
||||
c test whether the approximation sp(x) is an acceptable solution.
|
||||
fpms = fp-s
|
||||
if(abs(fpms).lt.acc) go to 660
|
||||
c test whether the maximal number of iterations is reached.
|
||||
if(iter.eq.maxit) go to 600
|
||||
c carry out one more step of the iteration process.
|
||||
p2 = p
|
||||
f2 = fpms
|
||||
if(ich3.ne.0) go to 580
|
||||
if((f2-f3) .gt. acc) go to 575
|
||||
c our initial choice of p is too large.
|
||||
p3 = p2
|
||||
f3 = f2
|
||||
p = p*con4
|
||||
if(p.le.p1) p = p1*con9 +p2*con1
|
||||
go to 595
|
||||
575 if(f2.lt.0.) ich3 = 1
|
||||
580 if(ich1.ne.0) go to 590
|
||||
if((f1-f2) .gt. acc) go to 585
|
||||
c our initial choice of p is too small
|
||||
p1 = p2
|
||||
f1 = f2
|
||||
p = p/con4
|
||||
if(p3.lt.0.) go to 595
|
||||
if(p.ge.p3) p = p2*con1 +p3*con9
|
||||
go to 595
|
||||
585 if(f2.gt.0.) ich1 = 1
|
||||
c test whether the iteration process proceeds as theoretically
|
||||
c expected.
|
||||
590 if(f2.ge.f1 .or. f2.le.f3) go to 610
|
||||
c find the new value for p.
|
||||
p = fprati(p1,f1,p2,f2,p3,f3)
|
||||
595 continue
|
||||
c error codes and messages.
|
||||
600 ier = 3
|
||||
go to 660
|
||||
610 ier = 2
|
||||
go to 660
|
||||
620 ier = 1
|
||||
go to 660
|
||||
630 ier = -1
|
||||
go to 660
|
||||
640 ier = -2
|
||||
c the least-squares constant function c1 is a solution of our problem.
|
||||
c a constant function is a spline of degree k with all b-spline
|
||||
c coefficients equal to that constant c1.
|
||||
do 650 i=1,k1
|
||||
rn = k1-i
|
||||
t(i) = x(1)-rn*per
|
||||
c(i) = c1
|
||||
j = i+k1
|
||||
rn = i-1
|
||||
t(j) = x(m)+rn*per
|
||||
650 continue
|
||||
n = nmin
|
||||
fp = fp0
|
||||
fpint(n) = fp0
|
||||
fpint(n-1) = 0.
|
||||
nrdata(n) = 0
|
||||
660 return
|
||||
end
|
||||
73
mcc/bsplines/fppocu.f
Normal file
73
mcc/bsplines/fppocu.f
Normal file
@@ -0,0 +1,73 @@
|
||||
recursive subroutine fppocu(idim,k,a,b,ib,db,nb,ie,de,ne,cp,np)
|
||||
implicit none
|
||||
c subroutine fppocu finds a idim-dimensional polynomial curve p(u) =
|
||||
c (p1(u),p2(u),...,pidim(u)) of degree k, satisfying certain derivative
|
||||
c constraints at the end points a and b, i.e.
|
||||
c (l)
|
||||
c if ib > 0 : pj (a) = db(idim*l+j), l=0,1,...,ib-1
|
||||
c (l)
|
||||
c if ie > 0 : pj (b) = de(idim*l+j), l=0,1,...,ie-1
|
||||
c
|
||||
c the polynomial curve is returned in its b-spline representation
|
||||
c ( coefficients cp(j), j=1,2,...,np )
|
||||
c ..
|
||||
c ..scalar arguments..
|
||||
integer idim,k,ib,nb,ie,ne,np
|
||||
real*8 a,b
|
||||
c ..array arguments..
|
||||
real*8 db(nb),de(ne),cp(np)
|
||||
c ..local scalars..
|
||||
real*8 ab,aki
|
||||
integer i,id,j,jj,l,ll,k1,k2
|
||||
c ..local array..
|
||||
real*8 work(6,6)
|
||||
c ..
|
||||
k1 = k+1
|
||||
k2 = 2*k1
|
||||
ab = b-a
|
||||
do 110 id=1,idim
|
||||
do 10 j=1,k1
|
||||
work(j,1) = 0.
|
||||
10 continue
|
||||
if(ib.eq.0) go to 50
|
||||
l = id
|
||||
do 20 i=1,ib
|
||||
work(1,i) = db(l)
|
||||
l = l+idim
|
||||
20 continue
|
||||
if(ib.eq.1) go to 50
|
||||
ll = ib
|
||||
do 40 j=2,ib
|
||||
ll = ll-1
|
||||
do 30 i=1,ll
|
||||
aki = k1-i
|
||||
work(j,i) = ab*work(j-1,i+1)/aki + work(j-1,i)
|
||||
30 continue
|
||||
40 continue
|
||||
50 if(ie.eq.0) go to 90
|
||||
l = id
|
||||
j = k1
|
||||
do 60 i=1,ie
|
||||
work(j,i) = de(l)
|
||||
l = l+idim
|
||||
j = j-1
|
||||
60 continue
|
||||
if(ie.eq.1) go to 90
|
||||
ll = ie
|
||||
do 80 jj=2,ie
|
||||
ll = ll-1
|
||||
j = k1+1-jj
|
||||
do 70 i=1,ll
|
||||
aki = k1-i
|
||||
work(j,i) = work(j+1,i) - ab*work(j,i+1)/aki
|
||||
j = j-1
|
||||
70 continue
|
||||
80 continue
|
||||
90 l = (id-1)*k2
|
||||
do 100 j=1,k1
|
||||
l = l+1
|
||||
cp(l) = work(j,1)
|
||||
100 continue
|
||||
110 continue
|
||||
return
|
||||
end
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user