Compare commits

..

158 Commits

Author SHA1 Message Date
01261272b4 . 2026-03-11 17:18:21 +03:00
e61146db69 fixed stupid bug 2026-03-11 12:36:45 +03:00
2eb7b214b2 add more errors to model 2026-03-11 11:03:10 +03:00
27b1cecc7b enc 2026-03-11 10:05:42 +03:00
9e84306a94 poll test works @high speed 2026-02-09 22:00:51 +03:00
c0c36388db test of polling sensors 2026-01-26 22:25:20 +03:00
ed24d0b9e2 add records to AsibFM700 config file according to Eddy's commit 2026-01-26 18:23:03 +03:00
bccc7a9b29 1st working approach after last changes 2026-01-22 21:18:01 +03:00
873f292a11 fixed race bug 2026-01-21 23:20:58 +03:00
f7cb279841 change macros to config parameters 2026-01-20 23:18:36 +03:00
fd96dc395b ... 2026-01-16 12:23:58 +03:00
Timur A. Fatkhullin
0aa0113be3 ... 2026-01-15 23:21:28 +03:00
01d5657b1b ... 2026-01-15 19:11:16 +03:00
09cf5f9c19 ... 2026-01-14 19:15:18 +03:00
66c3c7e6c7 ... 2025-12-29 16:11:23 +03:00
fd2776084a ... 2025-12-25 16:18:54 +03:00
Timur A. Fatkhullin
bbc35b02bb ... 2025-12-22 23:02:41 +03:00
Timur A. Fatkhullin
8ce6ffc41c ... 2025-12-22 22:56:49 +03:00
54d6c25171 ... 2025-12-22 17:13:04 +03:00
2c7d563994 ... 2025-12-19 12:01:36 +03:00
bc12777f18 ... 2025-12-18 18:21:17 +03:00
7948321cce ... 2025-12-17 17:25:00 +03:00
b12c0ec521 ... 2025-12-16 17:53:50 +03:00
Timur A. Fatkhullin
d417c03f59 ... 2025-12-16 02:22:03 +03:00
a8dd511366 ... 2025-12-15 17:26:26 +03:00
a30729fb37 ... 2025-12-14 05:32:04 +03:00
a70339c55e ... 2025-12-14 00:25:03 +03:00
6fca94e571 ... 2025-12-12 17:21:11 +03:00
255c34dbb2 ... 2025-12-11 17:40:23 +03:00
3c0c719e37 ... 2025-12-10 17:24:00 +03:00
Timur A. Fatkhullin
0ddd633bc9 ... 2025-12-10 00:18:55 +03:00
28ecf307a8 add saving slewing trajectory in a file 2025-12-09 16:55:46 +03:00
57467ce48f ... 2025-12-08 17:34:59 +03:00
196ed3be1b change time from double to struct timespec 2025-12-08 13:31:23 +03:00
acd26edc9c add PID-related items in mount config
rewrite AsibFM700ServoController methods according to new time point
representation in LibSidServo
2025-12-04 18:11:41 +03:00
da9cd51e5c timespec 2025-12-04 11:10:27 +03:00
d868810048 cmake fixes 2025-12-03 18:14:34 +03:00
Timur A. Fatkhullin
7c9612c3a2 MccSimpleSlewingModel: the code has been rewritten in accordance with the
changes in Eddy's LibSidServo
2025-12-03 00:35:52 +03:00
54419b8e60 ... 2025-12-02 18:05:08 +03:00
7dfb0d5e9b ... 2025-12-02 18:02:57 +03:00
bbf7314592 . 2025-12-02 12:33:16 +03:00
6dde28e8d9 some fixes 2025-12-01 17:28:18 +03:00
9066b3f091 ... 2025-12-01 17:18:04 +03:00
c514d4adcc Merge branch 'main' of ssh://95.140.147.151:/home/git/mountcontrol 2025-12-01 10:07:45 +03:00
cca58e8ba9 ... 2025-12-01 02:49:01 +03:00
bf55a45cf9 ... 2025-11-27 18:01:40 +03:00
a825a6935b MccGenericNetworkServer: fix client session thread pool behavior in
destructor
2025-11-27 09:20:42 +03:00
43638f383f ran client sessions in separated thread pool 2025-11-26 18:01:34 +03:00
a42f6dbc98 ... 2025-11-25 08:49:43 +03:00
Timur A. Fatkhullin
acced75fa2 ... 2025-11-24 21:52:22 +03:00
e548451617 ... 2025-11-24 18:00:46 +03:00
e529265a63 ... 2025-11-21 12:33:49 +03:00
b2c27a6f5c ... 2025-11-21 02:02:35 +03:00
6214b82a6c ... 2025-11-19 12:02:43 +03:00
Timur A. Fatkhullin
c6b47d8ad6 ... 2025-11-18 22:36:08 +03:00
273f239abb ... 2025-11-18 18:51:01 +03:00
Timur A. Fatkhullin
14e583a244 ... 2025-11-17 23:42:04 +03:00
771619b832 ... 2025-11-17 18:04:40 +03:00
Timur A. Fatkhullin
e0c8d8f39b ... 2025-11-17 03:07:54 +03:00
Timur A. Fatkhullin
0ce4430668 ... 2025-11-16 10:34:17 +03:00
Timur A. Fatkhullin
e18066e4a6 add logging in MccSimpleSlewingModel class 2025-11-16 00:58:56 +03:00
1c774d2d69 Asibfm700Mount is now MccGenericMount (not MccGenericFsmMount)
fix mount initialization (add EEPROM reading, assign correponded
mount config items)
rewrite computing distance to pzones in slewing mode (add braking
aceleration)
add more informative errors description for serialization (network
protocol)
2025-11-15 16:01:42 +03:00
9e8a7a62c9 ... 2025-11-14 17:27:44 +03:00
1ea5fb623d fixed model for STOPPED state 2025-11-14 14:07:07 +03:00
078e3f38f2 ... 2025-11-14 12:23:39 +03:00
94fb4c6a48 ... 2025-11-13 17:56:51 +03:00
b3a257fab6 ... 2025-11-12 18:50:23 +03:00
08ad1e665b ... 2025-11-11 18:10:06 +03:00
90acf1ee8c fix axis switch limit pzone calculations 2025-11-10 16:06:44 +03:00
15cf04f164 ... 2025-11-03 18:27:43 +03:00
6fc0b8bb4e ... 2025-11-02 14:38:44 +03:00
c0f274cec0 fix compilation with GCC version<15 2025-11-02 11:59:23 +03:00
Edward Emelianov
511956531e fixed nanotime 2025-11-01 19:51:56 +03:00
3f108fcc13 ... 2025-11-01 17:53:24 +03:00
683da9739d change system time function to UNIX time 2025-11-01 14:59:37 +03:00
a7fbae47f0 ... 2025-11-01 11:57:49 +03:00
8a202bd38c ... 2025-10-31 17:40:33 +03:00
d69ea51b0c various Asibfm700MountConfig class fixes 2025-10-31 12:22:16 +03:00
a1fa54c636 fix ASIO and cxxopts libraries compile/link errors 2025-10-31 11:02:52 +03:00
Timur A. Fatkhullin
cb362c6e49 rewrite MccGenericMount and MccGenericFsmMount class creation
Asibfm700MountNetServer is now started
2025-10-31 01:30:24 +03:00
f2be52d17c ... add dump of config for Asibfm700MountConfig class 2025-10-30 16:11:23 +03:00
3682ccdda6 ... 2025-10-30 11:58:11 +03:00
Timur A. Fatkhullin
85259fc6ad ... compiled! 2025-10-30 01:01:52 +03:00
620f8ba136 ... 2025-10-29 19:26:20 +03:00
50e79aa0ae ... 2025-10-29 18:47:24 +03:00
6a72ead855 cleanups of commented code 2025-10-29 16:15:58 +03:00
bc300bb3de ... 2025-10-29 15:07:53 +03:00
78e4bb182c ... 2025-10-28 18:01:22 +03:00
Timur A. Fatkhullin
85dfa2e9a5 ... 2025-10-28 01:11:34 +03:00
Timur A. Fatkhullin
bdfc5dbc1c ... 2025-10-26 02:22:39 +03:00
Timur A. Fatkhullin
ec27cd981a ... 2025-10-25 19:26:42 +03:00
47c57dca72 ... 2025-10-24 12:16:44 +03:00
e6b4604bfa ... 2025-10-23 18:08:44 +03:00
412f038eb0 ... 2025-10-23 12:13:07 +03:00
Timur A. Fatkhullin
80ec2382ea ... 2025-10-22 23:52:14 +03:00
42a4349c76 ... 2025-10-22 17:55:43 +03:00
Timur A. Fatkhullin
e50fbfc57e ... 2025-10-21 22:35:45 +03:00
49a2e2f9c1 ... 2025-10-21 17:48:21 +03:00
fc64642bd6 ... 2025-10-20 00:36:00 +03:00
Timur A. Fatkhullin
cbe106fe95 ... 2025-10-11 23:02:43 +03:00
f618fb64cb ... 2025-10-09 17:43:40 +03:00
Timur A. Fatkhullin
04272b8e1d ... 2025-10-09 01:09:27 +03:00
e0e10395fb ... 2025-10-08 18:13:46 +03:00
Timur A. Fatkhullin
27dccfe7c0 ... 2025-10-07 23:51:58 +03:00
8b16ac79b8 ... 2025-10-06 17:52:41 +03:00
58d62d85b3 ... 2025-10-06 12:09:06 +03:00
Timur A. Fatkhullin
9c13def8be ... 2025-10-04 00:46:07 +03:00
5fe2788cd7 ... 2025-10-03 12:11:21 +03:00
962504ed98 ... 2025-10-02 19:21:13 +03:00
Timur A. Fatkhullin
4d7e830798 ... 2025-10-01 06:35:44 +03:00
Timur A. Fatkhullin
3d769d79eb ... 2025-10-01 06:26:50 +03:00
0b7261a431 ... 2025-09-30 18:55:14 +03:00
c5aa3dc495 ... 2025-09-29 19:02:04 +03:00
98c46c2b8c ... 2025-09-28 19:31:03 +03:00
d8fae31406 ... 2025-09-25 17:12:12 +03:00
4a9ecf8639 ... 2025-09-25 12:11:48 +03:00
Timur A. Fatkhullin
b8383c1375 ... 2025-09-25 00:08:08 +03:00
f729799335 ... 2025-09-24 18:23:17 +03:00
b1a48d2b77 ... 2025-09-24 12:36:02 +03:00
fedc324410 ... 2025-09-22 17:46:52 +03:00
Timur A. Fatkhullin
1a4d998141 ... 2025-09-21 23:16:03 +03:00
0f955b3c91 ... 2025-09-19 12:12:12 +03:00
f5039a329b ... 2025-09-18 17:44:23 +03:00
83b7e0d924 ... 2025-09-17 21:57:05 +03:00
1087e043a8 ... 2025-09-17 18:21:32 +03:00
281ceacf89 ... 2025-09-17 12:51:28 +03:00
4e3a50acba ... 2025-09-16 19:06:37 +03:00
732cd33947 ... 2025-09-16 18:35:39 +03:00
Timur A. Fatkhullin
bb41710645 ... 2025-09-14 23:14:09 +03:00
Timur A. Fatkhullin
0b084f44f6 ... 2025-09-13 23:50:02 +03:00
Timur A. Fatkhullin
92b1a3cfd5 ... 2025-09-13 23:46:38 +03:00
3ae2d41fc8 ... 2025-09-13 13:59:54 +03:00
c7dd816481 ... 2025-09-12 18:31:15 +03:00
5f802ff57e ... 2025-09-12 12:53:05 +03:00
8e8cb543ae ... 2025-09-11 18:23:39 +03:00
ab49f927fb ... 2025-09-10 18:07:22 +03:00
00354d9b41 ... 2025-09-10 12:24:06 +03:00
2478c1e8d2 remove guiding model
now it are only slewing and tracking states
2025-09-03 18:28:52 +03:00
Timur A. Fatkhullin
460fc360c6 ... 2025-09-03 00:41:15 +03:00
Timur A. Fatkhullin
36ffde80f5 ... 2025-09-03 00:32:05 +03:00
fe6492e4fc ... 2025-09-02 16:49:58 +03:00
Timur A. Fatkhullin
de80acf315 ... 2025-09-02 00:45:23 +03:00
3d3b57a311 ... 2025-09-01 18:25:47 +03:00
227f501d6f ... 2025-09-01 12:32:31 +03:00
Timur A. Fatkhullin
218da42a1d ... 2025-09-01 01:15:23 +03:00
Timur A. Fatkhullin
c2627ecd89 ... 2025-08-31 01:54:15 +03:00
4696daa2ee ... 2025-08-28 13:42:33 +03:00
Timur A. Fatkhullin
2e5e1918e1 ... 2025-08-28 00:43:55 +03:00
45f655dc90 ... 2025-08-27 17:55:57 +03:00
9fb33e5bec ... 2025-08-27 12:17:52 +03:00
Timur A. Fatkhullin
31cf0a45dd ... 2025-08-27 08:51:46 +03:00
Timur A. Fatkhullin
4bf95c1043 ... 2025-08-27 00:04:06 +03:00
052d4e2eb4 ... 2025-08-26 19:56:32 +03:00
7556539084 ... 2025-08-26 13:38:33 +03:00
Timur A. Fatkhullin
8b1873b40b ... 2025-08-26 02:28:08 +03:00
0295d93cd3 ... 2025-08-25 13:40:54 +03:00
60cade4d1f ... 2025-08-24 04:03:45 +03:00
dc87ce0fb9 ... 2025-08-24 01:30:14 +03:00
169 changed files with 39557 additions and 1370 deletions

View File

@@ -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)

View 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&lt;int&gt;" 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&lt;int&gt;" 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>

View 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&lt;int&gt;" 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&lt;int&gt;" 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>

View File

@@ -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);
}

View File

@@ -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);

View File

@@ -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

View File

@@ -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

View File

@@ -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
}

View File

@@ -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];
}

View File

@@ -25,3 +25,4 @@
void confHelp();
conf_t *readServoConf(const char *filename);
void dumpConf();
const char *EcodeStr(mcc_errcodes_t e);

View File

@@ -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;
}
}

View File

@@ -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);

View File

@@ -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){

View File

@@ -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

View File

@@ -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)){

View File

@@ -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;

View File

@@ -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;

View File

@@ -1,4 +1,4 @@
Current configuration:
# Current configuration
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderDevPath=(null)

View File

@@ -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;
}

View File

@@ -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>

View File

@@ -22,6 +22,7 @@ examples/traectories.h
main.h
movingmodel.c
movingmodel.h
polltest/main.c
ramp.c
ramp.h
serial.h

View File

@@ -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, &param, 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, &param, 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,
};

View File

@@ -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))

View File

@@ -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);
}

View File

@@ -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
View 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;
}

View File

@@ -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){

View File

@@ -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:

View File

@@ -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;

View File

@@ -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");

View File

@@ -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
View 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()

View 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

View 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

View 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
View 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

View 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

View 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

View 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

View 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;
}
}

View 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

View 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

View 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;
}

View File

@@ -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)

View File

@@ -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;
};

View File

@@ -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

View File

@@ -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()

View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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