From 57c72b9de338ae4b87f50dc0971207e9d57067b6 Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Sun, 23 Nov 2025 21:25:41 -0700 Subject: [PATCH 1/8] EARLY TEST --- example-plugin/README.md | 21 +++ example-plugin/pyproject.toml | 19 +++ .../src/bsk_example_plugin/__init__.py | 7 + .../src/bsk_example_plugin/simple.py | 36 ++++ src/Basilisk/modules/__init__.py | 36 ++++ src/CMakeLists.txt | 13 ++ src/bsk_core/__init__.py | 10 ++ src/bsk_core/plugins.py | 120 +++++++++++++ src/tests/test_plugin_registry.py | 159 ++++++++++++++++++ 9 files changed, 421 insertions(+) create mode 100644 example-plugin/README.md create mode 100644 example-plugin/pyproject.toml create mode 100644 example-plugin/src/bsk_example_plugin/__init__.py create mode 100644 example-plugin/src/bsk_example_plugin/simple.py create mode 100644 src/Basilisk/modules/__init__.py create mode 100644 src/bsk_core/__init__.py create mode 100644 src/bsk_core/plugins.py create mode 100644 src/tests/test_plugin_registry.py diff --git a/example-plugin/README.md b/example-plugin/README.md new file mode 100644 index 0000000000..f366e93024 --- /dev/null +++ b/example-plugin/README.md @@ -0,0 +1,21 @@ +# Basilisk Example Plugin + +This standalone package demonstrates how third-party projects can integrate with +the Basilisk plugin system using Python entry points. + +## Installation + +```bash +pip install -e ./example-plugin +``` + +The package advertises the `basilisk.plugins` entry point group so it is +automatically discovered when Basilisk loads plugins at runtime. + +## Provided Module + +The `ExamplePluginModule` class is a minimal `SysModel` implementation. The +`register(registry)` function publishes this class to Basilisk, allowing it to +be accessed from `Basilisk.modules`. + +This project is intended purely for testing and documentation purposes. diff --git a/example-plugin/pyproject.toml b/example-plugin/pyproject.toml new file mode 100644 index 0000000000..11907bf66d --- /dev/null +++ b/example-plugin/pyproject.toml @@ -0,0 +1,19 @@ +[build-system] +requires = ["setuptools>=70"] +build-backend = "setuptools.build_meta" + +[project] +name = "bsk-example-plugin" +version = "0.1.0" +description = "Example Basilisk plugin showcasing Python-based registration." +readme = "README.md" +requires-python = ">=3.8" +dependencies = ["bsk"] +authors = [{ name = "Basilisk Developers" }] +license = { text = "ISC" } + +[project.entry-points."basilisk.plugins"] +example = "bsk_example_plugin.simple:register" + +[tool.setuptools.packages.find] +where = ["src"] diff --git a/example-plugin/src/bsk_example_plugin/__init__.py b/example-plugin/src/bsk_example_plugin/__init__.py new file mode 100644 index 0000000000..1799f3ac6f --- /dev/null +++ b/example-plugin/src/bsk_example_plugin/__init__.py @@ -0,0 +1,7 @@ +""" +Example Basilisk plugin used for integration and documentation. +""" + +from .simple import ExamplePluginModule, register + +__all__ = ["ExamplePluginModule", "register"] diff --git a/example-plugin/src/bsk_example_plugin/simple.py b/example-plugin/src/bsk_example_plugin/simple.py new file mode 100644 index 0000000000..8e570a1655 --- /dev/null +++ b/example-plugin/src/bsk_example_plugin/simple.py @@ -0,0 +1,36 @@ +""" +Minimal Basilisk plugin used to demonstrate Python-based registration. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from Basilisk.architecture import sysModel + +if TYPE_CHECKING: # pragma: no cover - import guard for typing tools + from bsk_core.plugins import PluginRegistry + + +class ExamplePluginModule(sysModel.SysModel): + """Small SysModel derivative that toggles flags when executed.""" + + def __init__(self): + super().__init__() + self.reset_called = False + self.update_called = False + + def Reset(self, current_sim_nanos): + self.reset_called = True + + def UpdateState(self, current_sim_nanos, call_time): + self.update_called = True + + +def register(registry: "PluginRegistry") -> None: + """Entry point invoked by Basilisk to register this module.""" + + registry.register_python_module("ExamplePluginModule", ExamplePluginModule) + + +__all__ = ["ExamplePluginModule", "register"] diff --git a/src/Basilisk/modules/__init__.py b/src/Basilisk/modules/__init__.py new file mode 100644 index 0000000000..a68778b4b8 --- /dev/null +++ b/src/Basilisk/modules/__init__.py @@ -0,0 +1,36 @@ +""" +Dynamic plugin namespace for Basilisk runtime modules. + +This module defers plugin discovery until attributes are accessed. Plugins are +expected to register their SysModel subclasses or factories via the global +registry that lives in :mod:`bsk_core.plugins`. +""" + +from __future__ import annotations + +from typing import Any, Iterable + +from bsk_core.plugins import GLOBAL_REGISTRY, load_all_plugins + +__all__ = ["GLOBAL_REGISTRY", "load_all_plugins"] + + +def _known_attribute_names() -> Iterable[str]: + load_all_plugins() + return tuple(set(GLOBAL_REGISTRY.py_modules) | set(GLOBAL_REGISTRY.factories)) + + +def __getattr__(name: str) -> Any: + load_all_plugins() + + if name in GLOBAL_REGISTRY.py_modules: + return GLOBAL_REGISTRY.py_modules[name] + + if name in GLOBAL_REGISTRY.factories: + return GLOBAL_REGISTRY.factories[name] + + raise AttributeError(f"module 'Basilisk.modules' has no attribute '{name}'") + + +def __dir__() -> list[str]: + return sorted(set(globals()) - {"__builtins__"} | set(_known_attribute_names())) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0c4f38f425..1993a1c659 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -721,6 +721,19 @@ else() "from Basilisk.architecture import messaging # ensure recorders() work without first importing messaging\n") endif() +file(MAKE_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk/modules") +configure_file( + "${CMAKE_SOURCE_DIR}/Basilisk/modules/__init__.py" + "${CMAKE_BINARY_DIR}/Basilisk/modules/__init__.py" + COPYONLY +) + +file(MAKE_DIRECTORY "${CMAKE_BINARY_DIR}/bsk_core") +file(GLOB BSK_CORE_SOURCES "${CMAKE_SOURCE_DIR}/bsk_core/*.py" "${CMAKE_SOURCE_DIR}/bsk_core/**") +if(BSK_CORE_SOURCES) + create_symlinks("${CMAKE_BINARY_DIR}/bsk_core" ${BSK_CORE_SOURCES}) +endif() + # TODO: Iterate through all dist directories and add __init__.py's where they don't exist file( GLOB_RECURSE DIST_DIRECTORIES diff --git a/src/bsk_core/__init__.py b/src/bsk_core/__init__.py new file mode 100644 index 0000000000..71e2261ecd --- /dev/null +++ b/src/bsk_core/__init__.py @@ -0,0 +1,10 @@ +""" +Lightweight core helpers that are shared across Basilisk's Python surface. + +The plugin system implemented in :mod:`bsk_core.plugins` is responsible for +discovering entry points and exposing their registrations to the runtime. +""" + +from .plugins import GLOBAL_REGISTRY, PluginRegistry, load_all_plugins + +__all__ = ["GLOBAL_REGISTRY", "PluginRegistry", "load_all_plugins"] diff --git a/src/bsk_core/plugins.py b/src/bsk_core/plugins.py new file mode 100644 index 0000000000..59d1490e87 --- /dev/null +++ b/src/bsk_core/plugins.py @@ -0,0 +1,120 @@ +""" +Runtime plugin registration support for Basilisk. + +Only Python-based registration is implemented today. C++ factories are stubbed +out to support future extensions without breaking the public API. +""" + +from __future__ import annotations + +from importlib import metadata +from typing import Any, Callable, Iterable, Optional + +from Basilisk.architecture import sysModel + +ENTRY_POINT_GROUP = "basilisk.plugins" +"""Python entry point group used to discover Basilisk plugins.""" + + +class PluginRegistry: + """Container for Basilisk plugin registrations.""" + + def __init__(self) -> None: + self.py_modules: dict[str, type[sysModel.SysModel]] = {} + self.factories: dict[str, Any] = {} + + def register_python_module(self, name: str, cls: type[sysModel.SysModel]) -> None: + """Register a Python :class:`~Basilisk.architecture.sysModel.SysModel` subclass.""" + if not isinstance(name, str) or not name: + raise TypeError("Module name must be a non-empty string") + if not isinstance(cls, type): + raise TypeError("Only classes can be registered as Python modules") + + try: + is_sysmodel = issubclass(cls, sysModel.SysModel) + except TypeError as exc: # cls is not a class or similar edge cases + raise TypeError("Only SysModel subclasses can be registered") from exc + + if not is_sysmodel: + raise TypeError( + f"Cannot register {cls!r} as '{name}': not a SysModel subclass" + ) + + existing = self.py_modules.get(name) + if existing is not None and existing is not cls: + raise ValueError( + f"Module name '{name}' already registered with {existing!r}" + ) + + self.py_modules[name] = cls + + def register_factory(self, name: str, factory: Any) -> None: + """ + Register a future C++ factory. + + No validation is performed yet; factories act as opaque callables or + objects until C++ support is implemented. + """ + if not isinstance(name, str) or not name: + raise TypeError("Factory name must be a non-empty string") + + existing = self.factories.get(name) + if existing is not None and existing is not factory: + raise ValueError(f"Factory name '{name}' already registered") + + self.factories[name] = factory + + +GLOBAL_REGISTRY = PluginRegistry() +"""Shared registry instance used across the Basilisk runtime.""" + +_PLUGINS_LOADED = False + + +def _iter_plugin_entry_points() -> Iterable[metadata.EntryPoint]: + """Return an iterable over all registered plugin entry points.""" + entry_points = metadata.entry_points() + if hasattr(entry_points, "select"): + return entry_points.select(group=ENTRY_POINT_GROUP) + return entry_points.get(ENTRY_POINT_GROUP, []) + + +def _resolve_register_callable(obj: Any) -> Callable[[PluginRegistry], None]: + """Normalize the value advertised by an entry point into a register callable.""" + if callable(obj): + return obj # The entry point points directly to register() + + register = getattr(obj, "register", None) + if callable(register): + return register + + raise TypeError( + "Basilisk plugin entry points must reference a callable or an object with " + "a callable 'register' attribute" + ) + + +def load_all_plugins(registry: Optional[PluginRegistry] = None) -> PluginRegistry: + """ + Discover and register all Basilisk plugins using ``importlib.metadata``. + + The discovery process is idempotent; repeated calls do not re-register + plugins. + """ + global _PLUGINS_LOADED + + if registry is None: + registry = GLOBAL_REGISTRY + + if _PLUGINS_LOADED: + return registry + + for entry_point in _iter_plugin_entry_points(): + register = _resolve_register_callable(entry_point.load()) + register(registry) + + _PLUGINS_LOADED = True + return registry + + +__all__ = ["GLOBAL_REGISTRY", "PluginRegistry", "load_all_plugins"] diff --git a/src/tests/test_plugin_registry.py b/src/tests/test_plugin_registry.py new file mode 100644 index 0000000000..7c607017e2 --- /dev/null +++ b/src/tests/test_plugin_registry.py @@ -0,0 +1,159 @@ +# +# ISC License +# +# Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# + +"""Smoke tests for the runtime plugin registry.""" + +from __future__ import annotations + +import importlib +from dataclasses import dataclass +from pathlib import Path +from typing import Callable + +import pytest + +from Basilisk.architecture import sysModel +from bsk_core import plugins + + +@pytest.fixture(autouse=True) +def reset_registry(): + """Ensure the global registry is clean before and after every test.""" + snapshot_py = dict(plugins.GLOBAL_REGISTRY.py_modules) + snapshot_factories = dict(plugins.GLOBAL_REGISTRY.factories) + snapshot_loaded = plugins._PLUGINS_LOADED + + plugins.GLOBAL_REGISTRY.py_modules.clear() + plugins.GLOBAL_REGISTRY.factories.clear() + plugins._PLUGINS_LOADED = False + + yield plugins.GLOBAL_REGISTRY + + plugins.GLOBAL_REGISTRY.py_modules.clear() + plugins.GLOBAL_REGISTRY.py_modules.update(snapshot_py) + + plugins.GLOBAL_REGISTRY.factories.clear() + plugins.GLOBAL_REGISTRY.factories.update(snapshot_factories) + + plugins._PLUGINS_LOADED = snapshot_loaded + + +class DummySysModel(sysModel.SysModel): + def __init__(self): + super().__init__() + + +def test_register_python_module_accepts_sysmodel(reset_registry): + registry = reset_registry + registry.register_python_module("Dummy", DummySysModel) + assert registry.py_modules["Dummy"] is DummySysModel + + +def test_register_python_module_rejects_non_sysmodel(reset_registry): + registry = reset_registry + with pytest.raises(TypeError): + registry.register_python_module("Bad", object) # type: ignore[arg-type] + + +def test_register_factory_allows_simple_storage(reset_registry): + registry = reset_registry + factory = object() + registry.register_factory("factory", factory) + assert registry.factories["factory"] is factory + + +@dataclass +class _FakeEntryPoint: + loader: Callable[[plugins.PluginRegistry], None] + + def load(self): + return self.loader + + +class _FakeEntryPoints: + def __init__(self, entries): + self._entries = entries + + def select(self, *, group): + if group == plugins.ENTRY_POINT_GROUP: + return self._entries + return [] + + +def test_load_all_plugins_discovers_entry_points(monkeypatch, reset_registry): + calls = [] + + def register(registry): + calls.append(registry) + registry.register_python_module("PluginSysModel", DummySysModel) + + fake_entry_points = _FakeEntryPoints([_FakeEntryPoint(register)]) + monkeypatch.setattr(plugins.metadata, "entry_points", lambda: fake_entry_points) + + registry = plugins.load_all_plugins() + assert "PluginSysModel" in registry.py_modules + assert calls == [registry] + + second = plugins.load_all_plugins() + assert second is registry + assert calls == [registry] + + +def test_modules_namespace_exposes_registered_plugins(monkeypatch, reset_registry): + modules_pkg = importlib.import_module("Basilisk.modules") + + def fake_loader(): + # Register a module and advertise that discovery has happened. + plugins.GLOBAL_REGISTRY.register_python_module("NamespaceSysModel", DummySysModel) + plugins._PLUGINS_LOADED = True + return plugins.GLOBAL_REGISTRY + + monkeypatch.setattr(modules_pkg, "load_all_plugins", fake_loader) + + resolved = modules_pkg.NamespaceSysModel + assert resolved is DummySysModel + + exported = dir(modules_pkg) + assert "NamespaceSysModel" in exported + + with pytest.raises(AttributeError): + getattr(modules_pkg, "DoesNotExist") + + +def test_example_plugin_discovery(monkeypatch, reset_registry): + repo_root = Path(__file__).resolve().parents[2] + plugin_src = repo_root / "example-plugin" / "src" + monkeypatch.syspath_prepend(str(plugin_src)) + + entry_point = plugins.metadata.EntryPoint( + name="example", + value="bsk_example_plugin.simple:register", + group=plugins.ENTRY_POINT_GROUP, + ) + monkeypatch.setattr(plugins, "_iter_plugin_entry_points", lambda: [entry_point]) + + registry = plugins.load_all_plugins() + assert "ExamplePluginModule" in registry.py_modules + + modules_pkg = importlib.import_module("Basilisk.modules") + cls = modules_pkg.ExamplePluginModule + instance = cls() + instance.Reset(0) + instance.UpdateState(0, 0) + assert instance.reset_called is True + assert instance.update_called is True From 9a6e14681bea1649d65f0c233934fb6d525662d6 Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Sun, 23 Nov 2025 21:42:11 -0700 Subject: [PATCH 2/8] CPP example --- example-plugin-cpp/CMakeLists.txt | 32 +++++++++++++ example-plugin-cpp/README.md | 41 +++++++++++++++++ example-plugin-cpp/pyproject.toml | 25 ++++++++++ example-plugin-cpp/src/cpp/example_plugin.cpp | 40 ++++++++++++++++ .../python/bsk_example_plugin_cpp/__init__.py | 7 +++ .../python/bsk_example_plugin_cpp/register.py | 28 +++++++++++ src/tests/test_plugin_registry.py | 46 +++++++++++++++++++ 7 files changed, 219 insertions(+) create mode 100644 example-plugin-cpp/CMakeLists.txt create mode 100644 example-plugin-cpp/README.md create mode 100644 example-plugin-cpp/pyproject.toml create mode 100644 example-plugin-cpp/src/cpp/example_plugin.cpp create mode 100644 example-plugin-cpp/src/python/bsk_example_plugin_cpp/__init__.py create mode 100644 example-plugin-cpp/src/python/bsk_example_plugin_cpp/register.py diff --git a/example-plugin-cpp/CMakeLists.txt b/example-plugin-cpp/CMakeLists.txt new file mode 100644 index 0000000000..0b7fa09bc8 --- /dev/null +++ b/example-plugin-cpp/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.18) +project(bsk_example_plugin_cpp LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(pybind11 CONFIG QUIET) +if(NOT pybind11_FOUND) + message(STATUS "Falling back to FetchContent for pybind11") + include(FetchContent) + FetchContent_Declare( + pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + GIT_TAG v2.12.0 + ) + FetchContent_MakeAvailable(pybind11) +endif() + +pybind11_add_module(_example_cpp MODULE src/cpp/example_plugin.cpp) +target_compile_features(_example_cpp PRIVATE cxx_std_17) + +install( + TARGETS _example_cpp + DESTINATION bsk_example_plugin_cpp + COMPONENT extensions +) + +install( + DIRECTORY src/python/bsk_example_plugin_cpp + DESTINATION . + COMPONENT python +) diff --git a/example-plugin-cpp/README.md b/example-plugin-cpp/README.md new file mode 100644 index 0000000000..a291a60da4 --- /dev/null +++ b/example-plugin-cpp/README.md @@ -0,0 +1,41 @@ +# Basilisk C++ Example Plugin + +This standalone project illustrates how a C++-backed module can integrate with +the Basilisk plugin system. The package is intentionally small and uses +`pybind11` to expose a factory that constructs a simple C++ `ExampleCppModule` +object. + +## Building + +```bash +pip install -e ./example-plugin-cpp +``` + +Requirements: + +- A Basilisk installation (for runtime only) +- A C++17 compiler +- `pybind11` (installed automatically via `pyproject.toml`) +- CMake ≥ 3.18 (provided by `scikit-build-core`) + +## Plugin Contract + +The package exposes a `register(registry)` function through the +`basilisk.plugins` entry-point group. When Basilisk calls this function at +runtime, the plugin registers a factory named `ExampleCppFactory`. The factory +returns instances of the C++ `ExampleCppModule` type, which mimics the shape of +future Basilisk C++ modules. + +After installation you can access the factory dynamically: + +```python +from basilisk import modules + +factory = modules.ExampleCppFactory +instance = factory() +instance.Reset(0) +instance.UpdateState(0, 0) +``` + +This project is meant for experimentation only; it does not ship with the core +Basilisk distribution. diff --git a/example-plugin-cpp/pyproject.toml b/example-plugin-cpp/pyproject.toml new file mode 100644 index 0000000000..46988dfcca --- /dev/null +++ b/example-plugin-cpp/pyproject.toml @@ -0,0 +1,25 @@ +[build-system] +requires = ["scikit-build-core>=0.9.3", "pybind11>=2.12"] +build-backend = "scikit_build_core.build" + +[project] +name = "bsk-example-plugin-cpp" +version = "0.1.0" +description = "Example Basilisk plugin implemented with a pybind11-backed C++ factory." +readme = "README.md" +requires-python = ">=3.9" +dependencies = ["bsk"] +authors = [{ name = "Basilisk Developers" }] +license = { text = "ISC" } + +[project.entry-points."basilisk.plugins"] +example_cpp = "bsk_example_plugin_cpp.register:register" + +[tool.scikit-build] +wheel.py-api = "cp38" + +[tool.scikit-build.cmake] +minimum-version = "3.18" + +[tool.scikit-build.install] +components = ["python", "extensions"] diff --git a/example-plugin-cpp/src/cpp/example_plugin.cpp b/example-plugin-cpp/src/cpp/example_plugin.cpp new file mode 100644 index 0000000000..ac53f95299 --- /dev/null +++ b/example-plugin-cpp/src/cpp/example_plugin.cpp @@ -0,0 +1,40 @@ +#include +#include + +namespace py = pybind11; + +namespace { + +class ExampleCppModule { + public: + void Reset(double /*current_sim_nanos*/) { reset_called_ = true; } + + void UpdateState(double /*current_sim_nanos*/, double /*call_time*/) { + update_called_ = true; + } + + bool reset_called() const { return reset_called_; } + bool update_called() const { return update_called_; } + + private: + bool reset_called_ = false; + bool update_called_ = false; +}; + +} // namespace + +PYBIND11_MODULE(_example_cpp, m) { + py::class_(m, "ExampleCppModule") + .def(py::init<>()) + .def("Reset", &ExampleCppModule::Reset) + .def("UpdateState", &ExampleCppModule::UpdateState) + .def_property_readonly("reset_called", &ExampleCppModule::reset_called) + .def_property_readonly("update_called", &ExampleCppModule::update_called); + + m.def( + "create_factory", + []() { + return py::cpp_function([]() { return ExampleCppModule(); }); + }, + "Return a callable factory that creates ExampleCppModule instances."); +} diff --git a/example-plugin-cpp/src/python/bsk_example_plugin_cpp/__init__.py b/example-plugin-cpp/src/python/bsk_example_plugin_cpp/__init__.py new file mode 100644 index 0000000000..c22beaacaf --- /dev/null +++ b/example-plugin-cpp/src/python/bsk_example_plugin_cpp/__init__.py @@ -0,0 +1,7 @@ +""" +Example C++-backed plugin package for Basilisk. +""" + +from .register import ExampleCppFactory, register + +__all__ = ["ExampleCppFactory", "register"] diff --git a/example-plugin-cpp/src/python/bsk_example_plugin_cpp/register.py b/example-plugin-cpp/src/python/bsk_example_plugin_cpp/register.py new file mode 100644 index 0000000000..c6202ace95 --- /dev/null +++ b/example-plugin-cpp/src/python/bsk_example_plugin_cpp/register.py @@ -0,0 +1,28 @@ +""" +Registration helpers for the example C++ Basilisk plugin. +""" + +from __future__ import annotations + +from importlib import import_module +from typing import Callable, Optional + +ExampleCppFactory: Optional[Callable[[], object]] = None + + +def _load_extension(): + module = import_module("bsk_example_plugin_cpp._example_cpp") + return module + + +def register(registry): + """ + Entry point invoked by Basilisk during plugin discovery. + """ + + extension = _load_extension() + factory = extension.create_factory() + registry.register_factory("ExampleCppFactory", factory) + + global ExampleCppFactory + ExampleCppFactory = factory diff --git a/src/tests/test_plugin_registry.py b/src/tests/test_plugin_registry.py index 7c607017e2..f70c7e5991 100644 --- a/src/tests/test_plugin_registry.py +++ b/src/tests/test_plugin_registry.py @@ -21,6 +21,8 @@ from __future__ import annotations import importlib +import sys +import types from dataclasses import dataclass from pathlib import Path from typing import Callable @@ -157,3 +159,47 @@ def test_example_plugin_discovery(monkeypatch, reset_registry): instance.UpdateState(0, 0) assert instance.reset_called is True assert instance.update_called is True + + +def test_example_cpp_plugin_discovery(monkeypatch, reset_registry): + repo_root = Path(__file__).resolve().parents[2] + plugin_src = repo_root / "example-plugin-cpp" / "src" / "python" + if not plugin_src.exists(): + pytest.skip("C++ example plugin sources missing") + + monkeypatch.syspath_prepend(str(plugin_src)) + + class StubExampleCppModule: + def __init__(self): + self.reset_called = False + self.update_called = False + + def Reset(self, current_sim_nanos): + self.reset_called = True + + def UpdateState(self, current_sim_nanos, call_time): + self.update_called = True + + def stub_factory(): + return StubExampleCppModule() + + stub_extension = types.SimpleNamespace(create_factory=lambda: stub_factory) + monkeypatch.setitem(sys.modules, "bsk_example_plugin_cpp._example_cpp", stub_extension) + + entry_point = plugins.metadata.EntryPoint( + name="example_cpp", + value="bsk_example_plugin_cpp.register:register", + group=plugins.ENTRY_POINT_GROUP, + ) + monkeypatch.setattr(plugins, "_iter_plugin_entry_points", lambda: [entry_point]) + + registry = plugins.load_all_plugins() + assert "ExampleCppFactory" in registry.factories + + modules_pkg = importlib.import_module("Basilisk.modules") + factory = modules_pkg.ExampleCppFactory + instance = factory() + instance.Reset(0) + instance.UpdateState(0, 0) + assert instance.reset_called is True + assert instance.update_called is True From e87cf3ffc67d4ef525e240ccb6661b6cc36ad962 Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Wed, 26 Nov 2025 10:26:58 -0700 Subject: [PATCH 3/8] testing sdk --- example-plugin-cpp/CMakeLists.txt | 7 ++ example-plugin-cpp/README.md | 5 +- example-plugin-cpp/src/cpp/example_plugin.cpp | 25 +--- sdk/include/bsk/plugin_sdk.hpp | 112 ++++++++++++++++++ src/CMakeLists.txt | 5 + 5 files changed, 130 insertions(+), 24 deletions(-) create mode 100644 sdk/include/bsk/plugin_sdk.hpp diff --git a/example-plugin-cpp/CMakeLists.txt b/example-plugin-cpp/CMakeLists.txt index 0b7fa09bc8..bdf406aac2 100644 --- a/example-plugin-cpp/CMakeLists.txt +++ b/example-plugin-cpp/CMakeLists.txt @@ -18,6 +18,13 @@ endif() pybind11_add_module(_example_cpp MODULE src/cpp/example_plugin.cpp) target_compile_features(_example_cpp PRIVATE cxx_std_17) +target_include_directories( + _example_cpp + PRIVATE + ${CMAKE_SOURCE_DIR}/../sdk/include + ${CMAKE_CURRENT_SOURCE_DIR}/../sdk/include + ${CMAKE_CURRENT_SOURCE_DIR}/../../sdk/include +) install( TARGETS _example_cpp diff --git a/example-plugin-cpp/README.md b/example-plugin-cpp/README.md index a291a60da4..a6788349a1 100644 --- a/example-plugin-cpp/README.md +++ b/example-plugin-cpp/README.md @@ -16,6 +16,8 @@ Requirements: - A Basilisk installation (for runtime only) - A C++17 compiler - `pybind11` (installed automatically via `pyproject.toml`) +- The simple Basilisk plugin SDK headers (`sdk/include/bsk/plugin_sdk.hpp`), bundled with + the Basilisk source tree for now - CMake ≥ 3.18 (provided by `scikit-build-core`) ## Plugin Contract @@ -24,7 +26,8 @@ The package exposes a `register(registry)` function through the `basilisk.plugins` entry-point group. When Basilisk calls this function at runtime, the plugin registers a factory named `ExampleCppFactory`. The factory returns instances of the C++ `ExampleCppModule` type, which mimics the shape of -future Basilisk C++ modules. +future Basilisk C++ modules. The helper macros in ``bsk/plugin_sdk.hpp`` hide +the pybind11 glue necessary to surface the class to Python. After installation you can access the factory dynamically: diff --git a/example-plugin-cpp/src/cpp/example_plugin.cpp b/example-plugin-cpp/src/cpp/example_plugin.cpp index ac53f95299..640d0c07e4 100644 --- a/example-plugin-cpp/src/cpp/example_plugin.cpp +++ b/example-plugin-cpp/src/cpp/example_plugin.cpp @@ -1,9 +1,4 @@ -#include -#include - -namespace py = pybind11; - -namespace { +#include class ExampleCppModule { public: @@ -21,20 +16,4 @@ class ExampleCppModule { bool update_called_ = false; }; -} // namespace - -PYBIND11_MODULE(_example_cpp, m) { - py::class_(m, "ExampleCppModule") - .def(py::init<>()) - .def("Reset", &ExampleCppModule::Reset) - .def("UpdateState", &ExampleCppModule::UpdateState) - .def_property_readonly("reset_called", &ExampleCppModule::reset_called) - .def_property_readonly("update_called", &ExampleCppModule::update_called); - - m.def( - "create_factory", - []() { - return py::cpp_function([]() { return ExampleCppModule(); }); - }, - "Return a callable factory that creates ExampleCppModule instances."); -} +BSK_PLUGIN_PYBIND_MODULE(_example_cpp, ExampleCppModule, "ExampleCppModule"); diff --git a/sdk/include/bsk/plugin_sdk.hpp b/sdk/include/bsk/plugin_sdk.hpp new file mode 100644 index 0000000000..5832812e77 --- /dev/null +++ b/sdk/include/bsk/plugin_sdk.hpp @@ -0,0 +1,112 @@ +#pragma once + +/** + * Minimal helper utilities for writing Basilisk C++ plugins. + * + * This header intentionally wraps a tiny amount of pybind11 boilerplate so that + * plugin authors can focus on the C++ implementation while Basilisk grows a + * richer native SDK. The helpers assume that the plugin exposes a default + * constructible module type with ``Reset`` and ``UpdateState`` methods. + * + * Usage: + * + * .. code-block:: cpp + * + * #include + * + * class MyCppModule { ... }; + * + * BSK_PLUGIN_PYBIND_MODULE(_my_module, MyCppModule, "MyCppModule"); + * + * This defines the required ``create_factory`` function and binds the class to + * Python so the plugin can be consumed via Basilisk's runtime registry. + */ + +#include +#include + +#include +#include + +namespace bsk::plugin { + +namespace detail { + +template +struct has_reset : std::false_type {}; + +template +struct has_reset().Reset(std::declval()))>> + : std::true_type {}; + +template +struct has_update_state : std::false_type {}; + +template +struct has_update_state< + T, std::void_t().UpdateState(std::declval(), std::declval()))>> + : std::true_type {}; + +template +struct has_reset_flag : std::false_type {}; + +template +struct has_reset_flag().reset_called())>> : std::true_type {}; + +template +struct has_update_flag : std::false_type {}; + +template +struct has_update_flag().update_called())>> : std::true_type {}; + +} // namespace detail + +template +pybind11::cpp_function make_factory() { + static_assert(std::is_default_constructible_v, + "Basilisk plugin modules must be default constructible"); + return pybind11::cpp_function([]() { return Module(); }); +} + +template +void bind_module(pybind11::module_& m, const char* python_name) { + auto cls = pybind11::class_(m, python_name); + + if constexpr (std::is_default_constructible_v) { + cls.def(pybind11::init<>()); + } + + if constexpr (detail::has_reset::value) { + cls.def("Reset", &Module::Reset); + } + + if constexpr (detail::has_update_state::value) { + cls.def("UpdateState", &Module::UpdateState); + } + + if constexpr (detail::has_reset_flag::value) { + cls.def_property_readonly("reset_called", &Module::reset_called); + } + + if constexpr (detail::has_update_flag::value) { + cls.def_property_readonly("update_called", &Module::update_called); + } +} + +template +void register_basic_plugin(pybind11::module_& m, const char* class_name) { + bind_module(m, class_name); + m.def("create_factory", []() { return make_factory(); }); +} + +} // namespace bsk::plugin + +/** + * Convenience macro that defines the required pybind11 module exporting the + * provided ``ModuleType`` as ``class_name``. It also generates the + * ``create_factory`` function consumed by the Basilisk runtime. + */ +#define BSK_PLUGIN_PYBIND_MODULE(module_name, ModuleType, class_name) \ + PYBIND11_MODULE(module_name, module) { \ + ::bsk::plugin::register_basic_plugin(module, class_name); \ + } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1993a1c659..ab6bca834a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -734,6 +734,11 @@ if(BSK_CORE_SOURCES) create_symlinks("${CMAKE_BINARY_DIR}/bsk_core" ${BSK_CORE_SOURCES}) endif() +set(BSK_PLUGIN_SDK_ROOT "${CMAKE_SOURCE_DIR}/../sdk") +if(EXISTS "${BSK_PLUGIN_SDK_ROOT}") + create_symlinks("${CMAKE_BINARY_DIR}/Basilisk" "${BSK_PLUGIN_SDK_ROOT}") +endif() + # TODO: Iterate through all dist directories and add __init__.py's where they don't exist file( GLOB_RECURSE DIST_DIRECTORIES From 80c2a1bb50f100eba1e7e9cf6e1cc2063d5fb0d0 Mon Sep 17 00:00:00 2001 From: Reece Humphreys Date: Fri, 12 Dec 2025 16:59:53 -0700 Subject: [PATCH 4/8] Checkpoint --- External/CMakeLists.txt | 52 + .../CustomCppModule/custom_cpp_module.cpp | 102 + External/README.md | 61 + .../msgPayloadDefC/CustomModuleMsgPayload.h | 31 + .../CustomModuleCppMsgPayload.h | 31 + External/pyproject.toml | 25 + .../Basilisk/ExternalModules/__init__.py | 13 + .../ExternalModules/customCppModule.py | 23 + .../ExternalModules/customPythonModule.py | 46 + .../python/bsk_external_plugin/__init__.py | 3 + .../python/bsk_external_plugin/register.py | 52 + External/tests/test_external_plugin.py | 149 + docs/source/Support/Developer.rst | 1 + docs/source/Support/Developer/bskSdkV1.rst | 162 + sdk/README.md | 12 + sdk/include/bsk/plugin_sdk.hpp | 5 +- sdk/pyproject.toml | 30 + sdk/src/bsk_sdk/__init__.py | 18 + .../_GeneralModuleFiles/sys_model.cpp | 31 + .../_GeneralModuleFiles/sys_model.h | 64 + .../_GeneralModuleFiles/sys_model.rst | 2 + .../_GeneralModuleFiles/sys_model_task.cpp | 132 + .../_GeneralModuleFiles/sys_model_task.h | 64 + .../architecture/messaging/CMakeLists.txt | 101 + .../messaging/cMsgCInterface/CMakeLists.txt | 90 + .../messaging/cMsgCInterface/msg_C.cpp.in | 156 + .../messaging/cMsgCInterface/msg_C.h.in | 60 + .../messaging/cMsgCInterfacePy/__init__.py | 7 + .../architecture/messaging/messaging.h | 386 ++ .../msgAutoSource/cMsgCInterfacePy.i.in | 65 + .../msgAutoSource/generatePackageInit.py | 26 + .../msgAutoSource/generateSWIGModules.py | 357 ++ .../msgAutoSource/msgInterfacePy.i.in | 56 + .../architecture/messaging/msgHeader.h | 30 + .../architecture/messaging/newMessaging.ih | 444 ++ .../architecture/utilities/BSpline.cpp | 725 +++ .../Basilisk/architecture/utilities/BSpline.h | 93 + .../architecture/utilities/CMakeLists.txt | 32 + .../architecture/utilities/astroConstants.h | 184 + .../architecture/utilities/avsEigenMRP.h | 843 +++ .../utilities/avsEigenSupport.cpp | 299 ++ .../architecture/utilities/avsEigenSupport.h | 64 + .../utilities/bilinearInterpolation.hpp | 58 + .../architecture/utilities/bskLogging.cpp | 153 + .../architecture/utilities/bskLogging.h | 111 + .../architecture/utilities/bskLogging.rst | 117 + .../architecture/utilities/bskSemaphore.h | 66 + .../architecture/utilities/bsk_Print.h | 59 + .../architecture/utilities/discretize.cpp | 95 + .../architecture/utilities/discretize.h | 78 + .../architecture/utilities/gauss_markov.cpp | 105 + .../architecture/utilities/gauss_markov.h | 100 + .../utilities/geodeticConversion.cpp | 168 + .../utilities/geodeticConversion.h | 43 + .../architecture/utilities/keplerianOrbit.cpp | 187 + .../architecture/utilities/keplerianOrbit.h | 98 + .../architecture/utilities/linearAlgebra.c | 2700 ++++++++++ .../architecture/utilities/linearAlgebra.h | 264 + .../utilities/linearInterpolation.hpp | 41 + .../architecture/utilities/macroDefinitions.h | 102 + .../moduleIdGenerator/moduleIdGenerator.cpp | 64 + .../moduleIdGenerator/moduleIdGenerator.h | 47 + .../architecture/utilities/orbitalMotion.c | 1018 ++++ .../architecture/utilities/orbitalMotion.h | 109 + .../utilities/rigidBodyKinematics.c | 4523 +++++++++++++++++ .../utilities/rigidBodyKinematics.h | 266 + .../architecture/utilities/saturate.cpp | 62 + .../architecture/utilities/saturate.h | 48 + .../architecture/utilities/signalCondition.c | 36 + .../architecture/utilities/signalCondition.h | 42 + .../architecture/utilities/simDefinitions.h | 29 + .../Basilisk/architecture/utilities/svd.c | 328 ++ .../Basilisk/architecture/utilities/svd.h | 35 + .../utilities/tests/CMakeLists.txt | 49 + .../utilities/tests/test_avsEigenMRP.cpp | 31 + .../utilities/tests/test_avsEigenSupport.cpp | 164 + .../tests/test_bilinearInterpolation.cpp | 49 + .../utilities/tests/test_discretize.cpp | 99 + .../utilities/tests/test_gaussMarkov.cpp | 215 + .../tests/test_geodeticConversion.cpp | 82 + .../tests/test_linearInterpolation.cpp | 42 + .../utilities/tests/test_orbitalMotion.cpp | 533 ++ .../utilities/tests/test_saturate.cpp | 37 + .../utilities/tests/unitTestComparators.h | 41 + .../architecture/utilities/ukfUtilities.c | 381 ++ .../architecture/utilities/ukfUtilities.h | 56 + sdk/src/bsk_sdk/include/bsk/plugin_sdk.hpp | 114 + sdk/src/bsk_sdk/include/pybind11/attr.h | 722 +++ .../bsk_sdk/include/pybind11/buffer_info.h | 208 + sdk/src/bsk_sdk/include/pybind11/cast.h | 2361 +++++++++ sdk/src/bsk_sdk/include/pybind11/chrono.h | 228 + sdk/src/bsk_sdk/include/pybind11/common.h | 2 + sdk/src/bsk_sdk/include/pybind11/complex.h | 74 + .../include/pybind11/conduit/README.txt | 15 + .../pybind11/conduit/pybind11_conduit_v1.h | 116 + .../conduit/pybind11_platform_abi_id.h | 87 + .../pybind11/conduit/wrap_include_python_h.h | 72 + .../include/pybind11/critical_section.h | 56 + .../bsk_sdk/include/pybind11/detail/class.h | 823 +++ .../bsk_sdk/include/pybind11/detail/common.h | 1348 +++++ .../include/pybind11/detail/cpp_conduit.h | 75 + .../bsk_sdk/include/pybind11/detail/descr.h | 226 + .../detail/dynamic_raw_ptr_cast_if_possible.h | 39 + .../pybind11/detail/exception_translation.h | 71 + .../detail/function_record_pyobject.h | 191 + .../bsk_sdk/include/pybind11/detail/init.h | 538 ++ .../include/pybind11/detail/internals.h | 799 +++ .../pybind11/detail/native_enum_data.h | 209 + .../detail/pybind11_namespace_macros.h | 82 + .../pybind11/detail/struct_smart_holder.h | 378 ++ .../pybind11/detail/type_caster_base.h | 1591 ++++++ .../bsk_sdk/include/pybind11/detail/typeid.h | 65 + .../pybind11/detail/using_smart_holder.h | 22 + .../pybind11/detail/value_and_holder.h | 90 + sdk/src/bsk_sdk/include/pybind11/eigen.h | 12 + .../bsk_sdk/include/pybind11/eigen/common.h | 9 + .../bsk_sdk/include/pybind11/eigen/matrix.h | 723 +++ .../bsk_sdk/include/pybind11/eigen/tensor.h | 521 ++ sdk/src/bsk_sdk/include/pybind11/embed.h | 320 ++ sdk/src/bsk_sdk/include/pybind11/eval.h | 161 + sdk/src/bsk_sdk/include/pybind11/functional.h | 147 + sdk/src/bsk_sdk/include/pybind11/gil.h | 199 + .../include/pybind11/gil_safe_call_once.h | 102 + sdk/src/bsk_sdk/include/pybind11/gil_simple.h | 37 + sdk/src/bsk_sdk/include/pybind11/iostream.h | 265 + .../bsk_sdk/include/pybind11/native_enum.h | 67 + sdk/src/bsk_sdk/include/pybind11/numpy.h | 2312 +++++++++ sdk/src/bsk_sdk/include/pybind11/operators.h | 202 + sdk/src/bsk_sdk/include/pybind11/options.h | 92 + sdk/src/bsk_sdk/include/pybind11/pybind11.h | 3645 +++++++++++++ sdk/src/bsk_sdk/include/pybind11/pytypes.h | 2680 ++++++++++ sdk/src/bsk_sdk/include/pybind11/stl.h | 666 +++ .../bsk_sdk/include/pybind11/stl/filesystem.h | 114 + sdk/src/bsk_sdk/include/pybind11/stl_bind.h | 858 ++++ .../bsk_sdk/include/pybind11/subinterpreter.h | 299 ++ .../pybind11/trampoline_self_life_support.h | 65 + .../pybind11/type_caster_pyobject_ptr.h | 61 + sdk/src/bsk_sdk/include/pybind11/typing.h | 298 ++ sdk/src/bsk_sdk/include/pybind11/warnings.h | 75 + sdk/tools/sync_headers.py | 46 + 140 files changed, 42634 insertions(+), 2 deletions(-) create mode 100644 External/CMakeLists.txt create mode 100644 External/ExternalModules/CustomCppModule/custom_cpp_module.cpp create mode 100644 External/README.md create mode 100644 External/msgPayloadDefC/CustomModuleMsgPayload.h create mode 100644 External/msgPayloadDefCpp/CustomModuleCppMsgPayload.h create mode 100644 External/pyproject.toml create mode 100644 External/src/python/Basilisk/ExternalModules/__init__.py create mode 100644 External/src/python/Basilisk/ExternalModules/customCppModule.py create mode 100644 External/src/python/Basilisk/ExternalModules/customPythonModule.py create mode 100644 External/src/python/bsk_external_plugin/__init__.py create mode 100644 External/src/python/bsk_external_plugin/register.py create mode 100644 External/tests/test_external_plugin.py create mode 100644 docs/source/Support/Developer/bskSdkV1.rst create mode 100644 sdk/README.md create mode 100644 sdk/pyproject.toml create mode 100644 sdk/src/bsk_sdk/__init__.py create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.rst create mode 100755 sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.cpp create mode 100755 sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/CMakeLists.txt create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/CMakeLists.txt create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.cpp.in create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.h.in create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterfacePy/__init__.py create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/messaging.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generatePackageInit.py create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generateSWIGModules.py create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/msgInterfacePy.i.in create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgHeader.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/newMessaging.ih create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/CMakeLists.txt create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/astroConstants.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenMRP.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bilinearInterpolation.hpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.rst create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskSemaphore.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bsk_Print.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearInterpolation.hpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/macroDefinitions.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/simDefinitions.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/CMakeLists.txt create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenMRP.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenSupport.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_bilinearInterpolation.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_discretize.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_gaussMarkov.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_geodeticConversion.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_linearInterpolation.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_orbitalMotion.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_saturate.cpp create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/unitTestComparators.h create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/ukfUtilities.c create mode 100644 sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/ukfUtilities.h create mode 100644 sdk/src/bsk_sdk/include/bsk/plugin_sdk.hpp create mode 100644 sdk/src/bsk_sdk/include/pybind11/attr.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/buffer_info.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/cast.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/chrono.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/common.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/complex.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/conduit/README.txt create mode 100644 sdk/src/bsk_sdk/include/pybind11/conduit/pybind11_conduit_v1.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/conduit/pybind11_platform_abi_id.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/conduit/wrap_include_python_h.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/critical_section.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/class.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/common.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/cpp_conduit.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/descr.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/dynamic_raw_ptr_cast_if_possible.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/exception_translation.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/function_record_pyobject.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/init.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/internals.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/native_enum_data.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/pybind11_namespace_macros.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/struct_smart_holder.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/type_caster_base.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/typeid.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/using_smart_holder.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/detail/value_and_holder.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/eigen.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/eigen/common.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/eigen/matrix.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/eigen/tensor.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/embed.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/eval.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/functional.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/gil.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/gil_safe_call_once.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/gil_simple.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/iostream.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/native_enum.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/numpy.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/operators.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/options.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/pybind11.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/pytypes.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/stl.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/stl/filesystem.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/stl_bind.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/subinterpreter.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/trampoline_self_life_support.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/type_caster_pyobject_ptr.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/typing.h create mode 100644 sdk/src/bsk_sdk/include/pybind11/warnings.h create mode 100755 sdk/tools/sync_headers.py diff --git a/External/CMakeLists.txt b/External/CMakeLists.txt new file mode 100644 index 0000000000..6c91d3d88d --- /dev/null +++ b/External/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.18) +project(bsk_external LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(Python3 COMPONENTS Interpreter Development.Module REQUIRED) + +execute_process( + COMMAND "${Python3_EXECUTABLE}" -c "import bsk_sdk; print(bsk_sdk.include_dir(), end='')" + OUTPUT_VARIABLE BSK_SDK_INCLUDE + RESULT_VARIABLE BSK_SDK_RESULT +) +if(NOT BSK_SDK_RESULT EQUAL 0) + message(FATAL_ERROR "Failed to locate bsk-sdk include directory. Is the package installed?") +endif() +string(STRIP "${BSK_SDK_INCLUDE}" BSK_SDK_INCLUDE) + +Python3_add_library(_custom_cpp MODULE ExternalModules/CustomCppModule/custom_cpp_module.cpp WITH_SOABI) +target_compile_features(_custom_cpp PRIVATE cxx_std_17) +target_include_directories( + _custom_cpp + PRIVATE + "${BSK_SDK_INCLUDE}" +) +target_link_libraries(_custom_cpp PRIVATE Python3::Module) + +set(BSK_ARCHITECTURE_SOURCES + "${BSK_SDK_INCLUDE}/Basilisk/architecture/_GeneralModuleFiles/sys_model.cpp" + "${BSK_SDK_INCLUDE}/Basilisk/architecture/utilities/bskLogging.cpp" + "${BSK_SDK_INCLUDE}/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp" +) + +target_sources(_custom_cpp PRIVATE ${BSK_ARCHITECTURE_SOURCES}) + +install( + TARGETS _custom_cpp + DESTINATION Basilisk/ExternalModules + COMPONENT extensions +) + +install( + DIRECTORY src/python/ + DESTINATION . + COMPONENT python +) + +install( + DIRECTORY msgPayloadDefC msgPayloadDefCpp + DESTINATION Basilisk + COMPONENT python +) diff --git a/External/ExternalModules/CustomCppModule/custom_cpp_module.cpp b/External/ExternalModules/CustomCppModule/custom_cpp_module.cpp new file mode 100644 index 0000000000..9cc4287a92 --- /dev/null +++ b/External/ExternalModules/CustomCppModule/custom_cpp_module.cpp @@ -0,0 +1,102 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace { + +struct CustomPluginMsgPayload { + std::array dataVector{}; +}; + +class CustomCppModule : public SysModel { + public: + CustomCppModule() + : output_writer_(dataOutMsg.addAuthor()), + input_reader_(input_channel_.addSubscriber()), + input_writer_(input_channel_.addAuthor()) { + this->ModelTag = "CustomCppModule"; + } + + void Reset(uint64_t /*current_sim_nanos*/) override { + reset_called_ = true; + update_called_ = false; + steps_ = 0; + last_input_ = {}; + last_output_ = {}; + last_update_nanos_ = 0; + } + + void UpdateState(uint64_t current_sim_nanos) override { + update_called_ = true; + ++steps_; + + if (input_reader_.isLinked() && input_reader_.isWritten()) { + last_input_ = input_reader_(); + } + + last_output_ = last_input_; + last_output_.dataVector[0] += static_cast(steps_); + last_output_.dataVector[2] = static_cast(current_sim_nanos) * 1e-9; + output_writer_(last_output_); + last_update_nanos_ = current_sim_nanos; + } + + void set_input_payload(const CustomPluginMsgPayload& payload) { + input_writer_(payload); + } + + CustomPluginMsgPayload last_input() const { return last_input_; } + CustomPluginMsgPayload last_output() const { return last_output_; } + uint64_t last_update_nanos() const { return last_update_nanos_; } + bool reset_called() const { return reset_called_; } + bool update_called() const { return update_called_; } + + private: + Message dataOutMsg; + Message input_channel_; + WriteFunctor output_writer_; + ReadFunctor input_reader_; + WriteFunctor input_writer_; + CustomPluginMsgPayload last_input_{}; + CustomPluginMsgPayload last_output_{}; + uint64_t last_update_nanos_ = 0; + bool reset_called_ = false; + bool update_called_ = false; + int steps_ = 0; +}; + +} // namespace + +PYBIND11_MODULE(_custom_cpp, module) { + namespace py = pybind11; + + py::class_(module, "CustomPluginMsgPayload") + .def(py::init<>()) + .def(py::init([](const std::vector& values) { + CustomPluginMsgPayload payload; + for (std::size_t i = 0; i < std::min(values.size(), payload.dataVector.size()); ++i) { + payload.dataVector[i] = values[i]; + } + return payload; + })) + .def_readwrite("dataVector", &CustomPluginMsgPayload::dataVector); + + auto cls = bsk::plugin::bind_module(module, "CustomCppModule"); + cls.def("set_input_payload", &CustomCppModule::set_input_payload, py::arg("payload")); + cls.def_property_readonly("last_input", &CustomCppModule::last_input); + cls.def_property_readonly("last_output", &CustomCppModule::last_output); + cls.def_property_readonly("last_update_nanos", &CustomCppModule::last_update_nanos); + cls.def_property_readonly("reset_called", &CustomCppModule::reset_called); + cls.def_property_readonly("update_called", &CustomCppModule::update_called); + + module.def("create_factory", []() { return bsk::plugin::make_factory(); }); +} diff --git a/External/README.md b/External/README.md new file mode 100644 index 0000000000..e73d6dabf2 --- /dev/null +++ b/External/README.md @@ -0,0 +1,61 @@ +# Basilisk External Plugin + +This package demonstrates how to distribute custom Basilisk modules as a +standalone plugin powered by the :mod:`bsk-sdk`. It contains both Python and +C++ examples and registers them with the Basilisk runtime through the plugin +entry-point system. + +## Building + +```bash +pip install -e ./sdk # install the published SDK locally +pip install scikit-build-core +pip install -e ./External --no-build-isolation +``` + +Requirements (beyond the Basilisk runtime): + +- Python 3.8+ +- A C++17 compiler +- ``bsk-sdk`` (published via ``./sdk`` in this repo for local development) +- ``scikit-build-core`` (build tooling; ``pybind11`` headers are bundled with the SDK) + +## Usage + +After installation the plugin is discovered automatically: + +```python +from Basilisk import modules + +cpp_factory = modules.CustomCppModule +instance = cpp_factory() +instance.Reset(0) +instance.UpdateState(0, 0) +``` + +The plugin also exposes a pure Python module: + +```python +from Basilisk import modules + +python_cls = modules.CustomPythonModule +module = python_cls() +module.Reset(0) +module.UpdateState(0, 0) + +The C++ factory mirrors Basilisk's ``SysModel`` concept and exposes a plugin +specific message payload: + +```python +from Basilisk import modules +from Basilisk.ExternalModules import customCppModule + +factory = modules.CustomCppModule +instance = factory() +payload = customCppModule.CustomPluginMsgPayload([1.0, 0.0, 0.0]) +instance.set_input_payload(payload) +instance.Reset(0) +instance.UpdateState(1_000_000_000) +print(instance.last_output().dataVector) +``` +``` diff --git a/External/msgPayloadDefC/CustomModuleMsgPayload.h b/External/msgPayloadDefC/CustomModuleMsgPayload.h new file mode 100644 index 0000000000..152f91f6a7 --- /dev/null +++ b/External/msgPayloadDefC/CustomModuleMsgPayload.h @@ -0,0 +1,31 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _CUSTOM_MODULE_OUT_H_ +#define _CUSTOM_MODULE_OUT_H_ + + +/*! @brief Structure used to define the output of the sub-module. This is the same + output message that is used by all sub-modules in the module folder. */ +typedef struct { + double dataVector[3]; //!< [units] sample output vector +}CustomModuleMsgPayload; + + +#endif diff --git a/External/msgPayloadDefCpp/CustomModuleCppMsgPayload.h b/External/msgPayloadDefCpp/CustomModuleCppMsgPayload.h new file mode 100644 index 0000000000..7f11ea7560 --- /dev/null +++ b/External/msgPayloadDefCpp/CustomModuleCppMsgPayload.h @@ -0,0 +1,31 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _CUSTOM_MODULE_CPP_OUT_H_ +#define _CUSTOM_MODULE_CPP_OUT_H_ + + +/*! @brief Structure used to define the output of the sub-module. This is the same + output message that is used by all sub-modules in the module folder. */ +typedef struct { + double dataVector[3]; //!< [units] sample output vector +}CustomModuleCppMsgPayload; + + +#endif diff --git a/External/pyproject.toml b/External/pyproject.toml new file mode 100644 index 0000000000..53d9cb7a94 --- /dev/null +++ b/External/pyproject.toml @@ -0,0 +1,25 @@ +[build-system] +requires = ["scikit-build-core>=0.9.3", "bsk-sdk>=1.0.0"] +build-backend = "scikit_build_core.build" + +[project] +name = "bsk-external" +version = "0.1.0" +description = "Standalone Basilisk plugin housing example external modules." +readme = "README.md" +requires-python = ">=3.8" +license = { text = "ISC" } +authors = [{ name = "Basilisk Developers" }] +dependencies = ["bsk"] + +[project.entry-points."basilisk.plugins"] +external = "bsk_external_plugin.register:register" + +[tool.scikit-build] +wheel.py-api = "cp38" + +[tool.scikit-build.cmake] +version = ">=3.18" + +[tool.scikit-build.install] +components = ["python", "extensions"] diff --git a/External/src/python/Basilisk/ExternalModules/__init__.py b/External/src/python/Basilisk/ExternalModules/__init__.py new file mode 100644 index 0000000000..aa2986288d --- /dev/null +++ b/External/src/python/Basilisk/ExternalModules/__init__.py @@ -0,0 +1,13 @@ +""" +External Basilisk modules distributed as a standalone plugin. + +The package mirrors Basilisk's legacy layout by exposing each module as a child +module so imports such as ``from Basilisk.ExternalModules import customCppModule`` +continue to work. +""" + +from __future__ import annotations + +from . import customCppModule, customPythonModule + +__all__ = ["customCppModule", "customPythonModule"] diff --git a/External/src/python/Basilisk/ExternalModules/customCppModule.py b/External/src/python/Basilisk/ExternalModules/customCppModule.py new file mode 100644 index 0000000000..74c3d294b4 --- /dev/null +++ b/External/src/python/Basilisk/ExternalModules/customCppModule.py @@ -0,0 +1,23 @@ +""" +Python helpers exposing the C++ CustomCppModule to Basilisk users. +""" + +from __future__ import annotations + +from importlib import import_module + +_extension = import_module("Basilisk.ExternalModules._custom_cpp") + +CustomCppModule = _extension.CustomCppModule +CustomPluginMsgPayload = _extension.CustomPluginMsgPayload + + +def customCppModule(): + """ + Backwards compatible factory returning a new :class:`CustomCppModule`. + """ + + return CustomCppModule() + + +__all__ = ["CustomCppModule", "CustomPluginMsgPayload", "customCppModule"] diff --git a/External/src/python/Basilisk/ExternalModules/customPythonModule.py b/External/src/python/Basilisk/ExternalModules/customPythonModule.py new file mode 100644 index 0000000000..19aead1369 --- /dev/null +++ b/External/src/python/Basilisk/ExternalModules/customPythonModule.py @@ -0,0 +1,46 @@ +""" +Pure Python example module that mimics the basic Basilisk SysModel contract. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import List + + +@dataclass +class CustomPythonModule: + """ + Simple stateful example of a Basilisk-like module implemented in Python. + """ + + dummy: float = 0.0 + input_vector: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) + data_vector: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) + reset_called: bool = False + update_called: bool = False + + def Reset(self, current_sim_nanos: float) -> None: # noqa: N802 - Basilisk naming convention + del current_sim_nanos + self.reset_called = True + self.dummy = 0.0 + self.data_vector = [0.0, 0.0, 0.0] + + def UpdateState(self, current_sim_nanos: float, call_time: float) -> None: # noqa: N802 + del current_sim_nanos + self.update_called = True + self.dummy += 1.0 + self.data_vector = [ + self.dummy + self.input_vector[0], + self.input_vector[1], + call_time, + ] + + +def customPythonModule() -> CustomPythonModule: + """Backwards compatible constructor for the module.""" + + return CustomPythonModule() + + +__all__ = ["CustomPythonModule", "customPythonModule"] diff --git a/External/src/python/bsk_external_plugin/__init__.py b/External/src/python/bsk_external_plugin/__init__.py new file mode 100644 index 0000000000..41727dae3a --- /dev/null +++ b/External/src/python/bsk_external_plugin/__init__.py @@ -0,0 +1,3 @@ +"""Namespace package for the Basilisk External plugin.""" + +__all__ = ["register"] diff --git a/External/src/python/bsk_external_plugin/register.py b/External/src/python/bsk_external_plugin/register.py new file mode 100644 index 0000000000..e9bc5b6403 --- /dev/null +++ b/External/src/python/bsk_external_plugin/register.py @@ -0,0 +1,52 @@ +""" +Entry point for the Basilisk External plugin. + +This module hooks the packaged custom modules into the Basilisk runtime through +``bsk_core.plugins``. +""" + +from __future__ import annotations + +from importlib import import_module +from typing import TYPE_CHECKING, Any, Callable + +if TYPE_CHECKING: # pragma: no cover - typing only + from bsk_core.plugins import PluginRegistry + + +def _load_cpp_extension() -> Any: + try: + return import_module("Basilisk.ExternalModules._custom_cpp") + except ModuleNotFoundError as exc: # pragma: no cover - build/runtime issue + raise ImportError( + "Unable to import the Basilisk External C++ extension. " + "Ensure the package was built with scikit-build-core." + ) from exc + + +def _register_cpp_factory(registry: "PluginRegistry") -> None: + extension = _load_cpp_extension() + factory = extension.create_factory() + + registry.register_factory("CustomCppModule", factory) + registry.register_factory("customCppModule", factory) + + +def _register_python_module(registry: "PluginRegistry") -> None: + module = import_module("Basilisk.ExternalModules.customPythonModule") + + def factory(): + return module.CustomPythonModule() + + registry.register_factory("CustomPythonModule", factory) + registry.register_factory("customPythonModule", factory) + + +def register(registry: "PluginRegistry") -> None: + """Register all external modules with the Basilisk runtime.""" + + _register_cpp_factory(registry) + _register_python_module(registry) + + +__all__ = ["register"] diff --git a/External/tests/test_external_plugin.py b/External/tests/test_external_plugin.py new file mode 100644 index 0000000000..e5549133d9 --- /dev/null +++ b/External/tests/test_external_plugin.py @@ -0,0 +1,149 @@ +from __future__ import annotations + +import sys +from pathlib import Path + +import pytest + +# Ensure the Basilisk sources under development are importable when running the +# tests directly from the repository. +REPO_ROOT = Path(__file__).resolve().parents[2] +SRC_ROOT = REPO_ROOT / "src" +if SRC_ROOT.exists(): + sys.path.insert(0, str(SRC_ROOT)) + +from bsk_core.plugins import PluginRegistry + +from bsk_external_plugin.register import register + + +@pytest.fixture() +def registry(): + return PluginRegistry() + + +@pytest.fixture(autouse=True) +def plugin_sys_path(monkeypatch): + test_root = Path(__file__).resolve().parents[2] + sdk_python = test_root / "src" + plugin_src = test_root / "External" / "src" / "python" + monkeypatch.syspath_prepend(str(sdk_python)) + monkeypatch.syspath_prepend(str(plugin_src)) + + +@pytest.fixture(autouse=True) +def stub_cpp_extension(monkeypatch): + class StubPayload: + def __init__(self, values=None): + self.dataVector = list(values or [0.0, 0.0, 0.0]) + + class StubCppModule: + def __init__(self): + self.reset_called = False + self.update_called = False + self._steps = 0 + self._input = StubPayload() + self._output = StubPayload() + self._last_update = 0 + + def Reset(self, current_sim_nanos): + del current_sim_nanos + self.reset_called = True + self.update_called = False + self._steps = 0 + self._output = StubPayload() + self._last_update = 0 + + def UpdateState(self, current_sim_nanos): + self.update_called = True + self._steps += 1 + vec = self._input.dataVector + self._output = StubPayload( + [vec[0] + float(self._steps), vec[1], float(current_sim_nanos) * 1e-9] + ) + self._last_update = current_sim_nanos + + def set_input_payload(self, payload): + self._input = payload + + def last_input(self): + return self._input + + def last_output(self): + return self._output + + def last_update_nanos(self): + return self._last_update + + class StubModule: + CustomCppModule = StubCppModule + CustomPluginMsgPayload = StubPayload + + @staticmethod + def create_factory(): + def factory(): + return StubCppModule() + + return factory + + monkeypatch.setitem(sys.modules, "Basilisk.ExternalModules._custom_cpp", StubModule) + + +def test_registers_python_and_cpp_modules(registry): + register(registry) + + assert "CustomCppModule" in registry.factories + assert "customCppModule" in registry.factories + assert "CustomPythonModule" in registry.factories + assert "customPythonModule" in registry.factories + + +def test_cpp_factory_behaves_like_module(registry): + register(registry) + + factory = registry.factories["CustomCppModule"] + instance = factory() + + assert instance.reset_called is False + assert instance.update_called is False + + instance.Reset(0) + assert instance.reset_called is True + + from Basilisk.ExternalModules import customCppModule as cpp_mod + + payload = cpp_mod.CustomPluginMsgPayload([1.0, -0.5, 0.7]) + instance.set_input_payload(payload) + instance.UpdateState(1_500_000_000) + assert instance.update_called is True + assert instance.last_input().dataVector == [1.0, -0.5, 0.7] + assert instance.last_output().dataVector == pytest.approx([2.0, -0.5, 1.5]) + + +def test_python_module_behaves_like_module(registry): + register(registry) + + factory = registry.factories["CustomPythonModule"] + instance = factory() + + instance.input_vector = [0.1, 0.2, 0.3] + instance.Reset(0.0) + assert instance.reset_called is True + + instance.UpdateState(0.0, 2.0) + assert instance.update_called is True + assert instance.dummy == pytest.approx(1.0) + assert instance.data_vector == pytest.approx([1.1, 0.2, 2.0]) + + +def test_legacy_wrappers_expose_modules(): + # Ensure the wrappers can be imported even before the plugin is registered. + from Basilisk.ExternalModules import customCppModule, customPythonModule + + cpp_instance = customCppModule.customCppModule() + assert isinstance(cpp_instance, customCppModule.CustomCppModule) + payload = customCppModule.CustomPluginMsgPayload() + assert payload.dataVector == [0.0, 0.0, 0.0] + + py_instance = customPythonModule.customPythonModule() + assert isinstance(py_instance, customPythonModule.CustomPythonModule) diff --git a/docs/source/Support/Developer.rst b/docs/source/Support/Developer.rst index b1720eee3b..b5667aeea3 100644 --- a/docs/source/Support/Developer.rst +++ b/docs/source/Support/Developer.rst @@ -16,5 +16,6 @@ The following support files help with writing Basilisk modules. Developer/createHtmlDocumentation Developer/bskModuleCheckoutList Developer/UnderstandingBasilisk + Developer/bskSdkV1 Developer/migratingBskModuleToBsk2 Developer/MigratingToPython3 diff --git a/docs/source/Support/Developer/bskSdkV1.rst b/docs/source/Support/Developer/bskSdkV1.rst new file mode 100644 index 0000000000..59dfbfb13f --- /dev/null +++ b/docs/source/Support/Developer/bskSdkV1.rst @@ -0,0 +1,162 @@ +Basilisk SDK Version 1 +====================== + +.. contents:: Outline + :local: + +Purpose +------- + +The Basilisk SDK (``bsk-sdk``) defines the public surface that external plugin +authors can rely on when integrating new simulation capabilities with the +core runtime. Version 1 focuses on establishing a stable contract for Python +and C++ plugin authors and capturing the minimal tooling that ships inside the +Basilisk source tree. + +Scope and Deliverables +---------------------- + +Version 1 guarantees the following artifacts: + +- ``bsk_core.plugins``: the runtime registry responsible for discovering + entry-point advertised plugins and exposing them under ``Basilisk.modules``. +- ``bsk-sdk``: a small Python package that publishes the SDK headers, the + bundled ``pybind11`` headers required by the helper macros, and a convenience + :func:`bsk_sdk.include_dir` helper for build scripts. +- A companion ``sync_headers.py`` utility (``sdk/tools``) keeps the vendored + Basilisk ``architecture`` headers in sync with the main source tree. +- ``sdk/include/bsk/plugin_sdk.hpp``: a single header that wraps the pybind11 + boilerplate required for C++ factories and enforces the default constructible + + ``Reset``/``UpdateState`` interface contract. The same header is shipped by + :mod:`bsk-sdk`. +- Reference implementations in ``example-plugin`` (pure Python), + ``example-plugin-cpp`` (C++ backed), and the standalone ``External`` plugin + package that exercises both module styles. + ``example-plugin-cpp`` (C++ backed) that demonstrate the expected packaging + and registration patterns. + +Any other files in the repository are explicitly *not* part of the SDK +agreement for this release. + +Plugin Registry API +------------------- + +The ``bsk_core.plugins.PluginRegistry`` class is the primary integration +point for third-party plugins. The registry is responsible for staging plugin +definitions until the runtime exports them under ``Basilisk.modules``. + +The public methods guaranteed in v1 are: + +.. code-block:: python + + class PluginRegistry: + def register_python_module(self, name: str, cls: type[sysModel.SysModel]) -> None: ... + def register_factory(self, name: str, factory: Any) -> None: ... + +``register_python_module`` accepts any subclass of +``Basilisk.architecture.sysModel.SysModel`` and exposes it as a class on +``Basilisk.modules`` using the provided name. ``register_factory`` stores an +opaque object under the supplied name. Factories are expected to be callables +returning Basilisk-compatible module instances, but v1 defers any runtime shape +validation to keep the surface area small. + +Plugins must advertise a ``register(registry)`` callable through the +``basilisk.plugins`` entry-point group. During startup Basilisk resolves the +entry-point, imports the containing module, and invokes the callable with the +shared registry instance. + +Python Plugin Pattern +--------------------- + +Pure-Python plugins should follow the pattern demonstrated in +``example-plugin/src/bsk_example_plugin/simple.py``: + +.. code-block:: python + + from Basilisk.architecture import sysModel + + class ExamplePluginModule(sysModel.SysModel): + def Reset(self, current_sim_nanos): + ... + + def UpdateState(self, current_sim_nanos, call_time): + ... + + def register(registry): + registry.register_python_module("ExamplePluginModule", ExamplePluginModule) + +The distribution's ``pyproject.toml`` must expose the ``register`` function via + +.. code-block:: toml + + [project.entry-points."basilisk.plugins"] + example = "bsk_example_plugin.simple:register" + +At runtime users import the module from ``Basilisk.modules``: + +.. code-block:: python + + from Basilisk import modules + + plugin_cls = modules.ExamplePluginModule + instance = plugin_cls() + instance.Reset(0) + instance.UpdateState(0, 0) + +C++ Plugin Pattern +------------------ + +Native extensions should include ``sdk/include/bsk/plugin_sdk.hpp`` to inherit +the pybind11 binding helpers. When building outside the Basilisk source tree +the :mod:`bsk-sdk` package exposes the headers via +``import bsk_sdk; bsk_sdk.include_dir()`` and ships the required ``pybind11`` +headers. Version 1 guarantees the availability of: + +- ``bsk::plugin::register_basic_plugin`` +- ``BSK_PLUGIN_PYBIND_MODULE`` + +The ``BSK_PLUGIN_PYBIND_MODULE`` macro defines both the pybind11 module and the +``create_factory`` callable consumed by the Basilisk runtime. The expected class +contract mirrors the Python case: default constructible with ``Reset`` and +``UpdateState`` methods. + +.. code-block:: cpp + + #include + + class ExampleCppModule { + public: + void Reset(double current_sim_nanos); + void UpdateState(double current_sim_nanos, double call_time); + }; + + BSK_PLUGIN_PYBIND_MODULE(_example_cpp, ExampleCppModule, "ExampleCppModule"); + +The companion Python package should lazily import the extension, extract the +factory, and register it: + +.. code-block:: python + + from importlib import import_module + + def register(registry): + ext = import_module("bsk_example_plugin_cpp._example_cpp") + factory = ext.create_factory() + registry.register_factory("ExampleCppFactory", factory) + +Limitations and Future Work +--------------------------- + +Version 1 intentionally leaves several items out of scope so they can be +designed with real-world feedback: + +- The SDK header is distributed from the Basilisk source tree and is not + published as a standalone artifact. +- Factories registered via ``register_factory`` are treated as opaque callables; + Basilisk does not verify their type or interface beyond name collisions. +- The helper header requires C++17 and a compatible pybind11 toolchain. +- Plugin lifecycle hooks beyond ``Reset``/``UpdateState`` will be designed as + future Basilisk modules adopt richer interfaces. + +Feedback on these gaps is welcome and will inform the roadmap for subsequent +SDK revisions. diff --git a/sdk/README.md b/sdk/README.md new file mode 100644 index 0000000000..a9dd67f574 --- /dev/null +++ b/sdk/README.md @@ -0,0 +1,12 @@ +# Basilisk SDK + +This package publishes the header-only Basilisk plugin SDK so that external +projects can build Basilisk-compatible plugins without vendoring the full +simulation codebase. It currently ships the ``bsk/plugin_sdk.hpp`` helper, a +vendored copy of the required ``pybind11`` headers, a subset of Basilisk's +``architecture`` headers, and a convenience +:func:`bsk_sdk.include_dir` accessor for build systems. + +To refresh the vendored Basilisk headers from the source tree run:: + + python sdk/tools/sync_headers.py diff --git a/sdk/include/bsk/plugin_sdk.hpp b/sdk/include/bsk/plugin_sdk.hpp index 5832812e77..ff2fa46acb 100644 --- a/sdk/include/bsk/plugin_sdk.hpp +++ b/sdk/include/bsk/plugin_sdk.hpp @@ -69,7 +69,7 @@ pybind11::cpp_function make_factory() { } template -void bind_module(pybind11::module_& m, const char* python_name) { +auto bind_module(pybind11::module_& m, const char* python_name) { auto cls = pybind11::class_(m, python_name); if constexpr (std::is_default_constructible_v) { @@ -91,11 +91,12 @@ void bind_module(pybind11::module_& m, const char* python_name) { if constexpr (detail::has_update_flag::value) { cls.def_property_readonly("update_called", &Module::update_called); } + return cls; } template void register_basic_plugin(pybind11::module_& m, const char* class_name) { - bind_module(m, class_name); + (void)bind_module(m, class_name); m.def("create_factory", []() { return make_factory(); }); } diff --git a/sdk/pyproject.toml b/sdk/pyproject.toml new file mode 100644 index 0000000000..f7eda25f95 --- /dev/null +++ b/sdk/pyproject.toml @@ -0,0 +1,30 @@ +[build-system] +requires = ["setuptools>=70"] +build-backend = "setuptools.build_meta" + +[project] +name = "bsk-sdk" +version = "1.0.0" +description = "Header-only SDK for developing Basilisk plugins." +readme = "README.md" +requires-python = ">=3.8" +license = { text = "ISC" } +authors = [{ name = "Basilisk Developers" }] +classifiers = [ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: ISC License (ISCL)", + "Operating System :: OS Independent", +] + +[project.urls] +homepage = "https://avslab.github.io/basilisk/" +source = "https://github.com/AVSLab/basilisk" +tracker = "https://github.com/AVSLab/basilisk/issues" + +[tool.setuptools] +package-dir = {"" = "src"} +packages = ["bsk_sdk"] +include-package-data = true + +[tool.setuptools.package-data] +"bsk_sdk" = ["include/**/*", "pybind11.LICENSE"] diff --git a/sdk/src/bsk_sdk/__init__.py b/sdk/src/bsk_sdk/__init__.py new file mode 100644 index 0000000000..b45a3ee5d2 --- /dev/null +++ b/sdk/src/bsk_sdk/__init__.py @@ -0,0 +1,18 @@ +""" +Utilities for locating the Basilisk SDK headers within a Python environment. + +The package installs the header-only SDK under ``include/bsk``. Plugin build +systems can use :func:`include_dir` to configure their compiler include paths. +""" + +from __future__ import annotations + +from importlib import resources + + +def include_dir() -> str: + """Return the absolute path to the installed SDK include directory.""" + return str(resources.files(__package__) / "include") + + +__all__ = ["include_dir"] diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.cpp new file mode 100644 index 0000000000..c203975926 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.cpp @@ -0,0 +1,31 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/utilities/moduleIdGenerator/moduleIdGenerator.h" + +SysModel::SysModel() + : moduleID(ModuleIdGenerator::GetInstance()->checkoutModuleID()) +{} + +SysModel::SysModel(const SysModel &obj) + : ModelTag{obj.ModelTag}, + RNGSeed{obj.RNGSeed}, + moduleID{ModuleIdGenerator::GetInstance()->checkoutModuleID()} +{} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.h new file mode 100644 index 0000000000..a42fe43d74 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.h @@ -0,0 +1,64 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _SysModel_HH_ +#define _SysModel_HH_ + +#include +#include +#include + +/*! @brief Simulation System Model Class */ +class SysModel +{ +public: + SysModel(); + + /** + * @brief Copy constructor for SysModel. + * + * This constructor initializes a new SysModel instance by copying data + * from another SysModel instance. + * + * @param obj The SysModel object to copy data from. + */ + SysModel(const SysModel &obj); + + virtual ~SysModel(){}; + + /** Initializes the module, create messages */ + virtual void SelfInit(){}; + + /** ??? */ + virtual void IntegratedInit(){}; + + /** Reads incoming messages, performs module actions, writes output messages */ + virtual void UpdateState(uint64_t CurrentSimNanos){}; + + /** Called at simulation initialization, resets module to specified time */ + virtual void Reset(uint64_t CurrentSimNanos){}; + + std::string ModelTag = ""; //!< Basilisk module tag name + uint64_t CallCounts = 0; //!< Counts on the model being called + uint32_t RNGSeed = 0x1badcad1; //!< Giving everyone a random seed for ease of MC + int64_t moduleID; //!< Module ID for this module (handed out by module_id_generator) +}; + + +#endif /* _SYS_MODEL_H_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.rst b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.rst new file mode 100644 index 0000000000..c56cea9aba --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model.rst @@ -0,0 +1,2 @@ + +Basilisk base system model class. diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.cpp new file mode 100755 index 0000000000..5c1f1e7722 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.cpp @@ -0,0 +1,132 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "sys_model_task.h" + +/*! A construction option that allows the user to set some task parameters. + Note that the only required argument is InputPeriod. + @param InputPeriod The amount of nanoseconds between calls to this Task. + @param FirstStartTime The amount of time in nanoseconds to hold a task dormant before starting. + After this time the task is executed at integer amounts of InputPeriod again + */ +SysModelTask::SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime) : + NextStartTime(FirstStartTime), TaskPeriod(InputPeriod), FirstTaskTime(FirstStartTime) +{ + this->NextPickupTime = this->NextStartTime + this->TaskPeriod; +} + +/*! This method self-initializes all of the models that have been added to the Task. + */ +void SysModelTask::SelfInitTaskList() const +{ + //! - Loop over all models and do the self init for each + for(auto const& modelPair : this->TaskModels) + { + SysModel* NonIt = modelPair.ModelPtr; + NonIt->SelfInit(); + } +} + + +/*! This method resets all of the models that have been added to the Task at the CurrentSimTime. + See sys_model_task.h for related method ResetTask() + @param CurrentSimTime The time to start at after reset +*/ +void SysModelTask::ResetTaskList(uint64_t CurrentSimTime) +{ + for (auto const& modelPair : this->TaskModels) + { + modelPair.ModelPtr->Reset(CurrentSimTime); + } + this->NextStartTime = CurrentSimTime; + this->NextPickupTime = this->NextStartTime + this->TaskPeriod; +} + +/*! This method executes all of the models on the Task during runtime. + Then, it sets its NextStartTime appropriately. + @param CurrentSimNanos The current simulation time in [ns] + */ +void SysModelTask::ExecuteTaskList(uint64_t CurrentSimNanos) +{ + //! - Loop over all of the models in the simulation and call their UpdateState + for(auto ModelPair = this->TaskModels.begin(); (ModelPair != this->TaskModels.end() && this->taskActive); + ModelPair++) + { + SysModel* NonIt = (ModelPair->ModelPtr); + NonIt->UpdateState(CurrentSimNanos); + NonIt->CallCounts += 1; + } + //! - NextStartTime is set to allow the scheduler to fit the next call in + this->NextStartTime += this->TaskPeriod; +} + +/*! This method adds a new model into the Task list. Note that the Priority + parameter is option as it defaults to -1 (lowest, latest) + @param NewModel The new model that we are adding to the Task + @param Priority The selected priority of the model being added (highest goes first) + */ +void SysModelTask::AddNewObject(SysModel *NewModel, int32_t Priority) +{ + ModelPriorityPair LocalPair; + + //! - Set the local pair with the requested priority and mode + LocalPair.CurrentModelPriority = Priority; + LocalPair.ModelPtr = NewModel; +// SystemMessaging::GetInstance()->addModuleToProcess(NewModel->moduleID, +// parentProc); + //! - Loop through the ModelPair vector and if Priority is higher than next, insert + for(auto ModelPair = this->TaskModels.begin(); ModelPair != this->TaskModels.end(); + ModelPair++) + { + if(Priority > ModelPair->CurrentModelPriority) + { + this->TaskModels.insert(ModelPair, LocalPair); + return; + } + } + //! - If we make it to the end of the loop, this is lowest priority, put it at end + this->TaskModels.push_back(LocalPair); +} + +/*! This method changes the period of a given task over to the requested period. + It attempts to keep the same offset relative to the original offset that + was specified at task creation. + @param newPeriod The period that the task should run at going forward + */ +void SysModelTask::updatePeriod(uint64_t newPeriod) +{ + //! - If the requested time is above the min time, set the next time based on the previous time plus the new period + if(this->NextStartTime > this->TaskPeriod) + { + uint64_t newStartTime = (this->NextStartTime/newPeriod)*newPeriod; + if(newStartTime <= (this->NextStartTime - this->TaskPeriod)) + { + newStartTime += newPeriod; + } + this->NextStartTime = newStartTime; + } + //! - Otherwise, we just should keep the original requested first call time for the task + else + { + this->NextStartTime = this->FirstTaskTime; + } + //! - Change the period of the task so that future calls will be based on the new period + this->TaskPeriod = newPeriod; + +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.h new file mode 100755 index 0000000000..4d43c6818a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/_GeneralModuleFiles/sys_model_task.h @@ -0,0 +1,64 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _SysModelTask_HH_ +#define _SysModelTask_HH_ + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/utilities/bskLogging.h" +#include +#include + +//! Structure used to pair a model and its requested priority +typedef struct { + int32_t CurrentModelPriority; //!< The current model priority. Higher goes first + SysModel *ModelPtr; //!< The model associated with this priority +}ModelPriorityPair; + +//! Class used to group a set of models into one "Task" of execution +class SysModelTask +{ + +public: + SysModelTask()=default; + explicit SysModelTask(uint64_t InputPeriod, uint64_t FirstStartTime=0); //!< class method + ~SysModelTask()=default; + void AddNewObject(SysModel *NewModel, int32_t Priority = -1); + void SelfInitTaskList() const; + //void CrossInitTaskList(); + void ExecuteTaskList(uint64_t CurrentSimTime); + void ResetTaskList(uint64_t CurrentSimTime); + void ResetTask() {this->NextStartTime = this->FirstTaskTime;} //!< Resets the task + void enableTask() {this->taskActive = true;} //!< Enables the task. Great comment huh? + void disableTask() {this->taskActive = false;} //!< Disables the task. I know. + void updatePeriod(uint64_t newPeriod); + void updateParentProc(std::string const& parent) {this->parentProc = parent;} //!< Allows the system to move task to a different process + + std::vector TaskModels{}; //!< -- Array that has pointers to all task sysModels + std::string TaskName{}; //!< -- Identifier for Task + std::string parentProc; //!< -- Process that calls this task + uint64_t NextStartTime=0; //!< [ns] Next time to start task + uint64_t NextPickupTime=0; //!< [ns] Next time read Task outputs + uint64_t TaskPeriod=100; //!< [ns] Cycle rate for Task + uint64_t FirstTaskTime=0; //!< [ns] Time to start Task for first time. After this time the normal periodic updates resume. + bool taskActive=true; //!< -- Flag indicating whether the Task has been disabled + BSKLogger bskLogger; //!< -- BSK Logging +}; + +#endif /* _SysModelTask_H_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/CMakeLists.txt b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/CMakeLists.txt new file mode 100644 index 0000000000..22c68febb7 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_policy(SET CMP0078 NEW) + +function(generate_messages searchDir generateCCode) + file( + GLOB_RECURSE message_files + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/" + "../${searchDir}/*Payload.h" "${EXTERNAL_MODULES_PATH}/${searchDir}/*Payload.h") + + file(MAKE_DIRECTORY ${CMAKE_BINARY_DIR}/autoSource/xmlWrap) + + foreach(msgFile ${message_files}) + get_filename_component(TARGET_NAME ${msgFile} NAME_WE) + + set(SWIG_XML_OUTPUT "${CMAKE_BINARY_DIR}/autoSource/xmlWrap/${TARGET_NAME}.xml") + add_custom_command( + OUTPUT ${SWIG_XML_OUTPUT} + COMMAND ${SWIG_EXECUTABLE} -c++ -xml -module dummy -o ${SWIG_XML_OUTPUT} ${msgFile} + DEPENDS ${msgFile} + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/" + COMMENT "Generating SWIG XML from ${msgFile}" + VERBATIM + ) + + get_filename_component(TARGET_DIR ${msgFile} DIRECTORY) + set(COMP_OUT_NAME "${CMAKE_BINARY_DIR}/autoSource/${TARGET_NAME}.i") + add_custom_command( + OUTPUT ${COMP_OUT_NAME} + COMMAND ${Python3_EXECUTABLE} generateSWIGModules.py ${COMP_OUT_NAME} ${msgFile} ${TARGET_NAME} ${searchDir} + ${generateCCode} ${SWIG_XML_OUTPUT} ${RECORDER_PROPERTY_ROLLBACK} + DEPENDS ${msgFile} ${SWIG_XML_OUTPUT} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/msgAutoSource/) + set_property(SOURCE ${COMP_OUT_NAME} PROPERTY CPLUSPLUS ON) + + if(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "") + set_property( + SOURCE ${COMP_OUT_NAME} + PROPERTY SWIG_FLAGS "-I${CMAKE_CURRENT_SOURCE_DIR}/../" "-I${CMAKE_CURRENT_SOURCE_DIR}/../../" + "-I${EXTERNAL_MODULES_PATH}/" "-I${CMAKE_BINARY_DIR}/autoSource/" "-I${Python3_INCLUDE_DIRS}" "-I${NUMPY_INCLUDE_DIR}") + include_directories("${EXTERNAL_MODULES_PATH}/") + else() + set_property( + SOURCE ${COMP_OUT_NAME} + PROPERTY SWIG_FLAGS "-I${CMAKE_CURRENT_SOURCE_DIR}/../" "-I${CMAKE_CURRENT_SOURCE_DIR}/../../" + "-I${CMAKE_BINARY_DIR}/autoSource/" "-I${Python3_INCLUDE_DIRS}" "-I${NUMPY_INCLUDE_DIR}") + endif(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "") + + include_directories(${Python3_INCLUDE_DIRS}) + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/") + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../../") + include_directories(${NUMPY_INCLUDE_DIR}) + + swig_add_library( + ${TARGET_NAME} + LANGUAGE "python" + TYPE MODULE + SOURCES ${COMP_OUT_NAME} OUTFILE_DIR "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/" # _wrap.c/.cxx file + OUTPUT_DIR "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/") + + add_dependencies(${TARGET_NAME} swigtrick) + set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "architecture/messaging/derivedCode") + set_target_properties(${SWIG_MODULE_${TARGET_NAME}_REAL_NAME} + PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging") + set_target_properties( + ${SWIG_MODULE_${TARGET_NAME}_REAL_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY_DEBUG + "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging") + set_target_properties( + ${SWIG_MODULE_${TARGET_NAME}_REAL_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY_RELEASE + "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging") + target_link_libraries(${SWIG_MODULE_${TARGET_NAME}_REAL_NAME} PUBLIC architectureLib) + endforeach() +endfunction(generate_messages) + +# Copy the cMsgCInterfacePy wrapper module to support backwards-compatibility. +# TODO: Deprecate this! +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cMsgCInterfacePy/__init__.py ${CMAKE_BINARY_DIR}/Basilisk/architecture/cMsgCInterfacePy/__init__.py COPYONLY) + +if(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "") + add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/__init__.py + COMMAND + ${Python3_EXECUTABLE} generatePackageInit.py "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/" + "../../msgPayloadDefC/" "../../msgPayloadDefCpp/" "${EXTERNAL_MODULES_PATH}/msgPayloadDefC/" + "${EXTERNAL_MODULES_PATH}/msgPayloadDefCpp/" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/msgAutoSource + VERBATIM) +else() + add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/__init__.py + COMMAND ${Python3_EXECUTABLE} generatePackageInit.py "${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/" + "../../msgPayloadDefC/" "../../msgPayloadDefCpp/" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/msgAutoSource + VERBATIM) +endif(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "") + +# Custom target for establishing dependency +add_custom_target(swigtrick DEPENDS ${CMAKE_BINARY_DIR}/Basilisk/architecture/messaging/__init__.py) + +# Dependency +generate_messages(msgPayloadDefCpp "False") +generate_messages(msgPayloadDefC "True") diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/CMakeLists.txt b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/CMakeLists.txt new file mode 100644 index 0000000000..0248fafd44 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/CMakeLists.txt @@ -0,0 +1,90 @@ +# Read the LICENSE file into a template variable +file(READ "${CMAKE_SOURCE_DIR}/../LICENSE" MSG_AUTOSOURCE_LICENSE) + +# Set the output directory for all generated messages +set(CMSGCINTERFACE_DIR ${CMAKE_BINARY_DIR}/autoSource/cMsgCInterface) + +function(GenCMessages TARGET searchDir) + #-------------------------------------------------------------------------- + # Generates C++ definitions for each C message file named + # "{searchDir}/msgPayloadDefC/*Payload.h". Generated source files are + # appended to the TARGET. + # + # Implementation notes: + # This function was ported from `messaging/architecture/GenCMessages.py`. + # Originally, `GenCMessages.py` was called from the Conanfile prior to + # configuring CMake, but this meant that CMake could not track which + # C-messages changed, making incremental builds impossible. + # Instead, this function tracks dependencies directly in CMake. + #-------------------------------------------------------------------------- + + # Find all C message payloads + file(GLOB_RECURSE payloadFiles CONFIGURE_DEPENDS "${searchDir}/msgPayloadDefC/*Payload.h") + foreach(payloadFile ${payloadFiles}) + + # Compute the name of the payload structure and generated files + get_filename_component(msgFileName "${payloadFile}" NAME_WLE) + string(REGEX REPLACE "^(.+)Payload$" "\\1" structName "${msgFileName}") + set(headerFile ${CMSGCINTERFACE_DIR}/${structName}_C.h) + set(objectFile ${CMSGCINTERFACE_DIR}/${structName}_C.cpp) + file(RELATIVE_PATH relativePayloadFile ${CMSGCINTERFACE_DIR} ${payloadFile}) + + # Set the template variables + set(MSG_AUTOSOURCE_TYPE ${structName}) + set(MSG_AUTOSOURCE_HEADER ${relativePayloadFile}) + + # Generate the corresponding .h header and .cpp object file + configure_file(msg_C.h.in ${headerFile} @ONLY) + configure_file(msg_C.cpp.in ${objectFile} @ONLY) + + # Mark these files as GENERATED + set(generatedFiles ${objectFile} ${headerFile}) + set_source_files_properties(${generatedFiles} PROPERTIES GENERATED ON) + + # Add these files as sources for the TARGET + target_sources(${TARGET} PRIVATE ${generatedFiles}) + endforeach() +endfunction(GenCMessages) + +SET(LIB_DIR "${CMAKE_SOURCE_DIR}/simulation/architecture/messaging/cMsgCInterface/") + +# Determine the name of the parent directory of _GeneralModuleFiles (i.e. dynamics, environment, etc) +get_filename_component(PARENT_DIR ${LIB_DIR} DIRECTORY) # Relative to the source directory (e.g. simulation/power) +string(FIND ${PARENT_DIR} "/" DIR_NAME_IDX REVERSE) +math(EXPR DIR_NAME_IDX "${DIR_NAME_IDX} + 1")# "simulation/" +string(SUBSTRING ${PARENT_DIR} ${DIR_NAME_IDX} -1 PARENT_DIR_NAME) # Should be "power" + +# Set the library name (ex. dynamicsLib, environmentLib, etc) +set(LIB_NAME "cMsgCInterface") + +# Add Target +add_library(${LIB_NAME} STATIC ${BSK_FWK_FILES}) +if(NOT WIN32) + target_compile_options(${LIB_NAME} PUBLIC "-fPIC") +endif() + +# Generate and add C message source files to the library +GenCMessages(${LIB_NAME} "../..") +GenCMessages(${LIB_NAME} "${EXTERNAL_MODULES_PATH}") + +# Link all necessary libraries +#target_link_libraries(${LIB_NAME} ArchitectureUtilities) +target_link_libraries(${LIB_NAME} ${PYTHON3_MODULE}) +#target_link_libraries(${LIB_NAME} Eigen3::Eigen3) + +# define build location, IDE generation specifications +set_target_properties(${LIB_NAME} PROPERTIES FOLDER ${PARENT_DIR}) +set_target_properties(${LIB_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_BINARY_DIR}/Basilisk") + +# Define the location for the executable part of the library (.dll) files (Windows Only) +# https://cmake.org/cmake/help/v3.17/manual/cmake-buildsystem.7.html#output-artifacts for details +set_target_properties(${LIB_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY_RELEASE "${CMAKE_BINARY_DIR}/Basilisk") + +# Define the location of the import library file (.lib) needed for the .dll (Windows Only) +set_target_properties(${LIB_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(${LIB_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_BINARY_DIR}/Basilisk") diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.cpp.in b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.cpp.in new file mode 100644 index 0000000000..db4fd202cb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.cpp.in @@ -0,0 +1,156 @@ +/* @MSG_AUTOSOURCE_LICENSE@ */ + + +/* Implementation generated automatically through CMake. + * See architecture/messaging/cMsgCInterface/CMakeLists.txt +*/ + +#include "@MSG_AUTOSOURCE_TYPE@_C.h" +#include "architecture/messaging/messaging.h" +#include "architecture/utilities/bsk_Print.h" +#include + +//! C interface to subscribe to a message +void @MSG_AUTOSOURCE_TYPE@_C_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source) { + subscriber->payloadPointer = &(source->payload); + subscriber->headerPointer = &(source->header); + subscriber->header.isLinked = 1; // set input message as linked + subscriber->headerPointer->isLinked = 1; // set output message as linked +}; + +//! C interface to check if subscriber is indeed subscribed to a message (1: subscribed, 0: not subscribed) +int8_t @MSG_AUTOSOURCE_TYPE@_C_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source) { + + return ((subscriber->payloadPointer == &(source->payload))&&(subscriber->headerPointer == &(source->header))); + +}; + +//! C interface to claim authorship to a message +void @MSG_AUTOSOURCE_TYPE@_C_addAuthor(@MSG_AUTOSOURCE_TYPE@_C *coownerMsg, @MSG_AUTOSOURCE_TYPE@_C *targetMsg) { + coownerMsg->payloadPointer = &(targetMsg->payload); + coownerMsg->headerPointer = &(targetMsg->header); +}; + +//! C interface to initialize the module output message +void @MSG_AUTOSOURCE_TYPE@_C_init(@MSG_AUTOSOURCE_TYPE@_C *owner) { + //! check if the msg pointer is not assigned already. If not, then connect message to itself. + if (owner->payloadPointer == 0) { + @MSG_AUTOSOURCE_TYPE@_C_addAuthor(owner, owner); + } +}; + +//! C interface to write to a message +void @MSG_AUTOSOURCE_TYPE@_C_write(@MSG_AUTOSOURCE_TYPE@Payload *data, @MSG_AUTOSOURCE_TYPE@_C *destination, int64_t moduleID, uint64_t callTime) { + *destination->payloadPointer = *data; + destination->headerPointer->isWritten = 1; + destination->headerPointer->timeWritten = callTime; + destination->headerPointer->moduleID = moduleID; + return; +}; + +//! C interface to return a zero'd copy of the message payload +@MSG_AUTOSOURCE_TYPE@Payload @MSG_AUTOSOURCE_TYPE@_C_zeroMsgPayload() { + @MSG_AUTOSOURCE_TYPE@Payload zeroMsg; + memset(&zeroMsg, 0x0, sizeof(@MSG_AUTOSOURCE_TYPE@Payload)); + return zeroMsg; +}; + + +//! C interface to read to a message +@MSG_AUTOSOURCE_TYPE@Payload @MSG_AUTOSOURCE_TYPE@_C_read(@MSG_AUTOSOURCE_TYPE@_C *source) { + if (!source->headerPointer->isWritten) { + BSK_PRINT(MSG_ERROR,"In C input msg, you are trying to read an un-written message of type @MSG_AUTOSOURCE_TYPE@."); + } + //! ensure the current message container has a copy of a subscribed message. + //! Does nothing if the message is writing to itself + source->payload = *source->payloadPointer; + + return *source->payloadPointer; +}; + +//! C interface to see if this message container has been subscribed to +int @MSG_AUTOSOURCE_TYPE@_C_isLinked(@MSG_AUTOSOURCE_TYPE@_C *data) { + return (int) data->header.isLinked; +}; + +//! C interface to see if this message container ever been written to +int @MSG_AUTOSOURCE_TYPE@_C_isWritten(@MSG_AUTOSOURCE_TYPE@_C *data) { + if (data->header.isLinked) { + return (int) data->headerPointer->isWritten; + } + BSK_PRINT(MSG_ERROR,"In C input msg, you are checking if an unconnected msg of type @MSG_AUTOSOURCE_TYPE@ is written."); + return 0; +}; + +//! C interface to see if this message container ever been written to +uint64_t @MSG_AUTOSOURCE_TYPE@_C_timeWritten(@MSG_AUTOSOURCE_TYPE@_C *data) { + if (data->header.isLinked) { + return data->headerPointer->timeWritten; + } + BSK_PRINT(MSG_ERROR,"In C input msg, you are requesting the write time of an unconnected msg of type @MSG_AUTOSOURCE_TYPE@."); + return 0; +}; + +//! C interface to get the moduleID of who wrote the message +int64_t @MSG_AUTOSOURCE_TYPE@_C_moduleID(@MSG_AUTOSOURCE_TYPE@_C *data) { + if (!data->header.isLinked) { + BSK_PRINT(MSG_ERROR,"In C input msg, you are requesting moduleID of an unconnected msg of type @MSG_AUTOSOURCE_TYPE@."); + return 0; + } + if (!data->headerPointer->isWritten) { + BSK_PRINT(MSG_ERROR,"In C input msg, you are requesting moduleID of an unwritten msg of type @MSG_AUTOSOURCE_TYPE@."); + return 0; + } + return data->headerPointer->moduleID; +}; + +//! method description +void @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source){ + Message<@MSG_AUTOSOURCE_TYPE@Payload>* source_t = (Message<@MSG_AUTOSOURCE_TYPE@Payload>*) source; + MsgHeader *msgPtr; + subscriber->payloadPointer = source_t->subscribeRaw(&(msgPtr)); + subscriber->headerPointer = msgPtr; + subscriber->header.isLinked = 1; // set input message as linked + subscriber->headerPointer->isLinked = 1; // set output message as linked +}; + +//! This method takes an address location as an integer, reinterprets it to be an object, and calls the CPP subscribe method from there +void @MSG_AUTOSOURCE_TYPE@_addr_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr){ + void *source = reinterpret_cast (sourceAddr); + @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(subscriber, source); +}; + +//! Disconnects the subscriber from any connected source, noop if not connected to any message +void @MSG_AUTOSOURCE_TYPE@_unsubscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber){ + // if `subscriber->payloadPointer` is true for InMsg that have + // been connected and for OutMsg that have not been initialized + // `isSubscribedTo(subscriber, subscriber)` is true for OutMsg that + // have been initialized. + // Overall, this checks that it's an InMsg and it's connected. + if (subscriber->payloadPointer && !@MSG_AUTOSOURCE_TYPE@_C_isSubscribedTo(subscriber, subscriber)) + { + subscriber->payloadPointer = 0; + subscriber->headerPointer = 0; + subscriber->header.isLinked = 0; + } +}; + +//! Cpp interface to check if subscriber is indeed subscribed to a message (1: subscribed, 0: not subscribed) +int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source) { + + MsgHeader *dummyMsgPtr; + Message<@MSG_AUTOSOURCE_TYPE@Payload>* source_t = (Message<@MSG_AUTOSOURCE_TYPE@Payload>*) source; + int8_t firstCheck = (subscriber->payloadPointer == source_t->getMsgPointers(&(dummyMsgPtr))); + int8_t secondCheck = (subscriber->headerPointer == dummyMsgPtr); + + return (firstCheck && secondCheck); + +}; + +//! Cpp interface to check if subscriber is indeed subscribed to a message-address (1: subscribed, 0: not subscribed) +int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedToAddr(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr) { + + void *source = reinterpret_cast (sourceAddr); + return (@MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(subscriber, source)); + +}; diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.h.in b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.h.in new file mode 100644 index 0000000000..d2380121a4 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterface/msg_C.h.in @@ -0,0 +1,60 @@ +/* @MSG_AUTOSOURCE_LICENSE@ */ + + +/* Header generated automatically through CMake. + * See architecture/messaging/cMsgCInterface/CMakeLists.txt +*/ + +#ifndef @MSG_AUTOSOURCE_TYPE@_C_H +#define @MSG_AUTOSOURCE_TYPE@_C_H + +#include +#include "@MSG_AUTOSOURCE_HEADER@" +#include "architecture/messaging/msgHeader.h" + +//! structure definition +typedef struct { + MsgHeader header; //!< message header, zero'd on construction + @MSG_AUTOSOURCE_TYPE@Payload payload; //!< message copy, zero'd on construction + @MSG_AUTOSOURCE_TYPE@Payload *payloadPointer; //!< pointer to message + MsgHeader *headerPointer; //!< pointer to message header +} @MSG_AUTOSOURCE_TYPE@_C; + +#ifdef __cplusplus +extern "C" { +#endif + +void @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source); + +void @MSG_AUTOSOURCE_TYPE@_C_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source); + +void @MSG_AUTOSOURCE_TYPE@_addr_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr); + +void @MSG_AUTOSOURCE_TYPE@_unsubscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber); + +int8_t @MSG_AUTOSOURCE_TYPE@_C_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source); +int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source); +int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedToAddr(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr); + +void @MSG_AUTOSOURCE_TYPE@_C_addAuthor(@MSG_AUTOSOURCE_TYPE@_C *coowner, @MSG_AUTOSOURCE_TYPE@_C *data); + +void @MSG_AUTOSOURCE_TYPE@_C_init(@MSG_AUTOSOURCE_TYPE@_C *owner); + +int @MSG_AUTOSOURCE_TYPE@_C_isLinked(@MSG_AUTOSOURCE_TYPE@_C *data); + +int @MSG_AUTOSOURCE_TYPE@_C_isWritten(@MSG_AUTOSOURCE_TYPE@_C *data); + +uint64_t @MSG_AUTOSOURCE_TYPE@_C_timeWritten(@MSG_AUTOSOURCE_TYPE@_C *data); + +int64_t @MSG_AUTOSOURCE_TYPE@_C_moduleID(@MSG_AUTOSOURCE_TYPE@_C *data); + +void @MSG_AUTOSOURCE_TYPE@_C_write(@MSG_AUTOSOURCE_TYPE@Payload *data, @MSG_AUTOSOURCE_TYPE@_C *destination, int64_t moduleID, uint64_t callTime); + +@MSG_AUTOSOURCE_TYPE@Payload @MSG_AUTOSOURCE_TYPE@_C_read(@MSG_AUTOSOURCE_TYPE@_C *source); + +@MSG_AUTOSOURCE_TYPE@Payload @MSG_AUTOSOURCE_TYPE@_C_zeroMsgPayload(); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterfacePy/__init__.py b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterfacePy/__init__.py new file mode 100644 index 0000000000..5b9625d083 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/cMsgCInterfacePy/__init__.py @@ -0,0 +1,7 @@ +""" +Kept only for backwards-compatibility. +""" +from warnings import warn +warn("The use of `cMsgCInterfacePy` is deprecated and will be removed after 2025/08/07. Use `messaging` instead", DeprecationWarning) + +from Basilisk.architecture.messaging import * diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/messaging.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/messaging.h new file mode 100644 index 0000000000..600ad67bfb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/messaging.h @@ -0,0 +1,386 @@ +/* +ISC License + +Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + +Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +*/ +#ifndef MESSAGING_H +#define MESSAGING_H +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include +#include "architecture/messaging/msgHeader.h" +#include "architecture/utilities/bskLogging.h" +#include +#include + +/*! forward-declare sim message for use by read functor */ +template +class Message; + +template +class Recorder; + +/*! Read functors have read-only access to messages*/ +template +class ReadFunctor{ +private: + messageType* payloadPointer; //!< -- pointer to the incoming msg data + MsgHeader *headerPointer; //!< -- pointer to the incoming msg header + bool initialized; //!< -- flag indicating if the input message is connect to another message + +public: + //!< -- BSK Logging + BSKLogger bskLogger; //!< -- bsk logging instance + messageType zeroMsgPayload ={}; //!< -- zero'd copy of the message payload type + + + //! constructor + ReadFunctor() : initialized(false) {}; + + //! constructor + ReadFunctor(messageType* payloadPtr, MsgHeader *headerPtr) : payloadPointer(payloadPtr), headerPointer(headerPtr), initialized(true){}; + + //! constructor + const messageType& operator()(){ + if (!this->initialized) { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are trying to read an un-connected message of type %s\nThis program is about to self destruct.", typeid(var).name()); + } + return *this->payloadPointer; + + }; + + //! check if this msg has been connected to + bool isLinked(){return this->initialized;}; // something that can be checked so that uninitialized messages aren't read. + + //! check if the message has been ever written to + bool isWritten(){ + if (this->initialized) { + return this->headerPointer->isWritten; + } else { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are checking if an unconnected msg of type %s is written.", typeid(var).name()); + return false; + } + }; + + //! return the time at which the message was written + uint64_t timeWritten(){ + if (!this->initialized) { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting the write time of an unconnected msg of type %s.", typeid(var).name()); + return 0; + } + return this->headerPointer->timeWritten; + }; + + //! return the moduleID of who wrote wrote the message + int64_t moduleID(){ + if (!this->initialized) { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting moduleID of an unconnected msg of type %s.", typeid(var).name()); + return 0; + } + if (!this->headerPointer->isWritten) { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting moduleID of an unwritten msg of type %s.", typeid(var).name()); + return 0; + } + return this->headerPointer->moduleID; + }; + + //! subscribe to a C message + void subscribeToC(void* source){ + // this method works by knowing that the first member of a C message is the header. + this->headerPointer = (MsgHeader*) source; + + // advance the address to connect to C-wrapped message payload + // this assumes the header memory is aligned with 0 additional padding + MsgHeader* pt = this->headerPointer; + this->payloadPointer = (messageType *) (++pt); + + + // set flag that this input message is connected to another message + this->initialized = true; // set input message as linked + this->headerPointer->isLinked = 1; // set source output message as linked + }; + //! Subscribe to the message located at the sourceAddr in memory + void subscribeToAddr(uint64_t sourceAddr) + { + //!Cast a pointer at the sourceAddr and call the regular subscribe + Message *source = + reinterpret_cast *> (sourceAddr); + subscribeTo(source); + } + //! Subscribe to the C message located at the sourceAddr in memory + void subscribeToCAddr(uint64_t sourceAddr) + { + //! Cast a void* pointer and call the regular C-subscribe method + void *source = reinterpret_cast (sourceAddr); + subscribeToC(source); + } + //! Subscribe to a C++ message + void subscribeTo(Message *source){ + *this = source->addSubscriber(); + this->initialized = true; + }; + + //! Unsubscribe to the connected message, noop if no message was connected + void unsubscribe(){ + this->payloadPointer = nullptr; + this->headerPointer = nullptr; + this->initialized = false; + } + + //! Check if self has been subscribed to a C message + uint8_t isSubscribedToC(void *source){ + + int8_t firstCheck = (this->headerPointer == (MsgHeader*) source); + MsgHeader* pt = this->headerPointer; + int8_t secondCheck = (this->payloadPointer == (messageType *) (++pt)); + + return (this->initialized && firstCheck && secondCheck); + + }; + //! Check if self has been subscribed to a Cpp message + uint8_t isSubscribedTo(Message *source){ + + MsgHeader *dummyMsgPtr; + int8_t firstCheck = (this->payloadPointer == source->getMsgPointers(&(dummyMsgPtr))); + int8_t secondCheck = (this->headerPointer == dummyMsgPtr); + + return (this->initialized && firstCheck && secondCheck ); + + }; + //! Check if self has been subscribed to the message at sourceAddr + uint8_t isSubscribedToAddr(uint64_t sourceAddr) + { + //!Cast a pointer at the sourceAddr and call the regular is-subscribe + Message *source = reinterpret_cast *> (sourceAddr); + return(isSubscribedTo(source)); + } + //! Check if self has been subscribed to the message at sourceAddr + uint8_t isSubscribedToCAddr(uint64_t sourceAddr) + { + //!Cast a void* pointer at the sourceAddr and call the regular is-subscribe + void *source = reinterpret_cast (sourceAddr); + return(isSubscribedToC(source)); + } + + //! Recorder method description + Recorder recorder(uint64_t timeDiff = 0){return Recorder(this, timeDiff);} +}; + +/*! Write Functor */ +template +class WriteFunctor{ +private: + messageType* payloadPointer; //!< pointer to the message payload + MsgHeader* headerPointer; //!< pointer to the message header +public: + //! write functor constructor + WriteFunctor(){}; + //! write functor constructor + WriteFunctor(messageType* payloadPointer, MsgHeader *headerPointer) : payloadPointer(payloadPointer), headerPointer(headerPointer){}; + //! write functor constructor + void operator()(messageType *payload, int64_t moduleID, uint64_t callTime){ + *this->payloadPointer = *payload; + this->headerPointer->isWritten = 1; + this->headerPointer->timeWritten = callTime; + this->headerPointer->moduleID = moduleID; + return; + } +}; + +template +class Recorder; + +/*! + * base class template for bsk messages + */ +template +class Message{ +private: + messageType payload = {}; //!< struct defining message payload, zero'd on creation + MsgHeader header = {}; //!< struct defining the message header, zero'd on creation + ReadFunctor read = ReadFunctor(&payload, &header); //!< read functor instance +public: + //! write functor to this message + WriteFunctor write = WriteFunctor(&payload, &header); + //! -- request read rights. returns reference to class ``read`` variable + ReadFunctor addSubscriber(); + //! -- request write rights. + WriteFunctor addAuthor(); + //! for plain ole c modules + messageType* subscribeRaw(MsgHeader **msgPtr); + + //! for plain ole c modules + messageType* getMsgPointers(MsgHeader **msgPtr); + + //! Recorder object + Recorder recorder(uint64_t timeDiff = 0){return Recorder(this, timeDiff);} + + messageType zeroMsgPayload = {}; //!< zero'd copy of the message payload structure + + //! check if this msg has been connected to + bool isLinked(){return this->header.isLinked;}; + + //! Return the memory size of the payload, be careful about dynamically sized things + uint64_t getPayloadSize() {return sizeof(messageType);}; +}; + + +template +ReadFunctor Message::addSubscriber(){ + this->header.isLinked = 1; + return this->read; +} + +template +WriteFunctor Message::addAuthor(){ + return this->write; +} + +template +messageType* Message::subscribeRaw(MsgHeader **msgPtr){ + *msgPtr = &this->header; + this->header.isLinked = 1; + return &this->payload; +} + +template +messageType* Message::getMsgPointers(MsgHeader **msgPtr){ + *msgPtr = &this->header; + return &this->payload; +} + +/*! Keep a time history of messages accessible to users from python */ +template +class Recorder : public SysModel{ +public: + Recorder(){}; + //! -- Use this to record cpp messages + Recorder(Message* message, uint64_t timeDiff = 0){ + this->timeInterval = timeDiff; + this->readMessage = message->addSubscriber(); + this->ModelTag = "Rec:" + findMsgName(std::string(typeid(*message).name())); + } + //! -- Use this to record C messages + Recorder(void* message, uint64_t timeDiff = 0){ + this->timeInterval = timeDiff; + + MsgHeader* msgPt = (MsgHeader *) message; + MsgHeader *pt = msgPt; + messageType* payloadPointer; + payloadPointer = (messageType *) (++pt); + + this->readMessage = ReadFunctor(payloadPointer, msgPt); + this->ModelTag = "Rec:"; + Message tempMsg; + std::string msgName = typeid(tempMsg).name(); + this->ModelTag += findMsgName(msgName); + } + //! -- Use this to keep track of what someone is reading + Recorder(ReadFunctor* messageReader, uint64_t timeDiff = 0){ + this->timeInterval = timeDiff; + this->readMessage = *messageReader; + if (!messageReader->isLinked()) { + messageType var; + bskLogger.bskLog(BSK_ERROR, "In C++ read functor, you are requesting to record an un-connected input message of type %s.", typeid(var).name()); + } + this->ModelTag = "Rec:" + findMsgName(std::string(typeid(*messageReader).name())); + } + ~Recorder(){}; + + //! -- self initialization + void SelfInit(){}; + //! -- cross initialization + void IntegratedInit(){}; + //! -- Read and record the message + void UpdateState(uint64_t CurrentSimNanos){ + if (CurrentSimNanos >= this->nextUpdateTime) { + // Log warning if message is invalid but don't change behavior + if (!this->readMessage.isLinked() || !this->readMessage.isWritten()) { + messageType var; + bskLogger.bskLog(BSK_WARNING, "Recording message of type %s that is not properly initialized or written", typeid(var).name()); + } + + // Record the message + this->msgRecordTimes.push_back(CurrentSimNanos); + this->msgWrittenTimes.push_back(this->readMessage.timeWritten()); + this->msgRecord.push_back(this->readMessage()); + this->nextUpdateTime += this->timeInterval; + } + }; + //! Reset method + void Reset(uint64_t CurrentSimNanos){ + this->msgRecord.clear(); //!< -- Can only reset to 0 for now + this->msgRecordTimes.clear(); + this->msgWrittenTimes.clear(); + this->nextUpdateTime = CurrentSimNanos; + }; + //! time recorded method + std::vector& times(){return this->msgRecordTimes;} + //! time written method + std::vector& timesWritten(){return this->msgWrittenTimes;} + //! record method + std::vector& record(){return this->msgRecord;}; + //! size of the record so far + size_t size(){return this->record().size();} + + //! determine message name + std::string findMsgName(std::string msgName) { + size_t locMsg = msgName.find("Payload"); + if (locMsg != std::string::npos) { + msgName.erase(locMsg, std::string::npos); + } + locMsg = msgName.find("Message"); + if (locMsg != std::string::npos) { + msgName.replace(locMsg, 7, ""); + } + for (int c = 0; c<10; c++) { + locMsg = msgName.rfind(std::to_string(c)); + if (locMsg != std::string::npos) { + msgName.erase(0, locMsg+1); + } + } + return msgName; + }; + + //! clear the recorded messages, i.e. purge the history + void clear(){ + this->msgRecord.clear(); + this->msgRecordTimes.clear(); + this->msgWrittenTimes.clear(); + }; + + BSKLogger bskLogger; //!< -- BSK Logging + + //! method to update the minimum time interval before recording the next message + void updateTimeInterval(uint64_t timeDiff) { + this->timeInterval = timeDiff; + }; + +private: + std::vector msgRecord; //!< vector of recorded messages + std::vector msgRecordTimes; //!< vector of times at which messages are recorded + std::vector msgWrittenTimes; //!< vector of times at which messages are written + uint64_t nextUpdateTime = 0; //!< [ns] earliest time at which the msg is recorded again + uint64_t timeInterval; //!< [ns] recording time intervale + +private: + ReadFunctor readMessage; //!< method description +}; + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in new file mode 100644 index 0000000000..516997318e --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in @@ -0,0 +1,65 @@ +%{{ +#include "cMsgCInterface/{type}_C.h" +#include "architecture/messaging/messaging.h" +%}} +%include "cMsgCInterface/{type}_C.h" +%include "architecture/messaging/msgHeader.h" +typedef struct {type}; +%extend {type}_C {{ + Recorder<{type}Payload> recorder(uint64_t timeDiff = 0) {{ + self->header.isLinked = 1; + return Recorder<{type}Payload>{{static_cast(self), timeDiff}}; + }} + + %pythoncode %{{ + + def subscribeTo(self, source): + """subscribe to another message source""" + from Basilisk.architecture.messaging import {type} + if type(source) == type(self): + {type}_C_subscribe(self, source) + elif type(source) == {type}: + {type}_cpp_subscribe(self, source) + elif type(source) == int: #Note that is assumes it is a uint64_t address in memory + {type}_addr_subscribe(self, source) + else: + raise Exception('tried to subscribe {type} to another message type') + + def unsubscribe(self): + """Unsubscribe to the connected message, noop if no message was connected""" + {type}_unsubscribe(self) + + def isSubscribedTo(self, source): + """check if self is subscribed to another message source""" + from Basilisk.architecture.messaging import {type} + if type(source) == type(self): + return ({type}_C_isSubscribedTo(self, source)) + elif type(source) == {type}: + return ({type}_cpp_isSubscribedTo(self, source)) + elif type(source) == int: #Note that this assumes it is a uint64_t address location in memory + return ({type}_cpp_isSubscribedToAddr(self, source)) + else: + return 0 + + + def init(self, data=None): + """returns a Msg copy connected to itself""" + {type}_C_addAuthor(self, self) + if data: + {type}_C_write(data, self, -1, 0) + return self + + def write(self, payload, time=0, moduleID=0): + """write the message payload. + The 2nd argument is time in nanoseconds. It is optional and defaults to 0. + The 3rd argument is the module ID which defaults to 0. + """ + {type}_C_addAuthor(self, self) + {type}_C_write(payload, self, moduleID, time) # msgs written in Python have 0 module ID + return self + + def read(self): + """read the message payload.""" + return {type}_C_read(self) + %}} +}}; diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generatePackageInit.py b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generatePackageInit.py new file mode 100644 index 0000000000..44ab82c79a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generatePackageInit.py @@ -0,0 +1,26 @@ +import os +import sys + +path = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(path + '/../../../../../Basilisk/src/architecture/messaging/msgAutoSource') + +if __name__ == "__main__": + moduleOutputPath = sys.argv[1] + isExist = os.path.exists(moduleOutputPath) + if not isExist: + os.makedirs(moduleOutputPath, exist_ok=True) + mainImportFid = open(moduleOutputPath + '/__init__.py', 'w') + for i in range(2, len(sys.argv)): + headerInputPath = sys.argv[i] + for filePre in os.listdir(headerInputPath): + if(filePre.endswith(".h") or filePre.endswith(".hpp")): + className = os.path.splitext(filePre)[0] + msgName = className.split('Payload')[0] + mainImportFid.write('from Basilisk.architecture.messaging.' + className + ' import *\n') + mainImportFid.close() + setOldPath = moduleOutputPath.split('messaging')[0] + '/cMsgCInterfacePy' + + # XXX: Disabled: don't make a symbolic link here, because when we build a + # Python wheel, the contents of the folder get copied, effectively doubling + # the size. Instead, see the new messaging/cMsgCInterface directory. + # os.symlink(moduleOutputPath, setOldPath) diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generateSWIGModules.py b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generateSWIGModules.py new file mode 100644 index 0000000000..cfdcfdbe08 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/generateSWIGModules.py @@ -0,0 +1,357 @@ +import sys +import re +from typing import Optional, Tuple, Callable, List +import xml.etree.ElementTree as ET + +# Maps numeric C/C++ types to numpy dtype enum names +C_TYPE_TO_NPY_ENUM = { + # Signed integers + "int8_t": "NPY_INT8", + + "short": "NPY_INT16", + "short int": "NPY_INT16", + "signed short": "NPY_INT16", + "signed short int": "NPY_INT16", + "int16_t": "NPY_INT16", + + "int": "NPY_INT32", + "signed int": "NPY_INT32", + "int32_t": "NPY_INT32", + + "long": "NPY_LONG", + "long int": "NPY_LONG", + "signed long": "NPY_LONG", + "signed long int": "NPY_LONG", + + "long long": "NPY_INT64", + "long long int": "NPY_INT64", + "signed long long": "NPY_INT64", + "signed long long int": "NPY_INT64", + "int64_t": "NPY_INT64", + + # Unsigned integers + "uint8_t": "NPY_UINT8", + + "unsigned short": "NPY_UINT16", + "unsigned short int": "NPY_UINT16", + "uint16_t": "NPY_UINT16", + + "unsigned": "NPY_UINT32", + "unsigned int": "NPY_UINT32", + "uint32_t": "NPY_UINT32", + + "unsigned long": "NPY_ULONG", + "unsigned long int": "NPY_ULONG", + + "unsigned long long": "NPY_UINT64", + "unsigned long long int": "NPY_UINT64", + "uint64_t": "NPY_UINT64", + + # Platform-size signed/unsigned integers + "intptr_t": "NPY_INTP", + "uintptr_t": "NPY_UINTP", + "ptrdiff_t": "NPY_INTP", + "ssize_t": "NPY_INTP", + "size_t": "NPY_UINTP", + + # Floating point types + "float16": "NPY_FLOAT16", + "half": "NPY_FLOAT16", + "float": "NPY_FLOAT32", + "float32": "NPY_FLOAT32", + "double": "NPY_FLOAT64", + "float64": "NPY_FLOAT64", + "long double": "NPY_LONGDOUBLE", +} + +def cleanSwigType(cppType: str) -> str: + """ + Cleans and normalizes a C++ template type string extracted from SWIG-generated XML, + restoring valid syntax and formatting for use in generated C++ or Python bindings. + + This function removes redundant parentheses and correctly reconstructs: + - Nested template structures (e.g., std::vector>) + - Function signatures within std::function (e.g., std::function) + - Multi-word C++ types (e.g., unsigned long long, const float&) + - Pointer and reference types (e.g., T*, T&, T&&) + - Fixed-size C-style arrays (e.g., int[10], const float(&)[3]) + + Parameters: + cppType (str): The raw type string from SWIG XML, possibly containing spurious + parentheses and HTML-escaped characters (<, >). + + Returns: + str: A cleaned-up and properly formatted C++ type string. + """ + import html + import re + + # Decode any XML-escaped characters (e.g., < to <) + cppType = html.unescape(cppType) + + # Tokenize while preserving C++ symbols + tokens = re.findall(r'\w+::|::|\w+|<|>|,|\(|\)|\[|\]|\*|&|&&|\S', cppType) + + def stripParensIfWrapped(typeStr: str) -> str: + """ + Remove surrounding parentheses if they are redundant and balanced. + + Args: + typeStr: A potentially parenthesized type string + + Returns: + str: The string with parentheses removed if they are unnecessary + """ + typeStr = typeStr.strip() + if typeStr.startswith('(') and typeStr.endswith(')'): + depth = 0 + for i, ch in enumerate(typeStr): + if ch == '(': + depth += 1 + elif ch == ')': + depth -= 1 + # If we close early, parentheses are internal – keep them + if depth == 0 and i != len(typeStr) - 1: + return typeStr + return typeStr[1:-1].strip() + return typeStr + + def isWord(token: str) -> bool: + """Return True if the token is a C++ identifier (e.g., 'int', 'const')""" + return re.fullmatch(r'\w+', token) is not None + + def parseType(index: int) -> Tuple[str, int]: + """ + Recursively parses a C++ type expression from tokens. + + Args: + index: Current token index + + Returns: + tuple: + str: parsed type string + int: next token index + """ + parts = [] + while index < len(tokens): + token = tokens[index] + + if token == '<': + index += 1 + inner, index = parseBracketBlock(index, '<', '>', parseType) + parts.append(f"<{inner}>") + + elif token == '(': + index += 1 + inner, index = parseBracketBlock(index, '(', ')', parseType) + parts.append(f"({inner})") + + elif token in [')', '>']: + break + + elif token == ',': + index += 1 + break + + else: + # Add space only between adjacent words + if parts: + prev = parts[-1] + if isWord(prev) and isWord(token): + parts.append(' ') + parts.append(token) + index += 1 + + return ''.join(parts), index + + def parseBracketBlock(index: int, openSym: str, closeSym: str, + parseFunc: Callable[[int], Tuple[str, int]]) -> Tuple[str, int]: + """ + Parses a bracketed block like <...> or (...) or [...] with nested content. + + Args: + index: Current token index (after the opening symbol) + openSym: Opening symbol, e.g. '<' + closeSym: Closing symbol, e.g. '>' + parseFunc: Recursive parse function (e.g., parseType) + + Returns: + tuple: + str: parsed type string + int: next token in + """ + items: List[str] = [] + while index < len(tokens): + if tokens[index] == closeSym: + cleaned = [stripParensIfWrapped(i) for i in items] + return ', '.join(cleaned), index + 1 + elif tokens[index] == ',': + index += 1 # skip separator + else: + item, index = parseFunc(index) + items.append(item) + return ', '.join(items), index + + cleaned, _ = parseType(0) + return cleaned.strip() + +def parseSwigDecl(decl: str): + """ + Parses a SWIG `decl` string and converts it into components of a C++ declarator. + + SWIG represents C++ type modifiers (pointers, references, arrays, functions) using a dot-delimited + postfix syntax attached to the base type. This function interprets those tokens and produces + the corresponding C++-style declarator suffix elements. + + SWIG encoding conventions: + - `p.` : pointer (adds a `*`) + - `r.` : reference (adds a `&`) + - `a(N).` : fixed-size array of N elements (adds `[N]`) + - `f().` : function type (currently ignored by this parser) + + Modifier order is postfix but applies from inside-out: + Example: `a(3).p.` means "pointer to array of 3" -> `*[3]` + `p.a(3).` means "array of 3 pointers" -> `[3]*` + + Parameters: + decl (str): The SWIG-encoded declarator string. Examples: + - "p." -> pointer + - "r." -> reference + - "a(5)." -> array of 5 elements + - "a(2).a(2).p.p." -> pointer to pointer to 2x2 array + + Returns: + tuple: + pointerPart (str): a string of '*' characters for pointer depth (e.g., '**') + referencePart (str): '&' if this is a reference, else '' + arrayParts (list[str]): array dimensions as strings, ordered from outermost to innermost. + For example, ['2', '3'] for `a(2).a(3).` means `[2][3]`. + + Example: + >>> parseSwigDecl("a(3).p.") + ('*', '', ['3']) # pointer to array of 3 -> '*[3]' + + >>> parseSwigDecl("p.a(3).") + ('*', '', ['3']) # array of 3 pointers -> '[3]*' + + >>> parseSwigDecl("p.p.a(2).a(2).r.") + ('**', '&', ['2', '2']) # reference to pointer to pointer to 2x2 array + """ + pointerPart = '' + referencePart = '' + arrayParts = [] + + # Match each declarator component + tokens = re.findall(r'(a\([^)]+\)|p\.|r\.|f\(\)\.)', decl) + + for token in tokens: + if token.startswith('a('): + # Array: a(N) -> N + size = re.match(r'a\(([^)]+)\)', token).group(1) + arrayParts.append(size) + elif token == 'p.': + pointerPart += '*' + elif token == 'r.': + referencePart = '&' # References appear after pointer + # Note: We skip f(). (function pointer) for now + + return pointerPart, referencePart, arrayParts + +def parseSwigXml(xmlPath: str, targetStructName: str, cpp: bool, recorderPropertyRollback: bool): + """ + Parses a SWIG-generated XML file and emits RECORDER_PROPERTY macros + for all struct/class member fields. + + Parameters: + xmlPath (str): Path to the XML file generated with `swig -xml` + targetStructName (str): Name of the payload (e.g. `MTBMsgPayload`). + cpp (bool): whether this is a C++ payload (we need to be extra + careful with the type since it might be templated.) + recorderPropertyRollback (bool): If true, non-numeric properties + are not given a special RECORDER_PROPERTY macro, thus recovering + the legacy output format for these fields. + + Returns: + str: macro declarations to be pasted into `msgInterfacePy.i.in` + """ + result = "" + + tree = ET.parse(xmlPath) + root = tree.getroot() + + # Iterate over all classes (C++ classes and structs) + for classNode in root.findall(".//class"): + classAttrs = extractAttributeMap(classNode.find("attributelist")) + structName = classAttrs.get("name") or classAttrs.get("tdname") + if structName != targetStructName: + continue + + # Each field appears as a child with ismember="1" + for cdecl in classNode.findall("cdecl"): + fieldAttrs = extractAttributeMap(cdecl.find("attributelist")) + if fieldAttrs.get("ismember") != "1": + continue # Skip non-member declarations + + fieldName = fieldAttrs.get("name") + baseType = fieldAttrs.get("type") + decl = fieldAttrs.get("decl", "") + + if not fieldName or not baseType: + continue + + if cpp: + baseType = cleanSwigType(baseType) + + typePointerPart, typeReferencePart, typeArrayParts = parseSwigDecl(decl) + typeWithPointerRef = f"{baseType}{typePointerPart}{typeReferencePart}" + + if typeWithPointerRef in C_TYPE_TO_NPY_ENUM and len(typeArrayParts) < 3: + npyType = C_TYPE_TO_NPY_ENUM[typeWithPointerRef] + macroName = f"RECORDER_PROPERTY_NUMERIC_{len(typeArrayParts)}" + result += f"{macroName}({targetStructName}, {fieldName}, {typeWithPointerRef}, {npyType});\n" + elif not recorderPropertyRollback: + fullType = f"{typeWithPointerRef}{''.join(f'[{i}]' for i in typeArrayParts)}" + result += f"RECORDER_PROPERTY({targetStructName}, {fieldName}, ({fullType}));\n" + + return result + +def extractAttributeMap(attributeListNode: Optional[ET.Element]): + """ + Converts an XML node into a Python dict. + + Parameters: + attributeListNode (Element | None): The element + + Returns: + dict[str, str]: Mapping of attribute name -> value + """ + if attributeListNode is None: + return {} + return { + attr.attrib['name']: attr.attrib['value'] + for attr in attributeListNode.findall("attribute") + if 'name' in attr.attrib and 'value' in attr.attrib + } + +if __name__ == "__main__": + moduleOutputPath = sys.argv[1] + headerinputPath = sys.argv[2] + payloadTypeName = sys.argv[3] + structType = payloadTypeName.split('Payload')[0] + baseDir = sys.argv[4] + generateCInfo = sys.argv[5] == 'True' + xmlWrapPath = sys.argv[6] + recorderPropertyRollback = bool(int(sys.argv[7])) + + with open('msgInterfacePy.i.in', 'r') as f: + swigTemplateData = f.read() + + with open('cMsgCInterfacePy.i.in', 'r') as f: + swigCTemplateData = f.read() + + extraContent = parseSwigXml(xmlWrapPath, payloadTypeName, not generateCInfo, recorderPropertyRollback) + + with open(moduleOutputPath, 'w') as moduleFileOut: + moduleFileOut.write(swigTemplateData.format(type=structType, baseDir=baseDir, extraContent=extraContent)) + if(generateCInfo): + moduleFileOut.write(swigCTemplateData.format(type=structType)) diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/msgInterfacePy.i.in b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/msgInterfacePy.i.in new file mode 100644 index 0000000000..e7be810f2a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgAutoSource/msgInterfacePy.i.in @@ -0,0 +1,56 @@ +%module(threads="1") {type}Payload +%{{ + #include "{baseDir}/{type}Payload.h" + #include "architecture/messaging/messaging.h" + #include "architecture/msgPayloadDefC/ReconfigBurnInfoMsgPayload.h" + #include "architecture/msgPayloadDefC/RWConfigElementMsgPayload.h" + #include "architecture/msgPayloadDefC/THRConfigMsgPayload.h" + #include "simulation/dynamics/reactionWheels/reactionWheelSupport.h" + #include + #include + #include +%}} +%include "messaging/newMessaging.ih" + +%include "std_vector.i" +%include "std_string.i" +%include "_GeneralModuleFiles/swig_eigen.i" +%include "_GeneralModuleFiles/swig_conly_data.i" +%include "stdint.i" +%template(TimeVector) std::vector>; +%template(DoubleVector) std::vector>; +%template(StringVector) std::vector>; + +%include "architecture/utilities/macroDefinitions.h" +%include "fswAlgorithms/fswUtilities/fswDefinitions.h" +%include "simulation/dynamics/reactionWheels/reactionWheelSupport.h" +ARRAYASLIST(FSWdeviceAvailability, PyLong_FromLong, PyLong_AsLong) +STRUCTASLIST(CSSUnitConfigMsgPayload) +STRUCTASLIST(AccPktDataMsgPayload) +STRUCTASLIST(RWConfigElementMsgPayload) +STRUCTASLIST(CSSArraySensorMsgPayload) + +%include "messaging/messaging.h" +%include "_GeneralModuleFiles/sys_model.h" + +%array_functions(THRConfigMsgPayload, ThrustConfigArray); +%array_functions(RWConfigElementMsgPayload, RWConfigArray); +%array_functions(ReconfigBurnInfoMsgPayload, ReconfigBurnArray); + +%rename(__subscribe_to) subscribeTo; // we want the users to have a unified "subscribeTo" interface +%rename(__subscribe_to_C) subscribeToC; // we want the users to have a unified "subscribeTo" interface +%rename(__is_subscribed_to) isSubscribedTo; // we want the users to have a unified "isSubscribedTo" interface +%rename(__is_subscribed_to_C) isSubscribedToC; // we want the users to have a unified "isSubscribedTo" interface +%rename(__time_vector) times; // It's not really useful to give the user back a time vector +%rename(__timeWritten_vector) timesWritten; +%rename(__record_vector) record; + +%pythoncode %{{ +import numpy as np +%}}; +%include "{baseDir}/{type}Payload.h" +{extraContent} +INSTANTIATE_TEMPLATES({type}, {type}Payload, {baseDir}) +%template({type}OutMsgsVector) std::vector, std::allocator> >; +%template({type}OutMsgsPtrVector) std::vector*, std::allocator*> >; +%template({type}InMsgsVector) std::vector, std::allocator> >; diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgHeader.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgHeader.h new file mode 100644 index 0000000000..8dcfafac34 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/msgHeader.h @@ -0,0 +1,30 @@ +/* +ISC License + +Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + +Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +*/ + +#ifndef msgHeader_h +#define msgHeader_h + +/*! @brief message system 2 header information structure */ +typedef struct { + int64_t isLinked; //!< flag if the message has is connected to another message + int64_t isWritten; //!< flag if the message conntent has ever been written + uint64_t timeWritten; //!< [ns] time the message was written + int64_t moduleID; //!< ID of the module who wrote the message, negative value for Python module, non-negative for C/C++ modules +}MsgHeader; + +#endif /* msgHeader_h */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/newMessaging.ih b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/newMessaging.ih new file mode 100644 index 0000000000..1142818c93 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/messaging/newMessaging.ih @@ -0,0 +1,444 @@ +/* +Copyright (c) 2020, Autonomous Vehicle Systems Lab, Univeristy of Colorado at Boulder + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + +%pythoncode %{ + import numpy as np + import inspect + import pprint + from Basilisk.utilities import deprecated +%}; +%{ +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/messaging/messaging.h" +#include "architecture/utilities/macroDefinitions.h" +#include "fswAlgorithms/fswUtilities/fswDefinitions.h" +#include +%} + +%{ +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include +%} + +%init %{ +import_array(); // Required to initialize NumPy C API +%} + +%define INSTANTIATE_TEMPLATES(messageType, messageTypePayload, folder) +%{ +#include "folder/messageTypePayload.h" +%} +%include "folder/messageTypePayload.h" + +%feature("shadow") messageTypePayload() +{ +def __init__(self, **fields): + """Constructs a new payload, zero'd by default. + + Keyword arguments can be passed to initialize the fields of + this payload. + """ + _ ## messageTypePayload.messageTypePayload ## _swiginit(self, _ ## messageTypePayload.new_ ## messageTypePayload()) + for field, value in fields.items(): + setattr(self, field, value) +} + +%extend messageTypePayload { +%pythoncode %{ + @classmethod + def __fields__(cls): + """Returns a list with all the payload's fields.""" + return tuple( + name + for name, _ in inspect.getmembers( + cls, + lambda v: isinstance(v, property) and v is not cls.thisown + ) + ) + + def __repr__(self): + """Formats the content of the payload""" + selfType = type(self) + fields = [ + f"{name}={getattr(self, name)!r}" + for name in selfType.__fields__() + ] + return selfType.__qualname__ + "(" + ', '.join(fields) + ")" +%} +} + +%pythoncode %{ +def _pprint_ ## messageTypePayload(pprinter, object, stream, indent, allowance, context, level): + """Formatter function to support pretty-printing (with ``pprint``).""" + selfType = type(object) + cls_name = selfType.__qualname__ + indent += len(cls_name) + 1 + fields = [ + (name, getattr(object, name)) + for name in selfType.__fields__() + ] + stream.write(cls_name + '(') + if fields: + delimnl = ',\n' + ' ' * indent + last_index = len(fields) - 1 + for i, (key, ent) in enumerate(fields): + last = i == last_index + stream.write(key) + stream.write('=') + pprinter._format(ent, stream, indent + len(key) + 1, + allowance if last else 1, + context, level) + if not last: + stream.write(delimnl) + + stream.write(')') + +pprint.PrettyPrinter._dispatch[messageTypePayload.__repr__] = _pprint_ ## messageTypePayload +%} + +%template(messageType ## Reader) ReadFunctor; +%extend ReadFunctor { + %pythoncode %{ + def subscribeTo(self, source): + if type(source) == messageType: + self.__subscribe_to(source) + return + + try: + from Basilisk.architecture.messaging.messageType ## Payload import messageType ## _C + if type(source) == messageType ## _C: + self.__subscribe_to_C(source) + return + except ImportError: + pass + + raise Exception('tried to subscribe ReadFunctor to output message type' + + str(type(source))) + + + def isSubscribedTo(self, source): + if type(source) == messageType: + return self.__is_subscribed_to(source) + + try: + from Basilisk.architecture.messaging.messageType ## Payload import messageType ## _C + except ImportError: + return 0 + + if type(source) == messageType ## _C: + return self.__is_subscribed_to_C(source) + else: + return 0 + %} +}; + +%template(messageType ## Writer) WriteFunctor; + +%template(messageType) Message; +%extend Message{ + %pythoncode %{ + def write(self, payload, time=0, moduleID=0): + """Write the message payload. + The 2nd argument is time in nanoseconds. It is optional and defaults to 0. + The 3rd argument is the module ID which defaults to 0. + """ + writeMsg = self.addAuthor() + writeMsg(payload, moduleID, time) # msgs written in python have 0 module ID + return self + + def read(self): + """Read the message payload.""" + readMsg = self.addSubscriber() + return readMsg() + %} +}; + +%template(messageType ## Recorder) Recorder; +%extend Recorder { + %pythoncode %{ + def times(self): + return np.array(self.__time_vector()) + + def timesWritten(self): + return np.array(self.__timeWritten_vector()) + + def explore_and_find_subattr(self, attr, attr_name, outer_content=None): + if isinstance(attr, (int, float, str, bool)): + # Short circuit the most common attribute types + content = attr + elif "method" in str(type(attr)): + # The attribute is a method, nothing to do here + content = None + elif isinstance(attr, list): + # The attribute is a list of yet to be determined types + if len(attr) > 0: + if "Basilisk" in str(type(attr[0])): + content = outer_content if outer_content else dict() + # The attribute is a list of swigged BSK objects + for el, k in zip(attr, range(len(attr))): + self.explore_and_find_subattr(el, attr_name + "[" + str(k) + "]", content) + else: + # The attribute is a list of common types + content = attr + elif "Basilisk" in str(type(attr)): + # The attribute is a swigged BSK object + # Check to see if the object is a vector and pull out the data + if "Vector" in str(type(attr)) or "vector" in str(type(attr)): + content = [] + for data in attr: + content.append(data) + else: + content = outer_content if outer_content else dict() + for subattr_name in dir(attr): + if not subattr_name.startswith("__") and subattr_name != "this": + self.explore_and_find_subattr(getattr(attr, subattr_name), + attr_name + "." + subattr_name, + content) + else: + # The attribute has a common type + content = attr + + if outer_content is None: + return content + else: + # Dont report methods + if content is not None: + outer_content[attr_name] = content + + # Cache if we can use the fast attr lookup, assuming that attr types are static + simple_attribute_map = {} + + # This __getattr__ is written in message.i. + # It lets us return message struct attribute record as lists for plotting, etc. + def __getattr__(self, name): + data = self.__record_vector() + data_record = [] + for rec in data.get_all(): + # Allow us to skip explore_and_find_subattr when we dont have a complex attr + if name not in self.simple_attribute_map: + simple_content = rec.__getattribute__(name) + content = self.explore_and_find_subattr(simple_content, name) + if content == simple_content: + self.simple_attribute_map[name] = True + else: + deprecated.deprecationWarn( + self.__getattr__.__module__ + "." + self.__getattr__.__qualname__, + "2026/07/27", + "Compiled Basilisk with '--recorderPropertyRollback True' and using " + "deprecated output format for non-numeric payload recorder attribute. Don't use " + "'--recorderPropertyRollback' and update the code to handle the new output format.") + self.simple_attribute_map[name] = False + else: + if self.simple_attribute_map[name]: + content = rec.__getattribute__(name) + else: + content = self.explore_and_find_subattr( + rec.__getattribute__(name), name + ) + + # One-element list of BSK objects or raw dicts + if isinstance(content, dict) and len(content) == 1 and "." not in str(list(content.keys())[0]): + data_record.append(next(iter(content.values()))) + else: + data_record.append(content) + + return np.array(data_record) + + def record(self): + return self.__record_vector + %} +}; + +typedef struct messageType; + +%template(messageType ## PayloadVector) std::vector>; +%extend std::vector>{ + std::vector get_all() { + return *self; + } + + %pythoncode %{ + # This __getattr__ is written in message.i. + # It lets us return message struct attribute record as lists for plotting, etc. + def __getattr__(self, name): + data_record = [] + for rec in self.get_all(): + data_record.append(rec.__getattribute__(name)) + return np.array(data_record) + %} +}; +%enddef + +%define RECORDER_PROPERTY(payloadType, fieldName, fieldType) +/* + * Exposes a general field (e.g. string, user-defined type) as a Python property. + * Creates a Python list and converts it to a NumPy array on the Python side. + * Slower than the numeric variants, but supports arbitrary types via SWIG typemaps. + */ +%typemap(out) Recorder* Recorder::_ ## fieldName ## _list +{ + size_t N = $1->record().size(); + $result = PyList_New(N); + for (size_t recordIndex = 0; recordIndex < N; ++recordIndex) + { + PyObject * resultItem; + auto& _swigResultHolder = $1->record().at(recordIndex). ## fieldName; + { + auto& result = _swigResultHolder; + $typemap(out, fieldType, result=resultItem); + } + PyList_SET_ITEM($result, recordIndex, resultItem); // steals ref + } + if (PyErr_Occurred()) SWIG_fail; +} + +%extend Recorder { + Recorder* _ ## fieldName ## _list() { + return $self; + } + + %pythoncode %{ + @property + def fieldName(self): + return np.array(self._ ## fieldName ## _list()) + %} +}; + +%enddef + +%define RECORDER_PROPERTY_NUMERIC_0(payloadType, fieldName, fieldType, npyTypeNum) +/* + * Exposes a scalar numeric field (e.g. double, int) as a NumPy array with shape [N]. + */ +%typemap(out) Recorder* Recorder::_ ## fieldName ## _array +{ + size_t N = $1->record().size(); + if (N == 0) { + npy_intp dims[1] = {0}; + $result = PyArray_SimpleNew(1, dims, NPY_FLOAT64); + } else { + npy_intp dims[1] = {static_cast(N)}; + $result = PyArray_SimpleNew(1, dims, npyTypeNum); + if (!$result) SWIG_fail; + + for (size_t i = 0; i < N; ++i ) { + void* ptr = PyArray_GETPTR1((PyArrayObject*)$result, i); + *reinterpret_cast(ptr) = $1->record().at(i). ## fieldName; + } + } + if (PyErr_Occurred()) SWIG_fail; +} + +%extend Recorder { + Recorder* _ ## fieldName ## _array() { + return $self; + } + + %pythoncode %{ + @property + def fieldName(self): + return self._ ## fieldName ## _array() + %} +}; + +%enddef + +%define RECORDER_PROPERTY_NUMERIC_1(payloadType, fieldName, fieldType, npyTypeNum) +%typemap(out) Recorder* Recorder::_ ## fieldName ## _array +{ + size_t N = $1->record().size(); + if (N == 0) { + npy_intp dims[1] = {0}; + $result = PyArray_SimpleNew(1, dims, NPY_FLOAT64); + } else { + /* Derive dimensions from compiled type */ + auto &first = $1->record().at(0).fieldName; + const size_t d1 = sizeof(first) / sizeof(first[0]); + const size_t elemsPerRecord = d1; + const size_t bytesPerRecord = elemsPerRecord * sizeof(fieldType); + + npy_intp dims[2] = {static_cast(N), (npy_intp) d1}; + $result = PyArray_SimpleNew(2, dims, npyTypeNum); + if (!$result) SWIG_fail; + + fieldType* dest = static_cast(PyArray_DATA((PyArrayObject*)$result)); + + for (size_t i = 0; i < N; ++i) { + const fieldType* src = &($1->record().at(i).fieldName[0]); + std::memcpy(dest + i * elemsPerRecord, src, bytesPerRecord); + } + } + if (PyErr_Occurred()) SWIG_fail; +} + +%extend Recorder { + Recorder* _ ## fieldName ## _array() { + return $self; + } + + %pythoncode %{ + @property + def fieldName(self): + return self._ ## fieldName ## _array() + %} +}; + +%enddef + +%define RECORDER_PROPERTY_NUMERIC_2(payloadType, fieldName, fieldType, npyTypeNum) +%typemap(out) Recorder* Recorder::_ ## fieldName ## _array +{ + size_t N = $1->record().size(); + if (N == 0) { + npy_intp dims[1] = {0}; + $result = PyArray_SimpleNew(1, dims, NPY_FLOAT64); + } else { + /* Derive dimensions from compiled type */ + auto &first = $1->record().at(0).fieldName; + const size_t d1 = sizeof(first) / sizeof(first[0]); + const size_t d2 = sizeof(first[0]) / sizeof(first[0][0]); + const size_t elemsPerRecord = d1 * d2; + const size_t bytesPerRecord = elemsPerRecord * sizeof(fieldType); + + npy_intp dims[3] = {static_cast(N), (npy_intp) d1, (npy_intp) d2}; + $result = PyArray_SimpleNew(3, dims, npyTypeNum); + if (!$result) SWIG_fail; + + fieldType* dest = static_cast(PyArray_DATA((PyArrayObject*)$result)); + + for (size_t i = 0; i < N; ++i) { + const fieldType* src = &($1->record().at(i).fieldName[0][0]); + std::memcpy(dest + i * elemsPerRecord, src, bytesPerRecord); + } + + if (PyErr_Occurred()) SWIG_fail; + } +} + +%extend Recorder { + Recorder* _ ## fieldName ## _array() { + return $self; + } + + %pythoncode %{ + @property + def fieldName(self): + return self._ ## fieldName ## _array() + %} +}; + +%enddef diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.cpp new file mode 100644 index 0000000000..5638da43f3 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.cpp @@ -0,0 +1,725 @@ +/* + ISC License + + Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "BSpline.h" +#include +#include +#include + +/*! This constructor initializes an Input structure for BSpline interpolation */ +InputDataSet::InputDataSet() +{ + return; +} + +/*! The constructor requires 3 N-dimensional vectors containing the coordinates of the waypoints */ +InputDataSet::InputDataSet(Eigen::VectorXd X1, Eigen::VectorXd X2, Eigen::VectorXd X3) +{ + this->X1 = X1; + this->X2 = X2; + this->X3 = X3; + this->XDot_0_flag = false; + this->XDot_N_flag = false; + this->XDDot_0_flag = false; + this->XDDot_N_flag = false; + this->T_flag = false; + this->AvgXDot_flag = false; + this->W_flag = false; + + uint64_t N1 = (uint64_t) X1.size(); + uint64_t N2 = (uint64_t) X2.size(); + uint64_t N3 = (uint64_t) X3.size(); + + if ((N1 != N2) || (N1 != N3) || (N2 != N3)) { + std::cout << "Error in BSpline.InputDataSet: \n the Input coordinate vectors X1, X2, X3 have different sizes. \n"; + } + + return; +} + +/*! Generic destructor */ +InputDataSet::~InputDataSet() +{ + return; +} + +/*! Set the first derivative of the starting point (optional) */ +void InputDataSet::setXDot_0(Eigen::Vector3d XDot_0) {this->XDot_0 = XDot_0; this->XDot_0_flag = true; return;} + +/*! Set the first derivative of the last point (optional) */ +void InputDataSet::setXDot_N(Eigen::Vector3d XDot_N) {this->XDot_N = XDot_N; this->XDot_N_flag = true; return;} + +/*! Set the second derivative of the starting point (optional) */ +void InputDataSet::setXDDot_0(Eigen::Vector3d XDDot_0) {this->XDDot_0 = XDDot_0; this->XDDot_0_flag = true; return;} + +/*! Set the second derivative of the last point (optional) */ +void InputDataSet::setXDDot_N(Eigen::Vector3d XDDot_N) {this->XDDot_N = XDDot_N; this->XDDot_N_flag = true; return;} + +/*! Set the time tags for each waypoint (optional). Cannot be imposed together with avg velocity norm below */ +void InputDataSet::setT(Eigen::VectorXd T) {this->T = T; this->T_flag = true; this->AvgXDot_flag = false; return;} + +/*! Set the average velocity norm (optional). Cannot be imposed together with time tag vector above */ +void InputDataSet::setAvgXDot(double AvgXDot) {this->AvgXDot = AvgXDot; this->AvgXDot_flag = true; this->T_flag = false; return;} + +/*! Set the weights for each waypoint. Weights are used in the LS approximation */ +void InputDataSet::setW(Eigen::VectorXd W) {this->W = W; this->W_flag = true; return;} + +/*! This constructor initializes an Output structure for BSpline interpolation */ +OutputDataSet::OutputDataSet() +{ + return; +} + +/*! Generic destructor */ +OutputDataSet::~OutputDataSet() +{ + return; +} + +/*! This method returns x, xDot and xDDot at the desired input time T */ +void OutputDataSet::getData(double T, double x[3], double xDot[3], double xDDot[3]) +{ + int N = (int) this->T.size() - 1; + double Ttot = this->T[N]; + + // if T < Ttot calculalte the values + if (T <= Ttot) { + double t = T / Ttot; + int Q = (int) this->C1.size(); + Eigen::VectorXd NN(Q), NN1(Q), NN2(Q); + basisFunction(t, this->U, Q, this->P, &NN[0], &NN1[0], &NN2[0]); + x[0] = NN.dot(this->C1); + x[1] = NN.dot(this->C2); + x[2] = NN.dot(this->C3); + xDot[0] = NN1.dot(this->C1) / Ttot; + xDot[1] = NN1.dot(this->C2) / Ttot; + xDot[2] = NN1.dot(this->C3) / Ttot; + xDDot[0] = NN2.dot(this->C1) / pow(Ttot,2); + xDDot[1] = NN2.dot(this->C2) / pow(Ttot,2); + xDDot[2] = NN2.dot(this->C3) / pow(Ttot,2); + } + // if t > Ttot return final value with zero derivatives + else { + x[0] = this->X1[N]; + x[1] = this->X2[N]; + x[2] = this->X3[N]; + xDot[0] = 0; + xDot[1] = 0; + xDot[2] = 0; + xDDot[0] = 0; + xDDot[1] = 0; + xDDot[2] = 0; + } +} + +/*! This method returns single coordinates of x, xDot and xDDot at the desired input time T. */ +/*! It is designed to be accessible from Python */ +double OutputDataSet::getStates(double T, int derivative, int index) +{ + int N = (int) this->T.size()-1; + double Ttot = this->T[N]; + + // if T < Ttot calculalte the values + if (T <= Ttot) { + double t = T / Ttot; + int Q = (int) this->C1.size(); + Eigen::VectorXd NN(Q), NN1(Q), NN2(Q); + basisFunction(t, this->U, Q, this->P, &NN[0], &NN1[0], &NN2[0]); + + switch (derivative) { + case 0 : + switch(index) { + case 0 : + return NN.dot(this->C1); + case 1 : + return NN.dot(this->C2); + case 2 : + return NN.dot(this->C3); + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + case 1 : + switch(index) { + case 0 : + return NN1.dot(this->C1) / Ttot; + case 1 : + return NN1.dot(this->C2) / Ttot; + case 2 : + return NN1.dot(this->C3) / Ttot; + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + case 2 : + switch(index) { + case 0 : + return NN2.dot(this->C1) / pow(Ttot,2); + case 1 : + return NN2.dot(this->C2) / pow(Ttot,2); + case 2 : + return NN2.dot(this->C3) / pow(Ttot,2); + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + default : + std::cout << "Error in Output.getStates: invalid derivative \n"; + return 1000; + } + } + // if t > Ttot return final value with zero derivatives + else { + switch (derivative) { + case 0 : + switch(index) { + case 0 : + return this->X1[N]; + case 1 : + return this->X2[N]; + case 2 : + return this->X3[N]; + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + case 1 : + switch(index) { + case 0 : + return 0; + case 1 : + return 0; + case 2 : + return 0; + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + case 2 : + switch(index) { + case 0 : + return 0; + case 1 : + return 0; + case 2 : + return 0; + default : + std::cout << "Error in Output.getStates: invalid index \n"; + return 1000; + } + default : + std::cout << "Error in Output.getStates: invalid derivative \n"; + return 1000; + } + } +} + +/*! This function takes the Input structure, performs the BSpline interpolation and outputs the result into Output structure */ +void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output) +{ + Output->P = P; + + // N = number of waypoints - 1 + int N = (int) Input.X1.size() - 1; + + // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on average + Eigen::VectorXd T(N+1); + double S = 0; + if (Input.T_flag == true) { + T = Input.T; + } + else { + T[0] = 0; + for (int n = 1; n < N+1; n++) { + T[n] = T[n-1] + pow( (pow(Input.X1[n]-Input.X1[n-1], 2) + pow(Input.X2[n]-Input.X2[n-1], 2) + pow(Input.X3[n]-Input.X3[n-1], 2)), 0.5 ); + S += T[n] - T[n-1]; + } + } + if (Input.AvgXDot_flag == true) { + for (int n = 0; n < N+1; n++) { + T[n] = T[n] / T[N] * S / Input.AvgXDot; + } + } + + double Ttot = T[N]; + + // build uk vector: normalized waypoint time tags + Eigen::VectorXd uk(N+1); + for (int n = 0; n < N+1; n++) { + uk[n] = T[n] / Ttot; + } + + // K = number of endpoint derivatives + int K = 0; + if (Input.XDot_0_flag == true) {K += 1;} + if (Input.XDot_N_flag == true) {K += 1;} + if (Input.XDDot_0_flag == true) {K += 1;} + if (Input.XDDot_N_flag == true) {K += 1;} + + // The maximum polynomial order is N + K. If a higher order is requested, print a BSK_ERROR + if (P > N + K) { + std::cout << "Error in BSpline.interpolate: \n the desired polynomial order P is too high. Mass matrix A will be singular. \n" ; + } + + int M = N + P + K + 1; + + // build knot vector U of size M + 1 + Eigen::VectorXd U(M+1); + double u; + for (int p = 0; p < P+1; p++) { + U[p] = 0; + } + for (int j = 0; j < M-2*P-1; j++) { + u = 0.0; + for (int i = j; i < j+P; i++) { + if (i >= uk.size()) { + u += uk[N] / P; + } + else { + u += uk[i] / P; + } + U[P+j+1] = u; + } + } + for (int p = 0; p < P+1; p++) { + U[M-P+p] = 1; + } + + // build stiffness matrix A of size (N+K+1)x(N+K+1) + Eigen::MatrixXd A(N+K+1,N+K+1); + // build vectors Q1, Q2, Q3 (left hand side of linear system) + Eigen::VectorXd Q1(N+K+1), Q2(N+K+1), Q3(N+K+1); + // populate A with zeros + for (int n = 0; n < N+K+1; n++) { + for (int m = 0; m < N+K+1; m++) { + A(n,m) = 0; + } + } + int n = -1; + // constrain first derivative at starting point + if (Input.XDot_0_flag == true) { + n += 1; + A(n,0) = -1; + A(n,1) = 1; + Q1[n] = U[P+1] / P * Input.XDot_0[0] * Ttot; + Q2[n] = U[P+1] / P * Input.XDot_0[1] * Ttot; + Q3[n] = U[P+1] / P * Input.XDot_0[2] * Ttot; + } + // constrain second derivative at starting point + if (Input.XDDot_0_flag == true) { + n += 1; + A(n,0) = U[P+2]; + A(n,1) = -(U[P+1] + U[P+2]); + A(n,2) = U[P+1]; + Q1[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[0] * pow(Ttot,2); + Q2[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[1] * pow(Ttot,2); + Q3[n] = ( pow(U[P+1],2) * U[P+2] / (P*(P-1)) ) * Input.XDDot_0[2] * pow(Ttot,2); + } + n += 1; + int m = -1; + int n0 = n; + Eigen::VectorXd NN(N+K+1), NN1(N+K+1), NN2(N+K+1); + // constrain waypoints + for (n = n0; n < N+n0+1; n++) { + m += 1; + basisFunction(uk[m], U, N+K+1, P, &NN[0], &NN1[0], &NN2[0]); + for (int b = 0; b < N+K+1; b++) { + A(n,b) = NN[b]; + } + Q1[n] = Input.X1[m]; + Q2[n] = Input.X2[m]; + Q3[n] = Input.X3[m]; + } + n = N+n0; + // constrain second derivative at final point + if (Input.XDDot_N_flag == true) { + n += 1; + A(n,N+K-2) = 1 - U[M-P-1]; + A(n,N+K-1) = -(2 - U[M-P-1] - U[M-P-2]); + A(n,N+K) = 1 - U[M-P-2]; + Q1[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[0] * pow(Ttot,2); + Q2[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[1] * pow(Ttot,2); + Q3[n] = ( pow((1-U[M-P-1]),2) * (1-U[M-P-2]) / (P*(P-1)) ) * Input.XDDot_N[2] * pow(Ttot,2); + } + // constrain first derivative at final point + if (Input.XDot_N_flag == true) { + n += 1; + A(n,N+K-1) = -1; + A(n,N+K) = 1; + Q1[n] = (1-U[M-P-1]) / P * Input.XDot_N[0] * Ttot; + Q2[n] = (1-U[M-P-1]) / P * Input.XDot_N[1] * Ttot; + Q3[n] = (1-U[M-P-1]) / P * Input.XDot_N[2] * Ttot; + } + + // solve linear systems + Eigen::MatrixXd B = A.inverse(); + Eigen::VectorXd C1 = B * Q1; + Eigen::VectorXd C2 = B * Q2; + Eigen::VectorXd C3 = B * Q3; + + Output->U = U; + Output->C1 = C1; + Output->C2 = C2; + Output->C3 = C3; + + double dt = 1.0 / (Num - 1); + double t = 0; + // store the interpolated trajectory information into Output structure + Output->T.resize(Num); + Output->X1.resize(Num); + Output->X2.resize(Num); + Output->X3.resize(Num); + Output->XD1.resize(Num); + Output->XD2.resize(Num); + Output->XD3.resize(Num); + Output->XDD1.resize(Num); + Output->XDD2.resize(Num); + Output->XDD3.resize(Num); + for (int i = 0; i < Num; i++) { + basisFunction(t, U, N+K+1, P, &NN[0], &NN1[0], &NN2[0]); + Output->T[i] = t * Ttot; + Output->X1[i] = NN.dot(C1); + Output->X2[i] = NN.dot(C2); + Output->X3[i] = NN.dot(C3); + Output->XD1[i] = NN1.dot(C1) / Ttot; + Output->XD2[i] = NN1.dot(C2) / Ttot; + Output->XD3[i] = NN1.dot(C3) / Ttot; + Output->XDD1[i] = NN2.dot(C1) / pow(Ttot,2); + Output->XDD2[i] = NN2.dot(C2) / pow(Ttot,2); + Output->XDD3[i] = NN2.dot(C3) / pow(Ttot,2); + t += dt; + } + + return; +} + +/*! This function takes the Input structure, performs the BSpline LS approximation and outputs the result into Output structure */ +void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Output) +{ + Output->P = P; + + // N = number of waypoints - 1 + int N = (int) Input.X1.size() - 1; + + // T = time tags; if not specified, it is computed from a cartesian distance assuming a constant velocity norm on average + Eigen::VectorXd T(N+1); + double S = 0; + if (Input.T_flag == true) { + T = Input.T; + } + else { + T[0] = 0; + for (int n = 1; n < N+1; n++) { + T[n] = T[n-1] + pow( (pow(Input.X1[n]-Input.X1[n-1], 2) + pow(Input.X2[n]-Input.X2[n-1], 2) + pow(Input.X3[n]-Input.X3[n-1], 2)), 0.5 ); + S += T[n] - T[n-1]; + } + } + if (Input.AvgXDot_flag == true) { + for (int n = 0; n < N+1; n++) { + T[n] = T[n] / T[N] * S / Input.AvgXDot; + } + } + + double Ttot = T[N]; + + // build uk vector: normalized waypoint time tags + Eigen::VectorXd uk(N+1); + for (int n = 0; n < N+1; n++) { + uk[n] = T[n] / Ttot; + } + + // The maximum polynomial order is N + K. If a higher order is requested, print a BSK_ERROR + if (P > Q) { + std::cout << "Error in BSpline.approximate: \n the desired polynomial order P can't be higher than the number of control points Q. \n" ; + } + + // build knot vector U of size Q + P + 2 + Eigen::VectorXd U(Q+P+2); + double d, alpha; + int i; + d = ((double)(N+1)) / ((double)abs(Q-P+1)); + for (int p = 0; p < P+1; p++) { + U[p] = 0; + } + for (int j = 1; j < Q-P+1; j++) { + i = int(j*d); + alpha = j*d - i; + U[P+j] = (1-alpha)*uk[i-1] + alpha*uk[i]; + } + for (int p = 0; p < P+1; p++) { + U[Q+p+1] = 1; + } + + // K = number of endpoint derivatives + int K = 0; + if (Input.XDot_0_flag == true) {K += 1;} + if (Input.XDot_N_flag == true) {K += 1;} + if (Input.XDDot_0_flag == true) {K += 1;} + if (Input.XDDot_N_flag == true) {K += 1;} + + // build stiffness matrix MD of size (K+2)x(K+2) + Eigen::MatrixXd MD(K+2,K+2); + // build vectors T1, T2, T3 (left hand side of linear system) + Eigen::VectorXd T1(K+2), T2(K+2), T3(K+2); + // populate MD with zeros + for (int n = 0; n < K+2; n++) { + for (int m = 0; m < K+2; m++) { + MD(n,m) = 0; + } + } + Eigen::VectorXd NN(Q+1), NN1(Q+1), NN2(Q+1); + basisFunction(uk[0], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + int n = 0; + MD(0,0) = NN[0]; + T1[0] = Input.X1[0]; + T2[0] = Input.X2[0]; + T3[0] = Input.X3[0]; + // constrain first derivative at starting point + if (Input.XDot_0_flag == true) { + n += 1; + MD(n,0) = NN1[0]; + MD(n,1) = NN1[1]; + T1[n] = Input.XDot_0[0] * Ttot; + T2[n] = Input.XDot_0[1] * Ttot; + T3[n] = Input.XDot_0[2] * Ttot; + } + // constrain second derivative at starting point + if (Input.XDDot_0_flag == true) { + n += 1; + MD(n,0) = NN2[0]; + MD(n,1) = NN2[1]; + MD(n,2) = NN2[2]; + T1[n] = Input.XDDot_0[0] * pow(Ttot,2); + T2[n] = Input.XDDot_0[1] * pow(Ttot,2); + T3[n] = Input.XDDot_0[2] * pow(Ttot,2); + } + basisFunction(uk[N], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + // constrain second derivative at ending point + if (Input.XDDot_N_flag == true) { + n += 1; + MD(n,K-1) = NN2[Q-2]; + MD(n,K) = NN2[Q-1]; + MD(n,K+1) = NN2[Q]; + T1[K-1] = Input.XDDot_N[0] * pow(Ttot,2); + T2[K] = Input.XDDot_N[1] * pow(Ttot,2); + T3[K+1] = Input.XDDot_N[2] * pow(Ttot,2); + } + // constrain first derivative at ending point + if (Input.XDot_N_flag == true) { + n += 1; + MD(n,K) = NN1[Q-1]; + MD(n,K+1) = NN1[Q]; + T1[n] = Input.XDot_N[0] * Ttot; + T2[n] = Input.XDot_N[1] * Ttot; + T3[n] = Input.XDot_N[2] * Ttot; + } + n += 1; + MD(n,K+1) = NN[Q]; + T1[n] = Input.X1[N]; + T2[n] = Input.X2[N]; + T3[n] = Input.X3[N]; + + // solve linear systems + Eigen::MatrixXd B = MD.inverse(); + Eigen::VectorXd C1_1 = B * T1; + Eigen::VectorXd C2_1 = B * T2; + Eigen::VectorXd C3_1 = B * T3; + + // populate Rk vectors with the base points for LS minimization + Eigen::VectorXd Rk1(N-1), Rk2(N-1), Rk3(N-1); + for (int n = 1; n < N; n++) { + basisFunction(uk[n], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + Rk1[n-1] = Input.X1[n] - NN[0]*C1_1[0] - NN[Q]*C1_1[K+1]; + Rk2[n-1] = Input.X2[n] - NN[0]*C2_1[0] - NN[Q]*C2_1[K+1]; + Rk3[n-1] = Input.X3[n] - NN[0]*C3_1[0] - NN[Q]*C3_1[K+1]; + if (Input.XDot_0_flag == true) { + Rk1[n-1] -= NN[1]*C1_1[1]; + Rk2[n-1] -= NN[1]*C2_1[1]; + Rk3[n-1] -= NN[1]*C3_1[1]; + } + if (Input.XDDot_0_flag == true) { + Rk1[n-1] -= NN[2]*C1_1[2]; + Rk2[n-1] -= NN[2]*C2_1[2]; + Rk3[n-1] -= NN[2]*C3_1[2]; + } + if (Input.XDDot_N_flag == true) { + Rk1[n-1] -= NN[Q-2]*C1_1[K-1]; + Rk2[n-1] -= NN[Q-2]*C2_1[K-1]; + Rk3[n-1] -= NN[Q-2]*C3_1[K-1]; + } + if (Input.XDot_N_flag == true) { + Rk1[n-1] -= NN[Q-1]*C1_1[K]; + Rk2[n-1] -= NN[Q-1]*C2_1[K]; + Rk3[n-1] -= NN[Q-1]*C3_1[K]; + } + } + + // populate LS matrix ND + Eigen::MatrixXd ND(N-1,Q-K-1); + for (int n = 0; n < N-1; n++) { + basisFunction(uk[1+n], U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + int k = 1; + if (Input.XDot_0_flag == true) {k += 1;} + if (Input.XDDot_0_flag == true) {k += 1;} + for (int b = 0; b < Q-K-1; b++) { + ND(n,b) = NN[k+b]; + } + } + + // populate weight matrix W + Eigen::MatrixXd W(N-1,N-1); + for (int n = 0; n < N-1; n++) { + for (int m = 0; m < N-1; m++) { + if (n == m) { + if (Input.W_flag) { + W(n,m) = Input.W[n+1]; + } + else { + W(n,m) = 1; + } + } + else { + W(n,m) = 0; + } + } + } + + Eigen::VectorXd R1(Q-K-1), R2(Q-K-1), R3(Q-K-1); + B = ND.transpose() * W; + R1 = B * Rk1; + R2 = B * Rk2; + R3 = B * Rk3; + + // compute LS values R for the control points + Eigen::MatrixXd NWN(Q-K-1,Q-K-1), NWN_inv(Q-K-1,Q-K-1); + NWN = B * ND; + NWN_inv = NWN.inverse(); + Eigen::VectorXd C1_2 = NWN_inv * R1; + Eigen::VectorXd C2_2 = NWN_inv * R2; + Eigen::VectorXd C3_2 = NWN_inv * R3; + + // build control point vectors C + Eigen::VectorXd C1(Q+1), C2(Q+1), C3(Q+1); + n = 0; + C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + if (Input.XDot_0_flag == true) { + n += 1; + C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + } + if (Input.XDDot_0_flag == true) { + n += 1; + C1[n] = C1_1[n]; C2[n] = C2_1[n]; C3[n] = C3_1[n]; + } + for (int q = 0; q < Q-K-1; q++) { + C1[n+q+1] = C1_2[q]; C2[n+q+1] = C2_2[q]; C3[n+q+1] = C3_2[q]; + } + if (Input.XDDot_N_flag == true) { + n += 1; + C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + } + if (Input.XDot_N_flag == true) { + n += 1; + C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + } + n += 1; + C1[Q-K-1+n] = C1_1[n]; C2[Q-K-1+n] = C2_1[n]; C3[Q-K-1+n] = C3_1[n]; + + Output->U = U; + Output->C1 = C1; + Output->C2 = C2; + Output->C3 = C3; + + double dt = 1.0 / (Num - 1); + double t = 0; + // store the interpolated trajectory information into Output structure + Output->T.resize(Num); + Output->X1.resize(Num); + Output->X2.resize(Num); + Output->X3.resize(Num); + Output->XD1.resize(Num); + Output->XD2.resize(Num); + Output->XD3.resize(Num); + Output->XDD1.resize(Num); + Output->XDD2.resize(Num); + Output->XDD3.resize(Num); + for (int i = 0; i < Num; i++) { + basisFunction(t, U, Q+1, P, &NN[0], &NN1[0], &NN2[0]); + Output->T[i] = t * Ttot; + Output->X1[i] = NN.dot(C1); + Output->X2[i] = NN.dot(C2); + Output->X3[i] = NN.dot(C3); + Output->XD1[i] = NN1.dot(C1) / Ttot; + Output->XD2[i] = NN1.dot(C2) / Ttot; + Output->XD3[i] = NN1.dot(C3) / Ttot; + Output->XDD1[i] = NN2.dot(C1) / pow(Ttot,2); + Output->XDD2[i] = NN2.dot(C2) / pow(Ttot,2); + Output->XDD3[i] = NN2.dot(C3) / pow(Ttot,2); + t += dt; + } + + return; +} + +/*! This function calculates the basis functions NN of order P, and derivatives NN1, NN2, for a given time t and knot vector U */ +void basisFunction(double t, Eigen::VectorXd U, int I, int P, double *NN, double *NN1, double *NN2) +{ + Eigen::MatrixXd N(I, P+1); + Eigen::MatrixXd N1(I, P+1); + Eigen::MatrixXd N2(I, P+1); + /* populate matrices with zeros */ + for (int i = 0; i < I; i++) { + for (int p = 0; p < P+1; p++) { + N(i,p) = 0; + N1(i,p) = 0; + N2(i,p) = 0; + } + } + /* zero order */ + for (int i = 0; i < I; i++) { + if ( (t >= U(i)) && (t < U(i+1)) ) { + N(i,0) = 1; + } + } + if (abs(t-1.0) < 1e-5) { + N(I-1,0) = 1; + } + /* higher order - De Boor formula */ + for (int p = 1; p < P+1; p++) { + for (int i = 0; i < I; i++) { + if (U[i+p]-U[i] != 0) { + N(i,p) += (t-U[i]) / (U[i+p]-U[i]) * N(i,p-1); + N1(i,p) += p / (U[i+p]-U[i]) * N(i,p-1); + N2(i,p) += p / (U[i+p]-U[i]) * N1(i,p-1); + } + if (U[i+p+1]-U[i+1] != 0) { + N(i,p) += (U[i+p+1]-t) / (U[i+p+1]-U[i+1]) * N(i+1,p-1); + N1(i,p) -= p / (U[i+p+1]-U[i+1]) * N(i+1,p-1); + N2(i,p) -= p / (U[i+p+1]-U[i+1]) * N1(i+1,p-1); + } + } + } + // output result + for (int i = 0; i < I; i++) { + *(NN+i) = N(i,P); + *(NN1+i) = N1(i,P); + *(NN2+i) = N2(i,P); + } + + return; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.h new file mode 100644 index 0000000000..6b86e6e394 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/BSpline.h @@ -0,0 +1,93 @@ +/* + ISC License + + Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + + +#include +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/macroDefinitions.h" + + +//! @brief The InputDataSet class contains the information about the points that must be interpolated. +//! It is used as a data structure to intialize the inputs that are passed to the interpolating function. + +class InputDataSet { +public: + InputDataSet(); + InputDataSet(Eigen::VectorXd X1, Eigen::VectorXd X2, Eigen::VectorXd X3); + ~InputDataSet(); + + void setXDot_0(Eigen::Vector3d XDot_0); + void setXDot_N(Eigen::Vector3d XDot_N); + void setXDDot_0(Eigen::Vector3d XDDot_0); + void setXDDot_N(Eigen::Vector3d XDDot_N); + void setT(Eigen::VectorXd T); + void setW(Eigen::VectorXd W); + void setAvgXDot(double AvgXDot); + + double AvgXDot; //!< desired average velocity norm + Eigen::VectorXd T; //!< time tags: specifies at what time each waypoint is hit + Eigen::VectorXd W; //!< weight vector for the LS approximation + Eigen::VectorXd X1; //!< coordinate #1 of the waypoints + Eigen::VectorXd X2; //!< coordinate #2 of the waypoints + Eigen::VectorXd X3; //!< coordinate #3 of the waypoints + Eigen::Vector3d XDot_0; //!< 3D vector containing the first derivative at starting point + Eigen::Vector3d XDot_N; //!< 3D vector containing the first derivative at final point + Eigen::Vector3d XDDot_0; //!< 3D vector containing the second derivative at starting point + Eigen::Vector3d XDDot_N; //!< 3D vector containing the second derivative at final point + bool T_flag; //!< indicates that time tags have been specified; if true, AvgXDot_flag is false + bool AvgXDot_flag; //!< indicates that avg velocity norm has been specified; if true, T_flag is false + bool W_flag; //!< indicates that weight vector has been specified + bool XDot_0_flag; //!< indicates that first derivative at starting point has been specified + bool XDot_N_flag; //!< indicates that first derivative at final point has been specified + bool XDDot_0_flag; //!< indicates that second derivative at starting point has been specified + bool XDDot_N_flag; //!< indicates that second derivative at final point has been specified +}; + +//! @brief The OutputDataSet class is used as a data structure to contain the interpolated function and its first- and +//! second-order derivatives, together with the time-tag vector T. +class OutputDataSet { +public: + OutputDataSet(); + ~OutputDataSet(); + void getData(double t, double x[3], double xDot[3], double xDDot[3]); + double getStates(double t, int derivative, int index); + + Eigen::VectorXd T; //!< time tags for each point of the interpolated trajectory + Eigen::VectorXd X1; //!< coordinate #1 of the interpolated trajectory + Eigen::VectorXd X2; //!< coordinate #2 of the interpolated trajectory + Eigen::VectorXd X3; //!< coordinate #3 of the interpolated trajectory + Eigen::VectorXd XD1; //!< first derivative of coordinate #1 of the interpolated trajectory + Eigen::VectorXd XD2; //!< first derivative of coordinate #2 of the interpolated trajectory + Eigen::VectorXd XD3; //!< first derivative of coordinate #3 of the interpolated trajectory + Eigen::VectorXd XDD1; //!< second derivative of coordinate #1 of the interpolated trajectory + Eigen::VectorXd XDD2; //!< second derivative of coordinate #2 of the interpolated trajectory + Eigen::VectorXd XDD3; //!< second derivative of coordinate #3 of the interpolated trajectory + + int P; //!< polynomial degree of the BSpline + Eigen::VectorXd U; //!< knot vector of the BSpline + Eigen::VectorXd C1; //!< coordinate #1 of the control points + Eigen::VectorXd C2; //!< coordinate #2 of the control points + Eigen::VectorXd C3; //!< coordinate #3 of the control points +}; + +void interpolate(InputDataSet Input, int Num, int P, OutputDataSet *Output); + +void approximate(InputDataSet Input, int Num, int Q, int P, OutputDataSet *Output); + +void basisFunction(double t, Eigen::VectorXd U, int I, int P, double *NN, double *NN1, double *NN2); \ No newline at end of file diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/CMakeLists.txt b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/CMakeLists.txt new file mode 100644 index 0000000000..cfa427bf10 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/CMakeLists.txt @@ -0,0 +1,32 @@ + +file(GLOB basilisk_src "*.h" "*.cpp" "*.c" "*.i") + +file(GLOB moduleId "moduleIdGenerator/*.h" "moduleIdGenerator/*.cpp" "moduleIdGenerator/*.i") + +add_library(ArchitectureUtilities STATIC ${basilisk_src}) +add_library(ModuleIdGenerator SHARED ${moduleId}) +string(LENGTH ${CMAKE_SOURCE_DIR} DIR_NAME_START) +math(EXPR DIR_NAME_START "${DIR_NAME_START} + 1") +string(SUBSTRING ${CMAKE_CURRENT_SOURCE_DIR} ${DIR_NAME_START} -1 DIR_NAME_STRING) +set_target_properties(ArchitectureUtilities PROPERTIES FOLDER "${DIR_NAME_STRING}") +set_target_properties(ModuleIdGenerator PROPERTIES FOLDER "${DIR_NAME_STRING}") + +if(NOT WIN32) + target_compile_options(ArchitectureUtilities PUBLIC "-fPIC") + target_compile_options(ModuleIdGenerator PUBLIC "-fPIC") +endif() + +set_target_properties(ArchitectureUtilities PROPERTIES ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(ArchitectureUtilities PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(ArchitectureUtilities PROPERTIES ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_BINARY_DIR}/Basilisk") + +set_target_properties(ModuleIdGenerator PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(ModuleIdGenerator PROPERTIES LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_BINARY_DIR}/Basilisk") +set_target_properties(ModuleIdGenerator PROPERTIES LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_BINARY_DIR}/Basilisk") + +if(WIN32) + add_custom_command( + TARGET ModuleIdGenerator + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy $ "${CMAKE_BINARY_DIR}/Basilisk/") +endif() diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/astroConstants.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/astroConstants.h new file mode 100644 index 0000000000..62fba553b0 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/astroConstants.h @@ -0,0 +1,184 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _ASTRO_CONSTANTS_H_ +#define _ASTRO_CONSTANTS_H_ + +#include + +#ifndef G_UNIVERSIAL +#define G_UNIVERSIAL 6.67259e-20 /* universial Gravitational Constant, units are in km^3/s^2/kg */ +#endif +#ifndef AU +#define AU 149597870.693 /* astronomical unit in units of kilometers */ +#endif +#ifndef AU2KM +#define AU2KM 149597870.693 /* convert astronomical unit to kilometers */ +#endif +#ifndef SPEED_LIGHT +#define SPEED_LIGHT 299792458 /* [m/s] convert astronomical unit to kilometers */ +#endif +#ifndef SOLAR_FLUX_EARTH +#define SOLAR_FLUX_EARTH 1372.5398 /* [W/m^2] solar flux at earth */ +#endif + + +/* common conversions */ +#ifndef MPI +#define MPI 3.141592653589793 +#endif +#ifndef MPI_2 +#define MPI_2 1.5707963267948966 +#endif +#ifndef M_E +#define M_E 2.718281828459045 +#endif +#ifndef D2R +#define D2R (MPI/180.) +#endif +#ifndef R2D +#define R2D 180./MPI +#endif +#ifndef NAN +#define NAN sqrt((float)-1) +#endif +#ifndef RPM +#define RPM (2.*MPI/60.) +#endif +#ifndef SEC2DAY +#define SEC2DAY 1.0 / (60.0 * 60.0 * 24.0); +#endif + +#ifndef EARTH_GRAV +#define EARTH_GRAV 9.80665 +#endif + +/* Gravitational Constants mu = G*m, where m is the planet of the attracting planet. All units are km^3/s^2. + * Values are obtained from SPICE kernels in http://naif.jpl.nasa.gov/pub/naif/generic_kernels/ + */ +#define MU_SUN 132712440023.310 +#define MU_MERCURY 22032.080 +#define MU_VENUS 324858.599 +#define MU_EARTH 398600.436 +#define MU_MOON 4902.799 +#define MU_MARS 42828.314 +#define MU_JUPITER 126712767.881 +#define MU_SATURN 37940626.068 +#define MU_URANUS 5794559.128 +#define MU_NEPTUNE 6836534.065 +#define MU_PLUTO 983.055 + +/* planet information for major solar system bodies. Units are in km. + * data taken from http://nssdc.gsfc.nasa.gov/planetary/planets.html + */ + +/* Sun */ +#define REQ_SUN 695000. /* km */ + +/* Mercury */ +#define REQ_MERCURY 2439.7 /* km */ +#define J2_MERCURY 60.0e-6 +#define SMA_MERCURY 0.38709893*AU +#define I_MERCURY 7.00487*D2R +#define E_MERCURY 0.20563069 +#define OMEGA_MERCURY (2*MPI/1407.6/3600.) /* Mercury's planetary rotation rate, rad/sec */ + +/* Venus */ +#define REQ_VENUS 6051.8 /* km */ +#define J2_VENUS 4.458e-6 +#define SMA_VENUS 0.72333199*AU +#define I_VENUS 3.39471*D2R +#define E_VENUS 0.00677323 +#define OMEGA_VENUS (2*MPI/(-5832.6)/3600.) /* Venus' planetary rotation rate, rad/sec */ + +/* Earth */ +#define REQ_EARTH 6378.1366 /* km, from SPICE */ +#define RP_EARTH 6356.7519 /* km, from SPICE */ +#define J2_EARTH 1082.616e-6 +#define J3_EARTH -2.53881e-6 +#define J4_EARTH -1.65597e-6 +#define J5_EARTH -0.15e-6 +#define J6_EARTH 0.57e-6 +#define SMA_EARTH 1.00000011*AU +#define I_EARTH 0.00005*D2R +#define E_EARTH 0.01671022 +#define OMEGA_EARTH (2*MPI/23.9345/3600.) /* Earth's planetary rotation rate, rad/sec */ + +/* Moon */ +#define REQ_MOON 1737.4 +#define J2_MOON 202.7e-6 +#define SMA_MOON 0.3844e6 +#define E_MOON 0.0549 +#define OMEGA_MOON (2*MPI/655.728/3600.) /* Moon's rotation rate, rad/sec */ + +/* Mars */ +#define REQ_MARS 3396.19 /* km */ +#define RP_MARS 3376.2 /* km */ +#define J2_MARS 1960.45e-6 +#define SMA_MARS 1.52366231*AU +#define I_MARS 1.85061*D2R +#define E_MARS 0.09341233 +#define OMEGA_MARS (2*MPI/24.6229/3600.) /* Mars' polar rotation rate, rad/sec */ + +/* Phobos */ +#define REQ_PHOBOS 11.2 /* km */ + +/* Deimos */ +#define REQ_DEIMOS 6.1 /* km */ + +/* Jupiter */ +#define REQ_JUPITER 71492. +#define J2_JUPITER 14736.e-6 +#define SMA_JUPITER 5.20336301*AU +#define I_JUPITER 1.30530*D2R +#define E_JUPITER 0.04839266 +#define OMEGA_JUPITER (2*MPI/9.9250/3600.) /* Jupiter's polar rotation rate, rad/sec */ + +/* Saturn */ +#define REQ_SATURN 60268. +#define J2_SATURN 16298.e-6 +#define SMA_SATURN 9.53707032*AU +#define I_SATURN 2.48446*D2R +#define E_SATURN 0.05415060 +#define OMEGA_SATURN (2*MPI/10.656/3600.) /* Saturn's polar rotation rate, rad/sec */ + +/* Uranus */ +#define REQ_URANUS 25559. +#define J2_URANUS 3343.43e-6 +#define SMA_URANUS 19.19126393*AU +#define I_URANUS 0.76986*D2R +#define E_URANUS 0.04716771 +#define OMEGA_URANUS (2*MPI/(-17.24)/3600.) /* Uranus' polar rotation rate, rad/sec */ + +/* Neptune */ +#define REQ_NEPTUNE 24746. +#define J2_NEPTUNE 3411.e-6 +#define SMA_NEPTUNE 30.06896348*AU +#define I_NEPTUNE 1.76917*D2R +#define E_NEPTUNE 0.00858587 +#define OMEGA_NEPTUNE (2*MPI/16.11/3600.) /* Neptune's polar rotation rate, rad/sec */ + +/* Pluto */ +#define REQ_PLUTO 1137. +#define SMA_PLUTO 39.48168677*AU +#define I_PLUTO 17.14175*D2R +#define E_PLUTO 0.24880766 +#define OMEGA_PLUTO (2*MPI/(-153.2928)/3600.) /* Pluto's polar rotation rate, rad/sec */ + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenMRP.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenMRP.h new file mode 100644 index 0000000000..32aa0f1280 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenMRP.h @@ -0,0 +1,843 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +/// \cond DO_NOT_DOCUMENT + + +#ifndef EIGEN_MRP_H +#define EIGEN_MRP_H + +//#include +//#include +//#include +//#include +//#include "Eigen/src/Geometry/Quaternion.h" + +namespace Eigen { + template class MRP; + + + + /*************************************************************************** + * Definition of MRPBase + * The implementation is at the end of the file + ***************************************************************************/ + + namespace internal { + template + struct MRPbase_assign_impl; + } + + /** + * \class MRPBase + * \brief Base class for MRP expressions + * \tparam Derived derived type (CRTP) + * \sa class MRP + */ + template + class MRPBase : public RotationBase + { + typedef RotationBase Base; + public: + using Base::operator*; + using Base::derived; + + typedef typename internal::traits::Scalar Scalar; //!< variable + typedef typename NumTraits::Real RealScalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + enum { + Flags = Eigen::internal::traits::Flags + }; + + // typedef typename Matrix Coefficients; + /** the type of a 3D vector */ + typedef Matrix Vector3; + /** the equivalent rotation matrix type */ + typedef Matrix Matrix3; + /** the equivalent angle-axis type */ + typedef AngleAxis AngleAxisType; + + /** default constructor */ + MRPBase() = default; + /** copy constructor */ + MRPBase(const MRPBase &/*rhs*/) = default; + + /** \returns the \c x coefficient */ + inline Scalar x() const { return this->derived().coeffs().coeff(0); } + /** \returns the \c y coefficient */ + inline Scalar y() const { return this->derived().coeffs().coeff(1); } + /** \returns the \c z coefficient */ + inline Scalar z() const { return this->derived().coeffs().coeff(2); } + + /** \returns a reference to the \c x coefficient */ + inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } + /** \returns a reference to the \c y coefficient */ + inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } + /** \returns a reference to the \c z coefficient */ + inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } + + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + inline const VectorBlock vec() const { return coeffs().template head<3>(); } + + /** \returns a vector expression of the imaginary part (x,y,z) */ + inline VectorBlock vec() { return coeffs().template head<3>(); } + + /** \returns a read-only vector expression of the coefficients (x,y,z) */ + inline const typename internal::traits::Coefficients& coeffs() const { return derived().coeffs(); } + + /** \returns a vector expression of the coefficients (x,y,z) */ + inline typename internal::traits::Coefficients& coeffs() { return derived().coeffs(); } + + EIGEN_STRONG_INLINE MRPBase& operator=(const MRPBase& other); //!< method + template EIGEN_STRONG_INLINE Derived& operator=(const MRPBase& other); //!< method + + // disabled this copy operator as it is giving very strange compilation errors when compiling + // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's + // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase + // we didn't have to add, in addition to templated operator=, such a non-templated copy operator. + // Derived& operator=(const MRPBase& other) + // { return operator=(other); } + + Derived& operator=(const AngleAxisType& aa); + template Derived& operator=(const MatrixBase& m); //!< method + + /** \returns a MRP representing an identity mapping or zero rotation + * \sa MatrixBase::Identity() + */ + static inline MRP Identity() { return MRP(Scalar(0), Scalar(0), Scalar(0)); } + + /** \sa MRPBase::Identity(), MatrixBase::setIdentity() + */ + inline MRPBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0); return *this; } + + /** \returns the squared norm of the MRP's coefficients + * \sa MRPBase::norm(), MatrixBase::squaredNorm() + */ + inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } + + /** \returns the norm of the MRP's coefficients + * \sa MRPBase::squaredNorm(), MatrixBase::norm() + */ + inline Scalar norm() const { return coeffs().norm(); } + + /** Normalizes the MRP \c *this + * \sa normalized(), MatrixBase::normalize() */ + inline void normalize() { coeffs().normalize(); } + /** \returns a normalized copy of \c *this + * \sa normalize(), MatrixBase::normalized() */ + inline MRP normalized() const { return MRP(coeffs().normalized()); } + + /** \returns the dot product of \c *this and \a other + * Geometrically speaking, the dot product of two unit MRPs + * corresponds to the cosine of half the angle between the two rotations. + * \sa angularDistance() + */ + template inline Scalar dot(const MRPBase& other) const { return coeffs().dot(other.coeffs()); } + + template Scalar angularDistance(const MRPBase& other) const; //!< method + + /** \returns an equivalent 3x3 rotation matrix */ + Matrix3 toRotationMatrix() const; + + /** \returns the MRP which transform \a a into \a b through a rotation */ + template + Derived& setFromTwoVectors(const MatrixBase& a, const MatrixBase& b); + + template EIGEN_STRONG_INLINE MRP operator* (const MRPBase& q) const; //!< method + template EIGEN_STRONG_INLINE Derived& operator*= (const MRPBase& q); + template EIGEN_STRONG_INLINE Derived& operator+= (const MRPBase& q); + template EIGEN_STRONG_INLINE Derived& operator-= (const MRPBase& q); + + /** \returns the MRP describing the shadow set */ + MRP shadow() const; + + /** \returns 3x3 [B] matrix for the MRP differential kinematic equations */ + Matrix3 Bmat() const; + + /** \returns the MRP describing the inverse rotation */ + MRP inverse() const; + + /** \returns the conjugated MRP */ + MRP conjugate() const; + + // template MRP slerp(const Scalar& t, const MRPBase& other) const; + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + template + bool isApprox(const MRPBase& other, const RealScalar& prec = NumTraits::dummy_precision()) const + { return coeffs().isApprox(other.coeffs(), prec); } + + /** return the result vector of \a v through the rotation*/ + EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; + + /** \returns \c *this with scalar type casted to \a NewScalarType + * + * Note that if \a NewScalarType is equal to the current scalar type of \c *this + * then this function smartly returns a const reference to \c *this. + */ + template + inline typename internal::cast_return_type >::type cast() const + { + return typename internal::cast_return_type >::type(derived()); + } + + }; + + /*************************************************************************** + * Definition/implementation of MRP + ***************************************************************************/ + + /** + * + * \class MRP + * + * \brief The MRP class used to represent 3D orientations and rotations + * + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. + * + * This class represents a MRP \f$ (x,y,z) \f$ that is a convenient representation of + * orientations and rotations of objects in three dimensions. Compared to other representations + * like Euler angles or 3x3 matrices, MRPs offer the following advantages: + * \li \b compact storage (3 scalars) + * \li \b efficient to compose (28 flops), + * + * The following two typedefs are provided for convenience: + * \li \c MRPf for \c float + * \li \c MRPd for \c double + * + * \warning Operations interpreting the MRP as rotation have undefined behavior if the MRP is not normalized. + * + * \sa class AngleAxis, class Transform + */ + + namespace internal { + template + /*! structure definition */ + struct traits > + { + /** struct definition */ + typedef MRP<_Scalar,_Options> PlainObject; + /** struct definition */ + typedef _Scalar Scalar; + /** struct definition */ + typedef Matrix<_Scalar,3,1,_Options> Coefficients; + enum{ + IsAligned = internal::traits::Flags & PacketAccessBit, + Flags = IsAligned ? (PacketAccessBit | LvalueBit) : LvalueBit + }; + }; + } + + template + class MRP : public MRPBase > + { + typedef MRPBase > Base; + enum { IsAligned = internal::traits::IsAligned }; + + public: + typedef _Scalar Scalar; //!< variable + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MRP) + using Base::operator*=; + + typedef typename internal::traits::Coefficients Coefficients; //!< variable + typedef typename Base::AngleAxisType AngleAxisType; //!< variable + + /** Default constructor leaving the MRP uninitialized. */ + inline MRP() {} + + /** Constructs and initializes the MRP \f$ (x,y,z) \f$ from + * its four coefficients \a x, \a y and \a z. + */ + inline MRP(const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z){} + + /** Constructs and initialize a MRP from the array data */ + inline MRP(const Scalar* data) : m_coeffs(data) {} + + /** Copy constructor */ + template EIGEN_STRONG_INLINE MRP(const MRPBase& other) { this->Base::operator=(other); } + + /** Constructs and initializes a MRP from the angle-axis \a aa */ + explicit inline MRP(const AngleAxisType& aa) { *this = aa; } + + /** Constructs and initializes a MRP from either: + * - a rotation matrix expression, + * - a 3D vector expression representing MRP coefficients. + */ + template + explicit inline MRP(const MatrixBase& other) { *this = other; } + + /** Explicit copy constructor with scalar conversion */ + // template + // explicit inline MRP(const MRP& other) + // { m_coeffs = other.coeffs().template cast(); } + + template + static MRP FromTwoVectors(const MatrixBase& a, const MatrixBase& b); + + inline Coefficients& coeffs() { return m_coeffs;} //!< method + inline const Coefficients& coeffs() const { return m_coeffs;} //!< method + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) + + protected: + Coefficients m_coeffs; //!< variable + +#ifndef EIGEN_PARSED_BY_DOXYGEN + /** + Check template parameters + */ + static EIGEN_STRONG_INLINE void _check_template_params() //!< method + { + EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, + INVALID_MATRIX_TEMPLATE_PARAMETERS) + } +#endif + }; + + /** + * single precision MRP type */ + typedef MRP MRPf; + /** + * double precision MRP type */ + typedef MRP MRPd; + + /*************************************************************************** + * Specialization of Map> + ***************************************************************************/ + + namespace internal { + /** struct definition */ + template + /** struct definition */ + struct traits, _Options> > : traits > + { + /** struct definition */ + typedef Map, _Options> Coefficients; + }; + } + + namespace internal { + template + /** struct definition */ + struct traits, _Options> > : traits > + { + /** struct definition */ + typedef Map, _Options> Coefficients; + /** struct definition */ + typedef traits > TraitsBase; + enum { + Flags = TraitsBase::Flags & ~LvalueBit + }; + }; + } + + /** + * \brief MRP expression mapping a constant memory buffer + * + * \tparam _Scalar the type of the MRP coefficients + * \tparam _Options see class Map + * + * This is a specialization of class Map for MRP. This class allows to view + * a 3 scalar memory buffer as an Eigen's MRP object. + * + * \sa class Map, class MRP, class MRPBase + */ + template + class Map, _Options > + : public MRPBase, _Options> > + { + typedef MRPBase, _Options> > Base; + + public: + typedef _Scalar Scalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped MRP object from the pointer \a coeffs + * + * The pointer \a coeffs must reference the three coefficients of MRP in the following order: + * \code *coeffs == {x, y, z} \endcode + * + * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ + EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} + + inline const Coefficients& coeffs() const { return m_coeffs;} //!< method + + protected: + const Coefficients m_coeffs; //!< variable + }; + + /** + * \brief Expression of a MRP from a memory buffer + * + * \tparam _Scalar the type of the MRP coefficients + * \tparam _Options see class Map + * + * This is a specialization of class Map for MRP. This class allows to view + * a 3 scalar memory buffer as an Eigen's MRP object. + * + * \sa class Map, class MRP, class MRPBase + */ + template + class Map, _Options > + : public MRPBase, _Options> > + { + typedef MRPBase, _Options> > Base; + + public: + typedef _Scalar Scalar; //!< variable + typedef typename internal::traits::Coefficients Coefficients; //!< variable + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) + using Base::operator*=; + + /** Constructs a Mapped MRP object from the pointer \a coeffs + * + * The pointer \a coeffs must reference the three coefficients of MRP in the following order: + * \code *coeffs == {x, y, z} \endcode + * + * If the template parameter _Options is set to Aligned, then the pointer coeffs must be aligned. */ + EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} + + inline Coefficients& coeffs() { return m_coeffs; } //!< method + inline const Coefficients& coeffs() const { return m_coeffs; } //!< method + + protected: + Coefficients m_coeffs; //!< variable + }; + + /** + * Map an unaligned array of single precision scalars as a MRP */ + typedef Map, 0> MRPMapf; + /** + * Map an unaligned array of double precision scalars as a MRP */ + typedef Map, 0> MRPMapd; + /** + * Map a 16-byte aligned array of single precision scalars as a MRP */ + typedef Map, Aligned> MRPMapAlignedf; + /** + * Map a 16-byte aligned array of double precision scalars as a MRP */ + typedef Map, Aligned> MRPMapAlignedd; + + /*************************************************************************** + * Implementation of MRPBase methods + ***************************************************************************/ + + // Generic MRP * MRP product + // This product can be specialized for a given architecture via the Arch template argument. + namespace internal { + /*! template definition */ + template struct mrp_product + { + static EIGEN_STRONG_INLINE MRP run(const MRPBase& a, const MRPBase& b){ + Scalar det; //!< variable + Scalar s1N2; //!< variable + Scalar s2N2; //!< variable + MRP s2 = b; + MRP answer; + s1N2 = a.squaredNorm(); + s2N2 = s2.squaredNorm(); + det = Scalar(1.0) + s1N2*s2N2 - 2*a.dot(b); + if (det < 0.01) { + s2 = s2.shadow(); + s2N2 = s2.squaredNorm(); + det = Scalar(1.0) + s1N2*s2N2 - 2*a.dot(b); + } + answer = MRP (((1-s1N2)*s2.vec() + (1-s2N2)*a.vec() - 2*b.vec().cross(a.vec()))/det); + if (answer.squaredNorm() > 1) + answer = answer.shadow(); + return answer; + } + }; + } + + /** \returns the concatenation of two rotations as a MRP-MRP product */ + template + template + EIGEN_STRONG_INLINE MRP::Scalar> + MRPBase::operator* (const MRPBase& other) const + { + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + return internal::mrp_product::Scalar, + internal::traits::IsAligned && internal::traits::IsAligned>::run(*this, other); + } + + /** \sa operator*(MRP) */ + template + template + EIGEN_STRONG_INLINE Derived& MRPBase::operator*= (const MRPBase& other) + { + derived() = derived() * other.derived(); + return derived(); + } + + /** \sa operator*(MRP) */ + template + template + EIGEN_STRONG_INLINE Derived& MRPBase::operator+= (const MRPBase& other) + { + derived().coeffs() = derived().coeffs() + other.derived().coeffs(); + return derived(); + } + /** \sa operator*(MRP) */ + template + template + EIGEN_STRONG_INLINE Derived& MRPBase::operator-= (const MRPBase& other) + { + derived().coeffs() = derived().coeffs() - other.derived().coeffs(); + return derived(); + } + + + /** Rotation of a vector by a MRP. + * \remarks If the MRP is used to rotate several points (>1) + * then it is much more efficient to first convert it to a 3x3 Matrix. + * Comparison of the operation cost for n transformations: + * - MRP2: 30n + * - Via a Matrix3: 24 + 15n + */ + template + EIGEN_STRONG_INLINE typename MRPBase::Vector3 + MRPBase::_transformVector(const Vector3& v) const + { + // Note that this algorithm comes from the optimization by hand + // of the conversion to a Matrix followed by a Matrix/Vector product. + // It appears to be much faster than the common algorithm found + // in the literature (30 versus 39 flops). It also requires two + // Vector3 as temporaries. + Vector3 uv = this->vec().cross(v); + uv += uv; + return v + this->w() * uv + this->vec().cross(uv); + } + + template + EIGEN_STRONG_INLINE MRPBase& MRPBase::operator=(const MRPBase& other) + { + coeffs() = other.coeffs(); + return derived(); + } + + template + template + EIGEN_STRONG_INLINE Derived& MRPBase::operator=(const MRPBase& other) + { + coeffs() = other.coeffs(); + return derived(); + } + + /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this + */ + template + EIGEN_STRONG_INLINE Derived& MRPBase::operator=(const AngleAxisType& aa) + { + using std::tan; + Scalar tanPhi = Scalar(0.25)*aa.angle(); // Scalar(0.25) to suppress precision loss warnings + this->vec() = tanPhi * aa.axis(); + return derived(); + } + + /** Set \c *this from the expression \a xpr: + * - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a MRP + * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix + * and \a xpr is converted to a MRP + */ + + template + template + inline Derived& MRPBase::operator=(const MatrixBase& xpr) + { + EIGEN_STATIC_ASSERT((internal::is_same::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + internal::MRPbase_assign_impl::run(*this, xpr.derived()); + return derived(); + } + + /** Convert the MRP sigma_B/N to a 3x3 rotation matrix [NB]. + */ + template + inline typename MRPBase::Matrix3 + MRPBase::toRotationMatrix(void) const + { + // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) + // if not inlined then the cost of the return by value is huge ~ +35%, + // however, not inlining this function is an order of magnitude slower, so + // it has to be inlined, and so the return by value is not an issue + Matrix3 res; + + const Scalar ps2 = Scalar(1) + this->coeffs().squaredNorm(); + const Scalar ms2 = Scalar(1) - this->coeffs().squaredNorm(); + const Scalar ms2Sq = ms2 * ms2; + const Scalar s1s2 = Scalar(8)*this->x()*this->y(); + const Scalar s1s3 = Scalar(8)*this->x()*this->z(); + const Scalar s2s3 = Scalar(8)*this->y()*this->z(); + const Scalar s1Sq = this->x()*this->x(); + const Scalar s2Sq = this->y()*this->y(); + const Scalar s3Sq = this->z()*this->z(); + + res.coeffRef(0,0) = Scalar(4)*(+s1Sq - s2Sq - s3Sq)+ms2Sq; + res.coeffRef(0,1) = s1s2 - Scalar(4)*this->z()*ms2; + res.coeffRef(0,2) = s1s3 + Scalar(4)*this->y()*ms2; + res.coeffRef(1,0) = s1s2 + Scalar(4)*this->z()*ms2; + res.coeffRef(1,1) = Scalar(4)*(-s1Sq + s2Sq - s3Sq)+ms2Sq; + res.coeffRef(1,2) = s2s3 - Scalar(4)*this->x()*ms2; + res.coeffRef(2,0) = s1s3 - Scalar(4)*this->y()*ms2; + res.coeffRef(2,1) = s2s3 + Scalar(4)*this->x()*ms2; + res.coeffRef(2,2) = Scalar(4)*(-s1Sq - s2Sq + s3Sq)+ms2Sq; + res = res / ps2 / ps2; + + return res; + } + + /** Sets \c *this to be a MRP representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns a reference to \c *this. + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ + template + template + inline Derived& MRPBase::setFromTwoVectors(const MatrixBase& a, const MatrixBase& b) + { + using std::acos; + using std::tan; + + Vector3 v0 = a.normalized(); + Vector3 v1 = b.normalized(); + Scalar c = v1.dot(v0); + + if (c <= 1 && c >= -1) { + Vector3 axis = v0.cross(v1); + axis.normalize(); + + this->vec() = axis*tan(acos(c)/Scalar(4.0)); + } else { + this->vec() << 0., 0., 0.; + } + return derived(); + } + + + /** Returns a MRP representing a rotation between + * the two arbitrary vectors \a a and \a b. In other words, the built + * rotation represent a rotation sending the line of direction \a a + * to the line of direction \a b, both lines passing through the origin. + * + * \returns resulting MRP + * + * Note that the two input vectors do \b not have to be normalized, and + * do not need to have the same norm. + */ + template + template + MRP MRP::FromTwoVectors(const MatrixBase& a, const MatrixBase& b) + { + MRP sig; + sig.setFromTwoVectors(a, b); + return sig; + } + + /** \returns the MRP Shadow set of \c *this + * + * \sa MRPBase::shadow() + */ + template + inline MRP::Scalar> MRPBase::shadow() const + { + Scalar n2 = this->squaredNorm(); + if (n2 > Scalar(0)) + return MRP(-this->coeffs() / n2); + else { + return MRP(0.,0.,0.); + } + } + + /** \returns the MRP [B] matrix of \c *this + * This is used in + * + * d(sigmda_B/N) = 1/4 [B(sigma_B/N)] omega_BN_B + * + * \sa MRPBase::shadow() + */ + template + inline typename MRPBase::Matrix3 MRPBase::Bmat() const + { + Matrix3 B; + Scalar ms2, s1s2, s1s3, s2s3; + + ms2 = Scalar(1) - this->coeffs().squaredNorm(); + s1s2 = this->x()*this->y(); + s1s3 = this->x()*this->z(); + s2s3 = this->y()*this->z(); + + B.coeffRef(0,0) = ms2 + Scalar(2)*this->x()*this->x(); + B.coeffRef(0,1) = Scalar(2)*(s1s2 - this->z()); + B.coeffRef(0,2) = Scalar(2)*(s1s3 + this->y()); + B.coeffRef(1,0) = Scalar(2)*(s1s2 + this->z()); + B.coeffRef(1,1) = ms2 + Scalar(2)*this->y()*this->y(); + B.coeffRef(1,2) = Scalar(2)*(s2s3 - this->x()); + B.coeffRef(2,0) = Scalar(2)*(s1s3 - this->y()); + B.coeffRef(2,1) = Scalar(2)*(s2s3 + this->x()); + B.coeffRef(2,2) = ms2 + Scalar(2)*this->z()*this->z(); + + return B; + } + + /** \returns the multiplicative inverse of \c *this + * Note that in most cases, i.e., if you simply want the opposite rotation, + * and/or the MRP is normalized, then it is enough to use the conjugate. + * + * \sa MRPBase::conjugate() + */ + template + inline MRP::Scalar> MRPBase::inverse() const + { + return MRP(-this->coeffs()); + } + + /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse + * if the MRP is normalized. + * The conjugate of a MRP represents the opposite rotation. + * + * \sa MRP2::inverse() + */ + template + inline MRP::Scalar> + MRPBase::conjugate() const + { + return MRP(-this->coeffs()); + } + + /** \returns the angle (in radian) between two rotations + * \sa dot() + */ + template + template + inline typename internal::traits::Scalar + MRPBase::angularDistance(const MRPBase& other) const + { + using std::atan; + using std::abs; + MRP d = (*this) * other.conjugate(); + return Scalar(4) * atan( d.norm() ); + } + + + + // /** \returns the spherical linear interpolation between the two MRPs + // * \c *this and \a other at the parameter \a t in [0;1]. + // * + // * This represents an interpolation for a constant motion between \c *this and \a other, + // * see also http://en.wikipedia.org/wiki/Slerp. + // */ + // template + // template + // MRP::Scalar> + // MRPBase::slerp(const Scalar& t, const MRPBase& other) const + // { + // using std::acos; + // using std::sin; + // using std::abs; + // static const Scalar one = Scalar(1) - NumTraits::epsilon(); + // Scalar d = this->dot(other); + // Scalar absD = abs(d); + // + // Scalar scale0; + // Scalar scale1; + // + // if(absD>=one) + // { + // scale0 = Scalar(1) - t; + // scale1 = t; + // } + // else + // { + // // theta is the angle between the 2 MRPs + // Scalar theta = acos(absD); + // Scalar sinTheta = sin(theta); + // + // scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; + // scale1 = sin( ( t * theta) ) / sinTheta; + // } + // if(d(scale0 * coeffs() + scale1 * other.coeffs()); + // } + + namespace internal { + + // set from a rotation matrix + // this maps the [NB] DCM to the equivalent sigma_B/N set + template + /*! struct definition */ + struct MRPbase_assign_impl + { + typedef typename Other::Scalar Scalar; //!< variable + typedef DenseIndex Index; //!< variable + /** Class Definition */ + template static inline void run(MRPBase& sig, const Other& mat) + { + Quaternion q; + Scalar num; + + /* convert DCM to quaternions */ + q = mat; + num = Scalar(1) + q.w(); + + /* convert quaternions to MRP */ + sig.x() = q.x()/num; + sig.y() = q.y()/num; + sig.z() = q.z()/num; + } + }; + + // set from a vector of coefficients assumed to be a MRP + template + /** + structure definition + */ + struct MRPbase_assign_impl + { + typedef typename Other::Scalar Scalar; //!< variable + /** Class definition */ + template static inline void run(MRPBase& q, const Other& vec) + { + q.coeffs() = vec; + } + }; + + } // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_MRP_H + +/// \endcond diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.cpp new file mode 100644 index 0000000000..04f712b75c --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.cpp @@ -0,0 +1,299 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include +#include +#include "avsEigenSupport.h" +#include "rigidBodyKinematics.h" +#include "architecture/utilities/macroDefinitions.h" + +/* + + Contains various support algorithms related to using the Eigen Library + + */ + +/*! This function provides a general conversion between an Eigen matrix and +an output C array. Note that this routine would convert an inbound type +to a MatrixXd and then transpose the matrix which would be inefficient +in a lot of cases. +@param inMat The source Eigen matrix that we are converting +@param outArray The destination array (sized by the user!) we copy into +*/ +void eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double *outArray) +{ + Eigen::MatrixXd tempMat = inMat.transpose(); + memcpy(outArray, tempMat.data(), inMat.rows()*inMat.cols()*sizeof(double)); +} + +/*! This function provides a general conversion between an Eigen matrix and +an output C array. Note that this routine would convert an inbound type +to a MatrixXd and then transpose the matrix which would be inefficient +in a lot of cases. + +@param inMat The source Eigen matrix that we are converting +@param outArray The destination array (sized by the user!) we copy into +*/ +void eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int *outArray) +{ + Eigen::MatrixXi tempMat = inMat.transpose(); + memcpy(outArray, tempMat.data(), inMat.rows()*inMat.cols()*sizeof(int)); +} + +/*! This function provides a direct conversion between a 3-vector and an +output C array. We are providing this function to save on the inline conversion +and the transpose that would have been performed by the general case. + +@param inMat The source Eigen matrix that we are converting +@param outArray The destination array we copy into +*/ +void eigenVector3d2CArray(Eigen::Vector3d & inMat, double *outArray) +{ + memcpy(outArray, inMat.data(), 3 * sizeof(double)); +} + +/*! This function provides a direct conversion between an MRP and an +output C array. We are providing this function to save on the inline conversion +and the transpose that would have been performed by the general case. + +@param inMat The source Eigen MRP that we are converting +@param outArray The destination array we copy into +*/ +void eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray) +{ + memcpy(outArray, inMat.data(), 3 * sizeof(double)); +} + +/*! This function provides a direct conversion between a 3x3 matrix and an +output C array. We are providing this function to save on the inline conversion +that would have been performed by the general case. + +@param inMat The source Eigen matrix that we are converting +@param outArray The destination array we copy into +*/ +void eigenMatrix3d2CArray(Eigen::Matrix3d & inMat, double *outArray) +{ + Eigen::MatrixXd tempMat = inMat.transpose(); + memcpy(outArray, tempMat.data(), 9 * sizeof(double)); +} + +/*! This function performs the general conversion between an input C array +and an Eigen matrix. Note that to use this function the user MUST size +the Eigen matrix ahead of time so that the internal map call has enough +information to ingest the C array. +@return Eigen::MatrixXd +@param inArray The input array (row-major) +@param nRows +@param nCols +*/ +Eigen::MatrixXd cArray2EigenMatrixXd(double *inArray, int nRows, int nCols) +{ + Eigen::MatrixXd outMat; + outMat.resize(nRows, nCols); + outMat = Eigen::Map(inArray, outMat.rows(), outMat.cols()); + return outMat; +} + +/*! This function performs the conversion between an input C array +3-vector and an output Eigen vector3d. This function is provided +in order to save an unnecessary conversion between types. +@return Eigen::Vector3d +@param inArray The input array (row-major) +*/ +Eigen::Vector3d cArray2EigenVector3d(double *inArray) +{ + return Eigen::Map(inArray, 3, 1); +} + +/*! This function performs the conversion between an input C array +3-vector and an output Eigen MRPd. This function is provided +in order to save an unnecessary conversion between types. +@return Eigen::MRPd +@param inArray The input array (row-major) +*/ +Eigen::MRPd cArray2EigenMRPd(double* inArray) +{ + Eigen::MRPd sigma_Eigen; + sigma_Eigen = cArray2EigenVector3d(inArray); + + return sigma_Eigen; +} + +/*! This function performs the conversion between an input C array +3x3-matrix and an output Eigen vector3d. This function is provided +in order to save an unnecessary conversion between types. +@return Eigen::Matrix3d +@param inArray The input array (row-major) +*/ +Eigen::Matrix3d cArray2EigenMatrix3d(double *inArray) +{ + return Eigen::Map(inArray, 3, 3).transpose(); +} + +/*! This function performs the conversion between an input C 3x3 +2D-array and an output Eigen vector3d. This function is provided +in order to save an unnecessary conversion between types +@return Eigen::Matrix3d +@param in2DArray The input 2D-array +*/ +Eigen::Matrix3d c2DArray2EigenMatrix3d(double in2DArray[3][3]) +{ + Eigen::Matrix3d outMat; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + outMat(i, j) = in2DArray[i][j]; + } + } + + return outMat; +} + +/*! This function returns the Eigen DCM that corresponds to a 1-axis rotation + by the angle theta. The DCM is the positive theta rotation from the original + frame to the final frame. + @return Eigen::Matrix3d + @param angle The input rotation angle + */ +Eigen::Matrix3d eigenM1(double angle) +{ + Eigen::Matrix3d mOut; + + mOut.setIdentity(); + + mOut(1,1) = cos(angle); + mOut(1,2) = sin(angle); + mOut(2,1) = -mOut(1,2); + mOut(2,2) = mOut(1,1); + + return mOut; +} + +/*! This function returns the Eigen DCM that corresponds to a 2-axis rotation + by the angle theta. The DCM is the positive theta rotation from the original + frame to the final frame. + @return Eigen::Matrix3d + @param angle The input rotation angle + */ +Eigen::Matrix3d eigenM2(double angle) +{ + Eigen::Matrix3d mOut; + + mOut.setIdentity(); + + mOut(0,0) = cos(angle); + mOut(0,2) = -sin(angle); + mOut(2,0) = -mOut(0,2); + mOut(2,2) = mOut(0,0); + + return mOut; +} + +/*! This function returns the Eigen DCM that corresponds to a 3-axis rotation + by the angle theta. The DCM is the positive theta rotation from the original + frame to the final frame. + @return Eigen::Matrix3d + @param angle The input rotation angle + */ +Eigen::Matrix3d eigenM3(double angle) +{ + Eigen::Matrix3d mOut; + + mOut.setIdentity(); + + mOut(0,0) = cos(angle); + mOut(0,1) = sin(angle); + mOut(1,0) = -mOut(0,1); + mOut(1,1) = mOut(0,0); + + return mOut; +} + +/*! This function returns the tilde matrix version of a vector. The tilde + matrix is the matrixi equivalent of a vector cross product, where + [tilde_a] b == a x b + @return Eigen::Matrix3d + @param vec The input vector + */ +Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec) +{ + Eigen::Matrix3d mOut; + + mOut(0,0) = mOut(1,1) = mOut(2,2) = 0.0; + + mOut(0,1) = -vec(2); + mOut(1,0) = vec(2); + mOut(0,2) = vec(1); + mOut(2,0) = -vec(1); + mOut(1,2) = -vec(0); + mOut(2,1) = vec(0); + + return mOut; +} + +/*! This function converts the Eigen DCM to an Eigen MRPd + @return Eigen::MRPd + @param dcm_Eigen The input DCM + */ +Eigen::MRPd eigenC2MRP(Eigen::Matrix3d dcm_Eigen) +{ + Eigen::MRPd sigma_Eigen; // output Eigen MRP + double dcm_Array[9]; // C array DCM + double sigma_Array[3]; // C array MRP + + eigenMatrix3d2CArray(dcm_Eigen, dcm_Array); + C2MRP(RECAST3X3 dcm_Array, sigma_Array); + sigma_Eigen = cArray2EigenVector3d(sigma_Array); + + return sigma_Eigen; +} + +/*! This function converts the Eigen MRPd to Vector3d + @return Eigen::Vector3d + @param mrp The input Vector3d variable + */ +Eigen::Vector3d eigenMRPd2Vector3d(Eigen::MRPd mrp) +{ + Eigen::Vector3d vec3d; + + vec3d[0] = mrp.x(); + vec3d[1] = mrp.y(); + vec3d[2] = mrp.z(); + + return vec3d; +} + +/*! This function solves for the zero of the passed function using the Newton Raphson Method +@return double +@param initialEstimate The initial value to use for newton-raphson +@param accuracy The desired upper bound for the error +@param f Function to find the zero of +@param fPrime First derivative of the function +*/ +double newtonRaphsonSolve(const double& initialEstimate, const double& accuracy, const std::function& f, const std::function& fPrime) { + double currentEstimate = initialEstimate; + for (int i = 0; i < 100; i++) { + if (std::abs(f(currentEstimate)) < accuracy) + break; + + double functionVal = f(currentEstimate); + double functionDeriv = fPrime(currentEstimate); + currentEstimate = currentEstimate - functionVal/functionDeriv; + } + return currentEstimate; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.h new file mode 100644 index 0000000000..8cdde9d480 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/avsEigenSupport.h @@ -0,0 +1,64 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + + +#ifndef _AVSEIGENSUPPORT_ +#define _AVSEIGENSUPPORT_ +#include +#include "avsEigenMRP.h" + + +//!@brief General conversion between any Eigen matrix and output array +void eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double *outArray); +//!@brief General conversion between any Eigen matrix and output array +void eigenMatrixXi2CArray(Eigen::MatrixXi inMat, int *outArray); +//!@brief Rapid conversion between 3-vector and output array +void eigenVector3d2CArray(Eigen::Vector3d & inMat, double *outArray); +//!@brief Rapid conversion between MRP and output array +void eigenMRPd2CArray(Eigen::Vector3d& inMat, double* outArray); +//!@brief Rapid conversion between 3x3 matrix and output array +void eigenMatrix3d2CArray(Eigen::Matrix3d & inMat, double *outArray); +//!@brief General conversion between a C array and an Eigen matrix +Eigen::MatrixXd cArray2EigenMatrixXd(double *inArray, int nRows, int nCols); +//!@brief Specific conversion between a C array and an Eigen 3-vector +Eigen::Vector3d cArray2EigenVector3d(double *inArray); +//!@brief Specific conversion between a C array and an Eigen MRPs +Eigen::MRPd cArray2EigenMRPd(double* inArray); +//!@brief Specfici conversion between a C array and an Eigen 3x3 matrix +Eigen::Matrix3d cArray2EigenMatrix3d(double *inArray); +//!@brief Specfici conversion between a C 2D array and an Eigen 3x3 matrix +Eigen::Matrix3d c2DArray2EigenMatrix3d(double in2DArray[3][3]); +//!@brief returns the first axis DCM with the input angle +Eigen::Matrix3d eigenM1(double angle); +//!@brief returns the second axis DCM with the input angle +Eigen::Matrix3d eigenM2(double angle); +//!@brief returns the third axis DCM with the input angle +Eigen::Matrix3d eigenM3(double angle); +//!@brief returns the tilde matrix representation of a vector (equivalent to a vector cross product) +Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec); +//!@brief converts MRPd to an Vector3d variable +Eigen::Vector3d eigenMRPd2Vector3d(Eigen::MRPd vec); +//!@brief maps the DCM to MRPs using Eigen variables +Eigen::MRPd eigenC2MRP(Eigen::Matrix3d); + +//!@brief solves for the zero of the provided function +double newtonRaphsonSolve(const double& initialEstimate, const double& accuracy, const std::function& f, const std::function& fPrime); + + +#endif /* _AVSEIGENSUPPORT_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bilinearInterpolation.hpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bilinearInterpolation.hpp new file mode 100644 index 0000000000..4d3360362e --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bilinearInterpolation.hpp @@ -0,0 +1,58 @@ +/* + ISC License + + Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef BILINEARINTERPOLATION_H +#define BILINEARINTERPOLATION_H + +#include + +/*! This function uses bilinear interpolation to solve for the value of an unknown function of two variables f(x,y) + at the point (x,y). +@return double +@param x1 Data point x1 +@param x2 Data point x2 +@param y1 Data point y1 +@param y2 Data point y2 +@param z11 Function value at point (x1, y1) +@param z12 Function value at point (x1, y2) +@param z21 Function value at point (x2, y1) +@param z22 Function value at point (x2, y2) +@param x Function x coordinate for interpolation +@param y Function y coordinate for interpolation +*/ +double bilinearInterpolation(double x1, + double x2, + double y1, + double y2, + double z11, + double z12, + double z21, + double z22, + double x, + double y) { + + assert(x1 < x && x < x2); + assert(y1 < y && y < y2); + + return 1 / ((x2 - x1) * (y2 - y1)) * (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) + + z12 * (x2 - x) * (y - y1) + + z22 * (x - x1) * (y - y1)); +} + +#endif // BILINEARINTERPOLATION_H diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.cpp new file mode 100644 index 0000000000..b1e6f8620f --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.cpp @@ -0,0 +1,153 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +/// \cond DO_NOT_DOCUMENT + +#include +#include +#include +#include "architecture/utilities/bskLogging.h" + +logLevel_t LogLevel = BSK_DEBUG; + +/*! This method sets the default logging verbosity + @param logLevel + */ +void setDefaultLogLevel(logLevel_t logLevel) +{ + LogLevel = logLevel; +} + +/*! This method gets the default logging verbosity */ +logLevel_t getDefaultLogLevel() +{ + return LogLevel; +} + +/*! This method prints the default logging verbosity */ +void printDefaultLogLevel() +{ + std::map logLevelMap + { + {0, "BSK_DEBUG"}, + {1, "BSK_INFORMATION"}, + {2, "BSK_WARNING"}, + {3, "BSK_ERROR"}, + {4, "BSK_SILENT"} + }; + const char* defaultLevelStr = logLevelMap[LogLevel]; + printf("Default Logging Level: %s\n", defaultLevelStr); +} + +/*! The constructor initialises the logger for a module and uses default verbostiy level for logging */ +BSKLogger::BSKLogger() +{ + //Default print verbosity + this->_logLevel = getDefaultLogLevel(); +} + +/*! The constructor initialises the logger for a module and uses a user defined verbostiy level for logging + @param logLevel + */ +BSKLogger::BSKLogger(logLevel_t logLevel) +{ + this->_logLevel = logLevel; +} + +/*! This method changes the logging verbosity after construction + */ +void BSKLogger::setLogLevel(logLevel_t logLevel) +{ + this->_logLevel = logLevel; +} + +/*! This method reads the current logging verbosity */ +void BSKLogger::printLogLevel() +{ + const char* currLevelStr = this->logLevelMap[this->_logLevel]; + printf("Current Logging Level: %s\n", currLevelStr); +} + +/*! Get the current log level value */ +int BSKLogger::getLogLevel() +{ + return this->_logLevel; +} + +/*! This method logs information. The current behavior is to simply print out the message and the targeted logging level. + This should be the main method called in user code. + @param targetLevel + @param info +*/ +void BSKLogger::bskLog(logLevel_t targetLevel, const char* info, ...) +{ + const char* targetLevelStr = this->logLevelMap[targetLevel]; + char formatMessage[MAX_LOGGING_LENGTH]; + va_list args; + va_start (args, info); + vsnprintf(formatMessage, sizeof(formatMessage), info, args); + va_end(args); + + // Raise an error that swig can pipe to python + if(targetLevel == BSK_ERROR) + { + throw BasiliskError(formatMessage); + } + // Otherwise, print the message accordingly + if(targetLevel >= this->_logLevel) + { + printf("%s: %s\n", targetLevelStr, formatMessage); + } +} + +/*! Section contains C interfacing to C++ object */ +EXTERN BSKLogger* _BSKLogger(void) +{ + return new BSKLogger(); +} + +EXTERN void _BSKLogger_d(BSKLogger* bskLogger) +{ + delete bskLogger; +} + +/*! This method reads the current logging verbosity */ +EXTERN void _printLogLevel(BSKLogger* bskLogger) +{ + bskLogger->printLogLevel(); +} + +/*! This method changes the logging verbosity after construction + @param bskLogger + @param logLevel + */ +EXTERN void _setLogLevel(BSKLogger* bskLogger, logLevel_t logLevel) +{ + bskLogger->setLogLevel(logLevel); +} + +/*! This method logs information. The current behavior is to simply print out the message and the targeted logging level. + This should be the main method called in user code. +*/ +EXTERN void _bskLog(BSKLogger* bskLogger, logLevel_t logLevel, const char* info) +{ + bskLogger->bskLog(logLevel, info); +} + +/// \endcond diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.h new file mode 100644 index 0000000000..f00daa31dc --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.h @@ -0,0 +1,111 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + + + +#ifndef _BSK_LOG_ +#define _BSK_LOG_ + + +//maximum length of info to log in a reference to BSKLogging in C, not relevant in C++ +#define MAX_LOGGING_LENGTH 255 + +typedef enum { + BSK_DEBUG, + BSK_INFORMATION, + BSK_WARNING, + BSK_ERROR, + BSK_SILENT // the coder should never use this flag when using bskLog(). It is used to turn off all output +} logLevel_t; + +extern logLevel_t LogLevel; +void printDefaultLogLevel(); + +/// \cond DO_NOT_DOCUMENT + +#ifdef __cplusplus +#include +#include +#include + +#ifdef SWIG +%include "std_except.i" +#endif + +void setDefaultLogLevel(logLevel_t logLevel); +logLevel_t getDefaultLogLevel(); + +/*! Custom Basilisk runtime error class */ +class BasiliskError : public std::runtime_error { +public: + explicit BasiliskError(const std::string& message) + : std::runtime_error(message) {} + + explicit BasiliskError(const char* message) + : std::runtime_error(message) {} +}; + + +/*! BSK logging class */ +class BSKLogger +{ + public: + BSKLogger(); + BSKLogger(logLevel_t logLevel); + virtual ~BSKLogger() = default; + void setLogLevel(logLevel_t logLevel); + void printLogLevel(); + int getLogLevel(); + void bskLog(logLevel_t targetLevel, const char* info, ...); + + //Provides a mapping from log level enum to str + public: + std::map logLevelMap + { + {0, "BSK_DEBUG"}, + {1, "\033[92mBSK_INFORMATION\033[0m"}, + {2, "\033[93mBSK_WARNING\033[0m"}, + {3, "\033[91mBSK_ERROR\033[0m"}, + {4, "BSK_SILENT"} + }; + + private: + logLevel_t _logLevel; +}; + +#else +typedef struct BSKLogger BSKLogger; +#endif + +#ifdef __cplusplus + #define EXTERN extern "C" +#else + #define EXTERN +#endif + +EXTERN BSKLogger* _BSKLogger(void); +EXTERN void _BSKLogger_d(BSKLogger*); +EXTERN void _printLogLevel(BSKLogger*); +EXTERN void _setLogLevel(BSKLogger*, logLevel_t); +EXTERN void _bskLog(BSKLogger*, logLevel_t, const char*); + + +/// \endcond + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.rst b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.rst new file mode 100644 index 0000000000..f9a9856485 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskLogging.rst @@ -0,0 +1,117 @@ +Executive Summary +----------------- +This support class ``bskLogger`` enables C++ ``bskLog()`` and ANSI-C ``_bskLog()`` method to be used to log various debug, information, warning and error messages. The verbosity, i.e. what level of messages are printed to the terminal, can be set from the Basilisk python script. + +.. table:: Verbosity Level Options + :widths: 25 25 100 + + +-----------------------+---------------------------------+---------------------------------------------------+ + | Level | Description | + +=======================+=================================+===================================================+ + | BSK_DEBUG | Can be used for debug information logging. Such ``bskLog`` statement should not be | + | | left in the final Basilisk code. | + +-----------------------+---------------------------------+---------------------------------------------------+ + | BSK_INFORMATION | General information messages | + +-----------------------+---------------------------------+---------------------------------------------------+ + | BSK_WARNING | Warnings about unexpected behavior, but not outright errors. | + +-----------------------+---------------------------------+---------------------------------------------------+ + | BSK_ERROR | Erroneous behavior that needs to be fixed. Errors raised at this level will raise a | + | | ``BasiliskError`` exception and stop the simulation immediately. | + +-----------------------+---------------------------------+---------------------------------------------------+ + | BSK_SILENT | This level is used to silence all `bskLog` statements. This should never be used | + | | with the `bskLog` method within the C++ or C code. | + +-----------------------+---------------------------------+---------------------------------------------------+ + + +Class Assumptions and Limitations +---------------------------------- +The ``bskLogger`` class is intended to be used primarily within the BSK modules. All the modules must have been initialized before the ``bskLog`` becomes effective. Thus, if ``bskLog`` is used during initialization to print a warning, this will not function as expected. + +For utility libraries such as ``linearAlgebra.c/h`` etc., this logging capabilities is not applicable as these libraries don't have access to the ``bskLogger`` instance. Rather, in such cases use the ``BSK_PRINT()`` macro instead. + + + +Using ``bskLogger`` From Python +------------------------------- +For an example of how to set the verbosity from Python, see :ref:`scenarioBskLog`. +The default verbosity is set to the lowest level ``BSK_DEBUG`` such that any ``bskLog`` method print out the associated message string. If this is the desired behavior, then no further actions are required. + +If the verbosity level is to be changed for a particular Basilisk script, then the following instructions explain how this can be done. At the top of the Basilisk python scrip be sure to include the ``bskLogging`` support package:: + + from Basilisk.architecture import bskLogging + +Setting Verbosity Globally for all BSK Modules +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The ``bskLog`` verbosity can be modified for all Basilisk modules by using:: + + bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) + +The verbosity options are listed in the table above. Note that this command must be included at the very beginning of +the Basilisk simulation script, certainly before the call for ``SimulationBaseClass.SimBaseClass()``. + +Changing Verbosity for a Particular BSK Module +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +It is possible to override the global verbosity setting and specify a different verbosity for a particular module. Assume we want to have a unique verbosity level for the ``simpleNav`` module. This can be done through:: + + sNavObject = simpleNav.SimpleNav() + scSim.AddModelToTask(simTaskName, sNavObject) + logger = bskLogging.BSKLogger() + logger.setLogLevel(bskLogging.BSK_INFORMATION) + sNavObject.bskLogger = logger + +Another option is to use the ``BSKLogger()`` constructor to provide the verbosity directly through:: + + sNavObject = simpleNav.SimpleNav() + sNavObject.bskLogger = bskLogging.BSKLogger(bskLogging.BSK_INFORMATION) + +Unlike change the global verbosity level, the module specific verbosity can be changed later on in the Basilisk +python script as the corresponding module is created and configured. + +Using ``bskLog`` in C++ Basilisk Modules +---------------------------------------- +The first step is to include the ``bskLogging`` support file with the module `*.h` file using: + +.. code-block:: cpp + + #include "architecture/utilities/bskLogging.h" + +Next, the module class must contain the following public variable: + +.. code-block:: cpp + + BSKLogger bskLogger; + +Within the ``*.cpp`` file, the ``bskLog()`` method can be called with: + +.. code-block:: cpp + + bskLogger.bskLog(BSK_INFORMATION, "%d %d", arg1, arg2); + + +Using ``_bskLog`` in C Basilisk Modules +--------------------------------------- +The first step is to include the ``bskLogging`` support file with the module ``*.h`` file using: + +.. code-block:: c + + #include "architecture/utilities/bskLogging.h" + +The C-module configuration structure must contain a pointer to the ``BSKLogger`` type using: + +.. code-block:: c + + BSKLogger *bskLogger; + +The ``_bskLog`` only accepts char*/string, so the formatting must be done before logging call. If it is a simple message without any variables being included, then you can use: + +.. code-block:: c + + _bskLog(configData->bskLogger, BSK_INFORMATION, "Fixed String"); + +If you want to print variables to the logging string, this must be done before calling ``_bskLog``, such as in this example: + +.. code-block:: c + + char info[MAX_LOGGING_LENGTH]; + sprintf(info, "Variable is too large (%d). Setting to max value.", variable); + _bskLog(configData->bskLogger, BSK_ERROR, info); diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskSemaphore.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskSemaphore.h new file mode 100644 index 0000000000..14d27377e7 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bskSemaphore.h @@ -0,0 +1,66 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef COMMON_UTILS_SEMAPHORE_H +#define COMMON_UTILS_SEMAPHORE_H +// https://riptutorial.com/cplusplus/example/30142/semaphore-cplusplus-11 + +#include +#include + + +/*! Basilisk semaphore class */ +class BSKSemaphore +{ + std::mutex mutex; + std::condition_variable cv; + size_t count; + +public: + /*! method description */ + BSKSemaphore(int count_in = 0) + : count(count_in) + { + } + + /*! release the lock */ + inline void release() + { + { + std::unique_lock lock(mutex); + ++count; + //notify the waiting thread + } + cv.notify_one(); + } + + /*! aquire the lock */ + inline void acquire() + { + std::unique_lock lock(mutex); + while (count == 0) + { + //wait on the mutex until notify is called + cv.wait(lock); + } + --count; + } +}; + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bsk_Print.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bsk_Print.h new file mode 100644 index 0000000000..05b7616de2 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/bsk_Print.h @@ -0,0 +1,59 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _BSK_PRINT_ +#define _BSK_PRINT_ + +#include + +typedef enum { + MSG_DEBUG, + MSG_INFORMATION, + MSG_WARNING, + MSG_ERROR, + MSG_SILENT // the coder should never use this flag when using BSK_PRINT(). It is used to turn off all BSK_PRINT() +} msgLevel_t_; + +/* specify the BSK printing verbosity level. + */ +#define MSG_LEVEL MSG_DEBUG + + +#define EXPAND(x) x + +#define BSK_MESSAGE(...) { printf(__VA_ARGS__); } + +#ifdef _WIN32 + +#define BSK_PRINT(X, _fmt, ...) if (EXPAND(X) >= MSG_LEVEL) {printf(#X ": " _fmt "\n", __VA_ARGS__);} +#define BSK_PRINT_BRIEF(X, _fmt, ...) if (EXPAND(X) >= MSG_LEVEL) {printf(#X ": " _fmt "\n", __VA_ARGS__);} + + +#else /* macOS and Linux */ + +#define WHERESTR "[FILE : %s, FUNC : %s, LINE : %d]:\n" +#define WHEREARG __FILE__,__func__,__LINE__ +#define BSK_PRINT(X, _fmt, ...) if(X >= MSG_LEVEL) \ + BSK_MESSAGE(WHERESTR #X ": " _fmt "\n", WHEREARG, ##__VA_ARGS__) +#define BSK_PRINT_BRIEF(X, _fmt, ...) if(X >= MSG_LEVEL) \ + BSK_MESSAGE(#X ": " _fmt "\n", ##__VA_ARGS__) + +#endif + +#endif /* BSK_PRINT_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.cpp new file mode 100644 index 0000000000..b3262ed2dd --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.cpp @@ -0,0 +1,95 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "discretize.h" +#include + +/*! The constructor initialies the random number generator used for the walks*/ +Discretize::Discretize() +{ + this->numStates = 0; + this->roundDirection = TO_ZERO; + this->carryError = false; +} + +/*! This lets the user initialized the discretization model to the right size*/ +Discretize::Discretize(uint8_t numStates) : Discretize() { + this->numStates = numStates; + this->LSB.resize(this->numStates); + this->LSB.fill(0.0); + this->discErrors.resize(this->numStates); + this->discErrors.fill(0.0); +} + +/*! The destructor is a placeholder for one that might do something*/ +Discretize::~Discretize() +{ +} + +///*! This method calculates the least significant bit size given the maximum state value, +// minimum state value, and number of bits to use.. +// */ +//void setLSBByBits(uint8_t numBits, double min, double max); + +/*!@brief Sets the round direction (toZero, fromZero, near) for discretization + @param direction + */ +void Discretize::setRoundDirection(roundDirection_t direction){ + + this->roundDirection = direction; + + return; +} + + +/*!@brief Discretizes the given truth vector according to a least significant bit (binSize) + @param undiscretizedVector + @return vector of discretized values*/ +Eigen::VectorXd Discretize::discretize(Eigen::VectorXd undiscretizedVector){ + + if (this->carryError){ + undiscretizedVector += this->discErrors; + } + + //discretize the data + Eigen::VectorXd workingVector = undiscretizedVector.cwiseQuotient(this->LSB); + workingVector = workingVector.cwiseAbs(); + + if (this->roundDirection == TO_ZERO){ + for (uint8_t i = 0; i < this->numStates; i++){ + workingVector[i] = floor(workingVector[i]); + } + }else if(this->roundDirection == FROM_ZERO){ + for (uint8_t i = 0; i < this->numStates; i++){ + workingVector[i] = ceil(workingVector[i]); + } + }else if(this->roundDirection == NEAR){ + for (uint8_t i = 0; i < this->numStates; i++){ + workingVector[i] = round(workingVector[i]); + } + } + + workingVector = workingVector.cwiseProduct(this->LSB); + for (uint8_t i = 0; i < this->numStates; i++){ + workingVector[i] = copysign(workingVector[i], undiscretizedVector[i]); + } + this->discErrors = undiscretizedVector - workingVector; + + return workingVector; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.h new file mode 100644 index 0000000000..e6baec7f0a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/discretize.h @@ -0,0 +1,78 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _discretize_HH_ +#define _discretize_HH_ + +#include +#include +#include + + +typedef enum { + TO_ZERO, + FROM_ZERO, + NEAR +} roundDirection_t; + +/*! This module discretizes data for output. It has the option to carry over discretization error or not. +*/ +class Discretize +{ + +public: + Discretize(); + Discretize(uint8_t numStates); + ~Discretize(); +// void setLSBByBits(uint8_t numBits, double min, double max); +// /*!@brief Method determines the size of an output data bin (bit-value) making sure that zero is +// a possible output and giving proportionate numbers of bits to the size of max and min void*/ + + /*!@brief Avoid calculating bit value (bin size) and just set it because a resolution is known + @param givenLSB + */ + void setLSB(Eigen::VectorXd givenLSB) {this->LSB = givenLSB;} + + void setRoundDirection(roundDirection_t direction); + + /*!@brief Sets the round direction (toZero, fromZero, near) for discretization + @param carryErrorIn + */ + void setCarryError(bool carryErrorIn){this->carryError = carryErrorIn;} + + /*!@brief Discretizes the given truth vector according to a least significant bit (binSize) + @param undiscretizedVector + @return vector of discretized values*/ + Eigen::VectorXd discretize(Eigen::VectorXd undiscretizedVector); + + /*!@brief Get the discretization errors + @return the errors due to discretization in a corresponding vector*/ + Eigen::VectorXd getDiscretizationErrors(){return(this->discErrors);} + + Eigen::VectorXd LSB; //!< -- size of bin, bit value, least significant bit + +private: + roundDirection_t roundDirection; //!< -- Direction to round when discretizing. "toZero", "fromZero", and "near" are the options. + uint8_t numStates; //!< -- Number of states to be discretized (length of vector fed in) + Eigen::VectorXd discErrors; //!< -- Errors from discretization. Can be returned to adjusted integrated values. + bool carryError; //!< -- true if discError should be added next time around, false if not. +}; + + +#endif /* _discretize_HH_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.cpp new file mode 100644 index 0000000000..ba9b50e9a0 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.cpp @@ -0,0 +1,105 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include +#include "gauss_markov.h" +#include "linearAlgebra.h" + +/*! The constructor initialies the random number generator used for the walks*/ +GaussMarkov::GaussMarkov() +{ + this->RNGSeed = 0x1badcad1; + this->numStates = 0; + initializeRNG(); +} + +GaussMarkov::GaussMarkov(uint64_t size, uint64_t newSeed) +{ + this->RNGSeed = newSeed; + this->numStates = size; + initializeRNG(); + this->propMatrix.resize(size,size); + this->propMatrix.fill(0.0); + this->currentState.resize((int64_t) size); + this->currentState.fill(0.0); + this->noiseMatrix.resize((int64_t) size, (int64_t) size); + this->noiseMatrix.fill(0.0); + this->stateBounds.resize((int64_t) size); + this->stateBounds.fill(DEFAULT_BOUND); +} + +void GaussMarkov::initializeRNG() { + //! - Set up standard normal distribution N(0,1) parameters for random number generation + std::normal_distribution::param_type updatePair(0.0, 1.0); + this->rGen.seed((unsigned int)this->RNGSeed); + this->rNum.param(updatePair); +} + +/*! The destructor is a placeholder for one that might do something*/ +GaussMarkov::~GaussMarkov() +{ +} + +/*! This method performs almost all of the work for the Gauss Markov random + walk. It uses the current random walk configuration, propagates the current + state, and then applies appropriate errors to the states to set the current + error level. +*/ +void GaussMarkov::computeNextState() +{ + Eigen::VectorXd errorVector; + Eigen::VectorXd ranNums; + + //! - Check for consistent sizes + if((this->propMatrix.size() != this->noiseMatrix.size()) || + ((uint64_t) this->propMatrix.size() != this->numStates*this->numStates)) + { + bskLogger.bskLog(BSK_ERROR, "Matrix size mismatch in Gauss Markov model"); + return; + } + if((uint64_t) this->stateBounds.size() != this->numStates) + { + bskLogger.bskLog(BSK_ERROR, "State bounds size mismatch in Gauss Markov model"); + return; + } + + //! - Generate base random numbers + ranNums.resize((int64_t) this->numStates); + for(size_t i = 0; i < this->numStates; i++) { + ranNums[i] = this->rNum(rGen); + } + + //! - Apply noise first + errorVector = this->noiseMatrix * ranNums; + + //! - Then propagate previous state + this->currentState = this->propMatrix * this->currentState; + + //! - Add noise to propagated state + this->currentState += errorVector; + + //! - Apply bounds if needed + for(size_t i = 0; i < this->numStates; i++) { + if(this->stateBounds[i] > 0.0) { + if(fabs(this->currentState[i]) > this->stateBounds[i]) { + this->currentState[i] = copysign(this->stateBounds[i], this->currentState[i]); + } + } + } +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.h new file mode 100644 index 0000000000..cc9d65455a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/gauss_markov.h @@ -0,0 +1,100 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _GaussMarkov_HH_ +#define _GaussMarkov_HH_ + +#include +#include +#include +#include +#include +#include "architecture/utilities/bskLogging.h" + + +/*! @brief This module is used to apply a second-order bounded Gauss-Markov random walk + on top of an upper level process. The intent is that the caller will perform + the set methods (setUpperBounds, setNoiseMatrix, setPropMatrix) as often as + they need to, call computeNextState, and then call getCurrentState cyclically +*/ +class GaussMarkov +{ + +public: + GaussMarkov(); + GaussMarkov(uint64_t size, uint64_t newSeed = 0x1badcad1); //!< class constructor + ~GaussMarkov(); + void computeNextState(); + + /*!@brief Method does just what it says, seeds the random number generator + @param newSeed The seed to use in the random number generator + */ + void setRNGSeed(uint64_t newSeed) {rGen.seed((unsigned int)newSeed); RNGSeed = newSeed;} + + /*!@brief Method returns the current random walk state from model + @return The private currentState which is the vector of random walk values*/ + Eigen::VectorXd getCurrentState() {return(currentState);} + + /*!@brief Set the upper bounds on the random walk to newBounds + @param newBounds the bounds to put on the random walk states + */ + void setUpperBounds(Eigen::VectorXd newBounds) { + // For normal distribution, ~99.7% of values fall within ±3σ + // So bounds should be at least 3x the standard deviation + for(int i = 0; i < noiseMatrix.rows(); i++) { + // Only check for warning if bounds are positive (random walk enabled) + // and noise is non-zero (random walk active) + if(newBounds[i] > 0 && noiseMatrix(i,i) > 0 && newBounds[i] < 3.0 * noiseMatrix(i,i)) { + bskLogger.bskLog(BSK_WARNING, "GaussMarkov bounds set tighter than 3σ - distribution will be truncated"); + } + } + stateBounds = newBounds; + } + + /*!@brief Set the noiseMatrix that is used to define error sigmas + @param noise The new value to use for the noiseMatrix variable (error sigmas) + */ + void setNoiseMatrix(Eigen::MatrixXd noise){noiseMatrix = noise;} + + /*!@brief Set the propagation matrix that is used to propagate the state. + @param prop The new value for the state propagation matrix + */ + void setPropMatrix(Eigen::MatrixXd prop){propMatrix = prop;} + + Eigen::VectorXd stateBounds; //!< -- Upper bounds to use for markov + Eigen::VectorXd currentState; //!< -- State of the markov model + Eigen::MatrixXd propMatrix; //!< -- Matrix to propagate error state with + Eigen::MatrixXd noiseMatrix; //!< -- Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with + BSKLogger bskLogger; //!< -- BSK Logging + +private: + void initializeRNG(); + uint64_t RNGSeed; //!< -- Seed for random number generator + std::minstd_rand rGen; //!< -- Random number generator for model + std::normal_distribution rNum; //!< -- Random number distribution for model + uint64_t numStates; //!< -- Number of states to generate noise for +}; + +//! Default bound for the Gauss-Markov model +static constexpr double DEFAULT_BOUND = -1.0; + +//! Minimum state factor for the Gauss-Markov model +static constexpr double MIN_STATE_FACTOR = 1E-10; + +#endif /* _GaussMarkov_HH_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.cpp new file mode 100644 index 0000000000..24cfa309cf --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.cpp @@ -0,0 +1,168 @@ +/* + ISC License + + Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "geodeticConversion.h" +#include "rigidBodyKinematics.h" +#include "avsEigenSupport.h" + + +/*! Converts from a planet-centered inertial position (i.e., J2000 ECI) to a planet-centered, planet-fixed position given a rotation matrix. +@param pciPosition: [m] Position vector in PCI coordinates +@param J20002Pfix: [-] 3x3 rotation matrix representing the rotation between PCPF and ECI frames +@return pcpfPosition: [m] Position vector in PCPF coordinates +*/ +Eigen::Vector3d PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]){ + // cArray2EigenMatrixd expects a column major input, thus the result is transposed + Eigen::MatrixXd dcm_PN = cArray2EigenMatrixXd(*J20002Pfix,3,3).transpose(); + Eigen::Vector3d pcpfPosition = dcm_PN * pciPosition; + + return pcpfPosition; +} + +/*! Converts from a planet-centered, planet-fixed coordinates to latitude/longitude/altitude (LLA) coordinates given a planet radius. +@param pcpfPosition: [m] Position vector in PCPF coordinates +@param planetEqRad: [m] Planetary radius, assumed to be constant (i.e., spherical) +@param planetPoRad: [m] Planetary polar used for elliptical surfaces if provided, otherwise spherical, +@return llaPosition: Final position in latitude/longitude/altitude coordinates + [0] : [rad] latitude above planetary equator + [1] : [rad] longitude across planetary meridian + [2] : [alt] altitude above planet radius +*/ +Eigen::Vector3d PCPF2LLA(Eigen::Vector3d pcpfPosition, double planetEqRad, double planetPoRad){ + + Eigen::Vector3d llaPosition; + llaPosition.fill(0.0); + if(planetPoRad < 0.0) { + llaPosition[2] = + pcpfPosition.norm() - planetEqRad; // Altitude is the height above assumed-spherical planet surface + llaPosition[1] = atan2(pcpfPosition[1], pcpfPosition[0]); + llaPosition[0] = atan2(pcpfPosition[2], sqrt(pow(pcpfPosition[0], 2.0) + pow(pcpfPosition[1], 2.0))); + } + else + { + + const int nr_kapp_iter = 10; + const double good_kappa_acc = 1.0E-13; + double planetEcc2 = 1.0 - planetPoRad*planetPoRad/(planetEqRad*planetEqRad); + double kappa = 1.0/(1.0-planetEcc2); + double p2 = pcpfPosition[0]*pcpfPosition[0] + pcpfPosition[1]*pcpfPosition[1]; + double z2 = pcpfPosition[2]*pcpfPosition[2]; + for(int i=0; i= 0) + { + planetEcc2 = 1.0 - planetPoRad*planetPoRad/(planetEqRad*planetEqRad); + } + double sPhi = sin(llaPosition[0]); + double nVal = planetEqRad/sqrt(1.0 - planetEcc2*sPhi*sPhi); + pcpfPosition[0] = (nVal+llaPosition[2])*cos(llaPosition[0])*cos(llaPosition[1]); + pcpfPosition[1] = (nVal+llaPosition[2])*cos(llaPosition[0])*sin(llaPosition[1]); + pcpfPosition[2] = ((1.0-planetEcc2)*nVal + llaPosition[2])*sPhi; + + return pcpfPosition; +} + +/*! Converts from a Lat/Long/Altitude coordinates to planet-centered inertialcoordinates given a planet radius and rotation matrix. +@param pcpfPosition: [m] Position vector in planet centered, planet fixed coordinates +@param J20002Pfix: [-] Rotation between inertial and pcf coordinates. +@return pciPosition: [m] Final position in the planet-centered inertial frame. +*/ +Eigen::Vector3d PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]) +{ + // cArray2EigenMatrixd expects a column major input, thus the result is transposed + Eigen::MatrixXd dcm_NP = cArray2EigenMatrixXd(*J20002Pfix,3,3); + Eigen::Vector3d pciPosition = dcm_NP * pcpfPosition; + + return pciPosition; +} + +/*! Converts from a planet-centered inertial coordinates to latitutde/longitude/altitude (LLA) coordinates given a planet radius and rotation matrix. +@param llaPosition: Final position in latitude/longitude/altitude coordinates + [0] : [rad] latitude above planetary equator + [1] : [rad] longitude across planetary meridian + [2] : [alt] altitude above planet radius +@param J20002Pfix: rotation matrix between inertial and PCPF frames +@param planetEqRad: [m] Planetary radius, assumed to be constant (i.e., spherical) +@param planetPoRad: [m] Planetary polar used for elliptical surfaces if provided, otherwise spherical, +@return pciPosition : [m] Position in inertial coordinates. +*/ +Eigen::Vector3d LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad) +{ + Eigen::Vector3d pcpfPosition = LLA2PCPF(llaPosition, planetEqRad, planetPoRad); + Eigen::Vector3d pciPosition = PCPF2PCI(pcpfPosition, J20002Pfix); + return pciPosition; +} + +Eigen::Matrix3d C_PCPF2SEZ(double lat, double longitude) +{ + double m1[3][3]; + double m2[3][3]; + Euler2(M_PI_2-lat, m1); + Euler3(longitude, m2); + + Eigen::MatrixXd rot2 = cArray2EigenMatrix3d(*m1); + Eigen::MatrixXd rot3 = cArray2EigenMatrix3d(*m2); + return rot2*rot3; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.h new file mode 100644 index 0000000000..e12d1bbd27 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/geodeticConversion.h @@ -0,0 +1,43 @@ +/* +ISC License + +Copyright (c) 2016-2017, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + +#ifndef _GEODETIC_CONV_H_ +#define _GEODETIC_CONV_H_ + +#include +#include + + +/*! @brief Collection of utility functions for converting in/out of planet-centric reference frames. + +The geodeticConversion library contains simple transformations between inertial coordinates and planet-fixed coordinates in a general way. + +No support is provided for non-spherical bodies. Transformations are scripted from Vallado. + + */ + +Eigen::Vector3d PCI2PCPF(Eigen::Vector3d pciPosition, double J20002Pfix[3][3]); +Eigen::Vector3d PCPF2LLA(Eigen::Vector3d pciPosition, double planetEqRadius, double planetPoRad=-1.0); +Eigen::Vector3d PCI2LLA(Eigen::Vector3d pciPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad=-1.0); +Eigen::Vector3d LLA2PCPF(Eigen::Vector3d llaPosition, double planetEqRad, double planetPoRad=-1.0); +Eigen::Vector3d PCPF2PCI(Eigen::Vector3d pcpfPosition, double J20002Pfix[3][3]); +Eigen::Vector3d LLA2PCI(Eigen::Vector3d llaPosition, double J20002Pfix[3][3], double planetEqRad, double planetPoRad=-1.0); +Eigen::Matrix3d C_PCPF2SEZ(double lat, double longitude); + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.cpp new file mode 100644 index 0000000000..6316ad0e29 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.cpp @@ -0,0 +1,187 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "keplerianOrbit.h" +#include +#include + +/*! This constructor initialized to an arbitrary orbit */ +KeplerianOrbit::KeplerianOrbit() +{ + this->change_orbit(); +} + +/*! The constructor requires orbital elements and a gravitational constant value */ +KeplerianOrbit::KeplerianOrbit(ClassicElements oe, const double mu) : mu(mu), + semi_major_axis(oe.a), + eccentricity(oe.e), + inclination(oe.i), + argument_of_periapsis(oe.omega), + right_ascension(oe.Omega), + true_anomaly(oe.f){ + this->change_orbit(); +} + +/*! The copy constructor works with python copy*/ +KeplerianOrbit::KeplerianOrbit(const KeplerianOrbit &orig) : mu(orig.mu), + semi_major_axis(orig.a()), + eccentricity(orig.e()), + inclination(orig.i()), + argument_of_periapsis(orig.omega()), + right_ascension(orig.RAAN()), + true_anomaly(orig.f()) +{ + this->change_orbit(); +} + +/*! Generic Destructor */ +KeplerianOrbit::~KeplerianOrbit() +{ +} + +/*! + body position vector relative to planet + */ +Eigen::Vector3d KeplerianOrbit::r_BP_P() const { + return this->position_BP_P; +} + +/*! + body velocity vector relative to planet + */ +Eigen::Vector3d KeplerianOrbit::v_BP_P() const{ + return this->velocity_BP_P; + +} + +/*! + angular momentum of body relative to planet + */ +Eigen::Vector3d KeplerianOrbit::h_BP_P() const{ + return this->orbital_angular_momentum_P; +} + +/*! return mean anomaly angle */ +double KeplerianOrbit::M() const {return this->mean_anomaly;} +/*! return mean orbit rate */ +double KeplerianOrbit::n() const {return this->mean_motion;}; //!< return mean orbit rate +/*! return orbit period */ +double KeplerianOrbit::P() const {return this->orbital_period;}; //!< return orbital period +/*! return true anomaly */ +double KeplerianOrbit::f() const {return this->true_anomaly;}; //!< return true anomaly +/*! return true anomaly rate */ +double KeplerianOrbit::fDot() const {return this->true_anomaly_rate;}; +/*! return right ascencion of the ascending node */ +double KeplerianOrbit::RAAN() const {return this->right_ascension;}; +/*! return argument of periapses */ +double KeplerianOrbit::omega() const {return this->argument_of_periapsis;}; +/*! return inclination angle */ +double KeplerianOrbit::i() const {return this->inclination;}; +/*! return eccentricty */ +double KeplerianOrbit::e() const {return this->eccentricity;}; +/*! return semi-major axis */ +double KeplerianOrbit::a() const {return this->semi_major_axis;}; +/*! return orbital angular momentum magnitude */ +double KeplerianOrbit::h() const {return this->h_BP_P().norm();}; +/*! return orbital energy */ +double KeplerianOrbit::Energy(){return this->orbital_energy;}; +/*! return orbit radius */ +double KeplerianOrbit::r() const {return this->r_BP_P().norm();}; +/*! return velocity magnitude */ +double KeplerianOrbit::v() const {return this->v_BP_P().norm();}; +/*! return radius at apoapses */ +double KeplerianOrbit::r_a() const {return this->r_apogee;}; +/*! return radius at periapses */ +double KeplerianOrbit::r_p() const {return this->r_perigee;}; +/*! return flight path angle */ +double KeplerianOrbit::fpa() const {return this->flight_path_angle;}; +/*! return eccentric anomaly angle */ +double KeplerianOrbit::E() const {return this->eccentric_anomaly;}; +/*! return semi-latus rectum or the parameter */ +double KeplerianOrbit::p() const {return this->semi_parameter;}; +/*! return radius rate */ +double KeplerianOrbit::rDot() const {return this->radial_rate;}; +/*! return escape velocity */ +double KeplerianOrbit::c3() const {return this->v_infinity;}; + +/*! set semi-major axis */ +void KeplerianOrbit::set_a(double a){this->semi_major_axis = a; this->change_orbit();}; +/*! set eccentricity */ +void KeplerianOrbit::set_e(double e){this->eccentricity = e; this->change_orbit();}; +/*! set inclination angle */ +void KeplerianOrbit::set_i(double i){this->inclination = i; this->change_orbit();}; +/*! set argument of periapsis */ +void KeplerianOrbit::set_omega(double omega){this->argument_of_periapsis = omega; this->change_orbit();}; +/*! set right ascension of the ascending node */ +void KeplerianOrbit::set_RAAN(double RAAN){this->right_ascension = RAAN; this->change_orbit();}; +/*! set true anomaly angle */ +void KeplerianOrbit::set_f(double f){this->true_anomaly = f; this->change_f();}; + + + +/*! This method returns the orbital element set for the orbit + @return ClassicElements oe + */ +ClassicElements KeplerianOrbit::oe(){ + ClassicElements elements; + elements.a = this->semi_major_axis; + elements.e = this->eccentricity; + elements.i = this->inclination; + elements.f = this->true_anomaly; + elements.omega = this->argument_of_periapsis; + elements.Omega = this->right_ascension; + elements.rApoap= this->r_apogee; + elements.rPeriap = this->r_perigee; + elements.alpha = 1.0 / elements.a; + elements.rmag = this->orbit_radius; + return elements; +} + +/*! This method populates all outputs from orbital elements coherently if any of the + * classical orbital elements are changed*/ +void KeplerianOrbit::change_orbit(){ + this->change_f(); + this->orbital_angular_momentum_P = this->position_BP_P.cross(this->velocity_BP_P); + this->mean_motion = sqrt(this->mu / pow(this->semi_major_axis, 3)); + this->orbital_period = 2 * M_PI / this->mean_motion; + this->semi_parameter = pow(this->h(), 2) / this->mu; + this->orbital_energy = -this->mu / 2 / this->a(); + this->r_apogee = this->a() * (1 + this->e()); + this->r_perigee = this->a() * (1 - this->e()); +} +/*! This method only changes the outputs dependent on true anomaly so that one + * orbit may be queried at various points along the orbit*/ +void KeplerianOrbit::change_f(){ + double r[3]; + double v[3]; + ClassicElements oe = this->oe(); // + elem2rv(this->mu, &oe, r, v); // + this->position_BP_P = cArray2EigenVector3d(r); // + this->velocity_BP_P = cArray2EigenVector3d(v); // + this->true_anomaly_rate = this->n() * pow(this->a(), 2) * sqrt(1 - pow(this->e(), 2)) / pow(this->r(), 2); // + this->radial_rate = this->r() * this->fDot() * this->e() * sin(this->f()) / (1 + this->e() * cos(this->f())); // + this->eccentric_anomaly = safeAcos(this->e() + cos(this->f()) / (1 + this->e() * cos(this->f()))); // + this->mean_anomaly = this->E() - this->e() * sin(this->E()); // + this->flight_path_angle = safeAcos(sqrt((1 - pow(this->e(), 2)) / (1 - pow(this->e(), 2)*pow(cos(this->E()), 2)))); // +} + +/*! This method sets the gravitational constants of the body being orbited */ +void KeplerianOrbit::set_mu(const double mu){ + this->mu = mu; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.h new file mode 100644 index 0000000000..b65f05343e --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/keplerianOrbit.h @@ -0,0 +1,98 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#pragma once + +#include +#include +#include "architecture/utilities/astroConstants.h" + + +//! @brief The KeplerianOrbit class represents an elliptical orbit and provides a coherent set of +//! common outputs such as position and velocity, orbital period, semi-parameter, etc. It uses the +//! utility orbitalMotion to do orbital element to position and velocity conversion. +class KeplerianOrbit { +public: + KeplerianOrbit(); + KeplerianOrbit(ClassicElements oe, const double mu); + KeplerianOrbit(const KeplerianOrbit &orig); + ~KeplerianOrbit(); + + + Eigen::Vector3d r_BP_P() const; //!< body position vector relative to planet + Eigen::Vector3d v_BP_P() const; //!< body velocity vector relative to planet + Eigen::Vector3d h_BP_P() const; //!< angular momentum of body relative to planet + double M() const; + double n() const; + double P() const; + double f() const; + double fDot() const; + double RAAN() const; + double omega() const; + double i() const; + double e() const; + double a() const; + double h() const; + double Energy(); + double r() const; + double v() const; + double r_a() const; + double r_p() const; + double fpa() const; + double E() const; + double p() const; + double rDot() const; + double c3() const; + ClassicElements oe(); + void set_mu(const double mu); + void set_a(double a); + void set_e(double e); + void set_i(double i); + void set_omega(double omega); + void set_RAAN(double RAAN); + void set_f(double f); + +private: + double mu = MU_EARTH*pow(10,9); // convert to m^3/s^2 + double semi_major_axis = 1E5*1000; // convert to meters + double eccentricity = 1E-5; + double inclination{}; + double argument_of_periapsis{}; + double right_ascension{}; + double true_anomaly{}; + double true_anomaly_rate{}; + double orbital_period{}; + double orbital_energy{}; + double v_infinity{}; + double orbit_radius{}; + double radial_rate{}; + double r_apogee{}; + double r_perigee{}; + double semi_parameter{}; + double flight_path_angle{}; + double eccentric_anomaly{}; + double mean_motion{}; + double mean_anomaly{}; + Eigen::Vector3d orbital_angular_momentum_P; + Eigen::Vector3d position_BP_P; + Eigen::Vector3d velocity_BP_P; +private: + void change_orbit(); + void change_f(); +}; diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.c new file mode 100644 index 0000000000..71db037ceb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.c @@ -0,0 +1,2700 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "linearAlgebra.h" +#include "architecture/utilities/bsk_Print.h" + +#include +#include +#include + + + +#define MOVE_DOUBLE(source, dim, destination) (memmove((void*)(destination), (void*)(source), sizeof(double)*(dim))) + +void vElementwiseMult(double *v1, size_t dim, + double *v2, double *result) +{ + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] * v2[i]; + } +} + +void vCopy(double *v, size_t dim, + double *result) +{ + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i]; + } +} + +void vSetZero(double *v, + size_t dim) +{ + size_t i; + for(i = 0; i < dim; i++) { + v[i] = 0.0; + } +} + +void vSetOnes(double *v, + size_t dim) +{ + size_t i; + for(i = 0; i < dim; i++) { + v[i] = 1.0; + } +} + +void vAdd(double *v1, size_t dim, + double *v2, + double *result) +{ + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] + v2[i]; + } +} + +void vSubtract(double *v1, size_t dim, + double *v2, + double *result) +{ + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] - v2[i]; + } +} + +void vScale(double scaleFactor, double *v, + size_t dim, + double *result) +{ + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i] * scaleFactor; + } +} + +double vDot(double *v1, size_t dim, + double *v2) +{ + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += v1[i] * v2[i]; + } + + return result; +} + +void vOuterProduct(double *v1, size_t dim1, + double *v2, size_t dim2, + void *result) +{ + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = v1[i] * v2[j]; + } + } + +} + +void vtMultM(double *v, + void *mx, size_t dim1, size_t dim2, + void *result) +{ + size_t dim11 = 1; + size_t dim12 = dim1; + size_t dim22 = dim2; + double *m_mx1 = (double *)v; + double *m_mx2 = (double *)mx; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; + } + } + } + + MOVE_DOUBLE(m_result, dim11 * dim22, result); +} + +void vtMultMt(double *v, + void *mx, size_t dim1, size_t dim2, + void *result) +{ + size_t dim11 = 1; + size_t dim12 = dim2; + size_t dim22 = dim1; + double *m_mx1 = (double *)v; + double *m_mx2 = (double *)mx; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, j, k)]; + } + } + } + + MOVE_DOUBLE(m_result, dim11 * dim22, result); +} + +double vNorm(double *v, size_t dim) +{ + return sqrt(vDot(v, dim, v)); +} + +double vMax(double *array, size_t dim) +{ + size_t i; + double result; + + result = array[0]; + for(i=1; iresult){ + result = array[i]; + } + } + return result; +} + + +double vMaxAbs(double *array, size_t dim) +{ + size_t i; + double result; + + result = fabs(array[0]); + for(i=1; iresult){ + result = fabs(array[i]); + } + } + return result; +} + + +void vNormalize(double *v, size_t dim, double *result) +{ + double norm = vNorm(v, dim); + + if(norm > DB0_EPS) { + vScale(1.0 / norm, v, dim, result); + } else { + vSetZero(result, dim); + } +} + +int vIsEqual(double *v1, size_t dim, + double *v2, + double accuracy) +{ + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i]) > accuracy) { + return 0; + } + } + return 1; +} + +int vIsZero(double *v, size_t dim, double accuracy) +{ + size_t i; + int result = 1; + for(i = 0; i < dim; i++) { + if(fabs(v[i]) > accuracy) { + result = 0; + break; + } + } + + return result; +} + +void vPrint(FILE *pFile, const char *name, double *v, size_t dim) +{ + size_t i; + fprintf(pFile, "%s = [", name); + for(i = 0; i < dim; i++) { + fprintf(pFile, "%20.15g", v[i]); + if(i != dim - 1) { + fprintf(pFile, ", "); + } + } + fprintf(pFile, "];\n"); +} + +/*I hope you allocated the output prior to calling this!*/ +void vSort(double *Input, double *Output, size_t dim) +{ + size_t i, j; + memcpy(Output, Input, dim*sizeof(double)); + for(i=0; iOutput[j+1]) + { + double temp = Output[j+1]; + Output[j+1] = Output[j]; + Output[j] = temp; + } + } + } +} + + +void v2Set(double v0, double v1, + double result[2]) +{ + result[0] = v0; + result[1] = v1; +} + +void v2SetZero(double v[2]) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + v[i] = 0.0; + } +} + +void v2Copy(double v[2], + double result[2]) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i]; + } +} + +void v2Scale(double scaleFactor, + double v[2], + double result[2]) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i] * scaleFactor; + } +} + +double v2Dot(double v1[2], + double v2[2]) +{ + size_t dim = 2; + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += v1[i] * v2[i]; + } + return result; +} + +int v2IsEqual(double v1[2], + double v2[2], + double accuracy) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i]) > accuracy) { + return 0; + } + } + return 1; +} + +int v2IsZero(double v[2], + double accuracy) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v[i]) > accuracy) { + return 0; + } + } + return 1; +} + +void v2Add(double v1[2], + double v2[2], + double result[2]) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] + v2[i]; + } +} + +void v2Subtract(double v1[2], + double v2[2], + double result[2]) +{ + size_t dim = 2; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] - v2[i]; + } +} + +double v2Norm(double v[2]) +{ + return sqrt(v2Dot(v, v)); +} + +void v2Normalize(double v[2], double result[2]) +{ + double norm = v2Norm(v); + if(norm > DB0_EPS) { + v2Scale(1. / norm, v, result); + } else { + v2SetZero(result); + } +} + + + + + + +void v3Set(double v0, double v1, double v2, + double result[3]) +{ + result[0] = v0; + result[1] = v1; + result[2] = v2; +} + +void v3Copy(double v[3], + double result[3]) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i]; + } +} + +void v3SetZero(double v[3]) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + v[i] = 0.0; + } +} + +void v3Add(double v1[3], + double v2[3], + double result[3]) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] + v2[i]; + } +} + +void v3Subtract(double v1[3], + double v2[3], + double result[3]) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v1[i] - v2[i]; + } +} + +void v3Scale(double scaleFactor, + double v[3], + double result[3]) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i] * scaleFactor; + } +} + +double v3Dot(double v1[3], + double v2[3]) +{ + return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; +} + +void v3OuterProduct(double v1[3], + double v2[3], + double result[3][3]) +{ + size_t dim = 3; + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + result[i][j] = v1[i] * v2[j]; + } + } +} + +void v3tMultM33(double v[3], + double mx[3][3], + double result[3]) +{ + size_t dim11 = 1; + size_t dim12 = 3; + size_t dim22 = 3; + size_t i; + size_t j; + size_t k; + double m_result[3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[j] += v[k] * mx[k][j]; + } + } + } + v3Copy(m_result, result); +} + +void v3tMultM33t(double v[3], + double mx[3][3], + double result[3]) +{ + size_t dim11 = 1; + size_t dim12 = 3; + size_t dim22 = 3; + size_t i; + size_t j; + size_t k; + double m_result[3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[j] += v[k] * mx[j][k]; + } + } + } + v3Copy(m_result, result); +} + +double v3Norm(double v[3]) +{ + return sqrt(v3Dot(v, v)); +} + +void v3Normalize(double v[3], double result[3]) +{ + double norm = v3Norm(v); + if(norm > DB0_EPS) { + v3Scale(1. / norm, v, result); + } else { + v3SetZero(result); + } +} + +int v3IsEqual(double v1[3], + double v2[3], + double accuracy) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i]) > accuracy) { + return 0; + } + } + return 1; +} + +int v3IsEqualRel(double v1[3], + double v2[3], + double accuracy) +{ + size_t dim = 3; + size_t i; + double norm; + norm = v3Norm(v1); + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i])/norm > accuracy) { + return 0; + } + } + return 1; +} + + + +int v3IsZero(double v[3], + double accuracy) +{ + size_t dim = 3; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v[i]) > accuracy) { + return 0; + } + } + return 1; +} + +void v3Print(FILE *pFile, const char *name, double v[3]) +{ + size_t dim = 3; + size_t i; + fprintf(pFile, "%s = [", name); + for(i = 0; i < dim; i++) { + fprintf(pFile, "%20.15g", v[i]); + if(i != dim - 1) { + fprintf(pFile, ", "); + } + } + fprintf(pFile, "]\n"); +} + +void v3Cross(double v1[3], + double v2[3], + double result[3]) +{ + double v1c[3]; + double v2c[3]; + v3Copy(v1, v1c); + v3Copy(v2, v2c); + result[0] = v1c[1] * v2c[2] - v1c[2] * v2c[1]; + result[1] = v1c[2] * v2c[0] - v1c[0] * v2c[2]; + result[2] = v1c[0] * v2c[1] - v1c[1] * v2c[0]; +} + +void v3Perpendicular(double v[3], + double result[3]) +{ + if (fabs(v[0]) > DB0_EPS) { + result[0] = -(v[1]+v[2]) / v[0]; + result[1] = 1; + result[2] = 1; + } + else if (fabs(v[1]) > DB0_EPS) { + result[0] = 1; + result[1] = -(v[0]+v[2]) / v[1]; + result[2] = 1; + } + else { + result[0] = 1; + result[1] = 1; + result[2] = -(v[0]+v[1]) / v[2]; + } + v3Normalize(result, result); +} + + +void v3Tilde(double v[3], + double result[3][3]) +{ + result[0][0] = 0.0; + result[0][1] = -v[2]; + result[0][2] = v[1]; + result[1][0] = v[2]; + result[1][1] = 0.0; + result[1][2] = -v[0]; + result[2][0] = -v[1]; + result[2][1] = v[0]; + result[2][2] = 0.0; +} + +void v3Sort(double v[3], + double result[3]) +{ + double temp; + v3Copy(v, result); + if(result[0] < result[1]) { + temp = result[0]; + result[0] = result[1]; + result[1] = temp; + } + if(result[1] < result[2]) { + if(result[0] < result[2]) { + temp = result[2]; + result[2] = result[1]; + result[1] = result[0]; + result[0] = temp; + } else { + temp = result[1]; + result[1] = result[2]; + result[2] = temp; + } + } +} + +void v3PrintScreen(const char *name, double vec[3]) +{ + printf("%s (%20.15g, %20.15g, %20.15g)\n", name, vec[0], vec[1], vec[2]); +} + +void v4Set(double v0, double v1, double v2, double v3, + double result[4]) +{ + result[0] = v0; + result[1] = v1; + result[2] = v2; + result[3] = v3; +} + +void v4Copy(double v[4], + double result[4]) +{ + size_t dim = 4; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i]; + } +} + +void v4SetZero(double v[4]) +{ + size_t dim = 4; + size_t i; + for(i = 0; i < dim; i++) { + v[i] = 0.0; + } +} + +double v4Dot(double v1[4], + double v2[4]) +{ + size_t dim = 4; + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += v1[i] * v2[i]; + } + return result; +} + +double v4Norm(double v[4]) +{ + return sqrt(v4Dot(v, v)); +} + +int v4IsEqual(double v1[4], + double v2[4], + double accuracy) +{ + size_t dim = 4; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i]) > accuracy) { + return 0; + } + } + return 1; +} + +int v4IsZero(double v[4], + double accuracy) +{ + size_t dim = 4; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v[i]) > accuracy) { + return 0; + } + } + return 1; +} + +void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, + double result[6]) +{ + result[0] = v0; + result[1] = v1; + result[2] = v2; + result[3] = v3; + result[4] = v4; + result[5] = v5; +} + +void v6Copy(double v[6], + double result[6]) +{ + size_t dim = 6; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i]; + } +} + +double v6Dot(double v1[6], + double v2[6]) +{ + size_t dim = 6; + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += v1[i] * v2[i]; + } + return result; +} + +void v6Scale(double scaleFactor, + double v[6], + double result[6]) +{ + size_t dim = 6; + size_t i; + for(i = 0; i < dim; i++) { + result[i] = v[i] * scaleFactor; + } +} + +void v6OuterProduct(double v1[6], + double v2[6], + double result[6][6]) +{ + size_t dim = 6; + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + result[i][j] = v1[i] * v2[j]; + } + } +} + +int v6IsEqual(double v1[6], + double v2[6], + double accuracy) +{ + size_t dim = 6; + size_t i; + for(i = 0; i < dim; i++) { + if(fabs(v1[i] - v2[i]) > accuracy) { + return 0; + } + } + return 1; +} + +void mLeastSquaresInverse(void *mx, size_t dim1, size_t dim2, void *result) +{ + /* + * Computes the least squares inverse. + */ + double *m_result = (double *)result; + double mxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double mxGrammian[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double mxGrammianInverse[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + + mTranspose(mx, dim1, dim2, mxTranspose); + mMultM(mxTranspose, dim2, dim1, mx, dim1, dim2, mxGrammian); + mInverse(mxGrammian, dim2, mxGrammianInverse); + mMultM(mxGrammianInverse, dim2, dim2, mxTranspose, dim2, dim1, m_result); + +} + +void mMinimumNormInverse(void *mx, size_t dim1, size_t dim2, void *result) +{ + /* + * Computes the minumum norm inverse. + */ + double *m_mx = (double *)mx; + double *m_result = (double *)result; + double mxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double mxMxTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double mxMxTransposeInverse[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + + mTranspose(m_mx, dim1, dim2, mxTranspose); + mMultM(m_mx, dim1, dim2, mxTranspose, dim2, dim1, mxMxTranspose); + mInverse(mxMxTranspose, dim1, mxMxTransposeInverse); + mMultM(mxTranspose, dim2, dim1, mxMxTransposeInverse, dim1, dim1, m_result); +} + +void mCopy(void *mx, size_t dim1, size_t dim2, + void *result) +{ + double *m_mx = (double *)mx; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = m_mx[MXINDEX(dim2, i, j)]; + } + } +} + +void mSetZero(void *result, size_t dim1, size_t dim2) +{ + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = 0.0; + } + } +} + +void mSetIdentity(void *result, size_t dim1, size_t dim2) +{ + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = (i == j) ? 1.0 : 0.0; + } + } +} + +void mDiag(void *v, size_t dim, void *result) +{ + double *m_v = (double *)v; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + m_result[MXINDEX(dim, i, j)] = (i == j) ? m_v[i] : 0.0; + } + } +} + +void mTranspose(void *mx, size_t dim1, size_t dim2, + void *result) +{ + double *m_mx = (double *)mx; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim1*dim2 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim1, j, i)] = m_mx[MXINDEX(dim2, i, j)]; + } + } + + MOVE_DOUBLE(m_result, dim2 * dim1, result); +} + +void mAdd(void *mx1, size_t dim1, size_t dim2, + void *mx2, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = m_mx1[MXINDEX(dim2, i, j)] + m_mx2[MXINDEX(dim2, i, j)]; + } + } +} + +void mSubtract(void *mx1, size_t dim1, size_t dim2, + void *mx2, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = m_mx1[MXINDEX(dim2, i, j)] - m_mx2[MXINDEX(dim2, i, j)]; + } + } +} + +void mScale(double scaleFactor, + void *mx, size_t dim1, size_t dim2, + void *result) +{ + double *m_mx = (double *)mx; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[MXINDEX(dim2, i, j)] = scaleFactor * m_mx[MXINDEX(dim2, i, j)]; + } + } +} + +void mMultM(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + if(dim12 != dim21) { + BSK_PRINT(MSG_ERROR, "Error: mMultM dimensions don't match."); + return; + } + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; + } + } + } + + MOVE_DOUBLE(m_result, dim11 * dim22, result); +} + +void mtMultM(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim12*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + if(dim11 != dim21) { + BSK_PRINT(MSG_ERROR, "Error: mtMultM dimensions don't match."); + return; + } + for(i = 0; i < dim12; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim11; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, k, j)]; + } + } + } + + MOVE_DOUBLE(m_result, dim12 * dim22, result); +} + +void mMultMt(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim11*dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + if(dim12 != dim22) { + BSK_PRINT(MSG_ERROR, "Error: mMultMt dimensions don't match."); + return; + } + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim21; j++) { + m_result[MXINDEX(dim21, i, j)] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[MXINDEX(dim21, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, j, k)]; + } + } + } + + MOVE_DOUBLE(m_result, dim11 * dim21, result); +} + +void mtMultMt(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim12*dim21 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + if(dim11 != dim22) { + BSK_PRINT(MSG_ERROR, "Error: mtMultMt dimensions don't match."); + return; + } + for(i = 0; i < dim12; i++) { + for(j = 0; j < dim21; j++) { + m_result[MXINDEX(dim21, i, j)] = 0.0; + for(k = 0; k < dim11; k++) { + m_result[MXINDEX(dim21, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, j, k)]; + } + } + } + + MOVE_DOUBLE(m_result, dim12 * dim21, result); +} + +void mMultV(void *mx, size_t dim1, size_t dim2, + void *v, + void *result) +{ + size_t dim11 = dim1; + size_t dim12 = dim2; + size_t dim22 = 1; + double *m_mx1 = (double *)mx; + double *m_mx2 = (double *)v; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim11*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, i, k)] * m_mx2[MXINDEX(dim22, k, j)]; + } + } + } + + MOVE_DOUBLE(m_result, dim11 * dim22, result); +} + +void mtMultV(void *mx, size_t dim1, size_t dim2, + void *v, + void *result) +{ + size_t dim11 = dim1; + size_t dim12 = dim2; + size_t dim22 = 1; + double *m_mx1 = (double *)mx; + double *m_mx2 = (double *)v; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim12*dim22 > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + size_t i; + size_t j; + size_t k; + for(i = 0; i < dim12; i++) { + for(j = 0; j < dim22; j++) { + m_result[MXINDEX(dim22, i, j)] = 0.0; + for(k = 0; k < dim11; k++) { + m_result[MXINDEX(dim22, i, j)] += m_mx1[MXINDEX(dim12, k, i)] * m_mx2[MXINDEX(dim22, k, j)]; + } + } + } + + MOVE_DOUBLE(m_result, dim12 * dim22, result); +} + +double mTrace(void *mx, size_t dim) +{ + double *m_mx = (double *)mx; + + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += m_mx[MXINDEX(dim, i, i)]; + } + + return result; +} + +double mDeterminant(void *mx, size_t dim) +{ + double *m_mx = (double *)mx; + + size_t i; + size_t j; + size_t k; + size_t ii; + double result = 0; + double mxTemp[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if ((dim-1)*(dim-1) > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + if(dim < 1) { + return 0; + } else if(dim == 1) { + result = m_mx[MXINDEX(dim, 0, 0)]; + } else if(dim == 2) { + result = m_mx[MXINDEX(dim, 0, 0)] * m_mx[MXINDEX(dim, 1, 1)] + - m_mx[MXINDEX(dim, 1, 0)] * m_mx[MXINDEX(dim, 0, 1)]; + } else { + for(k = 0; k < dim; k++) { + for(i = 1; i < dim; i++) { + ii = 0; + for(j = 0; j < dim; j++) { + if(j == k) { + continue; + } + mxTemp[MXINDEX(dim - 1, i - 1, ii)] = m_mx[MXINDEX(dim, i, j)]; + ii++; + } + } + result += pow(-1.0, 1.0 + k + 1.0) * m_mx[MXINDEX(dim, 0, k)] * mDeterminant(mxTemp, dim - 1); + } + } + return(result); +} + +void mCofactor(void *mx, size_t dim, void *result) +{ + /* The (j,i)th cofactor of A is defined as (-1)^(i + j)*det(A_(i,j)) + where A_(i,j) is the submatrix of A obtained from A by removing the ith row and jth column */ + size_t i; + size_t i0; + size_t i1; + size_t j; + size_t j0; + size_t j1; + double *m_mx = (double *)mx; + double m_mxij[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double det; + if (dim*dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + /* Form mx_(i,j) */ + i1 = 0; + for(i0 = 0; i0 < dim; i0++) { + if(i0 == i) { + continue; + } + j1 = 0; + for(j0 = 0; j0 < dim; j0++) { + if(j0 == j) { + continue; + } + m_mxij[MXINDEX(dim - 1, i1, j1)] = m_mx[MXINDEX(dim, i0, j0)]; + j1++; + } + i1++; + } + + /* Calculate the determinant */ + det = mDeterminant(m_mxij, dim - 1); + + /* Fill in the elements of the cofactor */ + m_result[MXINDEX(dim, i, j)] = pow(-1.0, i + j + 2.0) * det; + } + } + + MOVE_DOUBLE(m_result, dim * dim, result); +} + +int mInverse(void *mx, size_t dim, void *result) +{ + /* Inverse of a square matrix A with non zero determinant is adjoint matrix divided by determinant */ + /* The adjoint matrix is the square matrix X such that the (i,j)th entry of X is the (j,i)th cofactor of A */ + + size_t i; + size_t j; + int status = 0; + double det = mDeterminant(mx, dim); + double m_result[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + if (dim*dim > LINEAR_ALGEBRA_MAX_ARRAY_SIZE) + { + BSK_PRINT(MSG_ERROR,"Linear Algegra library array dimension input is too large."); + } + + if(fabs(det) > DB0_EPS) { + /* Find adjoint matrix */ + double m_adjoint[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + mCofactor(mx, dim, m_adjoint); + mTranspose(m_adjoint, dim, dim, m_adjoint); + /* Find inverse */ + mScale(1.0 / det, m_adjoint, dim, dim, m_result); + } else { + BSK_PRINT(MSG_ERROR, "Error: cannot invert singular matrix"); + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + m_result[MXINDEX(dim, i, j)] = 0.0; + } + } + status = 1; + } + + MOVE_DOUBLE(m_result, dim * dim, result); + return status; +} + +int mIsEqual(void *mx1, size_t dim1, size_t dim2, + void *mx2, + double accuracy) +{ + double *m_mx1 = (double *)mx1; + double *m_mx2 = (double *)mx2; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(m_mx1[MXINDEX(dim2, i, j)] - m_mx2[MXINDEX(dim2, i, j)]) > accuracy) { + return 0; + } + } + } + return 1; +} + +int mIsZero(void *mx, size_t dim1, size_t dim2, + double accuracy) +{ + double *m_mx = (double *)mx; + + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(m_mx[MXINDEX(dim2, i, j)]) > accuracy) { + return 0; + } + } + } + return 1; +} + +void mPrintScreen(const char *name, void *mx, size_t dim1, size_t dim2) +{ + double *m_mx = (double *)mx; + + size_t i; + size_t j; + printf("%s = [", name); + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + printf("%20.15g", m_mx[MXINDEX(dim2, i, j)]); + if(j != dim2 - 1) { + printf(", "); + } + } + if(i != dim1 - 1) { + printf(";\n"); + } + } + printf("];\n"); +} + + +void mPrint(FILE *pFile, const char *name, void *mx, size_t dim1, size_t dim2) +{ + double *m_mx = (double *)mx; + + size_t i; + size_t j; + fprintf(pFile, "%s = [", name); + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + fprintf(pFile, "%20.15g", m_mx[MXINDEX(dim2, i, j)]); + if(j != dim2 - 1) { + fprintf(pFile, ", "); + } + } + if(i != dim1 - 1) { + fprintf(pFile, ";\n"); + } + } + fprintf(pFile, "];\n"); +} + +void mGetSubMatrix(void *mx, size_t dim1, size_t dim2, + size_t dim1Start, size_t dim2Start, + size_t dim1Result, size_t dim2Result, void *result) +{ + double *m_mx = (double *)mx; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = dim1Start; i < dim1Start + dim1Result; i++) { + for(j = dim2Start; j < dim2Start + dim2Result; j++) { + m_result[MXINDEX(dim2Result, i - dim1Start, j - dim2Start)] = m_mx[MXINDEX(dim2, i, j)]; + } + } +} + +void mSetSubMatrix(void *mx, size_t dim1, size_t dim2, + void *result, size_t dim1Result, size_t dim2Result, + size_t dim1Start, size_t dim2Start) +{ + double *m_mx = (double *)mx; + double *m_result = (double *)result; + + size_t i; + size_t j; + for(i = dim1Start; i < dim1Start + dim1; i++) { + for(j = dim2Start; j < dim2Start + dim2; j++) { + m_result[MXINDEX(dim2Result, i, j)] = m_mx[MXINDEX(dim2, i - dim1Start, j - dim2Start)]; + } + } +} + +void m22Set(double m00, double m01, + double m10, double m11, + double m[2][2]) +{ + m[0][0] = m00; + m[0][1] = m01; + m[1][0] = m10; + m[1][1] = m11; +} + +void m22Copy(double mx[2][2], + double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx[i][j]; + } + } +} + +void m22SetZero(double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = 0.0; + } + } +} + +void m22SetIdentity(double result[2][2]) +{ + size_t dim = 2; + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + result[i][j] = (i == j) ? 1.0 : 0.0; + } + } +} + +void m22Transpose(double mx[2][2], + double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + double m_result[2][2]; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[j][i] = mx[i][j]; + } + } + m22Copy(m_result, result); +} + +void m22Add(double mx1[2][2], + double mx2[2][2], + double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] + mx2[i][j]; + } + } +} + +void m22Subtract(double mx1[2][2], + double mx2[2][2], + double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] - mx2[i][j]; + } + } +} + +void m22Scale(double scaleFactor, + double mx[2][2], + double result[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = scaleFactor * mx[i][j]; + } + } +} + +void m22MultM22(double mx1[2][2], + double mx2[2][2], + double result[2][2]) +{ + size_t dim11 = 2; + size_t dim12 = 2; + size_t dim22 = 2; + size_t i; + size_t j; + size_t k; + double m_result[2][2]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[k][j]; + } + } + } + m22Copy(m_result, result); +} + +void m22tMultM22(double mx1[2][2], + double mx2[2][2], + double result[2][2]) +{ + size_t dim11 = 2; + size_t dim12 = 2; + size_t dim22 = 2; + size_t i; + size_t j; + size_t k; + double m_result[2][2]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[k][i] * mx2[k][j]; + } + } + } + m22Copy(m_result, result); +} + +void m22MultM22t(double mx1[2][2], + double mx2[2][2], + double result[2][2]) +{ + size_t dim11 = 2; + size_t dim12 = 2; + size_t dim21 = 2; + size_t i; + size_t j; + size_t k; + double m_result[2][2]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim21; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[j][k]; + } + } + } + m22Copy(m_result, result); +} + +void m22MultV2(double mx[2][2], + double v[2], + double result[2]) +{ + size_t dim11 = 2; + size_t dim12 = 2; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[2]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[i][k] * v[k]; + } + } + } + v2Copy(m_result, result); +} + +void m22tMultV2(double mx[2][2], + double v[2], + double result[2]) +{ + size_t dim11 = 2; + size_t dim12 = 2; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[2]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[k][i] * v[k]; + } + } + } + v2Copy(m_result, result); +} + +double m22Trace(double mx[2][2]) +{ + size_t dim = 2; + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += mx[i][i]; + } + + return result; +} + +double m22Determinant(double mx[2][2]) +{ + double value; + value = mx[0][0] * mx[1][1] - mx[1][0] * mx[0][1]; + return value; +} + +int m22IsEqual(double mx1[2][2], + double mx2[2][2], + double accuracy) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +int m22IsZero(double mx[2][2], + double accuracy) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +void m22Print(FILE *pFile, const char *name, double mx[2][2]) +{ + size_t dim1 = 2; + size_t dim2 = 2; + size_t i; + size_t j; + fprintf(pFile, "%s = [", name); + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + fprintf(pFile, "%20.15g", mx[i][j]); + if(j != dim2 - 1) { + fprintf(pFile, ", "); + } + } + if(i != dim1 - 1) { + fprintf(pFile, ";\n"); + } + } + fprintf(pFile, "]\n"); +} + +int m22Inverse(double mx[2][2], double result[2][2]) +{ + double det = m22Determinant(mx); + double detInv; + double m_result[2][2]; + int status = 0; + + if(fabs(det) > DB0_EPS) { + detInv = 1.0 / det; + m_result[0][0] = mx[1][1] * detInv; + m_result[0][1] = -mx[0][1] * detInv; + m_result[1][0] = -mx[1][0] * detInv; + m_result[1][1] = mx[0][0] * detInv; + } else { + BSK_PRINT(MSG_ERROR, "Error: singular 2x2 matrix inverse"); + m22Set(0.0, 0.0, + 0.0, 0.0, + m_result); + status = 1; + } + m22Copy(m_result, result); + return status; +} + +void m22PrintScreen(const char *name, double mx[2][2]) +{ + int i; + printf("%s:\n", name); + for (i=0;i<2;i++) { + printf("%20.15g, %20.15g\n", mx[i][0], mx[i][1]); + } +} + +void m33Set(double m00, double m01, double m02, + double m10, double m11, double m12, + double m20, double m21, double m22, + double m[3][3]) +{ + m[0][0] = m00; + m[0][1] = m01; + m[0][2] = m02; + m[1][0] = m10; + m[1][1] = m11; + m[1][2] = m12; + m[2][0] = m20; + m[2][1] = m21; + m[2][2] = m22; +} + +void m33Copy(double mx[3][3], + double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx[i][j]; + } + } +} + +void m33SetZero(double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = 0.0; + } + } +} + +void m33SetIdentity(double result[3][3]) +{ + size_t dim = 3; + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + result[i][j] = (i == j) ? 1.0 : 0.0; + } + } +} + +void m33Transpose(double mx[3][3], + double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + double m_result[3][3]; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[j][i] = mx[i][j]; + } + } + m33Copy(m_result, result); +} + +void m33Add(double mx1[3][3], + double mx2[3][3], + double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] + mx2[i][j]; + } + } +} + +void m33Subtract(double mx1[3][3], + double mx2[3][3], + double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] - mx2[i][j]; + } + } +} + +void m33Scale(double scaleFactor, + double mx[3][3], + double result[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = scaleFactor * mx[i][j]; + } + } +} + +void m33MultM33(double mx1[3][3], + double mx2[3][3], + double result[3][3]) +{ + size_t dim11 = 3; + size_t dim12 = 3; + size_t dim22 = 3; + size_t i; + size_t j; + size_t k; + double m_result[3][3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[k][j]; + } + } + } + m33Copy(m_result, result); +} + +void m33tMultM33(double mx1[3][3], + double mx2[3][3], + double result[3][3]) +{ + size_t dim11 = 3; + size_t dim12 = 3; + size_t dim22 = 3; + size_t i; + size_t j; + size_t k; + double m_result[3][3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[k][i] * mx2[k][j]; + } + } + } + m33Copy(m_result, result); +} + +void m33MultM33t(double mx1[3][3], + double mx2[3][3], + double result[3][3]) +{ + size_t dim11 = 3; + size_t dim12 = 3; + size_t dim21 = 3; + size_t i; + size_t j; + size_t k; + double m_result[3][3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim21; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[j][k]; + } + } + } + m33Copy(m_result, result); +} + +void m33MultV3(double mx[3][3], + double v[3], + double result[3]) +{ + size_t dim11 = 3; + size_t dim12 = 3; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[i][k] * v[k]; + } + } + } + v3Copy(m_result, result); +} + +void m33tMultV3(double mx[3][3], + double v[3], + double result[3]) +{ + size_t dim11 = 3; + size_t dim12 = 3; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[3]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[k][i] * v[k]; + } + } + } + v3Copy(m_result, result); +} + +double m33Trace(double mx[3][3]) +{ + size_t dim = 3; + size_t i; + double result = 0.0; + for(i = 0; i < dim; i++) { + result += mx[i][i]; + } + + return result; +} + +double m33Determinant(double mx[3][3]) +{ + double value; + value = mx[0][0] * mx[1][1] * mx[2][2] + + mx[0][1] * mx[1][2] * mx[2][0] + + mx[0][2] * mx[1][0] * mx[2][1] + - mx[0][0] * mx[1][2] * mx[2][1] + - mx[0][1] * mx[1][0] * mx[2][2] + - mx[0][2] * mx[1][1] * mx[2][0]; + return value; +} + +int m33IsEqual(double mx1[3][3], + double mx2[3][3], + double accuracy) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +int m33IsZero(double mx[3][3], + double accuracy) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +void m33Print(FILE *pFile, const char *name, double mx[3][3]) +{ + size_t dim1 = 3; + size_t dim2 = 3; + size_t i; + size_t j; + fprintf(pFile, "%s = [", name); + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + fprintf(pFile, "%20.15g", mx[i][j]); + if(j != dim2 - 1) { + fprintf(pFile, ", "); + } + } + if(i != dim1 - 1) { + fprintf(pFile, ";\n"); + } + } + fprintf(pFile, "]\n"); +} + +int m33Inverse(double mx[3][3], double result[3][3]) +{ + double det = m33Determinant(mx); + double detInv; + double m_result[3][3]; + int status = 0; + + if(fabs(det) > DB0_EPS) { + detInv = 1.0 / det; + m_result[0][0] = (mx[1][1] * mx[2][2] - mx[1][2] * mx[2][1]) * detInv; + m_result[0][1] = -(mx[0][1] * mx[2][2] - mx[0][2] * mx[2][1]) * detInv; + m_result[0][2] = (mx[0][1] * mx[1][2] - mx[0][2] * mx[1][1]) * detInv; + m_result[1][0] = -(mx[1][0] * mx[2][2] - mx[1][2] * mx[2][0]) * detInv; + m_result[1][1] = (mx[0][0] * mx[2][2] - mx[0][2] * mx[2][0]) * detInv; + m_result[1][2] = -(mx[0][0] * mx[1][2] - mx[0][2] * mx[1][0]) * detInv; + m_result[2][0] = (mx[1][0] * mx[2][1] - mx[1][1] * mx[2][0]) * detInv; + m_result[2][1] = -(mx[0][0] * mx[2][1] - mx[0][1] * mx[2][0]) * detInv; + m_result[2][2] = (mx[0][0] * mx[1][1] - mx[0][1] * mx[1][0]) * detInv; + } else { + BSK_PRINT(MSG_ERROR, "Error: singular 3x3 matrix inverse"); + m33Set(0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + m_result); + status = 1; + } + m33Copy(m_result, result); + return status; +} + +void m33SingularValues(double mx[3][3], double result[3]) +{ + double sv[3]; + double a[3]; + double mxtmx[3][3]; + int i; + + m33tMultM33(mx, mx, mxtmx); + + /* Compute characteristic polynomial */ + a[0] = -m33Determinant(mxtmx); + a[1] = mxtmx[0][0] * mxtmx[1][1] - mxtmx[0][1] * mxtmx[1][0] + + mxtmx[0][0] * mxtmx[2][2] - mxtmx[0][2] * mxtmx[2][0] + + mxtmx[1][1] * mxtmx[2][2] - mxtmx[1][2] * mxtmx[2][1]; + a[2] = -mxtmx[0][0] - mxtmx[1][1] - mxtmx[2][2]; + + /* Solve cubic equation */ + cubicRoots(a, sv); + + /* take square roots */ + for(i = 0; i < 3; i++) { + sv[i] = sqrt(sv[i]); + } + + /* order roots */ + v3Sort(sv, result); +} + +void m33EigenValues(double mx[3][3], double result[3]) +{ + double sv[3]; + double a[3]; + + /* Compute characteristic polynomial */ + a[0] = -m33Determinant(mx); + a[1] = mx[0][0] * mx[1][1] - mx[0][1] * mx[1][0] + + mx[0][0] * mx[2][2] - mx[0][2] * mx[2][0] + + mx[1][1] * mx[2][2] - mx[1][2] * mx[2][1]; + a[2] = -mx[0][0] - mx[1][1] - mx[2][2]; + + /* Solve cubic equation */ + cubicRoots(a, sv); + + /* order roots */ + v3Sort(sv, result); +} + +double m33ConditionNumber(double mx[3][3]) +{ + double sv[3]; + m33SingularValues(mx, sv); + return (sv[0] / sv[2]); +} + +void m33PrintScreen(const char *name, double mx[3][3]) +{ + int i; + printf("%s:\n", name); + for (i=0;i<3;i++) { + printf("%20.15g, %20.15g, %20.15g\n", mx[i][0], mx[i][1], mx[i][2]); + } +} + +void m44Set(double m00, double m01, double m02, double m03, + double m10, double m11, double m12, double m13, + double m20, double m21, double m22, double m23, + double m30, double m31, double m32, double m33, + double m[4][4]) +{ + m[0][0] = m00; + m[0][1] = m01; + m[0][2] = m02; + m[0][3] = m03; + m[1][0] = m10; + m[1][1] = m11; + m[1][2] = m12; + m[1][3] = m13; + m[2][0] = m20; + m[2][1] = m21; + m[2][2] = m22; + m[2][3] = m23; + m[3][0] = m30; + m[3][1] = m31; + m[3][2] = m32; + m[3][3] = m33; +} + +void m44Copy(double mx[4][4], + double result[4][4]) +{ + size_t dim1 = 4; + size_t dim2 = 4; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx[i][j]; + } + } +} + +void m44SetZero(double result[4][4]) +{ + size_t dim1 = 4; + size_t dim2 = 4; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = 0.0; + } + } +} + +void m44MultV4(double mx[4][4], + double v[4], + double result[4]) +{ + size_t dim11 = 4; + size_t dim12 = 4; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[4]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[i][k] * v[k]; + } + } + } + v4Copy(m_result, result); +} + +double m44Determinant(double mx[4][4]) +{ + double value; + value = mx[0][3] * mx[1][2] * mx[2][1] * mx[3][0] + - mx[0][2] * mx[1][3] * mx[2][1] * mx[3][0] + - mx[0][3] * mx[1][1] * mx[2][2] * mx[3][0] + + mx[0][1] * mx[1][3] * mx[2][2] * mx[3][0] + + mx[0][2] * mx[1][1] * mx[2][3] * mx[3][0] + - mx[0][1] * mx[1][2] * mx[2][3] * mx[3][0] + - mx[0][3] * mx[1][2] * mx[2][0] * mx[3][1] + + mx[0][2] * mx[1][3] * mx[2][0] * mx[3][1] + + mx[0][3] * mx[1][0] * mx[2][2] * mx[3][1] + - mx[0][0] * mx[1][3] * mx[2][2] * mx[3][1] + - mx[0][2] * mx[1][0] * mx[2][3] * mx[3][1] + + mx[0][0] * mx[1][2] * mx[2][3] * mx[3][1] + + mx[0][3] * mx[1][1] * mx[2][0] * mx[3][2] + - mx[0][1] * mx[1][3] * mx[2][0] * mx[3][2] + - mx[0][3] * mx[1][0] * mx[2][1] * mx[3][2] + + mx[0][0] * mx[1][3] * mx[2][1] * mx[3][2] + + mx[0][1] * mx[1][0] * mx[2][3] * mx[3][2] + - mx[0][0] * mx[1][1] * mx[2][3] * mx[3][2] + - mx[0][2] * mx[1][1] * mx[2][0] * mx[3][3] + + mx[0][1] * mx[1][2] * mx[2][0] * mx[3][3] + + mx[0][2] * mx[1][0] * mx[2][1] * mx[3][3] + - mx[0][0] * mx[1][2] * mx[2][1] * mx[3][3] + - mx[0][1] * mx[1][0] * mx[2][2] * mx[3][3] + + mx[0][0] * mx[1][1] * mx[2][2] * mx[3][3]; + return value; +} + +int m44IsEqual(double mx1[4][4], + double mx2[4][4], + double accuracy) +{ + size_t dim1 = 4; + size_t dim2 = 4; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +int m44Inverse(double mx[4][4], double result[4][4]) +{ + double det = m44Determinant(mx); + double detInv; + double m_result[4][4]; + int status = 0; + + if(fabs(det) > DB0_EPS) { + detInv = 1.0 / det; + m_result[0][0] = (mx[1][2] * mx[2][3] * mx[3][1] - mx[1][3] * mx[2][2] * mx[3][1] + mx[1][3] * mx[2][1] * mx[3][2] - mx[1][1] * mx[2][3] * mx[3][2] - mx[1][2] * mx[2][1] * mx[3][3] + mx[1][1] * mx[2][2] * mx[3][3]) * detInv; + m_result[0][1] = (mx[0][3] * mx[2][2] * mx[3][1] - mx[0][2] * mx[2][3] * mx[3][1] - mx[0][3] * mx[2][1] * mx[3][2] + mx[0][1] * mx[2][3] * mx[3][2] + mx[0][2] * mx[2][1] * mx[3][3] - mx[0][1] * mx[2][2] * mx[3][3]) * detInv; + m_result[0][2] = (mx[0][2] * mx[1][3] * mx[3][1] - mx[0][3] * mx[1][2] * mx[3][1] + mx[0][3] * mx[1][1] * mx[3][2] - mx[0][1] * mx[1][3] * mx[3][2] - mx[0][2] * mx[1][1] * mx[3][3] + mx[0][1] * mx[1][2] * mx[3][3]) * detInv; + m_result[0][3] = (mx[0][3] * mx[1][2] * mx[2][1] - mx[0][2] * mx[1][3] * mx[2][1] - mx[0][3] * mx[1][1] * mx[2][2] + mx[0][1] * mx[1][3] * mx[2][2] + mx[0][2] * mx[1][1] * mx[2][3] - mx[0][1] * mx[1][2] * mx[2][3]) * detInv; + m_result[1][0] = (mx[1][3] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][0] * mx[3][2] + mx[1][0] * mx[2][3] * mx[3][2] + mx[1][2] * mx[2][0] * mx[3][3] - mx[1][0] * mx[2][2] * mx[3][3]) * detInv; + m_result[1][1] = (mx[0][2] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][2] * mx[3][0] + mx[0][3] * mx[2][0] * mx[3][2] - mx[0][0] * mx[2][3] * mx[3][2] - mx[0][2] * mx[2][0] * mx[3][3] + mx[0][0] * mx[2][2] * mx[3][3]) * detInv; + m_result[1][2] = (mx[0][3] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][0] * mx[3][2] + mx[0][0] * mx[1][3] * mx[3][2] + mx[0][2] * mx[1][0] * mx[3][3] - mx[0][0] * mx[1][2] * mx[3][3]) * detInv; + m_result[1][3] = (mx[0][2] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][2] * mx[2][0] + mx[0][3] * mx[1][0] * mx[2][2] - mx[0][0] * mx[1][3] * mx[2][2] - mx[0][2] * mx[1][0] * mx[2][3] + mx[0][0] * mx[1][2] * mx[2][3]) * detInv; + m_result[2][0] = (mx[1][1] * mx[2][3] * mx[3][0] - mx[1][3] * mx[2][1] * mx[3][0] + mx[1][3] * mx[2][0] * mx[3][1] - mx[1][0] * mx[2][3] * mx[3][1] - mx[1][1] * mx[2][0] * mx[3][3] + mx[1][0] * mx[2][1] * mx[3][3]) * detInv; + m_result[2][1] = (mx[0][3] * mx[2][1] * mx[3][0] - mx[0][1] * mx[2][3] * mx[3][0] - mx[0][3] * mx[2][0] * mx[3][1] + mx[0][0] * mx[2][3] * mx[3][1] + mx[0][1] * mx[2][0] * mx[3][3] - mx[0][0] * mx[2][1] * mx[3][3]) * detInv; + m_result[2][2] = (mx[0][1] * mx[1][3] * mx[3][0] - mx[0][3] * mx[1][1] * mx[3][0] + mx[0][3] * mx[1][0] * mx[3][1] - mx[0][0] * mx[1][3] * mx[3][1] - mx[0][1] * mx[1][0] * mx[3][3] + mx[0][0] * mx[1][1] * mx[3][3]) * detInv; + m_result[2][3] = (mx[0][3] * mx[1][1] * mx[2][0] - mx[0][1] * mx[1][3] * mx[2][0] - mx[0][3] * mx[1][0] * mx[2][1] + mx[0][0] * mx[1][3] * mx[2][1] + mx[0][1] * mx[1][0] * mx[2][3] - mx[0][0] * mx[1][1] * mx[2][3]) * detInv; + m_result[3][0] = (mx[1][2] * mx[2][1] * mx[3][0] - mx[1][1] * mx[2][2] * mx[3][0] - mx[1][2] * mx[2][0] * mx[3][1] + mx[1][0] * mx[2][2] * mx[3][1] + mx[1][1] * mx[2][0] * mx[3][2] - mx[1][0] * mx[2][1] * mx[3][2]) * detInv; + m_result[3][1] = (mx[0][1] * mx[2][2] * mx[3][0] - mx[0][2] * mx[2][1] * mx[3][0] + mx[0][2] * mx[2][0] * mx[3][1] - mx[0][0] * mx[2][2] * mx[3][1] - mx[0][1] * mx[2][0] * mx[3][2] + mx[0][0] * mx[2][1] * mx[3][2]) * detInv; + m_result[3][2] = (mx[0][2] * mx[1][1] * mx[3][0] - mx[0][1] * mx[1][2] * mx[3][0] - mx[0][2] * mx[1][0] * mx[3][1] + mx[0][0] * mx[1][2] * mx[3][1] + mx[0][1] * mx[1][0] * mx[3][2] - mx[0][0] * mx[1][1] * mx[3][2]) * detInv; + m_result[3][3] = (mx[0][1] * mx[1][2] * mx[2][0] - mx[0][2] * mx[1][1] * mx[2][0] + mx[0][2] * mx[1][0] * mx[2][1] - mx[0][0] * mx[1][2] * mx[2][1] - mx[0][1] * mx[1][0] * mx[2][2] + mx[0][0] * mx[1][1] * mx[2][2]) * detInv; + } else { + BSK_PRINT(MSG_ERROR, "Error: singular 4x4 matrix inverse"); + m44Set(0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + m_result); + status = 1; + } + m44Copy(m_result, result); + return status; +} + +void m66Set(double m00, double m01, double m02, double m03, double m04, double m05, + double m10, double m11, double m12, double m13, double m14, double m15, + double m20, double m21, double m22, double m23, double m24, double m25, + double m30, double m31, double m32, double m33, double m34, double m35, + double m40, double m41, double m42, double m43, double m44, double m45, + double m50, double m51, double m52, double m53, double m54, double m55, + double m[6][6]) +{ + m[0][0] = m00; + m[0][1] = m01; + m[0][2] = m02; + m[0][3] = m03; + m[0][4] = m04; + m[0][5] = m05; + m[1][0] = m10; + m[1][1] = m11; + m[1][2] = m12; + m[1][3] = m13; + m[1][4] = m14; + m[1][5] = m15; + m[2][0] = m20; + m[2][1] = m21; + m[2][2] = m22; + m[2][3] = m23; + m[2][4] = m24; + m[2][5] = m25; + m[3][0] = m30; + m[3][1] = m31; + m[3][2] = m32; + m[3][3] = m33; + m[3][4] = m34; + m[3][5] = m35; + m[4][0] = m40; + m[4][1] = m41; + m[4][2] = m42; + m[4][3] = m43; + m[4][4] = m44; + m[4][5] = m45; + m[5][0] = m50; + m[5][1] = m51; + m[5][2] = m52; + m[5][3] = m53; + m[5][4] = m54; + m[5][5] = m55; +} + +void m66Copy(double mx[6][6], + double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx[i][j]; + } + } +} + +void m66SetZero(double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = 0.0; + } + } +} + +void m66SetIdentity(double result[6][6]) +{ + size_t dim = 6; + size_t i; + size_t j; + for(i = 0; i < dim; i++) { + for(j = 0; j < dim; j++) { + result[i][j] = (i == j) ? 1.0 : 0.0; + } + } +} + +void m66Transpose(double mx[6][6], + double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + double m_result[6][6]; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + m_result[j][i] = mx[i][j]; + } + } + m66Copy(m_result, result); +} + +void m66Get33Matrix(size_t row, size_t col, + double m[6][6], + double mij[3][3]) +{ + size_t i; + size_t j; + /* WARNING! Doesn't error check that row = 0,1,N-1 and col = 0,1,N-1 */ + for(i = 0; i < 3; i++) { + for(j = 0; j < 3; j++) { + mij[i][j] = m[row * 3 + i][col * 3 + j]; + } + } +} + +void m66Set33Matrix(size_t row, size_t col, + double mij[3][3], + double m[6][6]) +{ + size_t i; + size_t j; + /* WARNING! Doesn't error check that row = 0,1,N-1 and col = 0,1,N-1 */ + for(i = 0; i < 3; i++) { + for(j = 0; j < 3; j++) { + m[row * 3 + i][col * 3 + j] = mij[i][j]; + } + } +} + +void m66Scale(double scaleFactor, + double mx[6][6], + double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = scaleFactor * mx[i][j]; + } + } +} + +void m66Add(double mx1[6][6], + double mx2[6][6], + double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] + mx2[i][j]; + } + } +} + +void m66Subtract(double mx1[6][6], + double mx2[6][6], + double result[6][6]) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = mx1[i][j] - mx2[i][j]; + } + } +} + +void m66MultM66(double mx1[6][6], + double mx2[6][6], + double result[6][6]) +{ + size_t dim11 = 6; + size_t dim12 = 6; + size_t dim22 = 6; + size_t i; + size_t j; + size_t k; + double m_result[6][6]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[k][j]; + } + } + } + m66Copy(m_result, result); +} + +void m66tMultM66(double mx1[6][6], + double mx2[6][6], + double result[6][6]) +{ + size_t dim11 = 6; + size_t dim12 = 6; + size_t dim22 = 6; + size_t i; + size_t j; + size_t k; + double m_result[6][6]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[k][i] * mx2[k][j]; + } + } + } + m66Copy(m_result, result); +} + +void m66MultM66t(double mx1[6][6], + double mx2[6][6], + double result[6][6]) +{ + size_t dim11 = 6; + size_t dim12 = 6; + size_t dim21 = 6; + size_t i; + size_t j; + size_t k; + double m_result[6][6]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim21; j++) { + m_result[i][j] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i][j] += mx1[i][k] * mx2[j][k]; + } + } + } + m66Copy(m_result, result); +} + +void m66MultV6(double mx[6][6], + double v[6], + double result[6]) +{ + size_t dim11 = 6; + size_t dim12 = 6; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[6]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[i][k] * v[k]; + } + } + } + v6Copy(m_result, result); +} + +void m66tMultV6(double mx[6][6], + double v[6], + double result[6]) +{ + size_t dim11 = 6; + size_t dim12 = 6; + size_t dim22 = 1; + size_t i; + size_t j; + size_t k; + double m_result[6]; + for(i = 0; i < dim11; i++) { + for(j = 0; j < dim22; j++) { + m_result[i] = 0.0; + for(k = 0; k < dim12; k++) { + m_result[i] += mx[k][i] * v[k]; + } + } + } + v6Copy(m_result, result); +} + +int m66IsEqual(double mx1[6][6], + double mx2[6][6], + double accuracy) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx1[i][j] - mx2[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +int m66IsZero(double mx[6][6], + double accuracy) +{ + size_t dim1 = 6; + size_t dim2 = 6; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + if(fabs(mx[i][j]) > accuracy) { + return 0; + } + } + } + return 1; +} + +void m99SetZero(double result[9][9]) +{ + size_t dim1 = 9; + size_t dim2 = 9; + size_t i; + size_t j; + for(i = 0; i < dim1; i++) { + for(j = 0; j < dim2; j++) { + result[i][j] = 0.0; + } + } +} + +void cubicRoots(double a[3], double result[3]) +{ + /* Solve cubic formula for x^3 + a[2]*x^2 + a[1]*x + a[0] = 0 */ + /* see http://mathworld.wolfram.com/CubicFormula.html */ + double a2sq = a[2] * a[2]; + double a2d3 = a[2] / 3.0; + double Q = (3.0 * a[1] - a2sq) / 9.0; + double R = (a[2] * (9 * a[1] - 2 * a2sq) - 27 * a[0]) / 54.0; + double RdsqrtnQ3 = R / sqrt(-Q * Q * Q); + + if(Q < 0.0 && fabs(RdsqrtnQ3) < 1.0) { + /* A = 2*sqrt(-Q) + * B = a[2]/3 + * result[0] = Acos(t) - B + * result[1] = Acos(t + 2pi/3) - B = A*(cos(t)*-0.5 - sin(t)*sqrt(3)*0.5) - B + * result[1] = Acos(t + 4pi/3) - B = A*(cos(t)*-0.5 + sin(t)*sqrt(3)*0.5) - B */ + double A = 2.0 * sqrt(-Q); + double td3 = safeAcos(RdsqrtnQ3) / 3.0; + double costd3 = cos(td3); + double sintd3 = sin(td3); + double sqrt3d2 = sqrt(3) * 0.5; + double temp1 = -0.5 * costd3; + double temp2 = sqrt3d2 * sintd3; + + result[0] = A * costd3 - a2d3; + result[1] = A * (temp1 - temp2) - a2d3; + result[2] = A * (temp1 + temp2) - a2d3; + } else { + double D = Q * Q * Q + R * R; + double sqrtD = sqrt(D); + double S = cbrt(R + sqrtD); + double T = cbrt(R - sqrtD); + + result[0] = -a2d3 + (S + T); + result[1] = -a2d3 - 0.5 * (S + T); + result[2] = result[1]; + } + +} + +double safeAcos (double x) { + if (x < -1.0) + return acos(-1); + else if (x > 1.0) + return acos(1) ; + return acos (x) ; +} + +double safeAsin (double x) { + if (x < -1.0) + return asin(-1); + else if (x > 1.0) + return asin(1) ; + return asin (x) ; +} + +double safeSqrt(double x) { + if (x < 0.0) + return 0.0; + return sqrt(x); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.h new file mode 100644 index 0000000000..aa16b45ad8 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearAlgebra.h @@ -0,0 +1,264 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _LINEARALGEBRA_H_ +#define _LINEARALGEBRA_H_ + +#include +#include + +/* Divide by zero epsilon value */ +#define DB0_EPS 1e-30 + +/* define a maximum array size for the functions that need + to allocate memory within their routine */ +#define LINEAR_ALGEBRA_MAX_ARRAY_SIZE (64*64) + +#define MXINDEX(dim2, row, col) ((row)*(dim2) + (col)) + +/* General vectors */ +#ifdef __cplusplus +extern "C" { +#endif + + /* N element vectors */ + void vElementwiseMult(double *v1, size_t dim, double *v2, double *result); + void vCopy(double *v, size_t dim, double *result); + void vSetZero(double *v, size_t dim); + void vSetOnes(double *v, size_t dim); + void vAdd(double *v1, size_t dim, double *v2, double *result); + void vSubtract(double *v1, size_t dim, double *v2, double *result); + void vScale(double scaleFactor, double *v, size_t dim, double *result); + double vDot(double *v1, size_t dim, double *v2); + void vOuterProduct(double *v1, size_t dim1, double *v2, size_t dim2, void *result); + void vtMultM(double *v, void *mx, size_t dim1, size_t dim2, void *result); + void vtMultMt(double *v, void *mx, size_t dim1, size_t dim2, void *result); + double vNorm(double *v, size_t dim); + double vMax(double *v, size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of a small 1-D array*/ + double vMaxAbs(double *v, size_t dim); /* Non-sorted, non-optimized algorithm for finding the max of the absolute values of the elements of a small 1-D array*/ + void vNormalize(double *v, size_t dim, double *result); + int vIsEqual(double *v1, size_t dim, double *v2, double accuracy); + int vIsZero(double *v, size_t dim, double accuracy); + void vPrint(FILE *pFile, const char *name, double *v, size_t dim); + void vSort(double *Input, double *Output, size_t dim); + + /* 2 element vectors */ + void v2Set(double v0, double v1, double result[2]); + void v2Copy(double v[2], double result[2]); + void v2Scale(double scaleFactor, double v[2], double result[2]); + void v2SetZero(double v[2]); + double v2Dot(double v1[2], double v2[2]); + int v2IsEqual(double v1[2], double v2[2], double accuracy); + int v2IsZero(double v[2], double accuracy); + void v2Add(double v1[2], double v2[2], double result[2]); + void v2Subtract(double v1[2], double v2[2], double result[2]); + double v2Norm(double v1[2]); + void v2Normalize(double v1[2], double result[2]); + + /* 3 element vectors */ + void v3Set(double v0, double v1, double v2, double result[3]); + void v3Copy(double v[3], double result[3]); + void v3SetZero(double v[3]); + void v3Add(double v1[3], double v2[3], double result[3]); + void v3Subtract(double v1[3], double v2[3], double result[3]); + void v3Scale(double scaleFactor, double v[3], double result[3]); + double v3Dot(double v1[3], double v2[3]); + void v3OuterProduct(double v1[3], double v2[3], double result[3][3]); + void v3tMultM33(double v[3], double mx[3][3], double result[3]); + void v3tMultM33t(double v[3], double mx[3][3], double result[3]); + double v3Norm(double v[3]); + void v3Normalize(double v[3], double result[3]); + int v3IsEqual(double v1[3], double v2[3], double accuracy); + int v3IsEqualRel(double v1[3], double v2[3], double accuracy); + int v3IsZero(double v[3], double accuracy); + void v3Print(FILE *pFile, const char *name, double v[3]); + void v3Cross(double v1[3], double v2[3], double result[3]); + void v3Perpendicular(double v[3], double result[3]); + void v3Tilde(double v[3], double result[3][3]); + void v3Sort(double v[3], double result[3]); + void v3PrintScreen(const char *name, double v[3]); + + /* 4 element vectors */ + void v4Set(double v0, double v1, double v2, double v3, double result[4]); + void v4Copy(double v[4], double result[4]); + void v4SetZero(double v[4]); + double v4Dot(double v1[4], double v2[4]); + double v4Norm(double v[4]); + int v4IsEqual(double v1[4], double v2[4], double accuracy); + int v4IsZero(double v[4], double accuracy); + + /* 6 element vectors */ + void v6Set(double v0, double v1, double v2, double v3, double v4, double v5, double result[6]); + void v6Copy(double v[6], double result[6]); + double v6Dot(double v1[6], double v2[6]); + void v6Scale(double scaleFactor, double v[6], double result[6]); + void v6OuterProduct(double v1[6], double v2[6], double result[6][6]); + int v6IsEqual(double v1[6], double v2[6], double accuracy); + + /* NxM matrices */ + void mLeastSquaresInverse(void *mx, size_t dim1, size_t dim2, void *result); + void mMinimumNormInverse(void *mx, size_t dim1, size_t dim2, void *result); + void mCopy(void *mx, size_t dim1, size_t dim2, void *result); + void mSetZero(void *result, size_t dim1, size_t dim2); + void mSetIdentity(void *result, size_t dim1, size_t dim2); + void mDiag(void *v, size_t dim, void *result); + void mTranspose(void *mx, size_t dim1, size_t dim2, void *result); + void mAdd(void *mx1, size_t dim1, size_t dim2, void *mx2, void *result); + void mSubtract(void *mx1, size_t dim1, size_t dim2, void *mx2, void *result); + void mScale(double scaleFactor, void *mx, size_t dim1, size_t dim2, void *result); + void mMultM(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result); + void mtMultM(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result); + void mMultMt(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result); + void mtMultMt(void *mx1, size_t dim11, size_t dim12, + void *mx2, size_t dim21, size_t dim22, + void *result); + void mMultV(void *mx, size_t dim1, size_t dim2, + void *v, + void *result); + void mtMultV(void *mx, size_t dim1, size_t dim2, + void *v, + void *result); + double mTrace(void *mx, size_t dim); + double mDeterminant(void *mx, size_t dim); + void mCofactor(void *mx, size_t dim, void *result); + int mInverse(void *mx, size_t dim, void *result); + int mIsEqual(void *mx1, size_t dim1, size_t dim2, void *mx2, double accuracy); + int mIsZero(void *mx, size_t dim1, size_t dim2, double accuracy); + void mPrintScreen(const char *name, void *mx, size_t dim1, size_t dim2); + void mPrint(FILE *pFile, const char *name, void *mx, size_t dim1, size_t dim2); + void mGetSubMatrix(void *mx, size_t dim1, size_t dim2, + size_t dim1Start, size_t dim2Start, + size_t dim1Result, size_t dim2Result, void *result); + void mSetSubMatrix(void *mx, size_t dim1, size_t dim2, + void *result, size_t dim1Result, size_t dim2Result, + size_t dim1Start, size_t dim2Start); + + /* 2x2 matrices */ + void m22Set(double m00, double m01, + double m10, double m11, + double m[2][2]); + void m22Copy(double mx[2][2], double result[2][2]); + void m22SetZero(double result[2][2]); + void m22SetIdentity(double result[2][2]); + void m22Transpose(double mx[2][2], double result[2][2]); + void m22Add(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22Subtract(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22Scale(double scaleFactor, double mx[2][2], double result[2][2]); + void m22MultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22tMultM22(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22MultM22t(double mx1[2][2], double mx2[2][2], double result[2][2]); + void m22MultV2(double mx[2][2], double v[2], double result[2]); + void m22tMultV2(double mx[2][2], double v[2], double result[2]); + double m22Trace(double mx[2][2]); + double m22Determinant(double mx[2][2]); + int m22IsEqual(double mx1[2][2], double mx2[2][2], double accuracy); + int m22IsZero(double mx[2][2], double accuracy); + void m22Print(FILE *pFile, const char *name, double mx[2][2]); + int m22Inverse(double mx[2][2], double result[2][2]); + void m22PrintScreen(const char *name, double mx[2][2]); + + /* 3x3 matrices */ + void m33Set(double m00, double m01, double m02, + double m10, double m11, double m12, + double m20, double m21, double m22, + double m[3][3]); + void m33Copy(double mx[3][3], double result[3][3]); + void m33SetZero(double result[3][3]); + void m33SetIdentity(double result[3][3]); + void m33Transpose(double mx[3][3], double result[3][3]); + void m33Add(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33Subtract(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33Scale(double scaleFactor, double mx[3][3], double result[3][3]); + void m33MultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33tMultM33(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33MultM33t(double mx1[3][3], double mx2[3][3], double result[3][3]); + void m33MultV3(double mx[3][3], double v[3], double result[3]); + void m33tMultV3(double mx[3][3], double v[3], double result[3]); + double m33Trace(double mx[3][3]); + double m33Determinant(double mx[3][3]); + int m33IsEqual(double mx1[3][3], double mx2[3][3], double accuracy); + int m33IsZero(double mx[3][3], double accuracy); + void m33Print(FILE *pfile, const char *name, double mx[3][3]); + int m33Inverse(double mx[3][3], double result[3][3]); + void m33SingularValues(double mx[3][3], double result[3]); + void m33EigenValues(double mx[3][3], double result[3]); + double m33ConditionNumber(double mx[3][3]); + void m33PrintScreen(const char *name, double mx[3][3]); + + /* 4x4 matrices */ + void m44Set(double m00, double m01, double m02, double m03, + double m10, double m11, double m12, double m13, + double m20, double m21, double m22, double m23, + double m30, double m31, double m32, double m33, + double m[4][4]); + void m44Copy(double mx[4][4], double result[4][4]); + void m44SetZero(double result[4][4]); + void m44MultV4(double mx[4][4], double v[4], double result[4]); + double m44Determinant(double mx[4][4]); + int m44IsEqual(double mx1[4][4], double mx2[4][4], double accuracy); + int m44Inverse(double mx[4][4], double result[4][4]); + + /* 6x6 matrices */ + void m66Set(double m00, double m01, double m02, double m03, double m04, double m05, + double m10, double m11, double m12, double m13, double m14, double m15, + double m20, double m21, double m22, double m23, double m24, double m25, + double m30, double m31, double m32, double m33, double m34, double m35, + double m40, double m41, double m42, double m43, double m44, double m45, + double m50, double m51, double m52, double m53, double m54, double m55, + double m[6][6]); + void m66Copy(double mx[6][6], double result[6][6]); + void m66SetZero(double result[6][6]); + void m66SetIdentity(double result[6][6]); + void m66Transpose(double mx[6][6], double result[6][6]); + void m66Get33Matrix(size_t row, size_t col, double m[6][6], double mij[3][3]); + void m66Set33Matrix(size_t row, size_t col, double mij[3][3], double m[6][6]); + void m66Scale(double scaleFactor, double mx[6][6], double result[6][6]); + void m66Add(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66Subtract(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66tMultM66(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultM66t(double mx1[6][6], double mx2[6][6], double result[6][6]); + void m66MultV6(double mx[6][6], double v[6], double result[6]); + void m66tMultV6(double mx[6][6], double v[6], double result[6]); + int m66IsEqual(double mx1[6][6], double mx2[6][6], double accuracy); + int m66IsZero(double mx[6][6], double accuracy); + + /* 9x9 matrices */ + void m99SetZero(double result[9][9]); + + /* additional routines */ + /* Solve cubic formula for x^3 + a[2]*x^2 + a[1]*x + a[0] = 0 */ + void cubicRoots(double a[3], double result[3]); + + double safeAcos(double x); + double safeAsin(double x); + double safeSqrt(double x); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearInterpolation.hpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearInterpolation.hpp new file mode 100644 index 0000000000..f7a26ae9b9 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/linearInterpolation.hpp @@ -0,0 +1,41 @@ +/* + ISC License + + Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef LINEARINTERPOLATION_H +#define LINEARINTERPOLATION_H + +#include + +/*! This function uses linear interpolation to solve for the value of an unknown function of a single variables f(x) + at the point x. +@return double +@param x1 Data point x1 +@param x2 Data point x2 +@param y1 Function value at point x1 +@param y2 Function value at point x2 +@param x Function x coordinate for interpolation +*/ +double linearInterpolation(double x1, double x2, double y1, double y2, double x) { + + assert(x1 < x && x < x2); + + return y1 * (x2 - x) / (x2 - x1) + y2 * (x - x1) / (x2 - x1); +} + +#endif // LINEARINTERPOLATION_H diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/macroDefinitions.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/macroDefinitions.h new file mode 100644 index 0000000000..45b4c38cfb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/macroDefinitions.h @@ -0,0 +1,102 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef SIM_FSW_MACROS_H +#define SIM_FSW_MACROS_H + +#define MAX_CIRCLE_NUM 10 +#define MAX_LIMB_PNTS 2000 +#define MAX_EFF_CNT 36 +#define MAX_NUM_CSS_SENSORS 32 +#define MAX_ST_VEH_COUNT 4 + +#define NANO2SEC 1e-9 +#define SEC2NANO 1e9 +#define RECAST6X6 (double (*)[6]) +#define RECAST3X3 (double (*)[3]) +#define RECAST2x2 (double (*)[2]) +#define SEC2HOUR 1./3600. + +#include // For uint64_t +#include // For DBL_MANT_DIG +#include // For NAN, fabs() +#include +#include + +/** + * Converts nanoseconds to seconds (double), with basic precision check. + * Returns NAN if conversion would lose precision. + */ +static inline double nanoToSec(uint64_t nanos) { + // Double precision loses exact integer values past 2^53 + const uint64_t MAX_SAFE_UINT64_FOR_DOUBLE = (1ULL << DBL_MANT_DIG); // Usually 2^53 + + if (nanos > MAX_SAFE_UINT64_FOR_DOUBLE) { + fprintf(stderr, + "[nano_to_sec] ERROR: Input %" PRIu64 + " exceeds safe conversion limit (~%" PRIu64 "). Precision may be lost. Returning NAN.\n", + nanos, MAX_SAFE_UINT64_FOR_DOUBLE); + return NAN; // Indicate precision loss + } + + return (double)nanos * NANO2SEC; // Convert to seconds +} + +/** + * Takes two times in nanoseconds, takes their difference and converts to seconds (double), + * with basic precision check. + * Returns NAN if conversion would lose precision. + */ +static inline double diffNanoToSec(uint64_t time1Nano, uint64_t time2Nano) { + double signedTimeDifference; + + if (time1Nano >= time2Nano) { + signedTimeDifference = nanoToSec(time1Nano - time2Nano); + } else { + signedTimeDifference = -nanoToSec(time2Nano - time1Nano); + } + + return signedTimeDifference; +} + +/** + * Converts seconds (double) to nanoseconds (uint64_t). + * Returns NAN on error (e.g. negative input or overflow) + */ +static inline uint64_t secToNano(double seconds) { + if (seconds < 0.0) { + fprintf(stderr, + "[secToNano] ERROR: Negative time value passed (%.15f seconds). Returning 0.\n", + seconds); + return 0; + } + + double result = seconds * SEC2NANO; + + if (result > (double)UINT64_MAX) { + fprintf(stderr, + "[secToNano] ERROR: Input time %.15f seconds exceeds uint64_t capacity. Returning 0.\n", + seconds); + return 0; + } + + return (uint64_t)(result + 0.5); // Round to nearest integer +} + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp new file mode 100644 index 0000000000..7ab9935fff --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.cpp @@ -0,0 +1,64 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "moduleIdGenerator.h" +#include + +/*! + * This constructor for TheInstance just sets it NULL + */ +ModuleIdGenerator* ModuleIdGenerator::TheInstance = NULL; + +/*! + * This constructor for ModuleIdGenerator initializes things + */ +ModuleIdGenerator::ModuleIdGenerator() +{ + this->nextModuleID = 1; +} + +/*! + * This desturctor for ModuleIdGenerator free memory + */ + +ModuleIdGenerator::~ModuleIdGenerator() +{ + delete TheInstance; + TheInstance = NULL; +} +/*! + * This gives a pointer to the messaging system to whoever asks for it. + * @return ModuleIdGenerator* TheInstance + */ +ModuleIdGenerator* ModuleIdGenerator::GetInstance() +{ + if(TheInstance == NULL) + { + TheInstance = new ModuleIdGenerator(); + } + return(TheInstance); +} + +/*! + * This method assigns a module ID to a new module and increments the NextModuleID counter + * @return uint64_t nextModuleID the newly minted module ID + */ +int64_t ModuleIdGenerator::checkoutModuleID() +{ + return(this->nextModuleID++); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h new file mode 100644 index 0000000000..90624cea68 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/moduleIdGenerator/moduleIdGenerator.h @@ -0,0 +1,47 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _ModuleIdGenerator_HH_ +#define _ModuleIdGenerator_HH_ + +#include + +/*! @brief module ID generating class */ +#ifdef _WIN32 +class __declspec( dllexport) ModuleIdGenerator +#else +class ModuleIdGenerator +#endif +{ +public: + int64_t checkoutModuleID(); //! -- Assigns next integer module ID + static ModuleIdGenerator* GetInstance(); //! -- returns a pointer to the sim instance of ModuleIdGenerator + +private: + int64_t nextModuleID; //!< the next module ID to give out when a module (SysModel sub-class) comes online + static ModuleIdGenerator *TheInstance; //!< instance of simulation module + + ModuleIdGenerator(); + ~ModuleIdGenerator(); + ModuleIdGenerator(ModuleIdGenerator const &) {}; + ModuleIdGenerator& operator =(ModuleIdGenerator const &){return(*this);}; + +}; + +#endif /* _ModuleIdGenerator_HH_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.c new file mode 100644 index 0000000000..99291e0ffb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.c @@ -0,0 +1,1018 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "orbitalMotion.h" + +#include +#include +#include + +#include "linearAlgebra.h" +#include "astroConstants.h" +#include "architecture/utilities/bsk_Print.h" + + +/*! + * Purpose: maps inertial position and velocity vectors in the Hill frame DCM HN + * Inputs: + * rc_N: inertial position vector + * vc_N: inertial velocity vector + * Outputs: + * HN: Hill frame DCM relative to inertial frame + */ +void hillFrame(double *rc_N, double *vc_N, double HN[3][3]) +{ + double ir_N[3]; /* orbit radial unit direction vector */ + double itheta_N[3]; /* along-track unit direction vector */ + double ih_N[3]; /* orbit plane normal unit direction vector */ + double hVec_N[3]; /* orbit angular momentum vector */ + + v3Normalize(rc_N, ir_N); + v3Cross(rc_N, vc_N, hVec_N); + v3Normalize(hVec_N, ih_N); + v3Cross(ih_N, ir_N, itheta_N); + v3Copy(ir_N, HN[0]); + v3Copy(itheta_N, HN[1]); + v3Copy(ih_N, HN[2]); +} + +/*! + * Purpose: maps Hill frame deputy states to inertial inertial position and velocity vectors + * Inputs: + * rc_N: chief inertial position vector + * vc_N: chief inertial velocity vector + * rho_H: deputy Hill position vector + * rhoPrime_H: deputy Hill velocity vector + * Outputs: + * rd_N: deputy inertial position vector + * vd_N: deputy inertial velocity vector + */ +void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, double *rd_N, double *vd_N) +{ + double HN[3][3]; /* DCM of Hill frame relative to inertial */ + double NH[3][3]; /* DCM of inertial frame relative to Hill */ + double hVec_N[3]; /* orbit angular momentum vector */ + double rc; /* chief orbit radius */ + double fDot; /* chief true anomaly rate */ + double omega_HN_H[3]; /* Hill frame angular velocity */ + double rho_N[3]; /* deputy relative to chief vector in N frame components */ + + hillFrame(rc_N, vc_N, HN); + m33Transpose(HN, NH); + v3Cross(rc_N, vc_N, hVec_N); + rc = v3Norm(rc_N); + fDot = v3Norm(hVec_N)/rc/rc; + v3Set(0, 0, fDot, omega_HN_H); + + /* compute inertial deputy position */ + m33MultV3(NH, rho_H, rho_N); + v3Add(rc_N, rho_N, rd_N); + + /* compute inertial deputy velocity */ + v3Cross(omega_HN_H, rho_H, vd_N); + v3Add(vd_N, rhoPrime_H, vd_N); + m33MultV3(NH, vd_N, vd_N); + v3Add(vd_N, vc_N, vd_N); +} + + +/*! + * Purpose: maps inertial frame deputy states to Hill inertial position and velocity vectors + * Inputs: + * rc_N: chief inertial position vector + * vc_N: chief inertial velocity vector + * rd_N: deputy inertial position vector + * vd_N: deputy inertial velocity vector + * Outputs: + * rho_H: deputy Hill position vector + * rhoPrime_H: deputy Hill velocity vector + */ +void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho_H, double *rhoPrime_H) +{ + double HN[3][3]; /* DCM of Hill frame relative to inertial */ + double hVec_N[3]; /* orbit angular momentum vector */ + double rc; /* chief orbit radius */ + double fDot; /* chief true anomaly rate */ + double omega_HN_H[3]; /* Hill frame angular velocity */ + double rho_N[3]; /* deputy relative to chief vector in N frame components */ + double rhoDot_N[3]; /* inertial derivative of deputy/chief vector */ + double rhoDot_H[3]; /* inertial derivative of deputy/chief vector */ + + hillFrame(rc_N, vc_N, HN); + v3Cross(rc_N, vc_N, hVec_N); + rc = v3Norm(rc_N); + fDot = v3Norm(hVec_N)/rc/rc; + v3Set(0, 0, fDot, omega_HN_H); + + /* compute Hill frame position */ + v3Subtract(rd_N, rc_N, rho_N); + m33MultV3(HN, rho_N, rho_H); + + /* compute Hill frame velocity */ + v3Subtract(vd_N, vc_N, rhoDot_N); + m33MultV3(HN, rhoDot_N, rhoDot_H); + v3Cross(omega_HN_H, rho_H, rhoPrime_H); + v3Subtract(rhoDot_H, rhoPrime_H, rhoPrime_H); +} + + +/*! + * Purpose: Maps eccentric anomaly angles into true anomaly angles. + * This function requires the orbit to be either circular or + * non-rectilinar elliptic orbit. + * Inputs: + * Ecc = eccentric anomaly (rad) + * e = eccentricity (0 <= e < 1) + * Outputs: + * f = true anomaly (rad) + */ +double E2f(double Ecc, double e) +{ + double f; + + if((e >= 0) && (e < 1)) { + f = 2 * atan2(sqrt(1 + e) * sin(Ecc / 2), sqrt(1 - e) * cos(Ecc / 2)); + } else { + f = NAN; + BSK_PRINT(MSG_ERROR, "E2f() received e = %g. The value of e should be 0 <= e < 1.", e); + } + + return f; +} + +/*! + * Purpose: Maps the eccentric anomaly angle into the corresponding + * mean elliptic anomaly angle. Both 2D and 1D elliptic + * orbit are allowed. + * Inputs: + * Ecc = eccentric anomaly (rad) + * e = eccentricity (0 <= e < 1) + * Outputs: + * M = mean elliptic anomaly (rad) + */ +double E2M(double Ecc, double e) +{ + double M; + + if((e >= 0) && (e < 1)) { + M = Ecc - e * sin(Ecc); + } else { + M = NAN; + BSK_PRINT(MSG_ERROR, "E2M() received e = %g. The value of e should be 0 <= e < 1.", e); + } + + return M; +} + +/*! + * Purpose: Maps true anomaly angles into eccentric anomaly angles. + * This function requires the orbit to be either circular or + * non-rectilinar elliptic orbit. + * Inputs: + * f = true anomaly angle (rad) + * e = eccentricity (0 <= e < 1) + * Outputs: + * Ecc = eccentric anomaly (rad) + */ +double f2E(double f, double e) +{ + double Ecc; + + if((e >= 0) && (e < 1)) { + Ecc = 2 * atan2(sqrt(1 - e) * sin(f / 2), sqrt(1 + e) * cos(f / 2)); + } else { + Ecc = NAN; + BSK_PRINT(MSG_ERROR, "f2E() received e = %g. The value of e should be 0 <= e < 1.", e); + } + + return Ecc; +} + +/*! + * Purpose: Maps true anomaly angles into hyperbolic anomaly angles. + * This function requires the orbit to be hyperbolic + * Inputs: + * f = true anomaly angle (rad) + * e = eccentricity (e > 1) + * Outputs: + * H = hyperbolic anomaly (rad) + */ +double f2H(double f, double e) +{ + double H; + + if(e > 1) { + H = 2 * atanh(sqrt((e - 1) / (e + 1)) * tan(f / 2)); + } else { + H = NAN; + BSK_PRINT(MSG_ERROR, "f2H() received e = %g. The value of e should be 1 < e.", e); + } + + return H; +} + +/*! + * Purpose: Maps hyperbolic anomaly angles into true anomaly angles. + * This function requires the orbit to be hyperbolic + * Inputs: + * H = hyperbolic anomaly (rad) + * e = eccentricity (e > 1) + * Outputs: + * f = true anomaly angle (rad) + */ +double H2f(double H, double e) +{ + double f; + + if(e > 1) { + f = 2 * atan(sqrt((e + 1) / (e - 1)) * tanh(H / 2)); + } else { + f = NAN; + BSK_PRINT(MSG_ERROR, "H2f() received e = %g. The value of e should be 1 < e.", e); + } + + return f; +} + +/*! + * Purpose: Maps the hyperbolic anomaly angle H into the corresponding + * mean hyperbolic anomaly angle N. + * Inputs: + * H = hyperbolic anomaly (rad) + * e = eccentricity (e > 1) + * Outputs: + * N = mean hyperbolic anomaly (rad) + */ +double H2N(double H, double e) +{ + double N; + + if(e > 1) { + N = e * sinh(H) - H; + } else { + N = NAN; + BSK_PRINT(MSG_ERROR, "H2N() received e = %g. The value of e should be 1 < e.", e); + } + + return N; +} + +/*! + * Purpose: Maps the mean elliptic anomaly angle into the corresponding + * eccentric anomaly angle. Both 2D and 1D elliptic + * orbit are allowed. + * Inputs: + * M = mean elliptic anomaly (rad) + * e = eccentricity (0 <= e < 1) + * Outputs: + * Ecc = eccentric anomaly (rad) + */ +double M2E(double M, double e) +{ + double small = 1e-13; + double dE = 10 * small; + double E1 = M; + int max = 200; + int count = 0; + + if((e >= 0) && (e < 1)) { + while(fabs(dE) > small) { + dE = (E1 - e * sin(E1) - M) / (1 - e * cos(E1)); + E1 -= dE; + if(++count > max) { + BSK_PRINT(MSG_ERROR, "iteration error in M2E(%f,%f)", M, e); + dE = 0.; + } + } + } else { + E1 = NAN; + BSK_PRINT(MSG_ERROR, "M2E() received e = %g. The value of e should be 0 <= e < 1.", e); + } + + return E1; +} + +/*! + * Purpose: Maps the mean hyperbolic anomaly angle N into the corresponding + * hyperbolic anomaly angle H. + * Inputs: + * N = mean hyperbolic anomaly (rad) + * e = eccentricity (e > 1) + * Outputs: + * H = hyperbolic anomaly (rad) + */ +double N2H(double N, double e) +{ + double small = 1e-13; + double dH = 10 * small; + double H1 = N; + int max = 200; + int count = 0; + if(fabs(H1) > 7.0) + { + H1 = N/fabs(N)*7.0; + } + + if(e > 1) { + while(fabs(dH) > small) { + dH = (e * sinh(H1) - H1 - N) / (e * cosh(H1) - 1); + H1 -= dH; + if(++count > max) { + BSK_PRINT(MSG_ERROR, "iteration error in N2H(%f,%f)", N, e); + dH = 0.; + } + } + } else { + H1 = NAN; + BSK_PRINT(MSG_ERROR, "N2H() received e = %g. The value of e should be e > 1.", e); + } + + return H1; +} + +/*! + * Purpose: Translates the orbit elements + * a - semi-major axis (km) + * e - eccentricity + * i - inclination (rad) + * AN - ascending node (rad) + * AP - argument of periapses (rad) + * f - true anomaly angle (rad) + * to the inertial Cartesian position and velocity vectors. + * The attracting body is specified through the supplied + * gravitational constant mu (units of km^3/s^2). + * + * The code can handle the following cases: + * circular: e = 0 a > 0 + * elliptical-2D: 0 < e < 1 a > 0 + * elliptical-1D: e = 1 a > 0 f = Ecc. Anom. here + * parabolic: e = 1 rp = -a + * hyperbolic: e > 1 a < 0 + * + * Note: to handle the parabolic case and distinguish it form the + * rectilinear elliptical case, instead of passing along the + * semi-major axis a in the "a" input slot, the negative radius + * at periapses is supplied. Having "a" be negative and e = 1 + * is a then a unique identified for the code for the parabolic + * case. + * Inputs: + * mu = gravitational parameter + * elements = orbital elements + * Outputs: + * rVec = position vector + * vVec = velocity vector + */ +void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec) +{ + double e; /* eccentricty */ + double a; /* semi-major axis */ + double f; /* true anomaly */ + double r; /* orbit radius */ + double v; /* orbit velocity magnitude */ + double i; /* orbit inclination angle */ + double p; /* the parameter or the semi-latus rectum */ + double AP; /* argument of perigee */ + double AN; /* argument of the ascending node */ + double theta; /* true latitude theta = omega + f */ + double h; /* orbit angular momentum magnitude */ + double ir[3]; /* orbit radius unit vector */ + double eps; /* small numerical value parameter */ + + /* define what is a small numerical value */ + eps = 1e-12; + + /* map classical elements structure into local variables */ + a = elements->a; + e = elements->e; + i = elements->i; + AN = elements->Omega; + AP = elements->omega; + f = elements->f; + + if((fabs(e-1.0) < eps) && (fabs(a) > eps)) { /* 1D rectilinear elliptic/hyperbolic orbit case */ + + double angle; /* eccentric/hyperbolic anomaly */ + angle = f; /* f is treated as ecc/hyp anomaly */ + /* orbit radius */ + if (elements->a > 0.0) { + r = a * (1 - e * cos(angle)); + } else { + r = a * (1 - e * cosh(angle)); + } + v = sqrt(2 * mu / r - mu / a); + ir[0] = cos(AN) * cos(AP) - sin(AN) * sin(AP) * cos(i); + ir[1] = sin(AN) * cos(AP) + cos(AN) * sin(AP) * cos(i); + ir[2] = sin(AP) * sin(i); + v3Scale(r, ir, rVec); + if(sin(angle) > 0) { + v3Scale(v, ir, vVec); + } else { + v3Scale(-v, ir, vVec); + } + } else { /* general 2D orbit case */ + /* evaluate semi-latus rectum */ + if(fabs(a) > eps) { + p = a * (1 - e * e); /* elliptic or hyperbolic */ + } else { + p = 2 * elements->rPeriap; /* parabolic */ + } + + r = p / (1 + e * cos(f)); /* orbit radius */ + theta = AP + f; /* true latitude angle */ + h = sqrt(mu * p); /* orbit ang. momentum mag. */ + + rVec[0] = r * (cos(AN) * cos(theta) - sin(AN) * sin(theta) * cos(i)); + rVec[1] = r * (sin(AN) * cos(theta) + cos(AN) * sin(theta) * cos(i)); + rVec[2] = r * (sin(theta) * sin(i)); + + vVec[0] = -mu / h * (cos(AN) * (sin(theta) + e * sin(AP)) + sin(AN) * (cos( + theta) + e * cos(AP)) * cos(i)); + vVec[1] = -mu / h * (sin(AN) * (sin(theta) + e * sin(AP)) - cos(AN) * (cos( + theta) + e * cos(AP)) * cos(i)); + vVec[2] = -mu / h * (-(cos(theta) + e * cos(AP)) * sin(i)); + } +} + + +/*! + * Purpose: Translates the orbit elements inertial Cartesian position + * vector rVec and velocity vector vVec into the corresponding + * classical orbit elements where + * a - semi-major axis (zero if parabolic) + * e - eccentricity + * i - inclination (rad) + * AN - ascending node (rad) + * AP - argument of periapses (rad) + * f - true anomaly angle (rad) + * if the orbit is rectilinear, then this will be the + * eccentric or hyperbolic anomaly + * rp - radius at periapses + * ra - radius at apoapses (zero if parabolic) + * The attracting body is specified through the supplied + * gravitational constant mu (units of km^3/s^2). + * Inputs: + * mu = gravitational parameter + * rVec = position vector + * vVec = velocity vector + * Outputs: + * elements = orbital elements + */ +void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements) +{ + double hVec[3]; /* orbit angular momentum vector */ + double ihHat[3]; /* normalized orbit angular momentum vector */ + double h; /* orbit angular momentum magnitude */ + double v3[3]; /* temp vector */ + double n1Hat[3]; /* 1st inertial frame base vector */ + double n3Hat[3]; /* 3rd inertial frame base vector */ + double nVec[3]; /* line of nodes vector */ + double inHat[3]; /* normalized line of nodes vector */ + double irHat[3]; /* normalized position vector */ + double r; /* current orbit radius */ + double v; /* orbit velocity magnitude */ + double eVec[3]; /* eccentricity vector */ + double ieHat[3]; /* normalized eccentricity vector */ + double p; /* the parameter, also called semi-latus rectum */ + double eps; /* small numerical value parameter */ + + /* define what is a small numerical value */ + eps = 1e-12; + + /* define inertial frame axes */ + v3Set(1.0, 0.0, 0.0, n1Hat); + v3Set(0.0, 0.0, 1.0, n3Hat); + + /* norms of position and velocity vectors */ + r = v3Norm(rVec); + elements->rmag = r; + v = v3Norm(vVec); + v3Normalize(rVec, irHat); + + /* Calculate the specific angular momentum and its magnitude */ + v3Cross(rVec, vVec, hVec); + h = v3Norm(hVec); + v3Normalize(hVec, ihHat); + p = h*h / mu; + + /* Calculate the line of nodes */ + v3Cross(n3Hat, hVec, nVec); + if (v3Norm(nVec) < eps) { + /* near equatorial orbits */ + v3Copy(n1Hat, inHat); + } else { + v3Normalize(nVec, inHat); + } + + /* Orbit eccentricity vector */ + v3Scale(v * v / mu - 1.0 / r, rVec, eVec); + v3Scale(v3Dot(rVec, vVec) / mu, vVec, v3); + v3Subtract(eVec, v3, eVec); + elements->e = v3Norm(eVec); + elements->rPeriap = p / (1.0 + elements->e); + + /* Orbit eccentricity unit vector */ + if (elements->e > eps) { + v3Normalize(eVec, ieHat); + } else { + /* near circular orbits, make i_e_hat equal to line of nodes */ + v3Copy(inHat, ieHat); + } + + /* compute semi-major axis */ + elements->alpha = 2.0 / r - v*v / mu; + if(fabs(elements->alpha) > eps) { + /* elliptic or hyperbolic case */ + elements->a = 1.0 / elements->alpha; + elements->rApoap = p / (1.0 - elements->e); + } else { + /* parabolic case */ + elements->a = 0.0; + elements->rApoap = 0.0; + } + + /* Calculate the inclination */ + elements->i = safeAcos(hVec[2] / h); + + /* Calculate Ascending Node Omega */ + v3Cross(n1Hat, inHat, v3); + elements->Omega = atan2(v3[2], inHat[0]); + if (elements->Omega < 0.0) { + elements->Omega += 2*M_PI; + } + + /* Calculate Argument of Periapses omega */ + v3Cross(inHat, ieHat, v3); + elements->omega = atan2(v3Dot(ihHat,v3), v3Dot(inHat, ieHat)); + if (elements->omega < 0.0) { + elements->omega += 2*M_PI; + } + + /* Calculate true anomaly angle f */ + v3Cross(ieHat, irHat, v3); + elements->f = atan2(v3Dot(ihHat,v3), v3Dot(ieHat, irHat)); + if (elements->f < 0.0) { + elements->f += 2*M_PI; + } +} + +/*! + * Purpose: This program computes the atmospheric density based on altitude + * supplied by user. This function uses a curve fit based on + * atmospheric data from the Standard Atmosphere 1976 Data. This + * function is valid for altitudes ranging from 100km to 1000km. + * + * Note: This code can only be applied to spacecraft orbiting the Earth + * Inputs: + * alt = altitude in km + * Outputs: + * density = density at the given altitude in kg/m^3 + */ +double atmosphericDensity(double alt) +{ + double logdensity; + double density; + double val; + + /* Smooth exponential drop-off after 1000 km */ + if(alt > 1000.) { + logdensity = (-7e-05) * alt - 14.464; + density = pow(10., logdensity); + return density; + } + + /* Calculating the density based on a scaled 6th order polynomial fit to the log of density */ + val = (alt - 526.8000) / 292.8563; + logdensity = 0.34047 * pow(val, 6) - 0.5889 * pow(val, 5) - 0.5269 * pow(val, 4) + + 1.0036 * pow(val, 3) + 0.60713 * pow(val, 2) - 2.3024 * val - 12.575; + + /* Calculating density by raising 10 to the log of density */ + density = pow(10., logdensity); + + return density; +} + +/*! + * Purpose: This program computes the Debye Length length for a given + * altitude and is valid for altitudes ranging + * from 200 km to GEO (35000km). However, all values above + * 1000 km are HIGHLY speculative at this point. + * Inputs: + * alt = altitude in km + * Outputs: + * debye = debye length given in m + */ +double debyeLength(double alt) +{ + double debyedist; + double a; + double X[N_DEBYE_PARAMETERS] = {200, 250, 300, 350, 400, 450, 500, 550, + 600, 650, 700, 750, 800, 850, 900, 950, 1000, 1050, 1100, 1150, + 1200, 1250, 1300, 1350, 1400, 1450, 1500, 1550, 1600, 1650, 1700, + 1750, 1800, 1850, 1900, 1950, 2000 + }; + double Y[N_DEBYE_PARAMETERS] = {5.64E-03, 3.92E-03, 3.24E-03, 3.59E-03, + 4.04E-03, 4.28E-03, 4.54E-03, 5.30E-03, 6.55E-03, 7.30E-03, 8.31E-03, + 8.38E-03, 8.45E-03, 9.84E-03, 1.22E-02, 1.37E-02, 1.59E-02, 1.75E-02, + 1.95E-02, 2.09E-02, 2.25E-02, 2.25E-02, 2.25E-02, 2.47E-02, 2.76E-02, + 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 3.21E-02, + 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02 + }; + int i; + + /* Flat debyeLength length for altitudes above 2000 km */ + if((alt > 2000.0) && (alt <= 30000.0)) { + alt = 2000.0; + } else if((alt > 30000.0) && (alt <= 35000.0)) { + debyedist = 0.1 * alt - 2999.7; + return debyedist; + } else if((alt < 200.0) || (alt > 35000.0)) { + BSK_PRINT(MSG_ERROR, "debyeLength() received alt = %g\nThe value of alt should be in the range of [200 35000]", alt); + debyedist = NAN; + return debyedist; + } + + /* Interpolation of data */ + for(i = 0; i < N_DEBYE_PARAMETERS - 1; i++) { + if(X[i + 1] > alt) { + break; + } + } + a = (alt - X[i]) / (X[i + 1] - X[i]); + debyedist = Y[i] + a * (Y[i + 1] - Y[i]); + + return debyedist; +} + +/*! + * Purpose: This program computes the atmospheric drag acceleration + * vector acting on a spacecraft. + * Note the acceleration vector output is inertial, and is + * only valid for altitudes up to 1000 km. + * Afterwards the drag force is zero. Only valid for Earth. + * Inputs: + * Cd = drag coefficient of the spacecraft + * A = cross-sectional area of the spacecraft in m^2 + * m = mass of the spacecraft in kg + * rvec = Inertial position vector of the spacecraft in km [x;y;z] + * vvec = Inertial velocity vector of the spacecraft in km/s [vx;vy;vz] + * Outputs: + * advec = The inertial acceleration vector due to atmospheric + * drag in km/sec^2 + */ +void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, + double *advec) +{ + double r; + double v; + double alt; + double ad; + double density; + + /* find the altitude and velocity */ + r = v3Norm(rvec); + v = v3Norm(vvec); + alt = r - REQ_EARTH; + + /* Checking if user supplied a orbital position is inside the earth */ + if(alt <= 0.) { + BSK_PRINT(MSG_ERROR, "atmosphericDrag() received rvec = [%g %g %g]The value of rvec should produce a positive altitude for the Earth.", rvec[1], rvec[2], rvec[3]); + v3Set(NAN, NAN, NAN, advec); + return; + } + + /* get the Atmospheric density at the given altitude in kg/m^3 */ + density = atmosphericDensity(alt); + + /* compute the magnitude of the drag acceleration */ + ad = ((-0.5) * density * (Cd * A / m) * (pow(v * 1000., 2))) / 1000.; + + /* computing the vector for drag acceleration */ + v3Scale(ad / v, vvec, advec); +} + +/*! + * Purpose: Computes the J2_EARTH-J6_EARTH zonal graviational perturbation + * accelerations. + * Inputs: + * rvec = Cartesian Position vector in kilometers [x;y;z]. + * num = Corresponds to which J components to use, + * must be an integer between 2 and 6. + * (note: Additive- 2 corresponds to J2_EARTH while 3 will + * correspond to J2_EARTH + J3_EARTH) + * Outputs: + * ajtot = The total acceleration vector due to the J + * perturbations in km/sec^2 [accelx;accely;accelz] + */ +void jPerturb(double *rvec, int num, double *ajtot, ...) +{ + double mu; + double req; + double J2, J3, J4, J5, J6; + double x; + double y; + double z; + double r; + double temp[3]; + double temp2[3]; + + CelestialObject_t planetID; + va_list args; + va_start(args, ajtot); + planetID = (CelestialObject_t) va_arg(args, int); + va_end(args); + + v3SetZero(ajtot); + + switch(planetID) { + case CELESTIAL_MERCURY: + mu = MU_MERCURY; + req = REQ_MERCURY; + J2 = J2_MERCURY; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_VENUS: + mu = MU_VENUS; + req = REQ_VENUS; + J2 = J2_VENUS; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_MOON: + mu = MU_MOON; + req = REQ_MOON; + J2 = J2_MOON; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_MARS: + mu = MU_MARS; + req = REQ_MARS; + J2 = J2_MARS; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_JUPITER: + mu = MU_JUPITER; + req = REQ_JUPITER; + J2 = J2_JUPITER; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_URANUS: + mu = MU_URANUS; + req = REQ_URANUS; + J2 = J2_URANUS; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_NEPTUNE: + mu = MU_NEPTUNE; + req = REQ_NEPTUNE; + J2 = J2_NEPTUNE; + J3 = 0.0; + J4 = 0.0; + J5 = 0.0; + J6 = 0.0; + break; + + case CELESTIAL_PLUTO: + case CELESTIAL_SUN: + return; + + default: + mu = MU_EARTH; + req = REQ_EARTH; + J2 = J2_EARTH; + J3 = J3_EARTH; + J4 = J4_EARTH; + J5 = J5_EARTH; + J6 = J6_EARTH; + break; + } + + + /* Calculate the J perturbations */ + x = rvec[0]; + y = rvec[1]; + z = rvec[2]; + r = v3Norm(rvec); + + /* Error Checking */ + if((num < 2) || (num > 6)) { + BSK_PRINT(MSG_ERROR, "jPerturb() received num = %d. The value of num should be 2 <= num <= 6.", num); + v3Set(NAN, NAN, NAN, ajtot); + return; + } + + /* Calculating the total acceleration based on user input */ + if(num >= 2) { + v3Set((1.0 - 5.0 * pow(z / r, 2.0)) * (x / r), + (1.0 - 5.0 * pow(z / r, 2.0)) * (y / r), + (3.0 - 5.0 * pow(z / r, 2.0)) * (z / r), ajtot); + v3Scale(-3.0 / 2.0 * J2 * (mu / pow(r, 2.0))*pow(req / r, 2.0), ajtot, ajtot); + } + if(num >= 3) { + v3Set(5.0 * (7.0 * pow(z / r, 3.0) - 3.0 * (z / r)) * (x / r), + 5.0 * (7.0 * pow(z / r, 3.0) - 3.0 * (z / r)) * (y / r), + -3.0 * (10.0 * pow(z / r, 2.0) - (35.0 / 3.0)*pow(z / r, 4.0) - 1.0), temp); + v3Scale(1.0 / 2.0 * J3 * (mu / pow(r, 2.0))*pow(req / r, 3.0), temp, temp2); + v3Add(ajtot, temp2, ajtot); + } + if(num >= 4) { + v3Set((3.0 - 42.0 * pow(z / r, 2.0) + 63.0 * pow(z / r, 4.0)) * (x / r), + (3.0 - 42.0 * pow(z / r, 2.0) + 63.0 * pow(z / r, 4.0)) * (y / r), + (15.0 - 70.0 * pow(z / r, 2) + 63.0 * pow(z / r, 4.0)) * (z / r), temp); + v3Scale(5.0 / 8.0 * J4 * (mu / pow(r, 2.0))*pow(req / r, 4.0), temp, temp2); + v3Add(ajtot, temp2, ajtot); + } + if(num >= 5) { + v3Set(3.0 * (35.0 * (z / r) - 210.0 * pow(z / r, 3.0) + 231.0 * pow(z / r, 5.0)) * (x / r), + 3.0 * (35.0 * (z / r) - 210.0 * pow(z / r, 3.0) + 231.0 * pow(z / r, 5.0)) * (y / r), + -(15.0 - 315.0 * pow(z / r, 2.0) + 945.0 * pow(z / r, 4.0) - 693.0 * pow(z / r, 6.0)), temp); + v3Scale(1.0 / 8.0 * J5 * (mu / pow(r, 2.0))*pow(req / r, 5.0), temp, temp2); + v3Add(ajtot, temp2, ajtot); + } + if(num >= 6) { + v3Set((35.0 - 945.0 * pow(z / r, 2) + 3465.0 * pow(z / r, 4.0) - 3003.0 * pow(z / r, 6.0)) * (x / r), + (35.0 - 945.0 * pow(z / r, 2.0) + 3465.0 * pow(z / r, 4.0) - 3003.0 * pow(z / r, 6.0)) * (y / r), + -(3003.0 * pow(z / r, 6.0) - 4851.0 * pow(z / r, 4.0) + 2205.0 * pow(z / r, 2.0) - 245.0) * (z / r), temp); + v3Scale(-1.0 / 16.0 * J6 * (mu / pow(r, 2.0))*pow(req / r, 6.0), temp, temp2); + v3Add(ajtot, temp2, ajtot); + } +} + +/*! + * Purpose: Computes the inertial solar radiation force vectors + * based on cross-sectional Area and mass of the spacecraft + * and the position vector of the planet to the sun. + * Note: It is assumed that the solar radiation pressure decreases + * quadratically with distance from sun (in AU) + * + * Solar Radiation Equations obtained from + * Earth Space and Planets Journal Vol. 51, 1999 pp. 979-986 + * Inputs: + * A = Cross-sectional area of the spacecraft that is facing + * the sun in m^2. + * m = The mass of the spacecraft in kg. + * sunvec = Position vector to the Sun in units of AU. + * Earth has a distance of 1 AU. + * Outputs: + * arvec = The inertial acceleration vector due to the effects + * of Solar Radiation pressure in km/sec^2. The vector + * components of the output are the same as the vector + * components of the sunvec input vector. + */ +void solarRad(double A, double m, double *sunvec, double *arvec) +{ + double flux; + double c; + double Cr; + double sundist; + + /* Solar Radiation Flux */ + flux = 1372.5398; /* Watts/m^2 */ + + /* Speed of light */ + c = 299792458.; /* m/s */ + + /* Radiation pressure coefficient */ + Cr = 1.3; + + /* Magnitude of position vector */ + sundist = v3Norm(sunvec); /* AU */ + + /* Computing the acceleration vector */ + v3Scale((-Cr * A * flux) / (m * c * pow(sundist, 3)) / 1000., sunvec, arvec); +} + +/*! maps classical mean orbit elements to Osculating elements */ +void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicElements *elements_p, double sgn) { + // Classical orbital elements = (a,e,i,Omega,omega,f) + // First-order J2 Mapping Between Mean and Osculating Orbital Elements + // sgn=1:mean2osc, sgn=-1:osc2mean + // Analytical Mechanics of Space Systems + // Hanspeter Schaub, John L. Junkins, 4th edition. + // [m] or [km] should be the same both for req and elements->a + + double a = elements->a; + double e = elements->e; + double i = elements->i; + double Omega = elements->Omega; + double omega = elements->omega; + double f = elements->f; + double E = f2E(f, e); + double M = E2M(E, e); + + double gamma2 = sgn * J2 / 2.0 * pow((req / a), 2.0); // (F.1),(F.2) + double eta = sqrt(1.0 - pow(e, 2.0)); + double gamma2p = gamma2 / pow(eta, 4.0); // (F.3) + double a_r = (1.0 + e * cos(f)) / pow(eta, 2.0); // (F.6) + + double ap = a + a * gamma2 * ((3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r, 3.0) - 1.0 / pow(eta, 3.0)) + + 3.0 * (1.0 - pow(cos(i), 2.0)) * pow(a_r, 3.0) * cos(2.0 * omega + 2.0 * f)); // (F.7) + + double de1 = gamma2p / 8.0 * e * pow(eta, 2.0) * (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) + / (1.0 - 5.0 * pow(cos(i), 2.0))) * cos(2.0 * omega); // (F.8) + + double de = de1 + pow(eta, 2.0) / 2.0 * (gamma2 * ((3.0 * pow(cos(i), 2.0) - 1.0) / pow(eta, 6.0) * (e * eta + e / (1.0 + eta) + 3.0 * cos(f) + + 3.0 * e * pow(cos(f), 2.0) + pow(e, 2.0) * pow(cos(f), 3.0)) + 3.0 * (1.0 - pow(cos(i), 2.0)) / pow(eta, 6.0) * (e + 3.0 * cos(f) + + 3 * e * pow(cos(f), 2) + pow(e, 2) * pow(cos(f), 3)) * cos(2 * omega + 2 * f)) + - gamma2p * (1.0 - pow(cos(i), 2.0)) * (3.0 * cos(2.0 * omega + f) + cos(2.0 * omega + 3.0 * f))); // (F.9) + + double di = -e * de1 / pow(eta, 2.0) / tan(i) + + gamma2p / 2.0 * cos(i) * sqrt(1.0 - pow(cos(i), 2.0)) * (3.0 * cos(2.0 * omega + 2.0 * f) + 3.0 * e * cos(2.0 * omega + f) + + e * cos(2.0 * omega + 3.0 * f)); // (F.10) + + double MpopOp = M + omega + Omega + gamma2p / 8.0 * pow(eta, 3.0) * (1.0 - 11.0 * pow(cos(i), 2.0) - 40.0 * pow(cos(i), 4.0) + / (1.0 - 5.0 * pow(cos(i), 2.0))) * sin(2.0 * omega) - gamma2p / 16.0 * (2.0 + pow(e, 2.0) - 11.0 * (2.0 + 3.0 * pow(e, 2.0)) + * pow(cos(i), 2.0) - 40.0 * (2.0 + 5.0 * pow(e, 2.0)) * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) - 400.0 * pow(e, 2.0) + * pow(cos(i), 6.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) + gamma2p / 4.0 * (-6.0 * (1.0 - 5.0 * pow(cos(i), 2.0)) + * (f - M + e * sin(f)) + (3.0 - 5.0 * pow(cos(i), 2.0)) * (3.0 * sin(2.0 * omega + 2.0 * f) + 3.0 * e * sin(2.0 * omega + f) + e + * sin(2.0 * omega + 3.0 * f))) - gamma2p / 8.0 * pow(e, 2.0) * cos(i) * (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) + + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) - gamma2p / 2.0 * cos(i) + * (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - e * sin(2.0 * omega + 3.0 * f)); // (F.11) + + double edM = gamma2p / 8.0 * e * pow(eta, 3.0) * (1.0 - 11.0 * pow(cos(i), 2.0) + - 40.0 * pow(cos(i), 4.0) / (1.0 - 5.0 * pow(cos(i), 2.0))) * sin(2.0 * omega) + - gamma2p / 4.0 * pow(eta, 3.0) * (2.0 * (3.0 * pow(cos(i), 2.0) - 1.0) * (pow(a_r * eta, 2.0) + a_r + 1.0) * sin(f) + + 3.0 * (1.0 - pow(cos(i), 2.0)) * ((-pow(a_r * eta, 2.0) - a_r + 1.0) * sin(2.0 * omega + f) + (pow(a_r * eta, 2.0) + a_r + + 1.0 / 3.0) * sin(2.0 * omega + 3.0 * f))); // (F.12) + + double dOmega = -gamma2p / 8.0 * pow(e, 2.0) * cos(i) * (11.0 + 80.0 * pow(cos(i), 2.0) / (1.0 - 5.0 * pow(cos(i), 2.0)) + + 200.0 * pow(cos(i), 4.0) / pow(1.0 - 5.0 * pow(cos(i), 2.0), 2.0)) * sin(2.0 * omega) - gamma2p / 2.0 * cos(i) + * (6.0 * (f - M + e * sin(f)) - 3.0 * sin(2.0 * omega + 2.0 * f) - 3.0 * e * sin(2.0 * omega + f) - e * sin(2.0 * omega + 3.0 * f)); // (F.13) + + double d1 = (e + de) * sin(M) + edM * cos(M); // (F.14) + double d2 = (e + de) * cos(M) - edM * sin(M); // (F.15) + + double Mp = atan2(d1, d2); // (F.16) + double ep = sqrt(d1 * d1 + d2 * d2); // (F.17) + + double d3 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * sin(Omega) + sin(i / 2.0) * dOmega * cos(Omega); // (F.18) + double d4 = (sin(i / 2.0) + cos(i / 2.0) * di / 2.0) * cos(Omega) - sin(i / 2.0) * dOmega * sin(Omega); // (F.19) + + double Omegap = atan2(d3, d4); // (F.20) + double ip = 2.0 * safeAsin(sqrt(d3 * d3 + d4 * d4)); // (F.21) + double omegap = MpopOp - Mp - Omegap; // (F.22) + + double Ep = M2E(Mp, ep); + double fp = E2f(Ep, ep); + + elements_p->a = ap; + elements_p->e = ep; + elements_p->i = ip; + elements_p->Omega = Omegap; + elements_p->omega = omegap; + elements_p->f = fp; +} + +/*! maps from classical orbit elements to equinoctial elements */ +void clElem2eqElem(ClassicElements *elements_cl, equinoctialElements *elements_eq) { + // conversion + // from classical orbital elements (a,e,i,Omega,omega,f) + // to equinoctial orbital elements (a,P1,P2,Q1,Q2,l,L) + elements_eq->a = elements_cl->a; + elements_eq->P1 = elements_cl->e * sin(elements_cl->Omega + elements_cl->omega); + elements_eq->P2 = elements_cl->e * cos(elements_cl->Omega + elements_cl->omega); + elements_eq->Q1 = tan(elements_cl->i / 2.0) * sin(elements_cl->Omega); + elements_eq->Q2 = tan(elements_cl->i / 2.0) * cos(elements_cl->Omega); + double E = f2E(elements_cl->f, elements_cl->e); + double M = E2M(E, elements_cl->e); + elements_eq->l = elements_cl->Omega + elements_cl->omega + M; + elements_eq->L = elements_cl->Omega + elements_cl->omega + elements_cl->f; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.h new file mode 100644 index 0000000000..5a7a7a4de6 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/orbitalMotion.h @@ -0,0 +1,109 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _ORBITAL_MOTION_0_H_ +#define _ORBITAL_MOTION_0_H_ + +#define N_DEBYE_PARAMETERS 37 + +/*! @brief Structure used to define classic orbit elements */ +typedef struct { + double a; //!< object semi-major axis + double e; //!< Eccentricity of the orbit + double i; //!< inclination of the orbital plane + double Omega; //!< Right ascension of the ascending node + double omega; //!< Argument of periapsis of the orbit + double f; //!< True anomaly of the orbit + double rmag; //!< Magnitude of the position vector (extra) + double alpha; //!< Inverted semi-major axis (extra) + double rPeriap; //!< Radius of periapsis (extra) + double rApoap; //!< Radius if apoapsis (extra) +} ClassicElements; + +/* Celestial object being orbited */ +typedef enum { + CELESTIAL_MERCURY, + CELESTIAL_VENUS, + CELESTIAL_EARTH, + CELESTIAL_MOON, + CELESTIAL_MARS, + CELESTIAL_PHOBOS, + CELESTIAL_DEIMOS, + CELESTIAL_JUPITER, + CELESTIAL_SATURN, + CELESTIAL_URANUS, + CELESTIAL_NEPTUNE, + CELESTIAL_PLUTO, + CELESTIAL_SUN, + MAX_CELESTIAL +} CelestialObject_t; + +/*! This structure contains the set of Keplerian orbital elements that define the + spacecraft translational state. It is operated on by the orbital element + routines and the OrbElemConvert module. +*/ + +/*! equinoctial elment struct definition */ +typedef struct { + double a; //!< semi-major axis + double P1; //!< e*sin(omega+Omega) + double P2; //!< e*cos(omega+Omega) + double Q1; //!< tan(i/2)*sin(Omega) + double Q2; //!< tan(i/2)*cos(Omega) + double l; //!< Omega+omega+M + double L; //!< Omega+omega+f +} equinoctialElements; + +#ifdef __cplusplus +extern "C" { +#endif + /* + E = eccentric anomaly + f = true anomaly + M = mean anomaly + H = hyperbolic anomaly + N = mean hyperbolic anomaly + */ + double E2f(double E, double e); + double E2M(double E, double e); + double f2E(double f, double e); + double f2H(double f, double e); + double H2f(double H, double e); + double H2N(double H, double e); + double M2E(double M, double e); + double N2H(double N, double e); + void elem2rv(double mu, ClassicElements *elements, double *rVec, double *vVec); + void rv2elem(double mu, double *rVec, double *vVec, ClassicElements *elements); + void clMeanOscMap(double req, double J2, ClassicElements *elements, ClassicElements *elements_p, double sgn); + void clElem2eqElem(ClassicElements *elements_cl, equinoctialElements *elements_eq); + + void hillFrame(double *rc_N, double *vc_N, double HN[3][3]); + void hill2rv(double *rc_N, double *vc_N, double *rho_H, double *rhoPrime_H, double *rd_N, double *vd_N); + void rv2hill(double *rc_N, double *vc_N, double *rd_N, double *vd_N, double *rho_H, double *rhoPrime_H); + + double atmosphericDensity(double alt); + double debyeLength(double alt); + void atmosphericDrag(double Cd, double A, double m, double *rvec, double *vvec, double *advec); + void jPerturb(double *rvec, int num, double *ajtot, ...); + void solarRad(double A, double m, double *sunvec, double *arvec); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.c new file mode 100644 index 0000000000..30678de13a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.c @@ -0,0 +1,4523 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "rigidBodyKinematics.h" + +#include "linearAlgebra.h" +#include "math.h" +#include "architecture/utilities/bsk_Print.h" +#include + +#define nearZero 0.0000000000001 + +/* + * Q = addEP(B1,B2) provides the Euler parameter vector + * which corresponds to performing to successive + * rotations B1 and B2. + */ +void addEP(double *b1, double *b2, double *result) +{ + result[0] = b2[0] * b1[0] - b2[1] * b1[1] - b2[2] * b1[2] - b2[3] * b1[3]; + result[1] = b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3]; + result[2] = b2[2] * b1[0] - b2[3] * b1[1] + b2[0] * b1[2] + b2[1] * b1[3]; + result[3] = b2[3] * b1[0] + b2[2] * b1[1] - b2[1] * b1[2] + b2[0] * b1[3]; +} + +/* + * addEuler121(E1,E2,Q) computes the overall (1-2-1) Euler + * angle vector corresponding to two successive + * (1-2-1) rotations E1 and E2. + */ +void addEuler121(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * addEuler131(E1,E2,Q) computes the overall (1-3-1) Euler + * angle vector corresponding to two successive + * (1-3-1) rotations E1 and E2. + */ +void addEuler131(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * addEuler123(E1,E2,Q) computes the overall (1-2-3) Euler + * angle vector corresponding to two successive + * (1-2-3) rotations E1 and E2. + */ +void addEuler123(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler1232C(e1, C1); + Euler1232C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler123(C, result); +} + +/* + * addEuler132(E1,E2,Q) computes the overall (1-3-2) Euler + * angle vector corresponding to two successive + * (1-3-2) rotations E1 and E2. + */ +void addEuler132(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler1322C(e1, C1); + Euler1322C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler132(C, result); +} + +/* + * addEuler212(E1,E2,Q) computes the overall (2-1-2) Euler + * angle vector corresponding to two successive + * (2-1-2) rotations E1 and E2. + */ +void addEuler212(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * addEuler213(E1,E2,Q) computes the overall (2-1-3) Euler + * angle vector corresponding to two successive + * (2-1-3) rotations E1 and E2. + */ +void addEuler213(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler2132C(e1, C1); + Euler2132C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler213(C, result); +} + +/* + * addEuler231(E1,E2,Q) computes the overall (2-3-1) Euler + * angle vector corresponding to two successive + * (2-3-1) rotations E1 and E2. + */ +void addEuler231(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler2312C(e1, C1); + Euler2312C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler231(C, result); +} + +/* + * addEuler232(E1,E2,Q) computes the overall (2-3-2) Euler + * angle vector corresponding to two successive + * (2-3-2) rotations E1 and E2. + */ +void addEuler232(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * addEuler312(E1,E2,Q) computes the overall (3-1-2) Euler + * angle vector corresponding to two successive + * (3-1-2) rotations E1 and E2. + */ +void addEuler312(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler3122C(e1, C1); + Euler3122C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler312(C, result); +} + +/* + * addEuler313(E1,E2,Q) computes the overall (3-1-3) Euler + * angle vector corresponding to two successive + * (3-1-3) rotations E1 and E2. + */ +void addEuler313(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * addEuler321(E1,E2,Q) computes the overall (3-2-1) Euler + * angle vector corresponding to two successive + * (3-2-1) rotations E1 and E2. + */ +void addEuler321(double *e1, double *e2, double *result) +{ + double C1[3][3]; + double C2[3][3]; + double C[3][3]; + + Euler3212C(e1, C1); + Euler3212C(e2, C2); + m33MultM33(C2, C1, C); + C2Euler321(C, result); +} + +/* + * addEuler323(E1,E2,Q) computes the overall (3-2-3) Euler + * angle vector corresponding to two successive + * (3-2-3) rotations E1 and E2. + */ +void addEuler323(double *e1, double *e2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double dum; + double cp3; + + cp1 = cos(e1[1]); + cp2 = cos(e2[1]); + sp1 = sin(e1[1]); + sp2 = sin(e2[1]); + dum = e1[2] + e2[0]; + + result[1] = safeAcos(cp1 * cp2 - sp1 * sp2 * cos(dum)); + cp3 = cos(result[1]); + result[0] = wrapToPi(e1[0] + atan2(sp1 * sp2 * sin(dum), cp2 - cp3 * cp1)); + result[2] = wrapToPi(e2[2] + atan2(sp1 * sp2 * sin(dum), cp1 - cp3 * cp2)); +} + +/* + * Q = addGibbs(Q1,Q2) provides the Gibbs vector + * which corresponds to performing to successive + * rotations Q1 and Q2. + */ +void addGibbs(double *q1, double *q2, double *result) +{ + double v1[3]; + double v2[3]; + + v3Cross(q1, q2, v1); + v3Add(q2, v1, v2); + v3Add(q1, v2, v1); + v3Scale(1. / (1. - v3Dot(q1, q2)), v1, result); +} + +/* + * addMRP(Q1,Q2,Q) provides the MRP vector + * which corresponds to performing to successive + * rotations Q1 and Q2. + */ +void addMRP(double *q1, double *q2, double *result) +{ + double v1[3]; + double v2[3]; + double s1[3]; + double det; + double mag; + + v3Copy(q1, s1); + det = (1 + v3Dot(s1, s1)*v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); + + if (fabs(det) < 0.1) { + mag = v3Dot(s1, s1); + v3Scale(-1./mag, s1, s1); + det = (1 + v3Dot(s1, s1)*v3Dot(q2, q2) - 2 * v3Dot(s1, q2)); + } + + v3Cross(s1, q2, v1); + v3Scale(2., v1, v2); + v3Scale(1 - v3Dot(q2, q2), s1, result); + v3Add(result, v2, result); + v3Scale(1 - v3Dot(s1, s1), q2, v1); + v3Add(result, v1, result); + v3Scale(1 / det, result, result); + + /* map MRP to inner set */ + mag = v3Dot(result, result); + if (mag > 1.0){ + v3Scale(-1./mag, result, result); + } +} + +/* + * addPRV(Q1,Q2,Q) provides the principal rotation vector + * which corresponds to performing to successive + * prinicipal rotations Q1 and Q2. + */ +void addPRV(double *qq1, double *qq2, double *result) +{ + double cp1; + double cp2; + double sp1; + double sp2; + double p; + double sp; + double e1[3]; + double e2[3]; + double compSum[3]; + double q1[4]; + double q2[4]; + + v3Add(qq1, qq2, compSum); + + if((v3Norm(qq1) < 1.0E-7 || v3Norm(qq2) < 1.0E-7)) + { + v3Add(qq1, qq2, result); + return; + } + + PRV2elem(qq1, q1); + PRV2elem(qq2, q2); + cp1 = cos(q1[0] / 2.); + cp2 = cos(q2[0] / 2.); + sp1 = sin(q1[0] / 2.); + sp2 = sin(q2[0] / 2.); + v3Set(q1[1], q1[2], q1[3], e1); + v3Set(q2[1], q2[2], q2[3], e2); + + p = 2 * safeAcos(cp1 * cp2 - sp1 * sp2 * v3Dot(e1, e2)); + if(fabs(p) < 1.0E-13) + { + v3SetZero(result); + return; + } + sp = sin(p / 2.); + v3Scale(cp1 * sp2, e2, q1); + v3Scale(cp2 * sp1, e1, q2); + v3Add(q1, q2, result); + v3Cross(e1, e2, q1); + v3Scale(sp1 * sp2, q1, q2); + v3Add(result, q2, result); + v3Scale(p / sp, result, result); +} + +/* + * BinvEP(Q,B) returns the 3x4 matrix which relates + * the derivative of Euler parameter vector Q to the + * body angular velocity vector w. + * w = 2 [B(Q)]^(-1) dQ/dt + */ +void BinvEP(double *q, double B[3][4]) +{ + B[0][0] = -q[1]; + B[0][1] = q[0]; + B[0][2] = q[3]; + B[0][3] = -q[2]; + B[1][0] = -q[2]; + B[1][1] = -q[3]; + B[1][2] = q[0]; + B[1][3] = q[1]; + B[2][0] = -q[3]; + B[2][1] = q[2]; + B[2][2] = -q[1]; + B[2][3] = q[0]; +} + +/* + * BinvEuler121(Q,B) returns the 3x3 matrix which relates + * the derivative of the (1-2-1) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler121(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c2; + B[0][1] = 0; + B[0][2] = 1; + B[1][0] = s2 * s3; + B[1][1] = c3; + B[1][2] = 0; + B[2][0] = s2 * c3; + B[2][1] = -s3; + B[2][2] = 0; +} + +/* + * BinvEuler123(Q,B) returns the 3x3 matrix which relates + * the derivative of the (1-2-3) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler123(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c2 * c3; + B[0][1] = s3; + B[0][2] = 0; + B[1][0] = -c2 * s3; + B[1][1] = c3; + B[1][2] = 0; + B[2][0] = s2; + B[2][1] = 0; + B[2][2] = 1; +} + +/* + * BinvEuler131(Q,B) returns the 3x3 matrix which relates + * the derivative of the (1-3-1) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler131(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c2; + B[0][1] = 0; + B[0][2] = 1; + B[1][0] = -s2 * c3; + B[1][1] = s3; + B[1][2] = 0; + B[2][0] = s2 * s3; + B[2][1] = c3; + B[2][2] = 0; +} + +/* + * BinvEuler132(Q,B) returns the 3x3 matrix which relates + * the derivative of the (1-3-2) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler132(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c2 * c3; + B[0][1] = -s3; + B[0][2] = 0; + B[1][0] = -s2; + B[1][1] = 0; + B[1][2] = 1; + B[2][0] = c2 * s3; + B[2][1] = c3; + B[2][2] = 0; +} + +/* + * BinvEuler212(Q,B) returns the 3x3 matrix which relates + * the derivative of the (2-1-2) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler212(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s2 * s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = c2; + B[1][1] = 0; + B[1][2] = 1; + B[2][0] = -s2 * c3; + B[2][1] = s3; + B[2][2] = 0; +} + +/* + * BinvEuler213(Q,B) returns the 3x3 matrix which relates + * the derivative of the (2-1-3) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler213(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c2 * s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = c2 * c3; + B[1][1] = -s3; + B[1][2] = 0; + B[2][0] = -s2; + B[2][1] = 0; + B[2][2] = 1; +} + +/* + * BinvEuler231(Q,B) returns the 3x3 matrix which relates + * the derivative of the (2-3-1) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler231(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s2; + B[0][1] = 0; + B[0][2] = 1; + B[1][0] = c2 * c3; + B[1][1] = s3; + B[1][2] = 0; + B[2][0] = -c2 * s3; + B[2][1] = c3; + B[2][2] = 0; +} + +/* + * BinvEuler232(Q,B) returns the 3x3 matrix which relates + * the derivative of the (2-3-2) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler232(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s2 * c3; + B[0][1] = -s3; + B[0][2] = 0; + B[1][0] = c2; + B[1][1] = 0; + B[1][2] = 1; + B[2][0] = s2 * s3; + B[2][1] = c3; + B[2][2] = 0; +} + +/* + * BinvEuler323(Q,B) returns the 3x3 matrix which relates + * the derivative of the (3-2-3) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler323(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = -s2 * c3; + B[0][1] = s3; + B[0][2] = 0; + B[1][0] = s2 * s3; + B[1][1] = c3; + B[1][2] = 0; + B[2][0] = c2; + B[2][1] = 0; + B[2][2] = 1; +} + +/* + * BinvEuler313(Q,B) returns the 3x3 matrix which relates + * the derivative of the (3-1-3) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler313(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s2 * s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = s2 * c3; + B[1][1] = -s3; + B[1][2] = 0; + B[2][0] = c2; + B[2][1] = 0; + B[2][2] = 1; +} + +/* + * BinvEuler321(Q,B) returns the 3x3 matrix which relates + * the derivative of the (3-2-1) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler321(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = -s2; + B[0][1] = 0; + B[0][2] = 1; + B[1][0] = c2 * s3; + B[1][1] = c3; + B[1][2] = 0; + B[2][0] = c2 * c3; + B[2][1] = -s3; + B[2][2] = 0; +} + +/* + * BinvEuler312(Q) returns the 3x3 matrix which relates + * the derivative of the (3-2-3) Euler angle vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvEuler312(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = -c2 * s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = s2; + B[1][1] = 0; + B[1][2] = 1; + B[2][0] = c2 * c3; + B[2][1] = s3; + B[2][2] = 0; +} + +/* + * BinvGibbs(Q,B) returns the 3x3 matrix which relates + * the derivative of Gibbs vector Q to the + * body angular velocity vector w. + * + * w = 2 [B(Q)]^(-1) dQ/dt + */ +void BinvGibbs(double *q, double B[3][3]) +{ + B[0][0] = 1; + B[0][1] = q[2]; + B[0][2] = -q[1]; + B[1][0] = -q[2]; + B[1][1] = 1; + B[1][2] = q[0]; + B[2][0] = q[1]; + B[2][1] = -q[0]; + B[2][2] = 1; + m33Scale(1. / (1 + v3Dot(q, q)), B, B); +} + +/* +* BinvMRP(Q,B) returns the 3x3 matrix which relates +* the derivative of MRP vector Q to the +* body angular velocity vector w. +* +* w = 4 [B(Q)]^(-1) dQ/dt +*/ +void BinvMRP(double *q, double B[3][3]) +{ + double s2; + + s2 = v3Dot(q, q); + B[0][0] = 1 - s2 + 2 * q[0] * q[0]; + B[0][1] = 2 * (q[0] * q[1] + q[2]); + B[0][2] = 2 * (q[0] * q[2] - q[1]); + B[1][0] = 2 * (q[1] * q[0] - q[2]); + B[1][1] = 1 - s2 + 2 * q[1] * q[1]; + B[1][2] = 2 * (q[1] * q[2] + q[0]); + B[2][0] = 2 * (q[2] * q[0] + q[1]); + B[2][1] = 2 * (q[2] * q[1] - q[0]); + B[2][2] = 1 - s2 + 2 * q[2] * q[2]; + m33Scale(1. / (1 + s2) / (1 + s2), B, B); +} + +/* + * BinvPRV(Q,B) returns the 3x3 matrix which relates + * the derivative of principal rotation vector Q to the + * body angular velocity vector w. + * + * w = [B(Q)]^(-1) dQ/dt + */ +void BinvPRV(double *q, double B[3][3]) +{ + double p; + double c1; + double c2; + + p = sqrt(v3Dot(q, q)); + c1 = (1 - cos(p)) / p / p; + c2 = (p - sin(p)) / p / p / p; + + B[0][0] = 1 - c2 * (q[1] * q[1] + q[2] * q[2]); + B[0][1] = c1 * q[2] + c2 * q[0] * q[1]; + B[0][2] = -c1 * q[1] + c2 * q[0] * q[2]; + B[1][0] = -c1 * q[2] + c2 * q[0] * q[1]; + B[1][1] = 1 - c2 * (q[0] * q[0] + q[2] * q[2]); + B[1][2] = c1 * q[0] + c2 * q[1] * q[2]; + B[2][0] = c1 * q[1] + c2 * q[2] * q[0]; + B[2][1] = -c1 * q[0] + c2 * q[2] * q[1]; + B[2][2] = 1 - c2 * (q[0] * q[0] + q[1] * q[1]); +} + +/* + * BmatEP(Q,B) returns the 4x3 matrix which relates the + * body angular velocity vector w to the derivative of + * Euler parameter vector Q. + * + * dQ/dt = 1/2 [B(Q)] w + */ +void BmatEP(double *q, double B[4][3]) +{ + B[0][0] = -q[1]; + B[0][1] = -q[2]; + B[0][2] = -q[3]; + B[1][0] = q[0]; + B[1][1] = -q[3]; + B[1][2] = q[2]; + B[2][0] = q[3]; + B[2][1] = q[0]; + B[2][2] = -q[1]; + B[3][0] = -q[2]; + B[3][1] = q[1]; + B[3][2] = q[0]; +} + +/* + * BmatEuler121(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (1-2-1) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler121(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = 0; + B[0][1] = s3; + B[0][2] = c3; + B[1][0] = 0; + B[1][1] = s2 * c3; + B[1][2] = -s2 * s3; + B[2][0] = s2; + B[2][1] = -c2 * s3; + B[2][2] = -c2 * c3; + m33Scale(1. / s2, B, B); +} + +/* + * BmatEuler131(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (1-3-1) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler131(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = 0; + B[0][1] = -c3; + B[0][2] = s3; + B[1][0] = 0; + B[1][1] = s2 * s3; + B[1][2] = s2 * c3; + B[2][0] = s2; + B[2][1] = c2 * c3; + B[2][2] = -c2 * s3; + m33Scale(1. / s2, B, B); +} + +/* + * BmatEuler123(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (1-2-3) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler123(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c3; + B[0][1] = -s3; + B[0][2] = 0; + B[1][0] = c2 * s3; + B[1][1] = c2 * c3; + B[1][2] = 0; + B[2][0] = -s2 * c3; + B[2][1] = s2 * s3; + B[2][2] = c2; + m33Scale(1. / c2, B, B); +} + +/* + * BmatEuler132(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (1-3-2) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler132(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c3; + B[0][1] = 0; + B[0][2] = s3; + B[1][0] = -c2 * s3; + B[1][1] = 0; + B[1][2] = c2 * c3; + B[2][0] = s2 * c3; + B[2][1] = c2; + B[2][2] = s2 * s3; + m33Scale(1. / c2, B, B); +} + +/* + * BmatEuler212(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (2-1-2) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler212(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s3; + B[0][1] = 0; + B[0][2] = -c3; + B[1][0] = s2 * c3; + B[1][1] = 0; + B[1][2] = s2 * s3; + B[2][0] = -c2 * s3; + B[2][1] = s2; + B[2][2] = c2 * c3; + m33Scale(1. / s2, B, B); +} + +/* + * BmatEuler213(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (2-1-3) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler213(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = c2 * c3; + B[1][1] = -c2 * s3; + B[1][2] = 0; + B[2][0] = s2 * s3; + B[2][1] = s2 * c3; + B[2][2] = c2; + m33Scale(1. / c2, B, B); +} + +/* + * BmatEuler231(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (2-3-1) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler231(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = 0; + B[0][1] = c3; + B[0][2] = -s3; + B[1][0] = 0; + B[1][1] = c2 * s3; + B[1][2] = c2 * c3; + B[2][0] = c2; + B[2][1] = -s2 * c3; + B[2][2] = s2 * s3; + m33Scale(1. / c2, B, B); +} + +/* + * B = BmatEuler232(Q) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (2-3-2) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler232(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = c3; + B[0][1] = 0; + B[0][2] = s3; + B[1][0] = -s2 * s3; + B[1][1] = 0; + B[1][2] = s2 * c3; + B[2][0] = -c2 * c3; + B[2][1] = s2; + B[2][2] = -c2 * s3; + m33Scale(1. / s2, B, B); +} + +/* + * BmatEuler312(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (3-1-2) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler312(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = -s3; + B[0][1] = 0; + B[0][2] = c3; + B[1][0] = c2 * c3; + B[1][1] = 0; + B[1][2] = c2 * s3; + B[2][0] = s2 * s3; + B[2][1] = c2; + B[2][2] = -s2 * c3; + m33Scale(1. / c2, B, B); +} + +/* + * BmatEuler313(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (3-1-3) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler313(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = s3; + B[0][1] = c3; + B[0][2] = 0; + B[1][0] = c3 * s2; + B[1][1] = -s3 * s2; + B[1][2] = 0; + B[2][0] = -s3 * c2; + B[2][1] = -c3 * c2; + B[2][2] = s2; + m33Scale(1. / s2, B, B); +} + +/* + * BmatEuler321(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (3-2-1) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler321(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = 0; + B[0][1] = s3; + B[0][2] = c3; + B[1][0] = 0; + B[1][1] = c2 * c3; + B[1][2] = -c2 * s3; + B[2][0] = c2; + B[2][1] = s2 * s3; + B[2][2] = s2 * c3; + m33Scale(1. / c2, B, B); +} + +/* + * BmatEuler323(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * (3-2-3) Euler angle vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatEuler323(double *q, double B[3][3]) +{ + double s2; + double c2; + double s3; + double c3; + + s2 = sin(q[1]); + c2 = cos(q[1]); + s3 = sin(q[2]); + c3 = cos(q[2]); + + B[0][0] = -c3; + B[0][1] = s3; + B[0][2] = 0; + B[1][0] = s2 * s3; + B[1][1] = s2 * c3; + B[1][2] = 0; + B[2][0] = c2 * c3; + B[2][1] = -c2 * s3; + B[2][2] = s2; + m33Scale(1. / s2, B, B); +} + +/* + * BmatGibbs(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * Gibbs vector Q. + * + * dQ/dt = 1/2 [B(Q)] w + */ +void BmatGibbs(double *q, double B[3][3]) +{ + B[0][0] = 1 + q[0] * q[0]; + B[0][1] = q[0] * q[1] - q[2]; + B[0][2] = q[0] * q[2] + q[1]; + B[1][0] = q[1] * q[0] + q[2]; + B[1][1] = 1 + q[1] * q[1]; + B[1][2] = q[1] * q[2] - q[0]; + B[2][0] = q[2] * q[0] - q[1]; + B[2][1] = q[2] * q[1] + q[0]; + B[2][2] = 1 + q[2] * q[2]; +} + +/* + * BmatMRP(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * MRP vector Q. + * + * dQ/dt = 1/4 [B(Q)] w + */ +void BmatMRP(double *q, double B[3][3]) +{ + double s2; + + s2 = v3Dot(q, q); + B[0][0] = 1 - s2 + 2 * q[0] * q[0]; + B[0][1] = 2 * (q[0] * q[1] - q[2]); + B[0][2] = 2 * (q[0] * q[2] + q[1]); + B[1][0] = 2 * (q[1] * q[0] + q[2]); + B[1][1] = 1 - s2 + 2 * q[1] * q[1]; + B[1][2] = 2 * (q[1] * q[2] - q[0]); + B[2][0] = 2 * (q[2] * q[0] - q[1]); + B[2][1] = 2 * (q[2] * q[1] + q[0]); + B[2][2] = 1 - s2 + 2 * q[2] * q[2]; +} + +/* + * BdotmatMRP(Q,dQ,B) returns the 3x3 matrix derivative of + * the BmatMRP matrix, and it is used to relate the + * body angular acceleration vector dw to the second order + * derivative of the MRP vector Q. + * + * (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) + */ +void BdotmatMRP(double *q, double *dq, double B[3][3]) +{ + double s; + + s = -2 * v3Dot(q, dq); + B[0][0] = s + 4 * ( q[0] * dq[0] ); + B[0][1] = 2 * (-dq[2] + q[0] * dq[1] + dq[0] * q[1] ); + B[0][2] = 2 * ( dq[1] + q[0] * dq[2] + dq[0] * q[2] ); + B[1][0] = 2 * ( dq[2] + q[0] * dq[1] + dq[0] * q[1] ); + B[1][1] = s + 4 * ( q[1] * dq[1] ); + B[1][2] = 2 * (-dq[0] + q[1] * dq[2] + dq[1] * q[2] ); + B[2][0] = 2 * (-dq[1] + q[0] * dq[2] + dq[0] * q[2] ); + B[2][1] = 2 * ( dq[0] + q[1] * dq[2] + dq[1] * q[2] ); + B[2][2] = s + 4 * ( q[2] * dq[2] ); +} + +/* + * BmatPRV(Q,B) returns the 3x3 matrix which relates the + * body angular velocity vector w to the derivative of + * principal rotation vector Q. + * + * dQ/dt = [B(Q)] w + */ +void BmatPRV(double *q, double B[3][3]) +{ + double p; + double c; + p = v3Norm(q); + c = 1. / p / p * (1. - p / 2. / tan(p / 2.)); + B[0][0] = 1 - c * (q[1] * q[1] + q[2] * q[2]); + B[0][1] = -q[2] / 2 + c * (q[0] * q[1]); + B[0][2] = q[1] / 2 + c * (q[0] * q[2]); + B[1][0] = q[2] / 2 + c * (q[0] * q[1]); + B[1][1] = 1 - c * (q[0] * q[0] + q[2] * q[2]); + B[1][2] = -q[0] / 2 + c * (q[1] * q[2]); + B[2][0] = -q[1] / 2 + c * (q[0] * q[2]); + B[2][1] = q[0] / 2 + c * (q[1] * q[2]); + B[2][2] = 1 - c * (q[0] * q[0] + q[1] * q[1]); +} + +/* + * C2EP(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding 4x1 Euler parameter vector Q, + * where the first component of Q is the non-dimensional + * Euler parameter Beta_0 >= 0. Transformation is done + * using the Stanley method. + * + */ +void C2EP(double C[3][3], double b[4]) +{ + double tr; + double b2[4]; + double max; + int i; + int j; + + tr = C[0][0] + C[1][1] + C[2][2]; + b2[0] = (1 + tr) / 4.; + b2[1] = (1 + 2 * C[0][0] - tr) / 4.; + b2[2] = (1 + 2 * C[1][1] - tr) / 4.; + b2[3] = (1 + 2 * C[2][2] - tr) / 4.; + + i = 0; + max = b2[0]; + for(j = 1; j < 4; j++) { + if(b2[j] > max) { + i = j; + max = b2[j]; + } + } + + switch(i) { + case 0: + b[0] = sqrt(b2[0]); + b[1] = (C[1][2] - C[2][1]) / 4 / b[0]; + b[2] = (C[2][0] - C[0][2]) / 4 / b[0]; + b[3] = (C[0][1] - C[1][0]) / 4 / b[0]; + break; + case 1: + b[1] = sqrt(b2[1]); + b[0] = (C[1][2] - C[2][1]) / 4 / b[1]; + if(b[0] < 0) { + b[1] = -b[1]; + b[0] = -b[0]; + } + b[2] = (C[0][1] + C[1][0]) / 4 / b[1]; + b[3] = (C[2][0] + C[0][2]) / 4 / b[1]; + break; + case 2: + b[2] = sqrt(b2[2]); + b[0] = (C[2][0] - C[0][2]) / 4 / b[2]; + if(b[0] < 0) { + b[2] = -b[2]; + b[0] = -b[0]; + } + b[1] = (C[0][1] + C[1][0]) / 4 / b[2]; + b[3] = (C[1][2] + C[2][1]) / 4 / b[2]; + break; + case 3: + b[3] = sqrt(b2[3]); + b[0] = (C[0][1] - C[1][0]) / 4 / b[3]; + if(b[0] < 0) { + b[3] = -b[3]; + b[0] = -b[0]; + } + b[1] = (C[2][0] + C[0][2]) / 4 / b[3]; + b[2] = (C[1][2] + C[2][1]) / 4 / b[3]; + break; + } +} + +/* + * C2Euler121(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (1-2-1) Euler angle set. + */ +void C2Euler121(double C[3][3], double *q) +{ + q[0] = atan2(C[0][1], -C[0][2]); + q[1] = safeAcos(C[0][0]); + q[2] = atan2(C[1][0], C[2][0]); +} + +/* + * C2Euler123(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (1-2-3) Euler angle set. + */ +void C2Euler123(double C[3][3], double *q) +{ + q[0] = atan2(-C[2][1], C[2][2]); + q[1] = safeAsin(C[2][0]); + q[2] = atan2(-C[1][0], C[0][0]); +} + +/* + * C2Euler131(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (1-3-1) Euler angle set. + */ +void C2Euler131(double C[3][3], double *q) +{ + q[0] = atan2(C[0][2], C[0][1]); + q[1] = safeAcos(C[0][0]); + q[2] = atan2(C[2][0], -C[1][0]); +} + +/* + * C2Euler132(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (1-3-2) Euler angle set. + */ +void C2Euler132(double C[3][3], double *q) +{ + q[0] = atan2(C[1][2], C[1][1]); + q[1] = safeAsin(-C[1][0]); + q[2] = atan2(C[2][0], C[0][0]); +} + +/* + * C2Euler212(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (2-1-2) Euler angle set. + */ +void C2Euler212(double C[3][3], double *q) +{ + q[0] = atan2(C[1][0], C[1][2]); + q[1] = safeAcos(C[1][1]); + q[2] = atan2(C[0][1], -C[2][1]); +} + +/* + * C2Euler213(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (2-1-3) Euler angle set. + */ +void C2Euler213(double C[3][3], double *q) +{ + q[0] = atan2(C[2][0], C[2][2]); + q[1] = safeAsin(-C[2][1]); + q[2] = atan2(C[0][1], C[1][1]); +} + +/* + * C2Euler231(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (2-3-1) Euler angle set. + */ +void C2Euler231(double C[3][3], double *q) +{ + q[0] = atan2(-C[0][2], C[0][0]); + q[1] = safeAsin(C[0][1]); + q[2] = atan2(-C[2][1], C[1][1]); +} + +/* + * C2Euler232(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (2-3-2) Euler angle set. + */ +void C2Euler232(double C[3][3], double *q) +{ + q[0] = atan2(C[1][2], -C[1][0]); + q[1] = safeAcos(C[1][1]); + q[2] = atan2(C[2][1], C[0][1]); +} + +/* + * C2Euler312(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (3-1-2) Euler angle set. + */ +void C2Euler312(double C[3][3], double *q) +{ + q[0] = atan2(-C[1][0], C[1][1]); + q[1] = safeAsin(C[1][2]); + q[2] = atan2(-C[0][2], C[2][2]); +} + +/* + * C2Euler313(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (3-1-3) Euler angle set. + */ +void C2Euler313(double C[3][3], double *q) +{ + q[0] = atan2(C[2][0], -C[2][1]); + q[1] = safeAcos(C[2][2]); + q[2] = atan2(C[0][2], C[1][2]); +} + +/* + * C2Euler321(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (3-2-1) Euler angle set. + */ +void C2Euler321(double C[3][3], double *q) +{ + q[0] = atan2(C[0][1], C[0][0]); + q[1] = safeAsin(-C[0][2]); + q[2] = atan2(C[1][2], C[2][2]); +} + +/* + * C2Euler323(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding (3-2-3) Euler angle set. + */ +void C2Euler323(double C[3][3], double *q) +{ + q[0] = atan2(C[2][1], C[2][0]); + q[1] = safeAcos(C[2][2]); + q[2] = atan2(C[1][2], -C[0][2]); +} + +/* + * C2Gibbs(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding 3x1 Gibbs vector Q. + */ +void C2Gibbs(double C[3][3], double *q) +{ + double b[4]; + + C2EP(C, b); + + q[0] = b[1] / b[0]; + q[1] = b[2] / b[0]; + q[2] = b[3] / b[0]; +} + +/* + * C2MRP(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding 3x1 MRP vector Q where the + * MRP vector is chosen such that |Q| <= 1. + */ +void C2MRP(double C[3][3], double *q) +{ + double b[4]; + + v4SetZero(b); + b[0] = 1.0; + C2EP(C, b); + + q[0] = b[1] / (1 + b[0]); + q[1] = b[2] / (1 + b[0]); + q[2] = b[3] / (1 + b[0]); +} + +/* + * C2PRV(C,Q) translates the 3x3 direction cosine matrix + * C into the corresponding 3x1 principal rotation vector Q, + * where the first component of Q is the principal rotation angle + * phi (0<= phi <= Pi) + */ +void C2PRV(double C[3][3], double *q) +{ + double beta[4]; + + C2EP(C,beta); + EP2PRV(beta,q); +} + +/* + * dEP(Q,W,dq) returns the Euler parameter derivative + * for a given Euler parameter vector Q and body + * angular velocity vector w. + * + * dQ/dt = 1/2 [B(Q)] w + */ +void dEP(double *q, double *w, double *dq) +{ + double B[4][3]; + int i; + int j; + + BmatEP(q, B); + m33MultV3(B, w, dq); + for(i = 0; i < 4; i++) { + dq[i] = 0.; + for(j = 0; j < 3; j++) { + dq[i] += B[i][j] * w[j]; + } + } + v3Scale(.5, dq, dq); + dq[3] = 0.5 * dq[3]; +} + +/* + * dEuler121(Q,W,dq) returns the (1-2-1) Euler angle derivative + * vector for a given (1-2-1) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler121(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler121(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler123(Q,W,dq) returns the (1-2-3) Euler angle derivative + * vector for a given (1-2-3) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler123(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler123(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler131(Q,W,dq) returns the (1-3-1) Euler angle derivative + * vector for a given (1-3-1) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler131(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler131(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler132(Q,W,dq) returns the (1-3-2) Euler angle derivative + * vector for a given (1-3-2) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler132(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler132(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler212(Q,W,dq) returns the (2-1-2) Euler angle derivative + * vector for a given (2-1-2) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler212(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler212(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler213(Q,W,dq) returns the (2-1-3) Euler angle derivative + * vector for a given (2-1-3) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler213(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler213(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler231(Q,W,dq) returns the (2-3-1) Euler angle derivative + * vector for a given (2-3-1) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler231(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler231(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler232(Q,W,dq) returns the (2-3-2) Euler angle derivative + * vector for a given (2-3-2) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler232(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler232(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler312(Q,W,dq) returns the (3-1-2) Euler angle derivative + * vector for a given (3-1-2) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler312(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler312(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler313(Q,W,dq) returns the (3-1-3) Euler angle derivative + * vector for a given (3-1-3) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler313(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler313(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler321(Q,W,dq) returns the (3-2-1) Euler angle derivative + * vector for a given (3-2-1) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler321(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler321(q, B); + m33MultV3(B, w, dq); +} + +/* + * dEuler323(Q,W,dq) returns the (3-2-3) Euler angle derivative + * vector for a given (3-2-3) Euler angle vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dEuler323(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatEuler323(q, B); + m33MultV3(B, w, dq); +} + +/* + * dGibbs(Q,W,dq) returns the Gibbs derivative + * for a given Gibbs vector Q and body + * angular velocity vector w. + * + * dQ/dt = 1/2 [B(Q)] w + */ +void dGibbs(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatGibbs(q, B); + m33MultV3(B, w, dq); + v3Scale(0.5, dq, dq); +} + +/* + * dMRP(Q,W,dq) returns the MRP derivative + * for a given MRP vector Q and body + * angular velocity vector w. + * + * dQ/dt = 1/4 [B(Q)] w + */ +void dMRP(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatMRP(q, B); + m33MultV3(B, w, dq); + v3Scale(0.25, dq, dq); +} + +/* + * dMRP2omega(Q,dQ,W) returns the angular rate W + * for a given MRP vector Q and + * MRP derivative dQ. + * + * w = 4 [B(Q)]^(-1) dQ/dt + */ +void dMRP2Omega(double *q, double *dq, double *w) +{ + double B[3][3]; + + BinvMRP(q, B); + m33MultV3(B, dq, w); + v3Scale(4, w, w); +} + +/* + * ddMRP(Q,dQ,W,dW) returns the second order MRP derivative + * for a given MRP vector Q, first MRP derivative dQ, body angular + * velocity vector w and body angular acceleration vector dw. + * + * (d^2Q)/(dt^2) = 1/4 ( [B(Q)] dw + [Bdot(Q,dQ)] w ) + */ +void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq) +{ + double B[3][3], Bdot[3][3]; + double s1[3], s2[3]; + int i; + + BmatMRP(q, B); + BdotmatMRP(q, dq, Bdot); + m33MultV3(B, dw, s1); + m33MultV3(Bdot, w, s2); + for(i = 0; i < 3; i++) { + ddq[i] = 0.25 * ( s1[i] + s2[i] ); + } +} + +/* + * ddMRP2omegaDot(Q,dQ,ddQ) returns the angular rate W + * for a given MRP vector Q and + * MRP derivative dQ. + * + * dW/dt = 4 [B(Q)]^(-1) ( ddQ - [Bdot(Q,dQ)] [B(Q)]^(-1) dQ ) + */ +void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw) +{ + double B[3][3], Bdot[3][3]; + double s1[3], s2[3], s3[3]; + int i; + + BinvMRP(q, B); + BdotmatMRP(q, dq, Bdot); + m33MultV3(B, dq, s1); + m33MultV3(Bdot, s1, s2); + for(i = 0; i < 3; i++) { + s3[i] = ddq[i] - s2[i]; + } + m33MultV3(B, s3, dw); + v3Scale(4, dw, dw); +} + +/* + * dPRV(Q,W,dq) returns the PRV derivative + * for a given PRV vector Q and body + * angular velocity vector w. + * + * dQ/dt = [B(Q)] w + */ +void dPRV(double *q, double *w, double *dq) +{ + double B[3][3]; + + BmatPRV(q, B); + m33MultV3(B, w, dq); +} + +/* + * elem2PRV(R,Q) translates a prinicpal rotation + * element set R into the corresponding principal + * rotation vector Q. + */ +void elem2PRV(double *r, double *q) +{ + q[0] = r[1] * r[0]; + q[1] = r[2] * r[0]; + q[2] = r[3] * r[0]; +} + +/* + * EP2C(Q,C) returns the direction cosine + * matrix in terms of the 4x1 Euler parameter vector + * Q. The first element is the non-dimensional Euler + * parameter, while the remain three elements form + * the Eulerparameter vector. + */ +void EP2C(double *q, double C[3][3]) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + C[0][0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + C[0][1] = 2 * (q1 * q2 + q0 * q3); + C[0][2] = 2 * (q1 * q3 - q0 * q2); + C[1][0] = 2 * (q1 * q2 - q0 * q3); + C[1][1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3; + C[1][2] = 2 * (q2 * q3 + q0 * q1); + C[2][0] = 2 * (q1 * q3 + q0 * q2); + C[2][1] = 2 * (q2 * q3 - q0 * q1); + C[2][2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; +} + +/* + * EP2Euler121(Q,E) translates the Euler parameter + * vector Q into the corresponding (1-2-1) Euler angle + * vector E. + */ +void EP2Euler121(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[3], q[2]); + t2 = atan2(q[1], q[0]); + + e[0] = t1 + t2; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[1] * q[1])); + e[2] = t2 - t1; +} + +/* + * EP2Euler123(Q,E) translates the Euler parameter vector + * Q into the corresponding (1-2-3) Euler angle set. + */ +void EP2Euler123(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(-2 * (q2 * q3 - q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + e[1] = safeAsin(2 * (q1 * q3 + q0 * q2)); + e[2] = atan2(-2 * (q1 * q2 - q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); +} + +/* + * EP2Euler131(Q,E) translates the Euler parameter + * vector Q into the corresponding (1-3-1) Euler angle + * vector E. + */ +void EP2Euler131(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[2], q[3]); + t2 = atan2(q[1], q[0]); + + e[0] = t2 - t1; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[1] * q[1])); + e[2] = t2 + t1; +} + +/* + * EP2Euler132(Q,E) translates the Euler parameter vector + * Q into the corresponding (1-3-2) Euler angle set. + */ +void EP2Euler132(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(2 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); + e[1] = safeAsin(-2 * (q1 * q2 - q0 * q3)); + e[2] = atan2(2 * (q1 * q3 + q0 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); +} + +/* + * EP2Euler212(Q,E) translates the Euler parameter + * vector Q into the corresponding (2-1-2) Euler angle + * vector E. + */ +void EP2Euler212(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[3], q[1]); + t2 = atan2(q[2], q[0]); + + e[0] = t2 - t1; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[2] * q[2])); + e[2] = t2 + t1; +} + +/* + * EP2Euler213(Q,E) translates the Euler parameter vector + * Q into the corresponding (2-1-3) Euler angle set. + */ +void EP2Euler213(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(2 * (q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + e[1] = safeAsin(-2 * (q2 * q3 - q0 * q1)); + e[2] = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); +} + +/* + * EP2Euler231(Q,E) translates the Euler parameter vector + * Q into the corresponding (2-3-1) Euler angle set. + */ +void EP2Euler231(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(-2 * (q1 * q3 - q0 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); + e[1] = safeAsin(2 * (q1 * q2 + q0 * q3)); + e[2] = atan2(-2 * (q2 * q3 - q0 * q1), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); +} + +/* + * EP2Euler232(Q,E) translates the Euler parameter + * vector Q into the corresponding (2-3-2) Euler angle + * vector E. + */ +void EP2Euler232(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[1], q[3]); + t2 = atan2(q[2], q[0]); + + e[0] = t1 + t2; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[2] * q[2])); + e[2] = t2 - t1; +} + +/* + * EP2Euler312(Q,E) translates the Euler parameter vector + * Q into the corresponding (3-1-2) Euler angle set. + */ +void EP2Euler312(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(-2 * (q1 * q2 - q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); + e[1] = safeAsin(2 * (q2 * q3 + q0 * q1)); + e[2] = atan2(-2 * (q1 * q3 - q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); +} + +/* + * EP2Euler313(Q,E) translates the Euler parameter + * vector Q into the corresponding (3-1-3) Euler angle + * vector E. + */ +void EP2Euler313(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[2], q[1]); + t2 = atan2(q[3], q[0]); + + e[0] = t1 + t2; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[3] * q[3])); + e[2] = t2 - t1; +} + +/* + * EP2Euler321(Q,E) translates the Euler parameter vector + * Q into the corresponding (3-2-1) Euler angle set. + */ +void EP2Euler321(double *q, double *e) +{ + double q0; + double q1; + double q2; + double q3; + + q0 = q[0]; + q1 = q[1]; + q2 = q[2]; + q3 = q[3]; + + e[0] = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); + e[1] = safeAsin(-2 * (q1 * q3 - q0 * q2)); + e[2] = atan2(2 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); +} + +/* + * EP2Euler323(Q,E) translates the Euler parameter + * vector Q into the corresponding (3-2-3) Euler angle + * vector E. + */ +void EP2Euler323(double *q, double *e) +{ + double t1; + double t2; + + t1 = atan2(q[1], q[2]); + t2 = atan2(q[3], q[0]); + + e[0] = t2 - t1; + e[1] = 2 * safeAcos(sqrt(q[0] * q[0] + q[3] * q[3])); + e[2] = t2 + t1; +} + +/* + * EP2Gibbs(Q1,Q) translates the Euler parameter vector Q1 + * into the Gibbs vector Q. + */ +void EP2Gibbs(double *q1, double *q) +{ + q[0] = q1[1] / q1[0]; + q[1] = q1[2] / q1[0]; + q[2] = q1[3] / q1[0]; +} + +/* + * EP2MRP(Q1,Q) translates the Euler parameter vector Q1 + * into the MRP vector Q. + */ +void EP2MRP(double *q1, double *q) +{ + if (q1[0] >= 0){ + q[0] = q1[1] / (1 + q1[0]); + q[1] = q1[2] / (1 + q1[0]); + q[2] = q1[3] / (1 + q1[0]); + } else { + q[0] = -q1[1] / (1 - q1[0]); + q[1] = -q1[2] / (1 - q1[0]); + q[2] = -q1[3] / (1 - q1[0]); + } +} + +/* + * EP2PRV(Q1,Q) translates the Euler parameter vector Q1 + * into the principal rotation vector Q. + */ +void EP2PRV(double *q1, double *q) +{ + double p; + double sp; + + p = 2 * safeAcos(q1[0]); + sp = sin(p / 2); + if (fabs(sp) < nearZero) { + q[0] = 0.0; + q[1] = 0.0; + q[2] = 0.0; + return; + } + q[0] = q1[1] / sp * p; + q[1] = q1[2] / sp * p; + q[2] = q1[3] / sp * p; +} + +/* + * Euler1(X,M) Elementary rotation matrix + * Returns the elementary rotation matrix about the + * first body axis. + */ +void Euler1(double x, double m[3][3]) +{ + m33SetIdentity(m); + m[1][1] = cos(x); + m[1][2] = sin(x); + m[2][1] = -m[1][2]; + m[2][2] = m[1][1]; +} + +/* + * Euler2(X,M) Elementary rotation matrix + * Returns the elementary rotation matrix about the + * second body axis. + */ +void Euler2(double x, double m[3][3]) +{ + m33SetIdentity(m); + m[0][0] = cos(x); + m[0][2] = -sin(x); + m[2][0] = -m[0][2]; + m[2][2] = m[0][0]; +} + +/* + * Euler3(X,M) Elementary rotation matrix + * Returns the elementary rotation matrix about the + * third body axis. + */ +void Euler3(double x, double m[3][3]) +{ + m33SetIdentity(m); + m[0][0] = cos(x); + m[0][1] = sin(x); + m[1][0] = -m[0][1]; + m[1][1] = m[0][0]; +} + +/* + * Euler1212C(Q,C) returns the direction cosine + * matrix in terms of the 1-2-1 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler1212C(double *q, double C[3][3]) +{ + double st1; + double ct1; + double st2; + double ct2; + double st3; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct2; + C[0][1] = st1 * st2; + C[0][2] = -ct1 * st2; + C[1][0] = st2 * st3; + C[1][1] = ct1 * ct3 - ct2 * st1 * st3; + C[1][2] = ct3 * st1 + ct1 * ct2 * st3; + C[2][0] = ct3 * st2; + C[2][1] = -ct2 * ct3 * st1 - ct1 * st3; + C[2][2] = ct1 * ct2 * ct3 - st1 * st3; +} + +/* + * Euler1212EP(E,Q) translates the 121 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler1212EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = cos(e2) * sin(e1 + e3); + q[2] = sin(e2) * cos(e1 - e3); + q[3] = sin(e2) * sin(e1 - e3); +} + +/* + * Euler1212Gibbs(E,Q) translates the (1-2-1) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler1212Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler1212EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler1212MRP(E,Q) translates the (1-2-1) Euler + * angle vector E into the MRP vector Q. + */ +void Euler1212MRP(double *e, double *q) +{ + double ep[4]; + + Euler1212EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler1212PRV(E,Q) translates the (1-2-1) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler1212PRV(double *e, double *q) +{ + double ep[4]; + + Euler1212EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler1232C(Q,C) returns the direction cosine + * matrix in terms of the 1-2-3 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler1232C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct2 * ct3; + C[0][1] = ct3 * st1 * st2 + ct1 * st3; + C[0][2] = st1 * st3 - ct1 * ct3 * st2; + C[1][0] = -ct2 * st3; + C[1][1] = ct1 * ct3 - st1 * st2 * st3; + C[1][2] = ct3 * st1 + ct1 * st2 * st3; + C[2][0] = st2; + C[2][1] = -ct2 * st1; + C[2][2] = ct1 * ct2; +} + +/* + * Euler1232EP(E,Q) translates the 123 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler1232EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 - s1 * s2 * s3; + q[1] = s1 * c2 * c3 + c1 * s2 * s3; + q[2] = c1 * s2 * c3 - s1 * c2 * s3; + q[3] = c1 * c2 * s3 + s1 * s2 * c3; +} + +/* + * Euler1232Gibbs(E,Q) translates the (1-2-3) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler1232Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler1232EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler1232MRP(E,Q) translates the (1-2-3) Euler + * angle vector E into the MRP vector Q. + */ +void Euler1232MRP(double *e, double *q) +{ + double ep[4]; + + Euler1232EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler1232PRV(E,Q) translates the (1-2-3) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler1232PRV(double *e, double *q) +{ + double ep[4]; + + Euler1232EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler1312C(Q,C) returns the direction cosine + * matrix in terms of the 1-3-1 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler1312C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct2; + C[0][1] = ct1 * st2; + C[0][2] = st1 * st2; + C[1][0] = -ct3 * st2; + C[1][1] = ct1 * ct2 * ct3 - st1 * st3; + C[1][2] = ct2 * ct3 * st1 + ct1 * st3; + C[2][0] = st2 * st3; + C[2][1] = -ct3 * st1 - ct1 * ct2 * st3; + C[2][2] = ct1 * ct3 - ct2 * st1 * st3; +} + +/* + * Euler1312EP(E,Q) translates the 131 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler1312EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = cos(e2) * sin(e1 + e3); + q[2] = sin(e2) * sin(-e1 + e3); + q[3] = sin(e2) * cos(-e1 + e3); +} + +/* + * Euler1312Gibbs(E,Q) translates the (1-3-1) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler1312Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler1312EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler1312MRP(E,Q) translates the (1-3-1) Euler + * angle vector E into the MRP vector Q. + */ +void Euler1312MRP(double *e, double *q) +{ + double ep[4]; + + Euler1312EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler1312PRV(E,Q) translates the (1-3-1) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler1312PRV(double *e, double *q) +{ + double ep[4]; + + Euler1312EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler1322C(Q,C) returns the direction cosine + * matrix in terms of the 1-3-2 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler1322C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct2 * ct3; + C[0][1] = ct1 * ct3 * st2 + st1 * st3; + C[0][2] = ct3 * st1 * st2 - ct1 * st3; + C[1][0] = -st2; + C[1][1] = ct1 * ct2; + C[1][2] = ct2 * st1; + C[2][0] = ct2 * st3; + C[2][1] = -ct3 * st1 + ct1 * st2 * st3; + C[2][2] = ct1 * ct3 + st1 * st2 * st3; +} + +/* + * Euler1322EP(E,Q) translates the 132 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler1322EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 + s1 * s2 * s3; + q[1] = s1 * c2 * c3 - c1 * s2 * s3; + q[2] = c1 * c2 * s3 - s1 * s2 * c3; + q[3] = c1 * s2 * c3 + s1 * c2 * s3; +} + +/* + * Euler1322Gibbs(E,Q) translates the (1-3-2) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler1322Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler1322EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler1322MRP(E,Q) translates the (1-3-2) Euler + * angle vector E into the MRP vector Q. + */ +void Euler1322MRP(double *e, double *q) +{ + double ep[4]; + + Euler1322EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler1322PRV(E,Q) translates the (1-3-2) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler1322PRV(double *e, double *q) +{ + double ep[4]; + + Euler1322EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler2122C(Q,C) returns the direction cosine + * matrix in terms of the 2-1-2 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler2122C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct3 - ct2 * st1 * st3; + C[0][1] = st2 * st3; + C[0][2] = -ct3 * st1 - ct1 * ct2 * st3; + C[1][0] = st1 * st2; + C[1][1] = ct2; + C[1][2] = ct1 * st2; + C[2][0] = ct2 * ct3 * st1 + ct1 * st3; + C[2][1] = -ct3 * st2; + C[2][2] = ct1 * ct2 * ct3 - st1 * st3; +} + +/* + * Euler2122EP(E,Q) translates the 212 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler2122EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = sin(e2) * cos(-e1 + e3); + q[2] = cos(e2) * sin(e1 + e3); + q[3] = sin(e2) * sin(-e1 + e3); +} + +/* + * Euler2122Gibbs(E,Q) translates the (2-1-2) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler2122Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler2122EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler2122MRP(E,Q) translates the (2-1-2) Euler + * angle vector E into the MRP vector Q. + */ +void Euler2122MRP(double *e, double *q) +{ + double ep[4]; + + Euler2122EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler2122PRV(E,Q) translates the (2-1-2) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler2122PRV(double *e, double *q) +{ + double ep[4]; + + Euler2122EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler2132C(Q,C) returns the direction cosine + * matrix in terms of the 2-1-3 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler2132C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct3 + st1 * st2 * st3; + C[0][1] = ct2 * st3; + C[0][2] = -ct3 * st1 + ct1 * st2 * st3; + C[1][0] = ct3 * st1 * st2 - ct1 * st3; + C[1][1] = ct2 * ct3; + C[1][2] = ct1 * ct3 * st2 + st1 * st3; + C[2][0] = ct2 * st1; + C[2][1] = -st2; + C[2][2] = ct1 * ct2; +} + +/* + * Euler2132EP(E,Q) translates the 213 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler2132EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 + s1 * s2 * s3; + q[1] = c1 * s2 * c3 + s1 * c2 * s3; + q[2] = s1 * c2 * c3 - c1 * s2 * s3; + q[3] = c1 * c2 * s3 - s1 * s2 * c3; +} + +/* + * Euler2132Gibbs(E,Q) translates the (2-1-3) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler2132Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler2132EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler2132MRP(E,Q) translates the (2-1-3) Euler + * angle vector E into the MRP vector Q. + */ +void Euler2132MRP(double *e, double *q) +{ + double ep[4]; + + Euler2132EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler2132PRV(E,Q) translates the (2-1-3) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler2132PRV(double *e, double *q) +{ + double ep[4]; + + Euler2132EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler2312C(Q,C) returns the direction cosine + * matrix in terms of the 2-3-1 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler2312C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct2; + C[0][1] = st2; + C[0][2] = -ct2 * st1; + C[1][0] = -ct1 * ct3 * st2 + st1 * st3; + C[1][1] = ct2 * ct3; + C[1][2] = ct3 * st1 * st2 + ct1 * st3; + C[2][0] = ct3 * st1 + ct1 * st2 * st3; + C[2][1] = -ct2 * st3; + C[2][2] = ct1 * ct3 - st1 * st2 * st3; +} + +/* + * Euler2312EP(E,Q) translates the 231 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler2312EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 - s1 * s2 * s3; + q[1] = c1 * c2 * s3 + s1 * s2 * c3; + q[2] = s1 * c2 * c3 + c1 * s2 * s3; + q[3] = c1 * s2 * c3 - s1 * c2 * s3; +} + +/* + * Euler2312Gibbs(E,Q) translates the (2-3-1) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler2312Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler2312EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler2312MRP(E,Q) translates the (2-3-1) Euler + * angle vector E into the MRP vector Q. + */ +void Euler2312MRP(double *e, double *q) +{ + double ep[4]; + + Euler2312EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler2312PRV(E,Q) translates the (2-3-1) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler2312PRV(double *e, double *q) +{ + double ep[4]; + + Euler2312EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler2322C(Q) returns the direction cosine + * matrix in terms of the 2-3-2 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler2322C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct2 * ct3 - st1 * st3; + C[0][1] = ct3 * st2; + C[0][2] = -ct2 * ct3 * st1 - ct1 * st3; + C[1][0] = -ct1 * st2; + C[1][1] = ct2; + C[1][2] = st1 * st2; + C[2][0] = ct3 * st1 + ct1 * ct2 * st3; + C[2][1] = st2 * st3; + C[2][2] = ct1 * ct3 - ct2 * st1 * st3; +} + +/* +* Euler2322EP(E,Q) translates the 232 Euler angle +* vector E into the Euler parameter vector Q. +*/ +void Euler2322EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = sin(e2) * sin(e1 - e3); + q[2] = cos(e2) * sin(e1 + e3); + q[3] = sin(e2) * cos(e1 - e3); +} + +/* + * Euler2322Gibbs(E) translates the (2-3-2) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler2322Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler2322EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler2322MRP(E,Q) translates the (2-3-2) Euler + * angle vector E into the MRP vector Q. + */ +void Euler2322MRP(double *e, double *q) +{ + double ep[4]; + + Euler2322EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler2322PRV(E,Q) translates the (2-3-2) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler2322PRV(double *e, double *q) +{ + double ep[4]; + + Euler2322EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler3122C(Q,C) returns the direction cosine + * matrix in terms of the 1-2-3 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler3122C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct3 - st1 * st2 * st3; + C[0][1] = ct3 * st1 + ct1 * st2 * st3; + C[0][2] = -ct2 * st3; + C[1][0] = -ct2 * st1; + C[1][1] = ct1 * ct2; + C[1][2] = st2; + C[2][0] = ct3 * st1 * st2 + ct1 * st3; + C[2][1] = st1 * st3 - ct1 * ct3 * st2; + C[2][2] = ct2 * ct3; +} + +/* + * Euler3122EP(E,Q) translates the 312 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler3122EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 - s1 * s2 * s3; + q[1] = c1 * s2 * c3 - s1 * c2 * s3; + q[2] = c1 * c2 * s3 + s1 * s2 * c3; + q[3] = s1 * c2 * c3 + c1 * s2 * s3; +} + +/* + * Euler3122Gibbs(E,Q) translates the (3-1-2) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler3122Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler3122EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler3122MRP(E,Q) translates the (3-1-2) Euler + * angle vector E into the MRP vector Q. + */ +void Euler3122MRP(double *e, double *q) +{ + double ep[4]; + + Euler3122EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler3122PRV(E,Q) translates the (3-1-2) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler3122PRV(double *e, double *q) +{ + double ep[4]; + + Euler3122EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler3132C(Q,C) returns the direction cosine + * matrix in terms of the 3-1-3 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler3132C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct3 * ct1 - st3 * ct2 * st1; + C[0][1] = ct3 * st1 + st3 * ct2 * ct1; + C[0][2] = st3 * st2; + C[1][0] = -st3 * ct1 - ct3 * ct2 * st1; + C[1][1] = -st3 * st1 + ct3 * ct2 * ct1; + C[1][2] = ct3 * st2; + C[2][0] = st2 * st1; + C[2][1] = -st2 * ct1; + C[2][2] = ct2; +} + +/* + * Euler3132EP(E,Q) translates the 313 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler3132EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = sin(e2) * cos(e1 - e3); + q[2] = sin(e2) * sin(e1 - e3); + q[3] = cos(e2) * sin(e1 + e3); +} + +/* + * Euler3132Gibbs(E,Q) translates the (3-1-3) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler3132Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler3132EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler3132MRP(E,Q) translates the (3-1-3) Euler + * angle vector E into the MRP vector Q. + */ +void Euler3132MRP(double *e, double *q) +{ + double ep[4]; + + Euler3132EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler3132PRV(E,Q) translates the (3-1-3) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler3132PRV(double *e, double *q) +{ + double ep[4]; + + Euler3132EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler3212C(Q,C) returns the direction cosine + * matrix in terms of the 3-2-1 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler3212C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct2 * ct1; + C[0][1] = ct2 * st1; + C[0][2] = -st2; + C[1][0] = st3 * st2 * ct1 - ct3 * st1; + C[1][1] = st3 * st2 * st1 + ct3 * ct1; + C[1][2] = st3 * ct2; + C[2][0] = ct3 * st2 * ct1 + st3 * st1; + C[2][1] = ct3 * st2 * st1 - st3 * ct1; + C[2][2] = ct3 * ct2; +} + +/* + * Euler3212EPE,Q) translates the 321 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler3212EP(double *e, double *q) +{ + double c1; + double c2; + double c3; + double s1; + double s2; + double s3; + + c1 = cos(e[0] / 2); + s1 = sin(e[0] / 2); + c2 = cos(e[1] / 2); + s2 = sin(e[1] / 2); + c3 = cos(e[2] / 2); + s3 = sin(e[2] / 2); + + q[0] = c1 * c2 * c3 + s1 * s2 * s3; + q[1] = c1 * c2 * s3 - s1 * s2 * c3; + q[2] = c1 * s2 * c3 + s1 * c2 * s3; + q[3] = s1 * c2 * c3 - c1 * s2 * s3; +} + +/* + * Euler3212Gibbs(E,Q) translates the (3-2-1) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler3212Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler3212EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler3212MRP(E,Q) translates the (3-2-1) Euler + * angle vector E into the MRP vector Q. + */ +void Euler3212MRP(double *e, double *q) +{ + double ep[4]; + + Euler3212EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler3212PRV(E,Q) translates the (3-2-1) Euler + * angle vector E into the principal rotation vector Q. + */ +void Euler3212PRV(double *e, double *q) +{ + double ep[4]; + + Euler3212EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Euler3232C(Q,C) returns the direction cosine + * matrix in terms of the 3-2-3 Euler angles. + * Input Q must be a 3x1 vector of Euler angles. + */ +void Euler3232C(double *q, double C[3][3]) +{ + double st1; + double st2; + double st3; + double ct1; + double ct2; + double ct3; + + st1 = sin(q[0]); + ct1 = cos(q[0]); + st2 = sin(q[1]); + ct2 = cos(q[1]); + st3 = sin(q[2]); + ct3 = cos(q[2]); + + C[0][0] = ct1 * ct2 * ct3 - st1 * st3; + C[0][1] = ct2 * ct3 * st1 + ct1 * st3; + C[0][2] = -ct3 * st2; + C[1][0] = -ct3 * st1 - ct1 * ct2 * st3; + C[1][1] = ct1 * ct3 - ct2 * st1 * st3; + C[1][2] = st2 * st3; + C[2][0] = ct1 * st2; + C[2][1] = st1 * st2; + C[2][2] = ct2; +} + +/* + * Euler3232EP(E,Q) translates the 323 Euler angle + * vector E into the Euler parameter vector Q. + */ +void Euler3232EP(double *e, double *q) +{ + double e1; + double e2; + double e3; + + e1 = e[0] / 2; + e2 = e[1] / 2; + e3 = e[2] / 2; + + q[0] = cos(e2) * cos(e1 + e3); + q[1] = sin(e2) * sin(-e1 + e3); + q[2] = sin(e2) * cos(-e1 + e3); + q[3] = cos(e2) * sin(e1 + e3); +} + +/* + * Euler3232Gibbs(E,Q) translates the (3-2-3) Euler + * angle vector E into the Gibbs vector Q. + */ +void Euler3232Gibbs(double *e, double *q) +{ + double ep[4]; + + Euler3232EP(e, ep); + EP2Gibbs(ep, q); +} + +/* + * Euler3232MRP(E,Q) translates the (3-2-3) Euler + * angle vector E into the MRP vector Q. + */ +void Euler3232MRP(double *e, double *q) +{ + double ep[4]; + + Euler3232EP(e, ep); + EP2MRP(ep, q); +} + +/* + * Euler3232PRV(E,Q) translates the (3-2-3) Euler + * angle vector Q1 into the principal rotation vector Q. + */ +void Euler3232PRV(double *e, double *q) +{ + double ep[4]; + + Euler3232EP(e, ep); + EP2PRV(ep, q); +} + +/* + * Gibbs2C(Q,C) returns the direction cosine + * matrix in terms of the 3x1 Gibbs vector Q. + */ +void Gibbs2C(double *q, double C[3][3]) +{ + double q1; + double q2; + double q3; + double d1; + + q1 = q[0]; + q2 = q[1]; + q3 = q[2]; + + d1 = v3Dot(q, q); + C[0][0] = 1 + 2 * q1 * q1 - d1; + C[0][1] = 2 * (q1 * q2 + q3); + C[0][2] = 2 * (q1 * q3 - q2); + C[1][0] = 2 * (q2 * q1 - q3); + C[1][1] = 1 + 2 * q2 * q2 - d1; + C[1][2] = 2 * (q2 * q3 + q1); + C[2][0] = 2 * (q3 * q1 + q2); + C[2][1] = 2 * (q3 * q2 - q1); + C[2][2] = 1 + 2 * q3 * q3 - d1; + m33Scale(1. / (1 + d1), C, C); +} + +/* + * Gibbs2EP(Q1,Q) translates the Gibbs vector Q1 + * into the Euler parameter vector Q. + */ +void Gibbs2EP(double *q1, double *q) +{ + q[0] = 1 / sqrt(1 + v3Dot(q1, q1)); + q[1] = q1[0] * q[0]; + q[2] = q1[1] * q[0]; + q[3] = q1[2] * q[0]; +} + +/* + * Gibbs2Euler121(Q,E) translates the Gibbs + * vector Q into the (1-2-1) Euler angle vector E. + */ +void Gibbs2Euler121(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler121(ep, e); +} + +/* + * Gibbs2Euler123(Q,E) translates the Gibbs + * vector Q into the (1-2-3) Euler angle vector E. + */ +void Gibbs2Euler123(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler123(ep, e); +} + +/* + * Gibbs2Euler131(Q,E) translates the Gibbs + * vector Q into the (1-3-1) Euler angle vector E. + */ +void Gibbs2Euler131(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler131(ep, e); +} + +/* + * Gibbs2Euler132(Q,E) translates the Gibbs + * vector Q into the (1-3-2) Euler angle vector E. + */ +void Gibbs2Euler132(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler132(ep, e); +} + +/* + * Gibbs2Euler212(Q,E) translates the Gibbs + * vector Q into the (2-1-2) Euler angle vector E. + */ +void Gibbs2Euler212(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler212(ep, e); +} + +/* + * Gibbs2Euler213(Q,E) translates the Gibbs + * vector Q into the (2-1-3) Euler angle vector E. + */ +void Gibbs2Euler213(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler213(ep, e); +} + +/* + * Gibbs2Euler231(Q,E) translates the Gibbs + * vector Q into the (2-3-1) Euler angle vector E. + */ +void Gibbs2Euler231(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler231(ep, e); +} + +/* + * Gibbs2Euler232(Q,E) translates the Gibbs + * vector Q into the (2-3-2) Euler angle vector E. + */ +void Gibbs2Euler232(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler232(ep, e); +} + +/* + * Gibbs2Euler312(Q,E) translates the Gibbs + * vector Q into the (3-1-2) Euler angle vector E. + */ +void Gibbs2Euler312(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler312(ep, e); +} + +/* + * Gibbs2Euler313(Q,E) translates the Gibbs + * vector Q into the (3-1-3) Euler angle vector E. + */ +void Gibbs2Euler313(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler313(ep, e); +} + +/* + * Gibbs2Euler321(Q,E) translates the Gibbs + * vector Q into the (3-2-1) Euler angle vector E. + */ +void Gibbs2Euler321(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler321(ep, e); +} + +/* + * Gibbs2Euler323(Q,E) translates the Gibbs + * vector Q into the (3-2-3) Euler angle vector E. + */ +void Gibbs2Euler323(double *q, double *e) +{ + double ep[4]; + + Gibbs2EP(q, ep); + EP2Euler323(ep, e); +} + +/* + * Gibbs2MRP(Q1,Q) translates the Gibbs vector Q1 + * into the MRP vector Q. + */ +void Gibbs2MRP(double *q1, double *q) +{ + v3Scale(1.0 / (1 + sqrt(1 + v3Dot(q1, q1))), q1, q); +} + +/* + * Gibbs2PRV(Q1,Q) translates the Gibbs vector Q1 + * into the principal rotation vector Q. + */ +void Gibbs2PRV(double *q1, double *q) +{ + double tp; + double p; + + tp = sqrt(v3Dot(q1, q1)); + p = 2 * atan(tp); + if (tp < nearZero) { + q[0] = 0.0; + q[1] = 0.0; + q[2] = 0.0; + return; + } + q[0] = q1[0] / tp * p; + q[1] = q1[1] / tp * p; + q[2] = q1[2] / tp * p; +} + +/* + * MRP2C(Q,C) returns the direction cosine + * matrix in terms of the 3x1 MRP vector Q. + */ +void MRP2C(double *q, double C[3][3]) +{ + double q1; + double q2; + double q3; + double S; + double d1; + double d; + + q1 = q[0]; + q2 = q[1]; + q3 = q[2]; + + d1 = v3Dot(q, q); + S = 1 - d1; + d = (1 + d1) * (1 + d1); + C[0][0] = 4 * (2 * q1 * q1 - d1) + S * S; + C[0][1] = 8 * q1 * q2 + 4 * q3 * S; + C[0][2] = 8 * q1 * q3 - 4 * q2 * S; + C[1][0] = 8 * q2 * q1 - 4 * q3 * S; + C[1][1] = 4 * (2 * q2 * q2 - d1) + S * S; + C[1][2] = 8 * q2 * q3 + 4 * q1 * S; + C[2][0] = 8 * q3 * q1 + 4 * q2 * S; + C[2][1] = 8 * q3 * q2 - 4 * q1 * S; + C[2][2] = 4 * (2 * q3 * q3 - d1) + S * S; + m33Scale(1. / d, C, C); +} + +/* + * MRP2EP(Q1,Q) translates the MRP vector Q1 + * into the Euler parameter vector Q. + */ +void MRP2EP(double *q1, double *q) +{ + double ps; + + ps = 1 + v3Dot(q1, q1); + q[0] = (1 - v3Dot(q1, q1)) / ps; + q[1] = 2 * q1[0] / ps; + q[2] = 2 * q1[1] / ps; + q[3] = 2 * q1[2] / ps; +} + +/* + * MRP2Euler121(Q,E) translates the MRP + * vector Q into the (1-2-1) Euler angle vector E. + */ +void MRP2Euler121(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler121(ep, e); +} + +/* + * MRP2Euler123(Q,E) translates the MRP + * vector Q into the (1-2-3) Euler angle vector E. + */ +void MRP2Euler123(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler123(ep, e); +} + +/* + * MRP2Euler131(Q,E) translates the MRP + * vector Q into the (1-3-1) Euler angle vector E. + */ +void MRP2Euler131(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler131(ep, e); +} + +/* + * MRP2Euler132(Q,E) translates the MRP + * vector Q into the (1-3-2) Euler angle vector E. + */ +void MRP2Euler132(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler132(ep, e); +} + +/* + * E = MRP2Euler212(Q) translates the MRP + * vector Q into the (2-1-2) Euler angle vector E. + */ +void MRP2Euler212(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler212(ep, e); +} + +/* + * MRP2Euler213(Q,E) translates the MRP + * vector Q into the (2-1-3) Euler angle vector E. + */ +void MRP2Euler213(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler213(ep, e); +} + +/* + * MRP2Euler231(Q,E) translates the MRP + * vector Q into the (2-3-1) Euler angle vector E. + */ +void MRP2Euler231(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler231(ep, e); +} + +/* + * MRP2Euler232(Q,E) translates the MRP + * vector Q into the (2-3-2) Euler angle vector E. + */ +void MRP2Euler232(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler232(ep, e); +} + +/* + * MRP2Euler312(Q,E) translates the MRP + * vector Q into the (3-1-2) Euler angle vector E. + */ +void MRP2Euler312(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler312(ep, e); +} + +/* + * MRP2Euler313(Q,E) translates the MRP + * vector Q into the (3-1-3) Euler angle vector E. + */ +void MRP2Euler313(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler313(ep, e); +} + +/* + * MRP2Euler321(Q,E) translates the MRP + * vector Q into the (3-2-1) Euler angle vector E. + */ +void MRP2Euler321(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler321(ep, e); +} + +/* + * MRP2Euler323(Q,E) translates the MRP + * vector Q into the (3-2-3) Euler angle vector E. + */ +void MRP2Euler323(double *q, double *e) +{ + double ep[4]; + + MRP2EP(q, ep); + EP2Euler323(ep, e); +} + +/* + * MRP2Gibbs(Q1,Q) translates the MRP vector Q1 + * into the Gibbs vector Q. + */ +void MRP2Gibbs(double *q1, double *q) +{ + v3Scale(2. / (1. - v3Dot(q1, q1)), q1, q); +} + +/* + * MRP2PRV(Q1,Q) translates the MRP vector Q1 + * into the principal rotation vector Q. + */ +void MRP2PRV(double *q1, double *q) +{ + double tp; + double p; + + tp = sqrt(v3Dot(q1, q1)); + if(tp < nearZero) + { + memset(q, 0x0, 3*sizeof(double)); + return; + } + p = 4 * atan(tp); + q[0] = q1[0] / tp * p; + q[1] = q1[1] / tp * p; + q[2] = q1[2] / tp * p; +} + +/* + * MRPswitch(Q,s2,s) checks to see if v3Norm(Q) is larger than s2. + * If yes, then the MRP vector Q is mapped to its shadow set. + */ +void MRPswitch(double *q, double s2, double *s) +{ + double q2; + + q2 = v3Dot(q, q); + if(q2 > s2 * s2) { + v3Scale(-1. / q2, q, s); + } else { + v3Copy(q, s); + } +} + +/* + * MRPshadow forces a switch from the current MRP to its shadow set + */ +void MRPshadow(double *qIn, double *qOut) +{ + double q2; + + q2 = v3Dot(qIn, qIn); + v3Scale(-1. / q2, qIn, qOut); + return; +} + +/* + * Makes sure that the angle x lies within +/- Pi. + */ +double wrapToPi(double x) +{ + double q; + + q = x; + + if(x > M_PI) { + q = x - 2 * M_PI; + } + + if(x < -M_PI) { + q = x + 2 * M_PI; + } + + return q; +} + +/* + * PRV2C(Q,C) returns the direction cosine + * matrix in terms of the 3x1 principal rotation vector + * Q. + */ +void PRV2C(double *q, double C[3][3]) +{ + double q0; + double q1; + double q2; + double q3; + double cp; + double sp; + double d1; + + if(v3Norm(q) == 0.0) + { + m33SetIdentity(C); + return; + } + + q0 = sqrt(v3Dot(q, q)); + q1 = q[0] / q0; + q2 = q[1] / q0; + q3 = q[2] / q0; + + cp = cos(q0); + sp = sin(q0); + d1 = 1 - cp; + C[0][0] = q1 * q1 * d1 + cp; + C[0][1] = q1 * q2 * d1 + q3 * sp; + C[0][2] = q1 * q3 * d1 - q2 * sp; + C[1][0] = q2 * q1 * d1 - q3 * sp; + C[1][1] = q2 * q2 * d1 + cp; + C[1][2] = q2 * q3 * d1 + q1 * sp; + C[2][0] = q3 * q1 * d1 + q2 * sp; + C[2][1] = q3 * q2 * d1 - q1 * sp; + C[2][2] = q3 * q3 * d1 + cp; +} + +/* + * PRV2elem(R,Q) translates a prinicpal rotation vector R + * into the corresponding principal rotation element set Q. + */ +void PRV2elem(double *r, double *q) +{ + q[0] = sqrt(v3Dot(r, r)); + if (q[0] < 1.0E-12) + { + q[1] = q[2] = q[3] = 0.0; + } + else + { + q[1] = r[0] / q[0]; + q[2] = r[1] / q[0]; + q[3] = r[2] / q[0]; + } +} + +/* + * PRV2EP(Q0,Q) translates the principal rotation vector Q1 + * into the Euler parameter vector Q. + */ +void PRV2EP(double *q0, double *q) +{ + double q1[4]; + double sp; + + PRV2elem(q0, q1); + sp = sin(q1[0] / 2); + q[0] = cos(q1[0] / 2); + q[1] = q1[1] * sp; + q[2] = q1[2] * sp; + q[3] = q1[3] * sp; +} + +/* + * PRV2Euler121(Q,E) translates the principal rotation + * vector Q into the (1-2-1) Euler angle vector E. + */ +void PRV2Euler121(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler121(ep, e); +} + +/* + * PRV2Euler123(Q,E) translates the principal rotation + * vector Q into the (1-2-3) Euler angle vector E. + */ +void PRV2Euler123(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler123(ep, e); +} + +/* + * PRV2Euler131(Q,E) translates the principal rotation + * vector Q into the (1-3-1) Euler angle vector E. + */ +void PRV2Euler131(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler131(ep, e); +} + +/* + * PRV2Euler132(Q,E) translates the principal rotation + * vector Q into the (1-3-2) Euler angle vector E. + */ +void PRV2Euler132(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler132(ep, e); +} + +/* + * PRV2Euler212(Q,E) translates the principal rotation + * vector Q into the (2-1-2) Euler angle vector E. + */ +void PRV2Euler212(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler212(ep, e); +} + +/* + * PRV2Euler213(Q,E) translates the principal rotation + * vector Q into the (2-1-3) Euler angle vector E. + */ +void PRV2Euler213(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler213(ep, e); +} + +/* + * PRV2Euler231(Q) translates the principal rotation + * vector Q into the (2-3-1) Euler angle vector E. + */ +void PRV2Euler231(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler231(ep, e); +} + +/* + * PRV2Euler232(Q,E) translates the principal rotation + * vector Q into the (2-3-2) Euler angle vector E. + */ +void PRV2Euler232(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler232(ep, e); +} + +/* + * PRV2Euler312(Q,E) translates the principal rotation + * vector Q into the (3-1-2) Euler angle vector E. + */ +void PRV2Euler312(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler312(ep, e); +} + +/* + * PRV2Euler313(Q,E) translates the principal rotation + * vector Q into the (3-1-3) Euler angle vector E. + */ +void PRV2Euler313(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler313(ep, e); +} + +/* + * PRV2Euler321(Q,E) translates the principal rotation + * vector Q into the (3-2-1) Euler angle vector E. + */ +void PRV2Euler321(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler321(ep, e); +} + +/* + * PRV2Euler323(Q,E) translates the principal rotation + * vector Q into the (3-2-3) Euler angle vector E. + */ +void PRV2Euler323(double *q, double *e) +{ + double ep[4]; + + PRV2EP(q, ep); + EP2Euler323(ep, e); +} + +/* + * PRV2Gibbs(Q0,Q) translates the principal rotation vector Q1 + * into the Gibbs vector Q. + */ +void PRV2Gibbs(double *q0, double *q) +{ + double q1[4]; + double tp; + + PRV2elem(q0, q1); + tp = tan(q1[0] / 2.); + q[0] = q1[1] * tp; + q[1] = q1[2] * tp; + q[2] = q1[3] * tp; +} + +/* + * PRV2MRP(Q0,Q) translates the principal rotation vector Q1 + * into the MRP vector Q. + */ +void PRV2MRP(double *q0, double *q) +{ + double q1[4]; + double tp; + + PRV2elem(q0, q1); + tp = tan(q1[0] / 4.); + q[0] = q1[1] * tp; + q[1] = q1[2] * tp; + q[2] = q1[3] * tp; +} + +/* + * subEP(B1,B2,Q) provides the Euler parameter vector + * which corresponds to relative rotation from B2 + * to B1. + */ +void subEP(double *b1, double *b2, double *q) +{ + q[0] = b2[0] * b1[0] + b2[1] * b1[1] + b2[2] * b1[2] + b2[3] * b1[3]; + q[1] = -b2[1] * b1[0] + b2[0] * b1[1] + b2[3] * b1[2] - b2[2] * b1[3]; + q[2] = -b2[2] * b1[0] - b2[3] * b1[1] + b2[0] * b1[2] + b2[1] * b1[3]; + q[3] = -b2[3] * b1[0] + b2[2] * b1[1] - b2[1] * b1[2] + b2[0] * b1[3]; +} + +/* + * subEuler121(E,E1,E2) computes the relative + * (1-2-1) Euler angle vector from E1 to E. + */ +void subEuler121(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double cp2; + double dum; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subEuler123(E,E1,E2) computes the relative + * (1-2-3) Euler angle vector from E1 to E. + */ +void subEuler123(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler1232C(e, C); + Euler1232C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler123(C2, e2); +} + +/* + * subEuler131(E,E1,E2) computes the relative + * (1-3-1) Euler angle vector from E1 to E. + */ +void subEuler131(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double dum; + double cp2; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subEuler132(E,E1,E2) computes the relative + * (1-3-2) Euler angle vector from E1 to E. + */ +void subEuler132(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler1322C(e, C); + Euler1322C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler132(C2, e2); +} + +/* + * subEuler212(E,E1,E2) computes the relative + * (2-1-2) Euler angle vector from E1 to E. + */ +void subEuler212(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double dum; + double cp2; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subEuler213(E,E1,E2) computes the relative + * (2-1-3) Euler angle vector from E1 to E. + */ +void subEuler213(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler2132C(e, C); + Euler2132C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler213(C2, e2); +} + +/* + * subEuler231(E,E1,E2) computes the relative + * (2-3-1) Euler angle vector from E1 to E. + */ +void subEuler231(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler2312C(e, C); + Euler2312C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler231(C2, e2); +} + +/* + * subEuler232(E,E1,E2) computes the relative + * (2-3-2) Euler angle vector from E1 to E. + */ +void subEuler232(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double dum; + double cp2; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subEuler312(E,E1,E2) computes the relative + * (3-1-2) Euler angle vector from E1 to E. + */ +void subEuler312(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler3122C(e, C); + Euler3122C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler312(C2, e2); +} + +/* + * subEuler313(E,E1,E2) computes the relative + * (3-1-3) Euler angle vector from E1 to E. + */ +void subEuler313(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double dum; + double cp2; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subEuler321(E,E1,E2) computes the relative + * (3-2-1) Euler angle vector from E1 to E. + */ +void subEuler321(double *e, double *e1, double *e2) +{ + double C[3][3]; + double C1[3][3]; + double C2[3][3]; + + Euler3212C(e, C); + Euler3212C(e1, C1); + m33MultM33t(C, C1, C2); + C2Euler321(C2, e2); +} + +/* + * subEuler323(E,E1,E2) computes the relative + * (3-2-3) Euler angle vector from E1 to E. + */ +void subEuler323(double *e, double *e1, double *e2) +{ + double cp; + double cp1; + double sp; + double sp1; + double dum; + double cp2; + + cp = cos(e[1]); + cp1 = cos(e1[1]); + sp = sin(e[1]); + sp1 = sin(e1[1]); + dum = e[0] - e1[0]; + + e2[1] = safeAcos(cp1 * cp + sp1 * sp * cos(dum)); + cp2 = cos(e2[1]); + e2[0] = wrapToPi(-e1[2] + atan2(sp1 * sp * sin(dum), cp2 * cp1 - cp)); + e2[2] = wrapToPi(e[2] - atan2(sp1 * sp * sin(dum), cp1 - cp * cp2)); +} + +/* + * subGibbs(Q1,Q2,Q) provides the Gibbs vector + * which corresponds to relative rotation from Q2 + * to Q1. + */ +void subGibbs(double *q1, double *q2, double *q) +{ + double d1[3]; + + v3Cross(q1, q2, d1); + v3Add(q1, d1, q); + v3Subtract(q, q2, q); + v3Scale(1. / (1. + v3Dot(q1, q2)), q, q); +} + +/* + * subMRP(Q1,Q2,Q) provides the MRP vector + * which corresponds to relative rotation from Q2 + * to Q1. + */ +void subMRP(double *q1, double *q2, double *q) +{ + double d1[3]; + double s1[3]; + double det; + double mag; + + v3Copy(q1, s1); + det = (1. + v3Dot(s1, s1)*v3Dot(q2, q2) + 2.*v3Dot(s1, q2)); + if (fabs(det) < 0.1) { + mag = v3Dot(s1, s1); + v3Scale(-1.0/mag, s1, s1); + det = (1. + v3Dot(s1, s1)*v3Dot(q2, q2) + 2.*v3Dot(s1, q2)); + } + + v3Cross(s1, q2, d1); + v3Scale(2., d1, q); + v3Scale(1. - v3Dot(q2, q2), s1, d1); + v3Add(q, d1, q); + v3Scale(1. - v3Dot(s1, s1), q2, d1); + v3Subtract(q, d1, q); + v3Scale(1. / det, q, q); + + /* map MRP to inner set */ + mag = v3Dot(q, q); + if (mag > 1.0){ + v3Scale(-1./mag, q, q); + } + +} + +/* + * subPRV(Q1,Q2,Q) provides the prinipal rotation vector + * which corresponds to relative principal rotation from Q2 + * to Q1. + */ +void subPRV(double *q10, double *q20, double *q) +{ + double q1[4]; + double q2[4]; + double cp1; + double cp2; + double sp1; + double sp2; + double e1[3]; + double e2[3]; + double p; + double sp; + + PRV2elem(q10, q1); + PRV2elem(q20, q2); + cp1 = cos(q1[0] / 2.); + cp2 = cos(q2[0] / 2.); + sp1 = sin(q1[0] / 2.); + sp2 = sin(q2[0] / 2.); + v3Copy(&(q1[1]), e1); + v3Copy(&(q2[1]), e2); + + p = 2.*safeAcos(cp1 * cp2 + sp1 * sp2 * v3Dot(e1, e2)); + sp = sin(p / 2.); + + v3Cross(e1, e2, q1); + v3Scale(sp1 * sp2, q1, q); + v3Scale(cp2 * sp1, e1, q1); + v3Add(q1, q, q); + v3Scale(cp1 * sp2, e2, q1); + v3Subtract(q, q1, q); + v3Scale(p / sp, q, q); +} + +/* + * Mi(theta, a, C) returns the rotation matrix corresponding + * to a single axis rotation about axis a by the angle theta + */ +void Mi(double theta, int a, double C[3][3]) +{ + double c; + double s; + + c = cos(theta); + s = sin(theta); + + switch(a) { + case 1: + C[0][0] = 1.; + C[0][1] = 0.; + C[0][2] = 0.; + C[1][0] = 0.; + C[1][1] = c; + C[1][2] = s; + C[2][0] = 0.; + C[2][1] = -s; + C[2][2] = c; + break; + + case 2: + C[0][0] = c; + C[0][1] = 0.; + C[0][2] = -s; + C[1][0] = 0.; + C[1][1] = 1.; + C[1][2] = 0.; + C[2][0] = s; + C[2][1] = 0.; + C[2][2] = c; + break; + + case 3: + C[0][0] = c; + C[0][1] = s; + C[0][2] = 0.; + C[1][0] = -s; + C[1][1] = c; + C[1][2] = 0.; + C[2][0] = 0.; + C[2][1] = 0.; + C[2][2] = 1.; + break; + + default: + BSK_PRINT(MSG_ERROR, "Mi() error: incorrect axis %d selected.", a); + } +} + +/* + * tilde(theta, mat) returns the the 3x3 cross product matrix + */ +void tilde(double *v, double mat[3][3]) +{ + m33SetZero(mat); + mat[0][1] = -v[2]; + mat[1][0] = v[2]; + mat[0][2] = v[1]; + mat[2][0] = -v[1]; + mat[1][2] = -v[0]; + mat[2][1] = v[0]; + + return; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.h new file mode 100644 index 0000000000..8d76755756 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/rigidBodyKinematics.h @@ -0,0 +1,266 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _RIGID_BODY_KINEMATICS_0_H_ +#define _RIGID_BODY_KINEMATICS_0_H_ +#include + +#ifdef __cplusplus +extern "C" { +#endif + void addEP(double *b1, double *b2, double *result); + void addEuler121(double *e1, double *e2, double *result); + void addEuler123(double *e1, double *e2, double *result); + void addEuler131(double *e1, double *e2, double *result); + void addEuler132(double *e1, double *e2, double *result); + void addEuler212(double *e1, double *e2, double *result); + void addEuler213(double *e1, double *e2, double *result); + void addEuler231(double *e1, double *e2, double *result); + void addEuler232(double *e1, double *e2, double *result); + void addEuler312(double *e1, double *e2, double *result); + void addEuler313(double *e1, double *e2, double *result); + void addEuler321(double *e1, double *e2, double *result); + void addEuler323(double *e1, double *e2, double *result); + void addGibbs(double *q1, double *q2, double *result); + void addMRP(double *q1, double *q2, double *result); + void addPRV(double *q1, double *q2, double *result); + void BinvEP(double *q, double B[3][4]); + void BinvEuler121(double *q, double B[3][3]); + void BinvEuler123(double *q, double B[3][3]); + void BinvEuler131(double *q, double B[3][3]); + void BinvEuler132(double *q, double B[3][3]); + void BinvEuler212(double *q, double B[3][3]); + void BinvEuler213(double *q, double B[3][3]); + void BinvEuler231(double *q, double B[3][3]); + void BinvEuler232(double *q, double B[3][3]); + void BinvEuler312(double *q, double B[3][3]); + void BinvEuler313(double *q, double B[3][3]); + void BinvEuler321(double *q, double B[3][3]); + void BinvEuler323(double *q, double B[3][3]); + void BinvGibbs(double *q, double B[3][3]); + void BinvMRP(double *q, double B[3][3]); + void BinvPRV(double *q, double B[3][3]); + void BmatEP(double *q, double B[4][3]); + void BmatEuler121(double *q, double B[3][3]); + void BmatEuler131(double *q, double B[3][3]); + void BmatEuler123(double *q, double B[3][3]); + void BmatEuler132(double *q, double B[3][3]); + void BmatEuler212(double *q, double B[3][3]); + void BmatEuler213(double *q, double B[3][3]); + void BmatEuler231(double *q, double B[3][3]); + void BmatEuler232(double *q, double B[3][3]); + void BmatEuler312(double *q, double B[3][3]); + void BmatEuler313(double *q, double B[3][3]); + void BmatEuler321(double *q, double B[3][3]); + void BmatEuler323(double *q, double B[3][3]); + void BmatGibbs(double *q, double B[3][3]); + void BmatMRP(double *q, double B[3][3]); + void BdotmatMRP(double *q, double *dq, double B[3][3]); + void BmatPRV(double *q, double B[3][3]); + void C2EP(double C[3][3], double b[4]); + void C2Euler121(double C[3][3], double *q); + void C2Euler123(double C[3][3], double *q); + void C2Euler131(double C[3][3], double *q); + void C2Euler132(double C[3][3], double *q); + void C2Euler212(double C[3][3], double *q); + void C2Euler213(double C[3][3], double *q); + void C2Euler231(double C[3][3], double *q); + void C2Euler232(double C[3][3], double *q); + void C2Euler312(double C[3][3], double *q); + void C2Euler313(double C[3][3], double *q); + void C2Euler321(double C[3][3], double *q); + void C2Euler323(double C[3][3], double *q); + void C2Gibbs(double C[3][3], double *q); + void C2MRP(double C[3][3], double *q); + void C2PRV(double C[3][3], double *q); + void dEP(double *q, double *w, double *dq); + void dEuler121(double *q, double *w, double *dq); + void dEuler123(double *q, double *w, double *dq); + void dEuler131(double *q, double *w, double *dq); + void dEuler132(double *q, double *w, double *dq); + void dEuler212(double *q, double *w, double *dq); + void dEuler213(double *q, double *w, double *dq); + void dEuler231(double *q, double *w, double *dq); + void dEuler232(double *q, double *w, double *dq); + void dEuler312(double *q, double *w, double *dq); + void dEuler313(double *q, double *w, double *dq); + void dEuler321(double *q, double *w, double *dq); + void dEuler323(double *q, double *w, double *dq); + void dGibbs(double *q, double *w, double *dq); + void dMRP(double *q, double *w, double *dq); + void dMRP2Omega(double *q, double *dq, double *w); + void ddMRP(double *q, double *dq, double *w, double *dw, double *ddq); + void ddMRP2dOmega(double *q, double *dq, double *ddq, double *dw); + void dPRV(double *q, double *w, double *dq); + void elem2PRV(double *r, double *q); + void EP2C(double *q, double C[3][3]); + void EP2Euler121(double *q, double *e); + void EP2Euler123(double *q, double *e); + void EP2Euler131(double *q, double *e); + void EP2Euler132(double *q, double *e); + void EP2Euler212(double *q, double *e); + void EP2Euler213(double *q, double *e); + void EP2Euler231(double *q, double *e); + void EP2Euler232(double *q, double *e); + void EP2Euler312(double *q, double *e); + void EP2Euler313(double *q, double *e); + void EP2Euler321(double *q, double *e); + void EP2Euler323(double *q, double *e); + void EP2Gibbs(double *q1, double *q); + void EP2MRP(double *q1, double *q); + void EP2PRV(double *q1, double *q); + void Euler1(double x, double m[3][3]); + void Euler2(double x, double m[3][3]); + void Euler3(double x, double m[3][3]); + void Euler1212C(double *q, double C[3][3]); + void Euler1212EP(double *e, double *q); + void Euler1212Gibbs(double *e, double *q); + void Euler1212MRP(double *e, double *q); + void Euler1212PRV(double *e, double *q); + void Euler1232C(double *q, double C[3][3]); + void Euler1232EP(double *e, double *q); + void Euler1232Gibbs(double *e, double *q); + void Euler1232MRP(double *e, double *q); + void Euler1232PRV(double *e, double *q); + void Euler1312C(double *q, double C[3][3]); + void Euler1312EP(double *e, double *q); + void Euler1312Gibbs(double *e, double *q); + void Euler1312MRP(double *e, double *q); + void Euler1312PRV(double *e, double *q); + void Euler1322C(double *q, double C[3][3]); + void Euler1322EP(double *e, double *q); + void Euler1322Gibbs(double *e, double *q); + void Euler1322MRP(double *e, double *q); + void Euler1322PRV(double *e, double *q); + void Euler2122C(double *q, double C[3][3]); + void Euler2122EP(double *e, double *q); + void Euler2122Gibbs(double *e, double *q); + void Euler2122MRP(double *e, double *q); + void Euler2122PRV(double *e, double *q); + void Euler2132C(double *q, double C[3][3]); + void Euler2132EP(double *e, double *q); + void Euler2132Gibbs(double *e, double *q); + void Euler2132MRP(double *e, double *q); + void Euler2132PRV(double *e, double *q); + void Euler2312C(double *q, double C[3][3]); + void Euler2312EP(double *e, double *q); + void Euler2312Gibbs(double *e, double *q); + void Euler2312MRP(double *e, double *q); + void Euler2312PRV(double *e, double *q); + void Euler2322C(double *q, double C[3][3]); + void Euler2322EP(double *e, double *q); + void Euler2322Gibbs(double *e, double *q); + void Euler2322MRP(double *e, double *q); + void Euler2322PRV(double *e, double *q); + void Euler3122C(double *q, double C[3][3]); + void Euler3122EP(double *e, double *q); + void Euler3122Gibbs(double *e, double *q); + void Euler3122MRP(double *e, double *q); + void Euler3122PRV(double *e, double *q); + void Euler3132C(double *q, double C[3][3]); + void Euler3132EP(double *e, double *q); + void Euler3132Gibbs(double *e, double *q); + void Euler3132MRP(double *e, double *q); + void Euler3132PRV(double *e, double *q); + void Euler3212C(double *q, double C[3][3]); + void Euler3212EP(double *e, double *q); + void Euler3212Gibbs(double *e, double *q); + void Euler3212MRP(double *e, double *q); + void Euler3212PRV(double *e, double *q); + void Euler3232C(double *q, double C[3][3]); + void Euler3232EP(double *e, double *q); + void Euler3232Gibbs(double *e, double *q); + void Euler3232MRP(double *e, double *q); + void Euler3232PRV(double *e, double *q); + void Gibbs2C(double *q, double C[3][3]); + void Gibbs2EP(double *q1, double *q); + void Gibbs2Euler121(double *q, double *e); + void Gibbs2Euler123(double *q, double *e); + void Gibbs2Euler131(double *q, double *e); + void Gibbs2Euler132(double *q, double *e); + void Gibbs2Euler212(double *q, double *e); + void Gibbs2Euler213(double *q, double *e); + void Gibbs2Euler231(double *q, double *e); + void Gibbs2Euler232(double *q, double *e); + void Gibbs2Euler312(double *q, double *e); + void Gibbs2Euler313(double *q, double *e); + void Gibbs2Euler321(double *q, double *e); + void Gibbs2Euler323(double *q, double *e); + void Gibbs2MRP(double *q1, double *q); + void Gibbs2PRV(double *q1, double *q); + void MRP2C(double *q, double C[3][3]); + void MRP2EP(double *q1, double *q); + void MRP2Euler121(double *q, double *e); + void MRP2Euler123(double *q, double *e); + void MRP2Euler131(double *q, double *e); + void MRP2Euler132(double *q, double *e); + void MRP2Euler212(double *q, double *e); + void MRP2Euler213(double *q, double *e); + void MRP2Euler231(double *q, double *e); + void MRP2Euler232(double *q, double *e); + void MRP2Euler312(double *q, double *e); + void MRP2Euler313(double *q, double *e); + void MRP2Euler321(double *q, double *e); + void MRP2Euler323(double *q, double *e); + void MRP2Gibbs(double *q1, double *q); + void MRP2PRV(double *q1, double *q); + void MRPswitch(double *q, double s2, double *s); + void MRPshadow(double *qIn, double *qOut); + double wrapToPi(double x); + void PRV2C(double *q, double C[3][3]); + void PRV2elem(double *r, double *q); + void PRV2EP(double *q0, double *q); + void PRV2Euler121(double *q, double *e); + void PRV2Euler123(double *q, double *e); + void PRV2Euler131(double *q, double *e); + void PRV2Euler132(double *q, double *e); + void PRV2Euler212(double *q, double *e); + void PRV2Euler213(double *q, double *e); + void PRV2Euler231(double *q, double *e); + void PRV2Euler232(double *q, double *e); + void PRV2Euler312(double *q, double *e); + void PRV2Euler313(double *q, double *e); + void PRV2Euler321(double *q, double *e); + void PRV2Euler323(double *q, double *e); + void PRV2Gibbs(double *q0, double *q); + void PRV2MRP(double *q0, double *q); + void subEP(double *b1, double *b2, double *q); + void subEuler121(double *e, double *e1, double *e2); + void subEuler123(double *e, double *e1, double *e2); + void subEuler131(double *e, double *e1, double *e2); + void subEuler132(double *e, double *e1, double *e2); + void subEuler212(double *e, double *e1, double *e2); + void subEuler213(double *e, double *e1, double *e2); + void subEuler231(double *e, double *e1, double *e2); + void subEuler232(double *e, double *e1, double *e2); + void subEuler312(double *e, double *e1, double *e2); + void subEuler313(double *e, double *e1, double *e2); + void subEuler321(double *e, double *e1, double *e2); + void subEuler323(double *e, double *e1, double *e2); + void subGibbs(double *q1, double *q2, double *q); + void subMRP(double *q1, double *q2, double *q); + void subPRV(double *q10, double *q20, double *q); + void Mi(double angle, int axis, double C[3][3]); + void tilde(double *v, double mat[3][3]); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.cpp new file mode 100644 index 0000000000..c39c0e071f --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.cpp @@ -0,0 +1,62 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "saturate.h" +#include +#include + +/*! The constructor initialies the random number generator used for the walks*/ +Saturate::Saturate() +{ +} + +Saturate::Saturate(int64_t size) : Saturate() { + this->numStates = size; + this->stateBounds.resize(numStates, 2); +} +/*! The destructor is a placeholder for one that might do something*/ +Saturate::~Saturate() +{ +} + +/*! + @brief This method should be used as the standard way to saturate an output. It will also be utilized by +other utilities + @param unsaturatedStates a vector of the unsaturated states + @return saturatedStates + */ +Eigen::VectorXd Saturate::saturate(Eigen::VectorXd unsaturatedStates) +{ + Eigen::VectorXd workingStates; + workingStates.resize(this->numStates); + for (int64_t i = 0; i < this->numStates; i++){ + workingStates[(int) i] = std::min(unsaturatedStates[i], this->stateBounds(i, 1)); + workingStates[(int) i] = std::max(workingStates[i], this->stateBounds(i, 0)); + } + return workingStates; + +} + +/*! + @brief sets upper and lower bounds for each state + @param bounds one row for each state. lower bounds in left column, upper in right column + */ +void Saturate::setBounds(Eigen::MatrixXd bounds) { + this->stateBounds = bounds; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.h new file mode 100644 index 0000000000..4550aced14 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/saturate.h @@ -0,0 +1,48 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _Saturate_HH_ +#define _Saturate_HH_ + +#include +#include + + +/*! @brief This class is used to saturate an output variable +*/ +class Saturate +{ + +public: + Saturate(); + Saturate(int64_t size); //!< class constructor + ~Saturate(); + void setBounds(Eigen::MatrixXd bounds); + Eigen::VectorXd saturate(Eigen::VectorXd unsaturatedStates); + /*!@brief Saturates the given unsaturated states + @param unsaturated States, a vector of the unsaturated states + @return saturatedStates*/ + +private: + int64_t numStates; //!< -- Number of states to generate noise for + Eigen::MatrixXd stateBounds; //!< -- one row for each state. lower bounds in left column, upper in right column +}; + + +#endif /* _saturate_HH_ */ diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.c new file mode 100644 index 0000000000..4dbaa82271 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.c @@ -0,0 +1,36 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/signalCondition.h" + + +/*! This method applies the low-pass filter configuration to the newMeas that + is passed in. The state is maintained in the LowPassFilterData structure + @param lpData The configuration data and history of the LP filter + @param newMeas The new measurement to take in to the filter + */ +void lowPassFilterSignal(double newMeas, LowPassFilterData *lpData) +{ + /*! See documentation of algorithm in documentation for LP torque filter module*/ + double hOmeg = lpData->hStep*lpData->omegCutoff; + lpData->currentState = (1.0/(2.0+hOmeg)* + (lpData->currentState*(2.0-hOmeg)+hOmeg*(newMeas+lpData->currentMeas))); + lpData->currentMeas = newMeas; + return; +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.h new file mode 100644 index 0000000000..bfd348c6be --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/signalCondition.h @@ -0,0 +1,42 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _SIGNAL_CONDITION_H_ +#define _SIGNAL_CONDITION_H_ + + +/*! struct definition */ +typedef struct { + double hStep; /*!< [s] filter time step (assumed to be fixed) */ + double omegCutoff; /*!< [rad/s] Cutoff frequency for the filter */ + double currentState; /*!< [-] Current state of the filter */ + double currentMeas; /*!< [-] Current measurement that we read */ +}LowPassFilterData; + +#ifdef __cplusplus +extern "C" { +#endif + + void lowPassFilterSignal(double newMeas, LowPassFilterData *lpData); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/simDefinitions.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/simDefinitions.h new file mode 100644 index 0000000000..7ecaef083e --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/simDefinitions.h @@ -0,0 +1,29 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#define EPOCH_YEAR 2019 +#define EPOCH_MONTH 01 +#define EPOCH_DAY 01 +#define EPOCH_HOUR 00 +#define EPOCH_MIN 00 +#define EPOCH_SEC 0.00 + +#define SIGNAL_NOMINAL 0 +#define SIGNAL_OFF 1 +#define SIGNAL_STUCK 2 \ No newline at end of file diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.c new file mode 100644 index 0000000000..71d752b255 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.c @@ -0,0 +1,328 @@ +/* + ISC License + + Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "svd.h" +#include "architecture/utilities/linearAlgebra.h" +#include +#include + +/* + * Originally Authored by Hanspeter Schaub. Sourced from: Recipes in C + * Revised by Henry Macanas to eliminate dynamic memory allocation, operate based + * on 0 based indexing. + * Sourced from: https://gist.github.com/sasekazu/32f966816ad6d9244259. + */ + +#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : - fabs(a)) + +static double maxarg1, maxarg2; +#define DMAX(a,b) (maxarg1 = (a),maxarg2 = (b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2)) + +static int iminarg1, iminarg2; +#define IMIN(a,b) (iminarg1 = (a),iminarg2 = (b),(iminarg1 < (iminarg2) ? (iminarg1) : iminarg2)) + +static double sqrarg; +#define SQR(a) ((sqrarg = (a)) == 0.0 ? 0.0 : sqrarg * sqrarg) + +// calculates sqrt( a^2 + b^2 ) +double pythag(double a, double b) { + double absa, absb; + + absa = fabs(a); + absb = fabs(b); + + if (absa > absb) + return (absa * sqrt(1.0 + SQR(absb/absa))); + else + return (absb == 0.0 ? 0.0 : absb * sqrt(1.0 + SQR(absa / absb))); +} + +/* + Modified from Numerical Recipes in C + Given a matrix a[dim1 * dim2], svdcmp() computes its singular value + decomposition, mx = U * W * Vt. mx is replaced by U when svdcmp + returns. The diagonal matrix W is output as a vector w[dim2]. + V (not V transpose) is output as the matrix V[dim2 * dim2]. + */ +int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v) { + int flag, i, its, j, jj, k, l, nm, cm; + double anorm, c, f, g, h, s, scale, x, y, z, max; + double rv1[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + vSetZero(rv1, dim2); + + g = scale = anorm = 0.0; + for (i = 0; i < dim2; i++) { + l = i + 1; + rv1[i] = scale * g; + g = s = scale = 0.0; + if (i < dim1) { + for (k = i; k < dim1; k++) + scale += fabs(mx[MXINDEX(dim2, k, i)]); + if (scale) { + for (k = i; k < dim1; k++) { + mx[MXINDEX(dim2, k, i)] /= scale; + s += mx[MXINDEX(dim2, k, i)] * mx[MXINDEX(dim2, k, i)]; + } + f = mx[MXINDEX(dim2, i, i)]; + g = -SIGN(sqrt(s),f); + h = f * g - s; + mx[MXINDEX(dim2, i, i)] = f - g; + for (j = l; j < dim2; j++) { + for (s = 0.0, k = i; k < dim1; k++) + s += mx[MXINDEX(dim2, k, i)] * mx[MXINDEX(dim2, k, j)]; + f = s / h; + for (k = i; k < dim1; k++) + mx[MXINDEX(dim2, k, j)] += f * mx[MXINDEX(dim2, k, i)]; + } + for (k = i; k < dim1; k++) + mx[MXINDEX(dim2, k, i)] *= scale; + } + } + w[i] = scale * g; + g = s = scale = 0.0; + if (i < dim1 && i != dim2 - 1) { + for (k = l; k < dim2; k++) + scale += fabs(mx[MXINDEX(dim2, i, k)]); + if (scale) { + for (k = l; k < dim2; k++) { + mx[MXINDEX(dim2, i, k)] /= scale; + s += mx[MXINDEX(dim2, i, k)] * mx[MXINDEX(dim2, i, k)]; + } + f = mx[MXINDEX(dim2, i, l)]; + g = -SIGN(sqrt(s),f); + h = f * g - s; + mx[MXINDEX(dim2, i, l)] = f - g; + for (k = l; k < dim2; k++) + rv1[k] = mx[MXINDEX(dim2, i, k)] / h; + for (j = l; j < dim1; j++) { + for (s = 0.0, k = l; k < dim2; k++) + s += mx[MXINDEX(dim2, j, k)] * mx[MXINDEX(dim2, i, k)]; + for (k = l; k < dim2; k++) + mx[MXINDEX(dim2, j, k)] += s * rv1[k]; + } + for (k = l; k < dim2; k++) + mx[MXINDEX(dim2, i, k)] *= scale; + } + } + anorm = DMAX(anorm, (fabs(w[i]) + fabs(rv1[i]))); + } + + for (i = (int) dim2 - 1; i >= 0; i--) { + if (i < dim2 - 1) { + if (g) { + for (j = l; j < dim2; j++) + v[MXINDEX(dim2, j, i)] = (mx[MXINDEX(dim2, i, j)] / mx[MXINDEX(dim2, i, l)]) / g; + for (j = l; j < dim2; j++) { + for (s = 0.0, k = l; k < dim2; k++) + s += mx[MXINDEX(dim2, i, k)] * v[MXINDEX(dim2, k, j)]; + for (k = l; k < dim2; k++) + v[MXINDEX(dim2, k, j)] += s * v[MXINDEX(dim2, k, i)]; + } + } + for (j = l; j < dim2; j++) + v[MXINDEX(dim2, i, j)] = v[MXINDEX(dim2, j, i)] = 0.0; + } + v[MXINDEX(dim2, i, i)] = 1.0; + g = rv1[i]; + l = i; + } + + for (i = IMIN((int) dim1, (int) dim2) - 1; i >= 0; i--) { + l = i + 1; + g = w[i]; + for (j = l; j < dim2; j++) + mx[MXINDEX(dim2, i, j)] = 0.0; + if (g) { + g = 1.0 / g; + for (j = l; j < dim2; j++) { + for (s = 0.0, k = l; k < dim1; k++) + s += mx[MXINDEX(dim2, k, i)] * mx[MXINDEX(dim2, k, j)]; + f = (s / mx[MXINDEX(dim2, i, i)]) * g; + for (k = i; k < dim1; k++) + mx[MXINDEX(dim2, k, j)] += f * mx[MXINDEX(dim2, k, i)]; + } + for (j = i; j < dim1; j++) + mx[MXINDEX(dim2, j, i)] *= g; + } else + for (j = i; j < dim1; j++) + mx[MXINDEX(dim2, j, i)] = 0.0; + ++mx[MXINDEX(dim2, i, i)]; + } + + for (k = (int) dim2 - 1; k >= 0; k--) { + for (its = 0; its < 30; its++) { + flag = 1; + for (l = k; l >= 0; l--) { + nm = l - 1; + if ((fabs(rv1[l]) + anorm) == anorm) { + flag = 0; + break; + } + if ((fabs(w[nm]) + anorm) == anorm) + break; + } + if (flag) { + c = 0.0; + s = 1.0; + for (i = l; i <= k; i++) { + f = s * rv1[i]; + rv1[i] = c * rv1[i]; + if ((fabs(f) + anorm) == anorm) + break; + g = w[i]; + h = pythag(f, g); + w[i] = h; + h = 1.0 / h; + c = g * h; + s = -f * h; + for (j = 0; j < dim1; j++) { + y = mx[MXINDEX(dim2, j, nm)]; + z = mx[MXINDEX(dim2, j, i)]; + mx[MXINDEX(dim2, j, nm)] = y * c + z * s; + mx[MXINDEX(dim2, j, i)] = z * c - y * s; + } + } + } + z = w[k]; + if (l == k) { + if (z < 0.0) { + w[k] = -z; + for (j = 0; j < dim2; j++) + v[MXINDEX(dim2, j, k)] = -v[MXINDEX(dim2, j, k)]; + } + break; + } + if (its == 29) + printf("no convergence in 30 svdcmp iterations\n"); + x = w[l]; + nm = k - 1; + y = w[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); + g = pythag(f, 1.0); + f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g,f)))- h)) / x; + c = s = 1.0; + for (j = l; j <= nm; j++) { + i = j + 1; + g = rv1[i]; + y = w[i]; + h = s * g; + g = c * g; + z = pythag(f, h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < dim2; jj++) { + x = v[MXINDEX(dim2, jj, j)]; + z = v[MXINDEX(dim2, jj, i)]; + v[MXINDEX(dim2, jj, j)] = x * c + z * s; + v[MXINDEX(dim2, jj, i)] = z * c - x * s; + } + z = pythag(f, h); + w[j] = z; + if (z) { + z = 1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < dim1; jj++) { + y = mx[MXINDEX(dim2, jj, j)]; + z = mx[MXINDEX(dim2, jj, i)]; + mx[MXINDEX(dim2, jj, j)] = y * c + z * s; + mx[MXINDEX(dim2, jj, i)] = z * c - y * s; + } + } + rv1[l] = 0.0; + rv1[k] = f; + w[k] = x; + } + } + + /* sort by largest singular value */ + for (i = 0; i < dim2 - 1; i++) { + max = w[i]; + cm = i; + for (j = i; j < dim2;j++) { + if (w[j]>max) { + max = w[j]; + cm = j; + } + } + if (i != cm) { + for (j = 0 ; j < dim1; j++) { + f = mx[MXINDEX(dim2, j, i)]; + mx[MXINDEX(dim2, j, i)] = mx[MXINDEX(dim2, j, cm)]; + mx[MXINDEX(dim2, j, cm)] = f; + } + for (j = 0; j < dim2; j++) { + f = v[MXINDEX(dim2, j, i)]; + v[MXINDEX(dim2, j, i)] = v[MXINDEX(dim2, j, cm)]; + v[MXINDEX(dim2, j, cm)] = f; + } + f = w[i]; + w[i] = w[cm]; + w[cm] = f; + } + } + + return (0); +} + +void solveSVD(double *mx, size_t dim1, size_t dim2, double *x, double *b, double minSV) +{ + double mxCopy[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double w[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double v[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double A[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double uTranspose[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double wInvDiag[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + double temp[LINEAR_ALGEBRA_MAX_ARRAY_SIZE]; + int j; + + vSetZero(w, dim2); + mSetZero(v, dim2, dim2); + mSetZero(A, dim1, dim2); + mSetZero(uTranspose, dim1, dim2); + mSetZero(wInvDiag, dim2, dim2); + mCopy(mx, dim1, dim2, mxCopy); + + svdcmp(mxCopy, dim1, dim2, w, v); + + // condition wInvDiag + for (j = 0; j < dim2; j++) + { + if (w[j] >= minSV) + wInvDiag[MXINDEX(dim2, j, j)] = 1.0 / w[j]; + } + + // compute A + mTranspose(mxCopy, dim1, dim2, uTranspose); + mMultM(v, dim2, dim2, wInvDiag, dim2, dim2, temp); + mMultM(temp, dim2, dim2, uTranspose, dim2, dim1, A); + + // solve for x + mMultV(A, dim2, dim1, b, x); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.h new file mode 100644 index 0000000000..c1997655bb --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/svd.h @@ -0,0 +1,35 @@ +/* + ISC License + + Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef _SVD_H_ +#define _SVD_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +int svdcmp(double *mx, size_t dim1, size_t dim2, double *w, double *v); +void solveSVD(double *u, size_t dim1, size_t dim2, double *x, double *b, double minSV); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/CMakeLists.txt b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/CMakeLists.txt new file mode 100644 index 0000000000..5c321a7a38 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/CMakeLists.txt @@ -0,0 +1,49 @@ +add_executable(test_discretize test_discretize.cpp) +target_link_libraries(test_discretize GTest::gtest_main) +target_link_libraries(test_discretize ArchitectureUtilities) + +add_executable(test_gaussMarkov test_gaussMarkov.cpp) +target_link_libraries(test_gaussMarkov GTest::gtest_main) +target_link_libraries(test_gaussMarkov ArchitectureUtilities) + +add_executable(test_orbitalMotion test_orbitalMotion.cpp) +target_link_libraries(test_orbitalMotion GTest::gtest_main) +target_link_libraries(test_orbitalMotion ArchitectureUtilities) + +add_executable(test_saturate test_saturate.cpp) +target_link_libraries(test_saturate GTest::gtest_main) +target_link_libraries(test_saturate ArchitectureUtilities) + +add_executable(test_geodeticConversion test_geodeticConversion.cpp) +target_link_libraries(test_geodeticConversion GTest::gtest_main) +target_link_libraries(test_geodeticConversion ArchitectureUtilities) + +add_executable(test_avsEigenMRP test_avsEigenMRP.cpp) +target_link_libraries(test_avsEigenMRP GTest::gtest_main) +target_link_libraries(test_avsEigenMRP ArchitectureUtilities) + +add_executable(test_avsEigenSupport test_avsEigenSupport.cpp) +target_link_libraries(test_avsEigenSupport GTest::gtest_main) +target_link_libraries(test_avsEigenSupport ArchitectureUtilities) + +add_executable(test_linearInterpolation test_linearInterpolation.cpp) +target_link_libraries(test_linearInterpolation GTest::gtest_main) +target_link_libraries(test_linearInterpolation ArchitectureUtilities) + +add_executable(test_bilinearInterpolation test_bilinearInterpolation.cpp) +target_link_libraries(test_bilinearInterpolation GTest::gtest_main) +target_link_libraries(test_bilinearInterpolation ArchitectureUtilities) + +if(CMAKE_HOST_SYSTEM_PROCESSOR STREQUAL "arm64" AND CMAKE_GENERATOR STREQUAL "Xcode") + set(CMAKE_GTEST_DISCOVER_TESTS_DISCOVERY_MODE PRE_TEST) +endif() + +gtest_discover_tests(test_discretize) +gtest_discover_tests(test_gaussMarkov) +gtest_discover_tests(test_orbitalMotion) +gtest_discover_tests(test_saturate) +gtest_discover_tests(test_geodeticConversion) +gtest_discover_tests(test_avsEigenMRP) +gtest_discover_tests(test_avsEigenSupport) +gtest_discover_tests(test_linearInterpolation) +gtest_discover_tests(test_bilinearInterpolation) diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenMRP.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenMRP.cpp new file mode 100644 index 0000000000..9ffa69ec67 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenMRP.cpp @@ -0,0 +1,31 @@ +/* + ISC License + + Copyright (c) 2024, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include +#include "architecture/utilities/avsEigenMRP.h" +#include + + +TEST(eigenMRP, testIdentity) { + Eigen::MRPd sigma; + sigma = sigma.Identity(); + + EXPECT_TRUE(sigma.norm() < 1e-10); +} + diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenSupport.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenSupport.cpp new file mode 100644 index 0000000000..5851678230 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_avsEigenSupport.cpp @@ -0,0 +1,164 @@ +/* + ISC License + + Copyright (c) 2024, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include +#include "architecture/utilities/avsEigenSupport.h" +#include + +// Test Vector3d conversions +TEST(avsEigenSupport, Vector3dConversions) { + Eigen::Vector3d vec(1, 2, 3); + double array[3]; + + eigenVector3d2CArray(vec, array); + Eigen::Vector3d result = cArray2EigenVector3d(array); + + EXPECT_TRUE(vec.isApprox(result)); +} + +// Test Matrix3d conversions +TEST(avsEigenSupport, Matrix3dConversions) { + Eigen::Matrix3d mat; + mat << 1, 2, 3, + 4, 5, 6, + 7, 8, 9; + + double array[9]; + eigenMatrix3d2CArray(mat, array); + + Eigen::Matrix3d result = cArray2EigenMatrix3d(array); + EXPECT_TRUE(mat.isApprox(result)); +} + +// Test 2D array to Matrix3d conversion +TEST(avsEigenSupport, C2DArrayToMatrix3d) { + double array2d[3][3] = { + {1, 2, 3}, + {4, 5, 6}, + {7, 8, 9} + }; + + Eigen::Matrix3d expected; + expected << 1, 2, 3, + 4, 5, 6, + 7, 8, 9; + + Eigen::Matrix3d result = c2DArray2EigenMatrix3d(array2d); + EXPECT_TRUE(expected.isApprox(result)); +} + +// Test rotation matrices +TEST(avsEigenSupport, RotationMatrices) { + double angle = M_PI / 4.0; // 45 degrees + + // Test M1 (rotation about x-axis) + Eigen::Matrix3d m1 = eigenM1(angle); + EXPECT_NEAR(m1(1,1), cos(angle), 1e-10); + EXPECT_NEAR(m1(1,2), sin(angle), 1e-10); + EXPECT_NEAR(m1(2,1), -sin(angle), 1e-10); + + // Test M2 (rotation about y-axis) + Eigen::Matrix3d m2 = eigenM2(angle); + EXPECT_NEAR(m2(0,0), cos(angle), 1e-10); + EXPECT_NEAR(m2(0,2), -sin(angle), 1e-10); + EXPECT_NEAR(m2(2,0), sin(angle), 1e-10); + + // Test M3 (rotation about z-axis) + Eigen::Matrix3d m3 = eigenM3(angle); + EXPECT_NEAR(m3(0,0), cos(angle), 1e-10); + EXPECT_NEAR(m3(0,1), sin(angle), 1e-10); + EXPECT_NEAR(m3(1,0), -sin(angle), 1e-10); +} + +// Test tilde matrix +TEST(avsEigenSupport, TildeMatrix) { + Eigen::Vector3d vec(1, 2, 3); + Eigen::Matrix3d tilde = eigenTilde(vec); + + // Test properties of skew-symmetric matrix + EXPECT_NEAR(tilde(0,0), 0, 1e-10); + EXPECT_NEAR(tilde(1,1), 0, 1e-10); + EXPECT_NEAR(tilde(2,2), 0, 1e-10); + EXPECT_NEAR(tilde(0,1), -vec(2), 1e-10); + EXPECT_NEAR(tilde(1,2), -vec(0), 1e-10); + EXPECT_NEAR(tilde(2,0), -vec(1), 1e-10); +} + +// Test MRP conversions +TEST(avsEigenSupport, MRPConversions) { + Eigen::Vector3d mrp(0.1, 0.2, 0.3); + double array[3]; + + eigenMRPd2CArray(mrp, array); + Eigen::MRPd result = cArray2EigenMRPd(array); + + EXPECT_NEAR(mrp[0], result.x(), 1e-10); + EXPECT_NEAR(mrp[1], result.y(), 1e-10); + EXPECT_NEAR(mrp[2], result.z(), 1e-10); +} + +// Test Newton-Raphson solver +TEST(avsEigenSupport, NewtonRaphson) { + // Test with a simple function f(x) = x^2 - 4 + auto f = [](double x) { return x*x - 4; }; + auto fPrime = [](double x) { return 2*x; }; + + double result = newtonRaphsonSolve(3.0, 1e-10, f, fPrime); + EXPECT_NEAR(result, 2.0, 1e-10); +} + +TEST(avsEigenSupport, MatrixXiConversions) { + Eigen::MatrixXi mat(2, 3); + mat << 1, 2, 3, + 4, 5, 6; + + int array[6]; + eigenMatrixXi2CArray(mat, array); + + // Verify the conversion + for(int i = 0; i < 6; i++) { + EXPECT_EQ(array[i], mat(i/3, i%3)); + } +} + +TEST(avsEigenSupport, DCMtoMRP) { + // Create a simple rotation DCM (90 degrees about 3rd axis) + Eigen::Matrix3d dcm; + dcm << 0, 1, 0, + -1, 0, 0, + 0, 0, 1; + + Eigen::MRPd mrp = eigenC2MRP(dcm); + + // MRP conversion values validated with software package + // from Analytical Mechanics of Space Systems + // by Schaub and Junkins + EXPECT_NEAR(mrp.x(), 0.0, 1e-6); + EXPECT_NEAR(mrp.y(), 0.0, 1e-6); + EXPECT_NEAR(mrp.z(), 0.41421356, 1e-6); +} + +TEST(avsEigenSupport, MRPdToVector3d) { + Eigen::MRPd mrp(0.1, 0.2, 0.3); + Eigen::Vector3d vec = eigenMRPd2Vector3d(mrp); + + EXPECT_NEAR(mrp.x(), vec[0], 1e-10); + EXPECT_NEAR(mrp.y(), vec[1], 1e-10); + EXPECT_NEAR(mrp.z(), vec[2], 1e-10); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_bilinearInterpolation.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_bilinearInterpolation.cpp new file mode 100644 index 0000000000..afbeb182f3 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_bilinearInterpolation.cpp @@ -0,0 +1,49 @@ +/* + ISC License + + Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/bilinearInterpolation.hpp" +#include +#include + +std::random_device rd; +std::default_random_engine generator(rd()); +std::uniform_real_distribution valueDistribution(-10, 10); +std::uniform_real_distribution boundDistribution(0, 2); + +TEST(BilinearInterpolationTest, HandlesNormalInputs) { + double x = valueDistribution(generator); + double x1 = x - boundDistribution(generator); + double x2 = x + boundDistribution(generator); + + double y = valueDistribution(generator); + double y1 = y - boundDistribution(generator); + double y2 = y + boundDistribution(generator); + + double z11 = valueDistribution(generator); + double z12 = valueDistribution(generator); + double z21 = valueDistribution(generator); + double z22 = valueDistribution(generator); + + // Bilinearly interpolate to solve for z + double z = 1 / ((x2 - x1) * (y2 - y1)) * (z11 * (x2 - x) * (y2 - y) + z21 * (x - x1) * (y2 - y) + + z12 * (x2 - x) * (y - y1) + + z22 * (x - x1) * (y - y1)); + + EXPECT_EQ(bilinearInterpolation(x1, x2, y1, y2, z11, z12, z21, z22, x, y), z); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_discretize.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_discretize.cpp new file mode 100644 index 0000000000..ba364c6a20 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_discretize.cpp @@ -0,0 +1,99 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/discretize.h" +#include +#include + + +TEST(Discretize, testRoundToZero) { + Discretize discretizor = Discretize(3); + discretizor.setRoundDirection(TO_ZERO); + discretizor.setCarryError(false); + + Eigen::Vector3d LSBs; + LSBs << 10.0, 10.0, 10.0; + discretizor.setLSB(LSBs); + + Eigen::Vector3d expected; + expected << 0, 10.0, 10.0; + Eigen::Vector3d states; + states << 0.1, 10.1, 11.1; + states = discretizor.discretize(states); + + EXPECT_TRUE(states == expected); +} + +TEST(Discretize, testRoundFromZero) { + Discretize discretizor = Discretize(3); + discretizor.setRoundDirection(FROM_ZERO); + discretizor.setCarryError(false); + + Eigen::Vector3d LSBs; + LSBs << 10.0, 10.0, 10.0; + discretizor.setLSB(LSBs); + + Eigen::Vector3d states; + states << 0.1, 10.1, 11.1; + Eigen::Vector3d expected; + expected << 10.0, 20.0, 20.0; + states = discretizor.discretize(states); + + EXPECT_TRUE(states == expected); +} + +TEST(Discretize, testRoundNear) { + Discretize discretizor = Discretize(3); + discretizor.setRoundDirection(NEAR); + discretizor.setCarryError(false); + + Eigen::Vector3d LSBs; + LSBs << 10.0, 10.0, 10.0; + discretizor.setLSB(LSBs); + + Eigen::Vector3d states; + states << 0.1, 10.1, 15.1; + Eigen::Vector3d expected; + expected << 0, 10, 20; + states = discretizor.discretize(states); + + EXPECT_TRUE(states == expected); +} + +TEST(Discretize, testRoundToZeroCarryError) { + Discretize discretizor = Discretize(3); + discretizor.setRoundDirection(TO_ZERO); + discretizor.setCarryError(true); + + Eigen::Vector3d LSBs; + LSBs << 10.0, 10.0, 10.0; + discretizor.setLSB(LSBs); + + Eigen::Vector3d expected; + expected << 0.0, 10.0, 10.0; + Eigen::Vector3d states; + states << 0.1, 10.1, 15; + + Eigen::Vector3d output; + for(uint64_t i = 0; i < 2; i++){ + output = discretizor.discretize(states); + EXPECT_TRUE(output == expected); + expected << 0, 10, 20; + } +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_gaussMarkov.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_gaussMarkov.cpp new file mode 100644 index 0000000000..8b50672a8d --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_gaussMarkov.cpp @@ -0,0 +1,215 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/gauss_markov.h" +#include +#include + + +Eigen::Vector2d calculateSD(const Eigen::MatrixXd& dat, int64_t numPts) +{ + // Calculate mean properly + Eigen::Vector2d means = dat.rowwise().mean(); + + // Calculate variance using numerically stable algorithm + Eigen::Vector2d variance = Eigen::Vector2d::Zero(); + for(int64_t i = 0; i < numPts; i++) { + Eigen::Vector2d diff = dat.col(i) - means; + variance += diff.cwiseProduct(diff); + } + variance /= (numPts - 1); // Use n-1 for sample standard deviation + + return variance.cwiseSqrt(); +} + +TEST(GaussMarkov, stdDeviationIsExpected) { + uint64_t seedIn = 1000; + + // Use proper Gauss-Markov propagation matrix (with decay) + Eigen::Matrix2d propIn; + propIn << 0.9,0,0,0.9; // Decay factor of 0.9 + + // Set noise matrix for std=1.0 in steady state + // For an autoregressive process of order 1 with coefficient a, + // steady state variance is: sigma^2/(1-a^2) + // So for a=0.9, noise variance should be (1-0.81) = 0.19 to get std=1.0 + Eigen::Matrix2d noiseMatrix; + noiseMatrix << sqrt(0.19),0,0,sqrt(0.19); + + Eigen::Vector2d bounds; + bounds << 100.0, 100.0; // Large bounds to avoid affecting distribution + + GaussMarkov errorModel = GaussMarkov(2); + errorModel.setRNGSeed(seedIn); + errorModel.setPropMatrix(propIn); + errorModel.setNoiseMatrix(noiseMatrix); + errorModel.setUpperBounds(bounds); + + int64_t numPts = 100000; + Eigen::MatrixXd noiseOut; + noiseOut.resize(2, numPts); + + // Longer warm up period to reach steady state + for(int64_t i = 0; i < 5000; i++) { + errorModel.computeNextState(); + } + + // Collect samples + for(int64_t i = 0; i < numPts; i++) { + errorModel.computeNextState(); + noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); + } + + Eigen::Vector2d stds = calculateSD(noiseOut, numPts); + + // Test with appropriate statistical tolerances + EXPECT_NEAR(stds(0), 1.0, 0.1); // Within 10% of expected std=1.0 + EXPECT_NEAR(stds(1), 1.0, 0.1); +} + +TEST(GaussMarkov, meanIsZero) { + uint64_t seedIn = 1000; + Eigen::Matrix2d propIn; + propIn << 1,0,0,1; + + // Set noise matrix as sqrt of covariance for std=1.0 + Eigen::Matrix2d noiseMatrix; + noiseMatrix << 1.0,0,0,1.0; // Square root of unit covariance + + Eigen::Vector2d bounds; + bounds << 1e-15, 1e-15; + + GaussMarkov errorModel = GaussMarkov(2); + errorModel.setRNGSeed(seedIn); + errorModel.setPropMatrix(propIn); + errorModel.setNoiseMatrix(noiseMatrix); + errorModel.setUpperBounds(bounds); + + int64_t numPts = 100000; + Eigen::MatrixXd noiseOut; + noiseOut.resize(2, numPts); + + // Warm up + for(int64_t i = 0; i < 1000; i++) { + errorModel.computeNextState(); + } + + // Collect samples + for(int64_t i = 0; i < numPts; i++) { + errorModel.computeNextState(); + noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); + } + + Eigen::Vector2d means = noiseOut.rowwise().mean(); + + // Test with appropriate statistical tolerances + double tol = 4.0 / sqrt(numPts); // 4-sigma confidence interval + EXPECT_LT(fabs(means(0)), tol); + EXPECT_LT(fabs(means(1)), tol); +} + +TEST(GaussMarkov, boundsAreRespected) { + uint64_t seedIn = 1500; + Eigen::Matrix2d propIn; + propIn << 1,0,0,1; + + // Adjust covariance matrix for std=1.0 + Eigen::Matrix2d covar; + covar << 0.5,0,0,0.005; + + Eigen::Vector2d bounds; + bounds << 10., 0.1; + GaussMarkov errorModel = GaussMarkov(2); + errorModel.setRNGSeed(seedIn); + errorModel.setPropMatrix(propIn); + errorModel.setNoiseMatrix(covar); + errorModel.setUpperBounds(bounds); + + int64_t numPts = 1000000; + Eigen::MatrixXd noiseOut; + noiseOut.resize(2, numPts); + + Eigen::Vector2d maxOut; + maxOut.fill(0.0); + Eigen::Vector2d minOut; + minOut.fill(0.0); + + for(int64_t i = 0; i < numPts; i++){ + errorModel.computeNextState(); + noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState(); + if (noiseOut(0,i) > maxOut(0)){ + maxOut(0) = noiseOut(0,i); + } + if (noiseOut(0,i) < minOut(0)){ + minOut(0) = noiseOut(0,i); + } + if (noiseOut(1,i) > maxOut(1)){ + maxOut(1) = noiseOut(1,i); + } + if (noiseOut(1,i) < minOut(1)){ + minOut(1) = noiseOut(1,i); + } + } + + // Adjust expected bounds for std=1.0 + EXPECT_LT(fabs(11.0 - maxOut(0)) / 11.0, 5e-1); + EXPECT_LT(fabs(0.11 - maxOut(1)) / 0.11, 5e-1); + EXPECT_LT(fabs(-11.0 - minOut(0)) / -11.0, 5e-1); + EXPECT_LT(fabs(-0.11 - minOut(1)) / -0.11, 5e-1); +} + +TEST(GaussMarkov, gaussianOnlyMode) { + uint64_t seedIn = 1000; + + // Setup matrices for pure Gaussian noise + Eigen::Matrix2d propIn; + propIn << 0.0,0,0,0.0; // No state propagation + + Eigen::Matrix2d noiseMatrix; + noiseMatrix << 1.0,0,0,1.0; // Unit variance + + Eigen::Vector2d bounds; + bounds << -1.0, -1.0; // Disable bounds/random walk + + GaussMarkov errorModel = GaussMarkov(2); + errorModel.setRNGSeed(seedIn); + errorModel.setPropMatrix(propIn); + errorModel.setNoiseMatrix(noiseMatrix); + errorModel.setUpperBounds(bounds); + + // Collect samples and verify standard normal distribution properties + int64_t numPts = 100000; + Eigen::MatrixXd samples; + samples.resize(2, numPts); + + for(int64_t i = 0; i < numPts; i++) { + errorModel.computeNextState(); + samples.col(i) = errorModel.getCurrentState(); + } + + // Calculate statistics + Eigen::Vector2d means = samples.rowwise().mean(); + Eigen::Vector2d stds = calculateSD(samples, numPts); + + // Test mean is ~0 and std is ~1 + EXPECT_NEAR(means(0), 0.0, 0.1); + EXPECT_NEAR(means(1), 0.0, 0.1); + EXPECT_NEAR(stds(0), 1.0, 0.1); + EXPECT_NEAR(stds(1), 1.0, 0.1); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_geodeticConversion.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_geodeticConversion.cpp new file mode 100644 index 0000000000..2b3947020c --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_geodeticConversion.cpp @@ -0,0 +1,82 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/geodeticConversion.h" +#include "architecture/utilities/linearAlgebra.h" +#include + + +TEST(GeodeticConversion, testPCI2PCPF) { + Eigen::Vector3d pciPosition; + pciPosition << 1., 2., 3.; + + double J20002Pfix[3][3]; + m33Set( 1, 0, 0, + 0, 0, 1, + 0,-1, 0, + J20002Pfix); + Eigen::Vector3d ans = PCI2PCPF(pciPosition, J20002Pfix); + + Eigen::Vector3d expected; + expected << 1, 3, -2; + EXPECT_TRUE(ans == expected); +} + +TEST(GeodeticConversion, testPCPF2PCI) { + Eigen::Vector3d pcpfPosition; + pcpfPosition << 1., 2., 3.; + + double J20002Pfix[3][3]; + m33Set( 1, 0, 0, + 0, 0, 1, + 0,-1, 0, + J20002Pfix); + Eigen::Vector3d ans = PCPF2PCI(pcpfPosition, J20002Pfix); + + Eigen::Vector3d expected; + expected << 1, -3, 2; + EXPECT_TRUE(ans == expected); +} + +TEST(GeodeticConversion, testLLA2PCPF) { + Eigen::Vector3d llaPosition; + llaPosition << 0.6935805104270613, 1.832425562445269, 1596.668; + + Eigen::Vector3d ans = LLA2PCPF(llaPosition, 6378.1363E3, 6356.7523E3); + + Eigen::Vector3d expected; + expected << -1270640.01, 4745322.63, 4056784.62; + EXPECT_LT((expected - ans).norm(), 100.0); +} + +TEST(GeodeticConversion, testPCPF2LLA) { + +Eigen::Vector3d pcpfPosition; +pcpfPosition << -1270640.01, 4745322.63, 4056784.62; + +Eigen::Vector3d ans = PCPF2LLA(pcpfPosition, 6378.1363E3, 6356.7523E3); + +Eigen::Vector3d llaPosition; +llaPosition << 0.6935805104270613, 1.832425562445269, 1596.668; + +EXPECT_LT(std::abs(llaPosition[0] - ans[0]), 0.0001); +EXPECT_LT(std::abs(llaPosition[1] - ans[1]), 0.0001); +EXPECT_LT(std::abs(llaPosition[2] - ans[2]), 100); + +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_linearInterpolation.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_linearInterpolation.cpp new file mode 100644 index 0000000000..935d5cf99a --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_linearInterpolation.cpp @@ -0,0 +1,42 @@ +/* + ISC License + + Copyright (c) 2024, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/linearInterpolation.hpp" +#include +#include + +std::random_device rd; +std::default_random_engine generator(rd()); +std::uniform_real_distribution valueDistribution(-10, 10); +std::uniform_real_distribution boundDistribution(0, 2); + +TEST(LinearInterpolationTest, HandlesNormalInputs) { + double x = valueDistribution(generator); + double x1 = x - boundDistribution(generator); + double x2 = x + boundDistribution(generator); + + double yUnused = valueDistribution(generator); + double y1 = yUnused - boundDistribution(generator); + double y2 = yUnused + boundDistribution(generator); + + // Linearly interpolate to solve for y + double y = y1 * (x2 - x) / (x2 - x1) + y2 * (x - x1) / (x2 - x1); + + EXPECT_EQ(linearInterpolation(x1, x2, y1, y2, x), y); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_orbitalMotion.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_orbitalMotion.cpp new file mode 100644 index 0000000000..7a6a23a557 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_orbitalMotion.cpp @@ -0,0 +1,533 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/orbitalMotion.h" +#include "architecture/utilities/astroConstants.h" +#include "architecture/utilities/linearAlgebra.h" +#include "architecture/utilities/rigidBodyKinematics.h" +#include "unitTestComparators.h" +#include + + +const double orbitalEnvironmentAccuracy = 1e-10; +const double orbitalElementsAccuracy = 1e-11; + +TEST(OrbitalMotion, atmosphericDensity_200km) { + double alt = 200.0; + double result = atmosphericDensity(alt); + EXPECT_NEAR(result, 1.64100656241e-10, orbitalEnvironmentAccuracy); +} + +TEST(OrbitalMotion, atmosphericDensity_2000km) { + double alt = 2000.0; + double result = atmosphericDensity(alt); + EXPECT_NEAR(result, 2.48885731828e-15, orbitalEnvironmentAccuracy); +} + +class DebeyeLengthTests :public ::testing::TestWithParam> {}; + +TEST_P(DebeyeLengthTests, checksDebeyeLengthAtAltitude) { + auto [altitude, expected] = GetParam(); + EXPECT_NEAR(expected, debyeLength(altitude), orbitalEnvironmentAccuracy); +} + +INSTANTIATE_TEST_SUITE_P( + OrbitalMotion, + DebeyeLengthTests, + ::testing::Values( + std::make_tuple(400.0, 0.00404), + std::make_tuple(1000.0, 0.0159), + std::make_tuple(10000.0, 0.0396), + std::make_tuple(34000.0, 400.30000000000018)) + ); + +TEST(OrbitalMotion, atmosphericDrag) { + double check[3]; + double ans[3]; + double r[3] = {6200.0, 100.0, 2000.0}; + double v[3] = {1.0, 9.0, 1.0}; + double A = 2.0; + double Cd = 0.2; + double m = 50.0; + + v3Set(-2.8245395411253663e-007, -2.5420855870128297e-006, -2.8245395411253663e-007, check); + atmosphericDrag(Cd, A, m, r, v, ans); + // @TODO refactor later to be a Google Test MATCHER + EXPECT_TRUE(v3IsEqual(ans, check, 11)); +} + +TEST(OrbitalMotion, jPerturb_order_6) { + double check[3]; + double ans[3]; + double r[3] = {6200.0, 100.0, 2000.0}; + int order = 6; + + v3Set(-7.3080959003487213e-006, -1.1787251452175358e-007, -1.1381118473672282e-005, check); + jPerturb(r, order, ans); + EXPECT_TRUE(v3IsEqual(ans, check, 11)); +} + +TEST(OrbitalMotion, solarRadiationPressure) { + double check[3]; + double ans[3]; + double A = 2.0; + double m = 50.0; + double r[3] = {1.0, 0.3, -0.2}; + + v3Set(-1.9825487816e-10, -5.94764634479e-11, 3.96509756319e-11, check); + solarRad(A, m, r, ans); + EXPECT_TRUE(v3IsEqual(ans, check, 11)); +} + +TEST(OrbitalMotion, elem2rv1DEccentric) +{ + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + elements.a = 7500.0; + elements.e = 1.0; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 23.0 * D2R; + elements.alpha = 1.0 / elements.a; + elements.rPeriap = elements.a*(1.-elements.e); + elements.rApoap = elements.a*(1.+elements.e); + elem2rv(MU_EARTH, &elements, r, v); + v3Set(-148.596902253492, -457.100381534593, 352.773096481799, r2); + v3Set(-8.93065944520745, -27.4716886950712, 21.2016289595043, v3_2); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) || !v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST(OrbitalMotion, elem2rv1DHyperbolic) +{ + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + elements.a = -7500.0; + elements.e = 1.0; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 23.0 * D2R; + elements.alpha = 1.0 / elements.a; + elements.rPeriap = elements.a*(1.-elements.e); + elements.rApoap = elements.a*(1.+elements.e); + elem2rv(MU_EARTH, &elements, r, v); + v3Set(-152.641873349816, -469.543156608544, 362.375968124408, r2); + v3Set(-9.17378720883851, -28.2195763820421, 21.7788208976681, v3_2); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +class TwoDimensionHyperbolic : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = -7500.0; + elements.e = 1.4; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 23.0 * D2R; + elements.alpha = 1.0 / elements.a; + elements.rPeriap = elements.a*(1.-elements.e); + elements.rApoap = elements.a*(1.+elements.e); + + v3Set(319.013136281857, -2796.71958333493, 1404.6919948109, r2); + v3Set(15.3433051336115, -5.87012423567412, -6.05659420479213, v3_2); + } +}; + +TEST_F(TwoDimensionHyperbolic, elem2rv) +{ + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(TwoDimensionHyperbolic, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, -7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 1.4, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, 40.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.Omega, 133.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.omega, 113.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 23.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 3145.881340612725, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap,3000.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, -18000., orbitalElementsAccuracy); +} + +class TwoDimensionParabolic : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.alpha = 0.0; /* zero orbit energy, i.e. parabolic */ + elements.a = 0.0; + elements.rPeriap = 7500.; + elements.e = 1.0; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(27862.6148209797, 795.70270010667, -17554.0435142669, r2); + v3Set(3.06499561197954, 2.21344887266898, -3.14760065404514, v3_2); + } +}; + +TEST_F(TwoDimensionParabolic, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(TwoDimensionParabolic, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_NEAR(elements.alpha, 0.0, orbitalElementsAccuracy); + EXPECT_NEAR(elements.a, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 1.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, 40.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.Omega, 133.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.omega, 113.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 32940.89997480352, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap,7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 0.0, orbitalElementsAccuracy); +} + +class TwoDimensionElliptical : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.5; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; /* true anomaly */ + elements.alpha = 1.0 / elements.a; + elements.rPeriap = elements.a*(1.-elements.e); + elements.rApoap = elements.a*(1.+elements.e); + + v3Set(6538.3506963942027, 186.7227227879431, -4119.3008399778619, r2); + v3Set(1.4414106130924005, 5.588901415902356, -4.0828931566657038, v3_2); + } +}; + +TEST_F(TwoDimensionElliptical, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(TwoDimensionElliptical, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, 40.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.Omega, 133.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.omega, 113.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); +} + +class NonCircularEquitorial : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.5; + elements.i = 0.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(7634.8714161163643, 1209.2448361913848, -0, r2); + v3Set(2.5282399359829868, 6.6023861555546057, -0, v3_2); + } +}; + +TEST_F(NonCircularEquitorial, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(NonCircularEquitorial, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.Omega, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.omega, (133+113)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); +} + +class NonCircularNearEquitorial : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + double eps2 = 1e-12 * 0.5; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.5; + elements.i = eps2; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(7634.87141611636, 1209.24483619139, -3.20424723337984e-09, r2); + v3Set(2.52823993598298, 6.60238615555461, -3.17592708317508e-12, v3_2); + } +}; + +TEST_F(NonCircularNearEquitorial, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(NonCircularNearEquitorial, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); + EXPECT_NEAR(elements.i, eps2, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, wrapToPi(elements.Omega+elements.omega), (133+113-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); +} + +class NonCircularNearEquitorial180Degree : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + double eps2 = 1e-12 * 0.5; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.5; + elements.i = eps2; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(7634.87141611636, 1209.24483619139, -3.20424723337984e-09, r2); + v3Set(2.52823993598298, 6.60238615555461, -3.17592708317508e-12, v3_2); + } +}; + +TEST_F(NonCircularNearEquitorial180Degree, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(NonCircularNearEquitorial180Degree, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.e, 0.5, orbitalElementsAccuracy); + EXPECT_NEAR(elements.i, eps2, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, wrapToPi(elements.Omega+elements.omega), (133+113-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, 123.0 * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7730.041048693483, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap,3750.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 11250.0, orbitalElementsAccuracy); +} + +class CircularInclined : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.0; + elements.i = 40.0 * D2R; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(6343.7735859429586, 181.16597468085499, -3996.7130970223939, r2); + v3Set(-1.8379619466304487, 6.5499717954886121, -2.6203988553352131, v3_2); + } +}; + +TEST_F(CircularInclined, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(CircularInclined, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, 40. * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.Omega, 133. * D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, wrapToPi(elements.omega+elements.f), (113+123-360.)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); +} + +class CircularEquitorial : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.0; + elements.i = 0.0; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(7407.6625544635335, 1173.2584878017262, 0, r2); + v3Set(-1.1404354122910105, 7.2004258117414572, 0, v3_2); + } +}; + +TEST_F(CircularEquitorial, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(CircularEquitorial, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); + EXPECT_NEAR(elements.i, 0.0, orbitalElementsAccuracy); + EXPECT_NEAR(elements.Omega, 0.0, orbitalElementsAccuracy); + EXPECT_NEAR(elements.omega, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, (133+113+123-360)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); +} + +class CircularEquitorialRetrograde : public ::testing::Test { +protected: + double r[3]; + double v[3]; + double r2[3]; + double v3_2[3]; + ClassicElements elements; + + void SetUp() override { + elements.a = 7500.0; + elements.e = 0.0; + elements.i = M_PI; + elements.Omega = 133.0 * D2R; + elements.omega = 113.0 * D2R; + elements.f = 123.0 * D2R; + v3Set(-1687.13290757899, -7307.77548588926, 0.0, r2); + v3Set(-7.10333318346184, 1.63993368302803, 0.0, v3_2); + } +}; + +TEST_F(CircularEquitorialRetrograde, elem2rv) { + elem2rv(MU_EARTH, &elements, r, v); + EXPECT_TRUE(v3IsEqualRel(r, r2, orbitalElementsAccuracy) && v3IsEqualRel(v, v3_2, orbitalElementsAccuracy)); +} + +TEST_F(CircularEquitorialRetrograde, rv2elem) { + rv2elem(MU_EARTH, r2, v3_2, &elements); + EXPECT_PRED3(isEqualRel, elements.a, 7500.00, orbitalElementsAccuracy); + EXPECT_NEAR(elements.e, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.i, M_PI, orbitalElementsAccuracy); + EXPECT_NEAR(elements.Omega, 0.0, orbitalElementsAccuracy); + EXPECT_NEAR(elements.omega, 0.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.f, (-133+113+123)*D2R, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rmag, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rPeriap, 7500.0, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements.rApoap, 7500.0, orbitalElementsAccuracy); +} + +TEST(OrbitalMotion, classicElementsToMeanElements) { + ClassicElements elements; + elements.a = 1000.0; + elements.e = 0.2; + elements.i = 0.2; + elements.Omega = 0.15; + elements.omega = 0.5; + elements.f = 0.2; + double req = 300.0; + double J2 = 1e-3; + ClassicElements elements_p; + clMeanOscMap(req, J2, &elements, &elements_p, 1); + EXPECT_PRED3(isEqualRel, elements_p.a, 1000.07546442015950560744386166334152, orbitalElementsAccuracy); + EXPECT_NEAR(elements_p.e, 0.20017786852908628358882481279579, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements_p.i, 0.20000333960738947425284095515963, orbitalElementsAccuracy); + EXPECT_NEAR(elements_p.Omega, 0.15007256499303692209856819772540, orbitalElementsAccuracy); + EXPECT_NEAR(elements_p.omega, 0.50011857315729335571319325026707, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements_p.f, 0.19982315726261962174348241205735, orbitalElementsAccuracy); +} + +TEST(OrbitalMotion, classicElementsToEquinoctialElements) { + ClassicElements elements; + elements.a = 1000.0; + elements.e = 0.2; + elements.i = 0.2; + elements.Omega = 0.15; + elements.omega = 0.5; + elements.f = 0.2; + equinoctialElements elements_eq; + clElem2eqElem(&elements, &elements_eq); + + EXPECT_PRED3(isEqualRel, elements_eq.a, 1000.00000000000000000000000000000000, orbitalElementsAccuracy); + EXPECT_NEAR(elements_eq.P1, 0.12103728114720790909331071816268, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements_eq.P2, 0.15921675970981119530023306651856, orbitalElementsAccuracy); + EXPECT_NEAR(elements_eq.Q1, 0.01499382601880069713906618034116, orbitalElementsAccuracy); + EXPECT_NEAR(elements_eq.Q2, 0.09920802187229026125603326136115, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements_eq.l, 0.78093005232114087732497864635661, orbitalElementsAccuracy); + EXPECT_PRED3(isEqualRel, elements_eq.L, 0.85000000000000008881784197001252, orbitalElementsAccuracy); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_saturate.cpp b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_saturate.cpp new file mode 100644 index 0000000000..fd7b60a5dc --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/test_saturate.cpp @@ -0,0 +1,37 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/saturate.h" +#include "architecture/utilities/linearAlgebra.h" +#include + + +TEST(Saturate, testSaturate) { + Eigen::Vector3d states; + states << -555, 1.27, 5000000.; + auto saturator = Saturate(3); + Eigen::MatrixXd bounds; + bounds.resize(3,2); + bounds << -400., 0, 5, 10, -1, 5000001; + saturator.setBounds(bounds); + states = saturator.saturate(states); + Eigen::Vector3d expected; + expected << -400, 5, 5000000; + EXPECT_TRUE(states == expected); +} diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/unitTestComparators.h b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/unitTestComparators.h new file mode 100644 index 0000000000..7351751f38 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/tests/unitTestComparators.h @@ -0,0 +1,41 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef UNITTESTCOMPARATORS_H +#define UNITTESTCOMPARATORS_H + +#include + +int isEqual(double a, double b, double accuracy) +{ + if(fabs(a - b) > accuracy) { + return 0; + } + return 1; +} + +int isEqualRel(double a, double b, double accuracy) +{ + if(fabs(a - b)/fabs(a) > accuracy) { + return 0; + } + return 1; +} + +#endif //UNITTESTCOMPARATORS_H diff --git a/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/ukfUtilities.c b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/ukfUtilities.c new file mode 100644 index 0000000000..b5c19d1b67 --- /dev/null +++ b/sdk/src/bsk_sdk/include/Basilisk/architecture/utilities/ukfUtilities.c @@ -0,0 +1,381 @@ +/* + ISC License + + Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/ukfUtilities.h" +#include "architecture/utilities/linearAlgebra.h" +#include "architecture/utilities/bsk_Print.h" +#include + +void ukfQRDJustR( + double *inMat, int32_t nRow, int32_t nCol, double *destMat) +{ + int32_t i, j, k, dimi, dimj; + double qMat[UKF_MAX_DIM*UKF_MAX_DIM]; + double sourceMat[UKF_MAX_DIM*UKF_MAX_DIM]; + double sum; + + mSetZero(qMat, UKF_MAX_DIM, UKF_MAX_DIM); + mSetZero(sourceMat, UKF_MAX_DIM, UKF_MAX_DIM); + mSetZero(destMat, (size_t) nCol, (size_t) nCol); + mCopy(inMat, (size_t) nRow, (size_t) nCol, sourceMat); + + for (i = 0; i= 0; i--) + { + if (sourceMat[i*mat_dim + i] == 0) + { + BSK_PRINT(MSG_WARNING, "Can't invert zero matrix."); + return -1; + } + destMat[mat_dim*i + i] = 1.0 / sourceMat[i*mat_dim + i]; + for (j = mat_dim - 1; j >= i + 1; j--) + { + destMat[mat_dim*j + i] = 0.0; + for (k = i + 1; k <= j; k++) + { + destMat[j*mat_dim + i] -= sourceMat[mat_dim*k + i] * + destMat[j*mat_dim + k]; + } + destMat[j*mat_dim + i] *= destMat[mat_dim*i + i]; + } + } + + + return 0; +} + +int32_t ukfUInv(double *sourceMat, int32_t nRow, int32_t nCol, double *destMat) +{ + int i, j, k, mat_dim; + + mSetZero(destMat, (size_t) nRow, (size_t) nCol); + if (nRow != nCol) + { + BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfLInv."); + return -1; + } + mat_dim = nRow; + for (i = mat_dim - 1; i >= 0; i--) + { + if (sourceMat[i*mat_dim + i] == 0) + { + BSK_PRINT(MSG_WARNING, "Can't invert zero matrix."); + return -1; + } + destMat[mat_dim*i + i] = 1.0 / sourceMat[i*mat_dim + i]; + for (j = mat_dim - 1; j >= i + 1; j--) + { + destMat[mat_dim*i + j] = 0.0; + for (k = i + 1; k <= j; k++) + { + destMat[i*mat_dim + j] -= sourceMat[mat_dim*i + k] * + destMat[k*mat_dim + j]; + } + destMat[i*mat_dim + j] *= destMat[mat_dim*i + i]; + } + } + return 0; +} + +int32_t ukfLUD(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat, int32_t *indx) +{ + double vv[UKF_MAX_DIM]; + int32_t rowIndicator, i, j, k, imax; + double big, dum, sum, temp; + double TINY = 1.0E-14; + + mSetZero(destMat, (size_t) nRow, (size_t) nCol); + for(i=0; i big ? temp : big; + } + if (big < TINY) + { + BSK_PRINT(MSG_WARNING, "Singlular matrix encountered in ukfLUD LU decomposition."); + return -1; + } + vv[i] = 1.0 / big; + } + for (j = 0; j < nRow; j++) + { + for (i = 0; i < j; i++) + { + sum = destMat[i*nRow + j]; + for (k = 0; k < i; k++) + { + sum -= destMat[i*nRow + k] * destMat[k*nRow + j]; + } + destMat[i*nRow + j] = sum; + } + big = 0.0; + imax = j; + for (i = j; i < nRow; i++) + { + sum = destMat[i*nRow + j]; + for (k = 0; k < j; k++) + { + sum -= destMat[i*nRow + k] * destMat[k*nRow + j]; + } + destMat[i*nRow + j] = sum; + dum = vv[i] * fabs(sum); + if (dum >= big) + { + big = dum; + imax = i; + } + } + if (j != imax) + { + for (k = 0; k < nRow; k++) + { + dum = destMat[imax*nRow + k]; + destMat[imax*nRow + k] = destMat[j*nRow + k]; + destMat[j*nRow + k] = dum; + } + rowIndicator += 1; + vv[imax] = vv[j]; + } + indx[j] = imax; + if (destMat[j*nRow + j] == 0.0) + { + destMat[j*nRow + j] = TINY; + } + if (j != nRow-1) + { + dum = 1.0 / destMat[j*nRow + j]; + for (i = j + 1; i < nRow; i++) + { + destMat[i*nRow + j] *= dum; + } + } + } + return 0; +} + +int32_t ukfLUBckSlv(double *sourceMat, int32_t nRow, int32_t nCol, + int32_t *indx, double *bmat, double *destMat) +{ + int i, ip, j; + int ii = -1; + double sum; + + vSetZero(destMat, (size_t) nRow); + if (nRow != nCol) + { + BSK_PRINT(MSG_WARNING, "Can't get a linear solution of non-square matrix in ukfLUBckSlv."); + return -1; + } + vCopy(bmat, (size_t) nRow, destMat); + + for (i = 0; i < nRow; i++) + { + ip = indx[i]; + sum = destMat[ip]; + destMat[ip] = destMat[i]; + if (ii >= 0) + { + for (j = ii; j <= i-1; j++) + { + sum -= sourceMat[i*nRow + j] * destMat[j]; + } + } + else if ((int) sum) + { + ii = i; + } + destMat[i] = sum; + } + for (i = nRow - 1; i >= 0; i--) + { + sum = destMat[i]; + for (j = i + 1; j < nRow; j++) + { + sum -= sourceMat[i*nRow + j] * destMat[j]; + } + if (sourceMat[i*nRow + i] == 0){ + BSK_PRINT(MSG_WARNING, "Dividing by zero."); + return -1;} + destMat[i] = sum / sourceMat[i*nRow + i]; + } + return 0; +} + +int32_t ukfMatInv(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat) +{ + double LUMatrix[UKF_MAX_DIM*UKF_MAX_DIM]; + double invCol[UKF_MAX_DIM]; + double colSolve[UKF_MAX_DIM]; + int indx[UKF_MAX_DIM]; + int32_t i, j, badCall; + + mSetZero(destMat, (size_t) nRow, (size_t) nCol); + if (nRow != nCol) + { + BSK_PRINT(MSG_WARNING, "Can't invert a non-square matrix in ukfMatInv."); + return -1; + } + ukfLUD(sourceMat, nRow, nCol, LUMatrix, indx); + for (j = 0; j < nRow; j++) + { + vSetZero(colSolve, (size_t) nRow); + colSolve[j] = 1.0; + badCall = ukfLUBckSlv(LUMatrix, nRow, nCol, indx, colSolve, invCol); + for (i = 0; i < nRow; i++) + { + destMat[i*nRow + j] = invCol[i]; + } + } + return badCall; +} + +int32_t ukfCholDecomp(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat) +{ + int32_t i, j, k; + double sigma; + + mSetZero(destMat, (size_t) nRow, (size_t) nCol); + if (nRow != nCol) + { + BSK_PRINT(MSG_WARNING, "Can't get a lower-triangular inverse of non-square matrix in ukfCholDecomp."); + return -1; + } + + for (i = 0; i +#include +#include + +#define UKF_MAX_DIM 20 + + +#ifdef __cplusplus +extern "C" { +#endif + + void ukfQRDJustR( + double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); + int32_t ukfLInv( + double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); + int32_t ukfUInv( + double *sourceMat, int32_t nRow, int32_t nCol, double *destMat); + int32_t ukfLUD(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat, int32_t *indx); + int32_t ukfLUBckSlv(double *sourceMat, int32_t nRow, int32_t nCol, + int32_t *indx, double *bmat, double *destMat); + int32_t ukfMatInv(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat); + int32_t ukfCholDecomp(double *sourceMat, int32_t nRow, int32_t nCol, + double *destMat); + int32_t ukfCholDownDate(double *rMat, double *xVec, double beta, int32_t nStates, + double *rOut); + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/sdk/src/bsk_sdk/include/bsk/plugin_sdk.hpp b/sdk/src/bsk_sdk/include/bsk/plugin_sdk.hpp new file mode 100644 index 0000000000..6aeaad1249 --- /dev/null +++ b/sdk/src/bsk_sdk/include/bsk/plugin_sdk.hpp @@ -0,0 +1,114 @@ +#pragma once + +/** + * Minimal helper utilities for writing Basilisk C++ plugins. + * + * This header intentionally wraps a tiny amount of pybind11 boilerplate so that + * plugin authors can focus on the C++ implementation while Basilisk grows a + * richer native SDK. The helpers assume that the plugin exposes a default + * constructible module type with ``Reset`` and ``UpdateState`` methods. + * + * Usage: + * + * .. code-block:: cpp + * + * #include + * + * class MyCppModule { ... }; + * + * BSK_PLUGIN_PYBIND_MODULE(_my_module, MyCppModule, "MyCppModule"); + * + * This defines the required ``create_factory`` function and binds the class to + * Python so the plugin can be consumed via Basilisk's runtime registry. + */ + +#include +#include + +#include +#include + +namespace bsk::plugin { + +namespace detail { + +template +struct has_reset : std::false_type {}; + +template +struct has_reset().Reset(std::declval()))>> + : std::true_type {}; + +template +struct has_update_state : std::false_type {}; + +template +struct has_update_state< + T, std::void_t().UpdateState(std::declval(), std::declval()))>> + : std::true_type {}; + +template +struct has_reset_flag : std::false_type {}; + +template +struct has_reset_flag().reset_called())>> : std::true_type {}; + +template +struct has_update_flag : std::false_type {}; + +template +struct has_update_flag().update_called())>> : std::true_type {}; + +} // namespace detail + +template +pybind11::cpp_function make_factory() { + static_assert(std::is_default_constructible_v, + "Basilisk plugin modules must be default constructible"); + return pybind11::cpp_function([]() { return Module(); }); +} + +template +auto bind_module(pybind11::module_& m, const char* python_name) { + auto cls = pybind11::class_(m, python_name); + + if constexpr (std::is_default_constructible_v) { + cls.def(pybind11::init<>()); + } + + if constexpr (detail::has_reset::value) { + cls.def("Reset", &Module::Reset); + } + + if constexpr (detail::has_update_state::value) { + cls.def("UpdateState", &Module::UpdateState); + } + + if constexpr (detail::has_reset_flag::value) { + cls.def_property_readonly("reset_called", &Module::reset_called); + } + + if constexpr (detail::has_update_flag::value) { + cls.def_property_readonly("update_called", &Module::update_called); + } + + return cls; +} + +template +void register_basic_plugin(pybind11::module_& m, const char* class_name) { + (void)bind_module(m, class_name); + m.def("create_factory", []() { return make_factory(); }); +} + +} // namespace bsk::plugin + +/** + * Convenience macro that defines the required pybind11 module exporting the + * provided ``ModuleType`` as ``class_name``. It also generates the + * ``create_factory`` function consumed by the Basilisk runtime. + */ +#define BSK_PLUGIN_PYBIND_MODULE(module_name, ModuleType, class_name) \ + PYBIND11_MODULE(module_name, module) { \ + ::bsk::plugin::register_basic_plugin(module, class_name); \ + } diff --git a/sdk/src/bsk_sdk/include/pybind11/attr.h b/sdk/src/bsk_sdk/include/pybind11/attr.h new file mode 100644 index 0000000000..9b631fa48d --- /dev/null +++ b/sdk/src/bsk_sdk/include/pybind11/attr.h @@ -0,0 +1,722 @@ +/* + pybind11/attr.h: Infrastructure for processing custom + type and function attributes + + Copyright (c) 2016 Wenzel Jakob + + All rights reserved. Use of this source code is governed by a + BSD-style license that can be found in the LICENSE file. +*/ + +#pragma once + +#include "detail/common.h" +#include "cast.h" +#include "trampoline_self_life_support.h" + +#include + +PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) + +/// \addtogroup annotations +/// @{ + +/// Annotation for methods +struct is_method { + handle class_; + explicit is_method(const handle &c) : class_(c) {} +}; + +/// Annotation for setters +struct is_setter {}; + +/// Annotation for operators +struct is_operator {}; + +/// Annotation for classes that cannot be subclassed +struct is_final {}; + +/// Annotation for parent scope +struct scope { + handle value; + explicit scope(const handle &s) : value(s) {} +}; + +/// Annotation for documentation +struct doc { + const char *value; + explicit doc(const char *value) : value(value) {} +}; + +/// Annotation for function names +struct name { + const char *value; + explicit name(const char *value) : value(value) {} +}; + +/// Annotation indicating that a function is an overload associated with a given "sibling" +struct sibling { + handle value; + explicit sibling(const handle &value) : value(value.ptr()) {} +}; + +/// Annotation indicating that a class derives from another given type +template +struct base { + + PYBIND11_DEPRECATED( + "base() was deprecated in favor of specifying 'T' as a template argument to class_") + base() = default; +}; + +/// Keep patient alive while nurse lives +template +struct keep_alive {}; + +/// Annotation indicating that a class is involved in a multiple inheritance relationship +struct multiple_inheritance {}; + +/// Annotation which enables dynamic attributes, i.e. adds `__dict__` to a class +struct dynamic_attr {}; + +/// Annotation which enables the buffer protocol for a type +struct buffer_protocol {}; + +/// Annotation which enables releasing the GIL before calling the C++ destructor of wrapped +/// instances (pybind/pybind11#1446). +struct release_gil_before_calling_cpp_dtor {}; + +/// Annotation which requests that a special metaclass is created for a type +struct metaclass { + handle value; + + PYBIND11_DEPRECATED("py::metaclass() is no longer required. It's turned on by default now.") + metaclass() = default; + + /// Override pybind11's default metaclass + explicit metaclass(handle value) : value(value) {} +}; + +/// Specifies a custom callback with signature `void (PyHeapTypeObject*)` that +/// may be used to customize the Python type. +/// +/// The callback is invoked immediately before `PyType_Ready`. +/// +/// Note: This is an advanced interface, and uses of it may require changes to +/// work with later versions of pybind11. You may wish to consult the +/// implementation of `make_new_python_type` in `detail/classes.h` to understand +/// the context in which the callback will be run. +struct custom_type_setup { + using callback = std::function; + + explicit custom_type_setup(callback value) : value(std::move(value)) {} + + callback value; +}; + +/// Annotation that marks a class as local to the module: +struct module_local { + const bool value; + constexpr explicit module_local(bool v = true) : value(v) {} +}; + +/// Annotation to mark enums as an arithmetic type +struct arithmetic {}; + +/// Mark a function for addition at the beginning of the existing overload chain instead of the end +struct prepend {}; + +/** \rst + A call policy which places one or more guard variables (``Ts...``) around the function call. + + For example, this definition: + + .. code-block:: cpp + + m.def("foo", foo, py::call_guard()); + + is equivalent to the following pseudocode: + + .. code-block:: cpp + + m.def("foo", [](args...) { + T scope_guard; + return foo(args...); // forwarded arguments + }); + \endrst */ +template +struct call_guard; + +template <> +struct call_guard<> { + using type = detail::void_type; +}; + +template +struct call_guard { + static_assert(std::is_default_constructible::value, + "The guard type must be default constructible"); + + using type = T; +}; + +template +struct call_guard { + struct type { + T guard{}; // Compose multiple guard types with left-to-right default-constructor order + typename call_guard::type next{}; + }; +}; + +/// @} annotations + +PYBIND11_NAMESPACE_BEGIN(detail) +/* Forward declarations */ +enum op_id : int; +enum op_type : int; +struct undefined_t; +template +struct op_; +void keep_alive_impl(size_t Nurse, size_t Patient, function_call &call, handle ret); + +/// Internal data structure which holds metadata about a keyword argument +struct argument_record { + const char *name; ///< Argument name + const char *descr; ///< Human-readable version of the argument value + handle value; ///< Associated Python object + bool convert : 1; ///< True if the argument is allowed to convert when loading + bool none : 1; ///< True if None is allowed when loading + + argument_record(const char *name, const char *descr, handle value, bool convert, bool none) + : name(name), descr(descr), value(value), convert(convert), none(none) {} +}; + +/// Internal data structure which holds metadata about a bound function (signature, overloads, +/// etc.) +#define PYBIND11_DETAIL_FUNCTION_RECORD_ABI_ID "v1" // PLEASE UPDATE if the struct is changed. +struct function_record { + function_record() + : is_constructor(false), is_new_style_constructor(false), is_stateless(false), + is_operator(false), is_method(false), is_setter(false), has_args(false), + has_kwargs(false), prepend(false) {} + + /// Function name + char *name = nullptr; /* why no C++ strings? They generate heavier code.. */ + + // User-specified documentation string + char *doc = nullptr; + + /// Human-readable version of the function signature + char *signature = nullptr; + + /// List of registered keyword arguments + std::vector args; + + /// Pointer to lambda function which converts arguments and performs the actual call + handle (*impl)(function_call &) = nullptr; + + /// Storage for the wrapped function pointer and captured data, if any + void *data[3] = {}; + + /// Pointer to custom destructor for 'data' (if needed) + void (*free_data)(function_record *ptr) = nullptr; + + /// Return value policy associated with this function + return_value_policy policy = return_value_policy::automatic; + + /// True if name == '__init__' + bool is_constructor : 1; + + /// True if this is a new-style `__init__` defined in `detail/init.h` + bool is_new_style_constructor : 1; + + /// True if this is a stateless function pointer + bool is_stateless : 1; + + /// True if this is an operator (__add__), etc. + bool is_operator : 1; + + /// True if this is a method + bool is_method : 1; + + /// True if this is a setter + bool is_setter : 1; + + /// True if the function has a '*args' argument + bool has_args : 1; + + /// True if the function has a '**kwargs' argument + bool has_kwargs : 1; + + /// True if this function is to be inserted at the beginning of the overload resolution chain + bool prepend : 1; + + /// Number of arguments (including py::args and/or py::kwargs, if present) + std::uint16_t nargs; + + /// Number of leading positional arguments, which are terminated by a py::args or py::kwargs + /// argument or by a py::kw_only annotation. + std::uint16_t nargs_pos = 0; + + /// Number of leading arguments (counted in `nargs`) that are positional-only + std::uint16_t nargs_pos_only = 0; + + /// Python method object + PyMethodDef *def = nullptr; + + /// Python handle to the parent scope (a class or a module) + handle scope; + + /// Python handle to the sibling function representing an overload chain + handle sibling; + + /// Pointer to next overload + function_record *next = nullptr; +}; +// The main purpose of this macro is to make it easy to pin-point the critically related code +// sections. +#define PYBIND11_ENSURE_PRECONDITION_FOR_FUNCTIONAL_H_PERFORMANCE_OPTIMIZATIONS(...) \ + static_assert( \ + __VA_ARGS__, \ + "Violation of precondition for pybind11/functional.h performance optimizations!") + +/// Special data structure which (temporarily) holds metadata about a bound class +struct type_record { + PYBIND11_NOINLINE type_record() + : multiple_inheritance(false), dynamic_attr(false), buffer_protocol(false), + module_local(false), is_final(false), release_gil_before_calling_cpp_dtor(false) {} + + /// Handle to the parent scope + handle scope; + + /// Name of the class + const char *name = nullptr; + + // Pointer to RTTI type_info data structure + const std::type_info *type = nullptr; + + /// How large is the underlying C++ type? + size_t type_size = 0; + + /// What is the alignment of the underlying C++ type? + size_t type_align = 0; + + /// How large is the type's holder? + size_t holder_size = 0; + + /// The global operator new can be overridden with a class-specific variant + void *(*operator_new)(size_t) = nullptr; + + /// Function pointer to class_<..>::init_instance + void (*init_instance)(instance *, const void *) = nullptr; + + /// Function pointer to class_<..>::dealloc + void (*dealloc)(detail::value_and_holder &) = nullptr; + + /// Function pointer for casting alias class (aka trampoline) pointer to + /// trampoline_self_life_support pointer. Sidesteps cross-DSO RTTI issues + /// on platforms like macOS (see PR #5728 for details). + get_trampoline_self_life_support_fn get_trampoline_self_life_support + = [](void *) -> trampoline_self_life_support * { return nullptr; }; + + /// List of base classes of the newly created type + list bases; + + /// Optional docstring + const char *doc = nullptr; + + /// Custom metaclass (optional) + handle metaclass; + + /// Custom type setup. + custom_type_setup::callback custom_type_setup_callback; + + /// Multiple inheritance marker + bool multiple_inheritance : 1; + + /// Does the class manage a __dict__? + bool dynamic_attr : 1; + + /// Does the class implement the buffer protocol? + bool buffer_protocol : 1; + + /// Is the class definition local to the module shared object? + bool module_local : 1; + + /// Is the class inheritable from python classes? + bool is_final : 1; + + /// Solves pybind/pybind11#1446 + bool release_gil_before_calling_cpp_dtor : 1; + + holder_enum_t holder_enum_v = holder_enum_t::undefined; + + PYBIND11_NOINLINE void add_base(const std::type_info &base, void *(*caster)(void *) ) { + auto *base_info = detail::get_type_info(base, false); + if (!base_info) { + std::string tname(base.name()); + detail::clean_type_id(tname); + pybind11_fail("generic_type: type \"" + std::string(name) + + "\" referenced unknown base type \"" + tname + "\""); + } + + // SMART_HOLDER_BAKEIN_FOLLOW_ON: Refine holder compatibility checks. + bool this_has_unique_ptr_holder = (holder_enum_v == holder_enum_t::std_unique_ptr); + bool base_has_unique_ptr_holder + = (base_info->holder_enum_v == holder_enum_t::std_unique_ptr); + if (this_has_unique_ptr_holder != base_has_unique_ptr_holder) { + std::string tname(base.name()); + detail::clean_type_id(tname); + pybind11_fail("generic_type: type \"" + std::string(name) + "\" " + + (this_has_unique_ptr_holder ? "does not have" : "has") + + " a non-default holder type while its base \"" + tname + "\" " + + (base_has_unique_ptr_holder ? "does not" : "does")); + } + + bases.append((PyObject *) base_info->type); + +#ifdef PYBIND11_BACKWARD_COMPATIBILITY_TP_DICTOFFSET + dynamic_attr |= base_info->type->tp_dictoffset != 0; +#else + dynamic_attr |= (base_info->type->tp_flags & Py_TPFLAGS_MANAGED_DICT) != 0; +#endif + + if (caster) { + base_info->implicit_casts.emplace_back(type, caster); + } + } +}; + +inline function_call::function_call(const function_record &f, handle p) : func(f), parent(p) { + args.reserve(f.nargs); + args_convert.reserve(f.nargs); +} + +/// Tag for a new-style `__init__` defined in `detail/init.h` +struct is_new_style_constructor {}; + +/** + * Partial template specializations to process custom attributes provided to + * cpp_function_ and class_. These are either used to initialize the respective + * fields in the type_record and function_record data structures or executed at + * runtime to deal with custom call policies (e.g. keep_alive). + */ +template +struct process_attribute; + +template +struct process_attribute_default { + /// Default implementation: do nothing + static void init(const T &, function_record *) {} + static void init(const T &, type_record *) {} + static void precall(function_call &) {} + static void postcall(function_call &, handle) {} +}; + +/// Process an attribute specifying the function's name +template <> +struct process_attribute : process_attribute_default { + static void init(const name &n, function_record *r) { r->name = const_cast(n.value); } +}; + +/// Process an attribute specifying the function's docstring +template <> +struct process_attribute : process_attribute_default { + static void init(const doc &n, function_record *r) { r->doc = const_cast(n.value); } +}; + +/// Process an attribute specifying the function's docstring (provided as a C-style string) +template <> +struct process_attribute : process_attribute_default { + static void init(const char *d, function_record *r) { r->doc = const_cast(d); } + static void init(const char *d, type_record *r) { r->doc = d; } +}; +template <> +struct process_attribute : process_attribute {}; + +/// Process an attribute indicating the function's return value policy +template <> +struct process_attribute : process_attribute_default { + static void init(const return_value_policy &p, function_record *r) { r->policy = p; } +}; + +/// Process an attribute which indicates that this is an overloaded function associated with a +/// given sibling +template <> +struct process_attribute : process_attribute_default { + static void init(const sibling &s, function_record *r) { r->sibling = s.value; } +}; + +/// Process an attribute which indicates that this function is a method +template <> +struct process_attribute : process_attribute_default { + static void init(const is_method &s, function_record *r) { + r->is_method = true; + r->scope = s.class_; + } +}; + +/// Process an attribute which indicates that this function is a setter +template <> +struct process_attribute : process_attribute_default { + static void init(const is_setter &, function_record *r) { r->is_setter = true; } +}; + +/// Process an attribute which indicates the parent scope of a method +template <> +struct process_attribute : process_attribute_default { + static void init(const scope &s, function_record *r) { r->scope = s.value; } +}; + +/// Process an attribute which indicates that this function is an operator +template <> +struct process_attribute : process_attribute_default { + static void init(const is_operator &, function_record *r) { r->is_operator = true; } +}; + +template <> +struct process_attribute + : process_attribute_default { + static void init(const is_new_style_constructor &, function_record *r) { + r->is_new_style_constructor = true; + } +}; + +inline void check_kw_only_arg(const arg &a, function_record *r) { + if (r->args.size() > r->nargs_pos && (!a.name || a.name[0] == '\0')) { + pybind11_fail("arg(): cannot specify an unnamed argument after a kw_only() annotation or " + "args() argument"); + } +} + +inline void append_self_arg_if_needed(function_record *r) { + if (r->is_method && r->args.empty()) { + r->args.emplace_back("self", nullptr, handle(), /*convert=*/true, /*none=*/false); + } +} + +/// Process a keyword argument attribute (*without* a default value) +template <> +struct process_attribute : process_attribute_default { + static void init(const arg &a, function_record *r) { + append_self_arg_if_needed(r); + r->args.emplace_back(a.name, nullptr, handle(), !a.flag_noconvert, a.flag_none); + + check_kw_only_arg(a, r); + } +}; + +/// Process a keyword argument attribute (*with* a default value) +template <> +struct process_attribute : process_attribute_default { + static void init(const arg_v &a, function_record *r) { + if (r->is_method && r->args.empty()) { + r->args.emplace_back( + "self", /*descr=*/nullptr, /*parent=*/handle(), /*convert=*/true, /*none=*/false); + } + + if (!a.value) { +#if defined(PYBIND11_DETAILED_ERROR_MESSAGES) + std::string descr("'"); + if (a.name) { + descr += std::string(a.name) + ": "; + } + descr += a.type + "'"; + if (r->is_method) { + if (r->name) { + descr += " in method '" + (std::string) str(r->scope) + "." + + (std::string) r->name + "'"; + } else { + descr += " in method of '" + (std::string) str(r->scope) + "'"; + } + } else if (r->name) { + descr += " in function '" + (std::string) r->name + "'"; + } + pybind11_fail("arg(): could not convert default argument " + descr + + " into a Python object (type not registered yet?)"); +#else + pybind11_fail("arg(): could not convert default argument " + "into a Python object (type not registered yet?). " + "#define PYBIND11_DETAILED_ERROR_MESSAGES or compile in debug mode for " + "more information."); +#endif + } + r->args.emplace_back(a.name, a.descr, a.value.inc_ref(), !a.flag_noconvert, a.flag_none); + + check_kw_only_arg(a, r); + } +}; + +/// Process a keyword-only-arguments-follow pseudo argument +template <> +struct process_attribute : process_attribute_default { + static void init(const kw_only &, function_record *r) { + append_self_arg_if_needed(r); + if (r->has_args && r->nargs_pos != static_cast(r->args.size())) { + pybind11_fail("Mismatched args() and kw_only(): they must occur at the same relative " + "argument location (or omit kw_only() entirely)"); + } + r->nargs_pos = static_cast(r->args.size()); + } +}; + +/// Process a positional-only-argument maker +template <> +struct process_attribute : process_attribute_default { + static void init(const pos_only &, function_record *r) { + append_self_arg_if_needed(r); + r->nargs_pos_only = static_cast(r->args.size()); + if (r->nargs_pos_only > r->nargs_pos) { + pybind11_fail("pos_only(): cannot follow a py::args() argument"); + } + // It also can't follow a kw_only, but a static_assert in pybind11.h checks that + } +}; + +/// Process a parent class attribute. Single inheritance only (class_ itself already guarantees +/// that) +template +struct process_attribute::value>> + : process_attribute_default { + static void init(const handle &h, type_record *r) { r->bases.append(h); } +}; + +/// Process a parent class attribute (deprecated, does not support multiple inheritance) +template +struct process_attribute> : process_attribute_default> { + static void init(const base &, type_record *r) { r->add_base(typeid(T), nullptr); } +}; + +/// Process a multiple inheritance attribute +template <> +struct process_attribute : process_attribute_default { + static void init(const multiple_inheritance &, type_record *r) { + r->multiple_inheritance = true; + } +}; + +template <> +struct process_attribute : process_attribute_default { + static void init(const dynamic_attr &, type_record *r) { r->dynamic_attr = true; } +}; + +template <> +struct process_attribute { + static void init(const custom_type_setup &value, type_record *r) { + r->custom_type_setup_callback = value.value; + } +}; + +template <> +struct process_attribute : process_attribute_default { + static void init(const is_final &, type_record *r) { r->is_final = true; } +}; + +template <> +struct process_attribute : process_attribute_default { + static void init(const buffer_protocol &, type_record *r) { r->buffer_protocol = true; } +}; + +template <> +struct process_attribute : process_attribute_default { + static void init(const metaclass &m, type_record *r) { r->metaclass = m.value; } +}; + +template <> +struct process_attribute : process_attribute_default { + static void init(const module_local &l, type_record *r) { r->module_local = l.value; } +}; + +template <> +struct process_attribute + : process_attribute_default { + static void init(const release_gil_before_calling_cpp_dtor &, type_record *r) { + r->release_gil_before_calling_cpp_dtor = true; + } +}; + +/// Process a 'prepend' attribute, putting this at the beginning of the overload chain +template <> +struct process_attribute : process_attribute_default { + static void init(const prepend &, function_record *r) { r->prepend = true; } +}; + +/// Process an 'arithmetic' attribute for enums (does nothing here) +template <> +struct process_attribute : process_attribute_default {}; + +template +struct process_attribute> : process_attribute_default> {}; + +/** + * Process a keep_alive call policy -- invokes keep_alive_impl during the + * pre-call handler if both Nurse, Patient != 0 and use the post-call handler + * otherwise + */ +template +struct process_attribute> + : public process_attribute_default> { + template = 0> + static void precall(function_call &call) { + keep_alive_impl(Nurse, Patient, call, handle()); + } + template = 0> + static void postcall(function_call &, handle) {} + template = 0> + static void precall(function_call &) {} + template = 0> + static void postcall(function_call &call, handle ret) { + keep_alive_impl(Nurse, Patient, call, ret); + } +}; + +/// Recursively iterate over variadic template arguments +template +struct process_attributes { + static void init(const Args &...args, function_record *r) { + PYBIND11_WORKAROUND_INCORRECT_MSVC_C4100(r); + PYBIND11_WORKAROUND_INCORRECT_GCC_UNUSED_BUT_SET_PARAMETER(r); + using expander = int[]; + (void) expander{ + 0, ((void) process_attribute::type>::init(args, r), 0)...}; + } + static void init(const Args &...args, type_record *r) { + PYBIND11_WORKAROUND_INCORRECT_MSVC_C4100(r); + PYBIND11_WORKAROUND_INCORRECT_GCC_UNUSED_BUT_SET_PARAMETER(r); + using expander = int[]; + (void) expander{0, + (process_attribute::type>::init(args, r), 0)...}; + } + static void precall(function_call &call) { + PYBIND11_WORKAROUND_INCORRECT_MSVC_C4100(call); + using expander = int[]; + (void) expander{0, + (process_attribute::type>::precall(call), 0)...}; + } + static void postcall(function_call &call, handle fn_ret) { + PYBIND11_WORKAROUND_INCORRECT_MSVC_C4100(call, fn_ret); + PYBIND11_WORKAROUND_INCORRECT_GCC_UNUSED_BUT_SET_PARAMETER(fn_ret); + using expander = int[]; + (void) expander{ + 0, (process_attribute::type>::postcall(call, fn_ret), 0)...}; + } +}; + +template +using is_call_guard = is_instantiation; + +/// Extract the ``type`` from the first `call_guard` in `Extras...` (or `void_type` if none found) +template +using extract_guard_t = typename exactly_one_t, Extra...>::type; + +/// Check the number of named arguments at compile time +template ::value...), + size_t self = constexpr_sum(std::is_same::value...)> +constexpr bool expected_num_args(size_t nargs, bool has_args, bool has_kwargs) { + PYBIND11_WORKAROUND_INCORRECT_MSVC_C4100(nargs, has_args, has_kwargs); + return named == 0 || (self + named + size_t(has_args) + size_t(has_kwargs)) == nargs; +} + +PYBIND11_NAMESPACE_END(detail) +PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/sdk/src/bsk_sdk/include/pybind11/buffer_info.h b/sdk/src/bsk_sdk/include/pybind11/buffer_info.h new file mode 100644 index 0000000000..75aec0ba30 --- /dev/null +++ b/sdk/src/bsk_sdk/include/pybind11/buffer_info.h @@ -0,0 +1,208 @@ +/* + pybind11/buffer_info.h: Python buffer object interface + + Copyright (c) 2016 Wenzel Jakob + + All rights reserved. Use of this source code is governed by a + BSD-style license that can be found in the LICENSE file. +*/ + +#pragma once + +#include "detail/common.h" + +PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) + +PYBIND11_NAMESPACE_BEGIN(detail) + +// Default, C-style strides +inline std::vector c_strides(const std::vector &shape, ssize_t itemsize) { + auto ndim = shape.size(); + std::vector strides(ndim, itemsize); + if (ndim > 0) { + for (size_t i = ndim - 1; i > 0; --i) { + strides[i - 1] = strides[i] * shape[i]; + } + } + return strides; +} + +// F-style strides; default when constructing an array_t with `ExtraFlags & f_style` +inline std::vector f_strides(const std::vector &shape, ssize_t itemsize) { + auto ndim = shape.size(); + std::vector strides(ndim, itemsize); + for (size_t i = 1; i < ndim; ++i) { + strides[i] = strides[i - 1] * shape[i - 1]; + } + return strides; +} + +template +struct compare_buffer_info; + +PYBIND11_NAMESPACE_END(detail) + +/// Information record describing a Python buffer object +struct buffer_info { + void *ptr = nullptr; // Pointer to the underlying storage + ssize_t itemsize = 0; // Size of individual items in bytes + ssize_t size = 0; // Total number of entries + std::string format; // For homogeneous buffers, this should be set to + // format_descriptor::format() + ssize_t ndim = 0; // Number of dimensions + std::vector shape; // Shape of the tensor (1 entry per dimension) + std::vector strides; // Number of bytes between adjacent entries + // (for each per dimension) + bool readonly = false; // flag to indicate if the underlying storage may be written to + + buffer_info() = default; + + buffer_info(void *ptr, + ssize_t itemsize, + const std::string &format, + ssize_t ndim, + detail::any_container shape_in, + detail::any_container strides_in, + bool readonly = false) + : ptr(ptr), itemsize(itemsize), size(1), format(format), ndim(ndim), + shape(std::move(shape_in)), strides(std::move(strides_in)), readonly(readonly) { + if (ndim != (ssize_t) shape.size() || ndim != (ssize_t) strides.size()) { + pybind11_fail("buffer_info: ndim doesn't match shape and/or strides length"); + } + for (size_t i = 0; i < (size_t) ndim; ++i) { + size *= shape[i]; + } + } + + template + buffer_info(T *ptr, + detail::any_container shape_in, + detail::any_container strides_in, + bool readonly = false) + : buffer_info(private_ctr_tag(), + ptr, + sizeof(T), + format_descriptor::format(), + static_cast(shape_in->size()), + std::move(shape_in), + std::move(strides_in), + readonly) {} + + buffer_info(void *ptr, + ssize_t itemsize, + const std::string &format, + ssize_t size, + bool readonly = false) + : buffer_info(ptr, itemsize, format, 1, {size}, {itemsize}, readonly) {} + + template + buffer_info(T *ptr, ssize_t size, bool readonly = false) + : buffer_info(ptr, sizeof(T), format_descriptor::format(), size, readonly) {} + + template + buffer_info(const T *ptr, ssize_t size, bool readonly = true) + : buffer_info( + const_cast(ptr), sizeof(T), format_descriptor::format(), size, readonly) {} + + explicit buffer_info(Py_buffer *view, bool ownview = true) + : buffer_info( + view->buf, + view->itemsize, + view->format, + view->ndim, + {view->shape, view->shape + view->ndim}, + /* Though buffer::request() requests PyBUF_STRIDES, ctypes objects + * ignore this flag and return a view with NULL strides. + * When strides are NULL, build them manually. */ + view->strides + ? std::vector(view->strides, view->strides + view->ndim) + : detail::c_strides({view->shape, view->shape + view->ndim}, view->itemsize), + (view->readonly != 0)) { + // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) + this->m_view = view; + // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) + this->ownview = ownview; + } + + buffer_info(const buffer_info &) = delete; + buffer_info &operator=(const buffer_info &) = delete; + + buffer_info(buffer_info &&other) noexcept { (*this) = std::move(other); } + + buffer_info &operator=(buffer_info &&rhs) noexcept { + ptr = rhs.ptr; + itemsize = rhs.itemsize; + size = rhs.size; + format = std::move(rhs.format); + ndim = rhs.ndim; + shape = std::move(rhs.shape); + strides = std::move(rhs.strides); + std::swap(m_view, rhs.m_view); + std::swap(ownview, rhs.ownview); + readonly = rhs.readonly; + return *this; + } + + ~buffer_info() { + if (m_view && ownview) { + PyBuffer_Release(m_view); + delete m_view; + } + } + + Py_buffer *view() const { return m_view; } + Py_buffer *&view() { return m_view; } + + /* True if the buffer item type is equivalent to `T`. */ + // To define "equivalent" by example: + // `buffer_info::item_type_is_equivalent_to(b)` and + // `buffer_info::item_type_is_equivalent_to(b)` may both be true + // on some platforms, but `int` and `unsigned` will never be equivalent. + // For the ground truth, please inspect `detail::compare_buffer_info<>`. + template + bool item_type_is_equivalent_to() const { + return detail::compare_buffer_info::compare(*this); + } + +private: + struct private_ctr_tag {}; + + buffer_info(private_ctr_tag, + void *ptr, + ssize_t itemsize, + const std::string &format, + ssize_t ndim, + detail::any_container &&shape_in, + detail::any_container &&strides_in, + bool readonly) + : buffer_info( + ptr, itemsize, format, ndim, std::move(shape_in), std::move(strides_in), readonly) {} + + Py_buffer *m_view = nullptr; + bool ownview = false; +}; + +PYBIND11_NAMESPACE_BEGIN(detail) + +template +struct compare_buffer_info { + static bool compare(const buffer_info &b) { + // NOLINTNEXTLINE(bugprone-sizeof-expression) Needed for `PyObject *` + return b.format == format_descriptor::format() && b.itemsize == (ssize_t) sizeof(T); + } +}; + +template +struct compare_buffer_info::value>> { + static bool compare(const buffer_info &b) { + return (size_t) b.itemsize == sizeof(T) + && (b.format == format_descriptor::value + || ((sizeof(T) == sizeof(long)) + && b.format == (std::is_unsigned::value ? "L" : "l")) + || ((sizeof(T) == sizeof(size_t)) + && b.format == (std::is_unsigned::value ? "N" : "n"))); + } +}; + +PYBIND11_NAMESPACE_END(detail) +PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/sdk/src/bsk_sdk/include/pybind11/cast.h b/sdk/src/bsk_sdk/include/pybind11/cast.h new file mode 100644 index 0000000000..c635791fee --- /dev/null +++ b/sdk/src/bsk_sdk/include/pybind11/cast.h @@ -0,0 +1,2361 @@ +/* + pybind11/cast.h: Partial template specializations to cast between + C++ and Python types + + Copyright (c) 2016 Wenzel Jakob + + All rights reserved. Use of this source code is governed by a + BSD-style license that can be found in the LICENSE file. +*/ + +#pragma once + +#include "detail/common.h" +#include "detail/descr.h" +#include "detail/native_enum_data.h" +#include "detail/type_caster_base.h" +#include "detail/typeid.h" +#include "pytypes.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) + +PYBIND11_WARNING_DISABLE_MSVC(4127) + +PYBIND11_NAMESPACE_BEGIN(detail) + +template +class type_caster : public type_caster_base {}; +template +using make_caster = type_caster>; + +// Shortcut for calling a caster's `cast_op_type` cast operator for casting a type_caster to a T +template +typename make_caster::template cast_op_type cast_op(make_caster &caster) { + using result_t = typename make_caster::template cast_op_type; // See PR #4893 + return caster.operator result_t(); +} +template +typename make_caster::template cast_op_type::type> +cast_op(make_caster &&caster) { + using result_t = typename make_caster::template cast_op_type< + typename std::add_rvalue_reference::type>; // See PR #4893 + return std::move(caster).operator result_t(); +} + +template +class type_caster_enum_type { +private: + using Underlying = typename std::underlying_type::type; + +public: + static constexpr auto name = const_name(); + + template + static handle cast(SrcType &&src, return_value_policy, handle parent) { + handle native_enum + = global_internals_native_enum_type_map_get_item(std::type_index(typeid(EnumType))); + if (native_enum) { + return native_enum(static_cast(src)).release(); + } + return type_caster_base::cast( + std::forward(src), + // Fixes https://github.com/pybind/pybind11/pull/3643#issuecomment-1022987818: + return_value_policy::copy, + parent); + } + + template + static handle cast(SrcType *src, return_value_policy policy, handle parent) { + return cast(*src, policy, parent); + } + + bool load(handle src, bool convert) { + handle native_enum + = global_internals_native_enum_type_map_get_item(std::type_index(typeid(EnumType))); + if (native_enum) { + if (!isinstance(src, native_enum)) { + return false; + } + type_caster underlying_caster; + if (!underlying_caster.load(src.attr("value"), convert)) { + pybind11_fail("native_enum internal consistency failure."); + } + value = static_cast(static_cast(underlying_caster)); + return true; + } + if (!pybind11_enum_) { + pybind11_enum_.reset(new type_caster_base()); + } + return pybind11_enum_->load(src, convert); + } + + template + using cast_op_type = detail::cast_op_type; + + // NOLINTNEXTLINE(google-explicit-constructor) + operator EnumType *() { + if (!pybind11_enum_) { + return &value; + } + return pybind11_enum_->operator EnumType *(); + } + + // NOLINTNEXTLINE(google-explicit-constructor) + operator EnumType &() { + if (!pybind11_enum_) { + return value; + } + return pybind11_enum_->operator EnumType &(); + } + +private: + std::unique_ptr> pybind11_enum_; + EnumType value; +}; + +template +struct type_caster_enum_type_enabled : std::true_type {}; + +template +struct type_uses_type_caster_enum_type { + static constexpr bool value + = std::is_enum::value && type_caster_enum_type_enabled::value; +}; + +template +class type_caster::value>> + : public type_caster_enum_type {}; + +template ::value, int> = 0> +bool isinstance_native_enum_impl(handle obj, const std::type_info &tp) { + handle native_enum = global_internals_native_enum_type_map_get_item(tp); + if (!native_enum) { + return false; + } + return isinstance(obj, native_enum); +} + +template ::value, int> = 0> +bool isinstance_native_enum_impl(handle, const std::type_info &) { + return false; +} + +template +bool isinstance_native_enum(handle obj, const std::type_info &tp) { + return isinstance_native_enum_impl>(obj, tp); +} + +template +class type_caster> { +private: + using caster_t = make_caster; + caster_t subcaster; + using reference_t = type &; + using subcaster_cast_op_type = typename caster_t::template cast_op_type; + + static_assert( + std::is_same::type &, subcaster_cast_op_type>::value + || std::is_same::value, + "std::reference_wrapper caster requires T to have a caster with an " + "`operator T &()` or `operator const T &()`"); + +public: + bool load(handle src, bool convert) { return subcaster.load(src, convert); } + static constexpr auto name = caster_t::name; + static handle + cast(const std::reference_wrapper &src, return_value_policy policy, handle parent) { + // It is definitely wrong to take ownership of this pointer, so mask that rvp + if (policy == return_value_policy::take_ownership + || policy == return_value_policy::automatic) { + policy = return_value_policy::automatic_reference; + } + return caster_t::cast(&src.get(), policy, parent); + } + template + using cast_op_type = std::reference_wrapper; + explicit operator std::reference_wrapper() { return cast_op(subcaster); } +}; + +#define PYBIND11_TYPE_CASTER(type, py_name) \ +protected: \ + type value; \ + \ +public: \ + static constexpr auto name = py_name; \ + template >::value, \ + int> \ + = 0> \ + static ::pybind11::handle cast( \ + T_ *src, ::pybind11::return_value_policy policy, ::pybind11::handle parent) { \ + if (!src) \ + return ::pybind11::none().release(); \ + if (policy == ::pybind11::return_value_policy::take_ownership) { \ + auto h = cast(std::move(*src), policy, parent); \ + delete src; \ + return h; \ + } \ + return cast(*src, policy, parent); \ + } \ + operator type *() { return &value; } /* NOLINT(bugprone-macro-parentheses) */ \ + operator type &() { return value; } /* NOLINT(bugprone-macro-parentheses) */ \ + operator type &&() && { return std::move(value); } /* NOLINT(bugprone-macro-parentheses) */ \ + template \ + using cast_op_type = ::pybind11::detail::movable_cast_op_type + +template +using is_std_char_type = any_of, /* std::string */ +#if defined(PYBIND11_HAS_U8STRING) + std::is_same, /* std::u8string */ +#endif + std::is_same, /* std::u16string */ + std::is_same, /* std::u32string */ + std::is_same /* std::wstring */ + >; + +template +struct type_caster::value && !is_std_char_type::value>> { + using _py_type_0 = conditional_t; + using _py_type_1 = conditional_t::value, + _py_type_0, + typename std::make_unsigned<_py_type_0>::type>; + using py_type = conditional_t::value, double, _py_type_1>; + +public: + bool load(handle src, bool convert) { + py_type py_value; + + if (!src) { + return false; + } + +#if !defined(PYPY_VERSION) + auto index_check = [](PyObject *o) { return PyIndex_Check(o); }; +#else + // In PyPy 7.3.3, `PyIndex_Check` is implemented by calling `__index__`, + // while CPython only considers the existence of `nb_index`/`__index__`. + auto index_check = [](PyObject *o) { return hasattr(o, "__index__"); }; +#endif + + if (std::is_floating_point::value) { + if (convert || PyFloat_Check(src.ptr())) { + py_value = (py_type) PyFloat_AsDouble(src.ptr()); + } else { + return false; + } + } else if (PyFloat_Check(src.ptr()) + || (!convert && !PYBIND11_LONG_CHECK(src.ptr()) && !index_check(src.ptr()))) { + return false; + } else { + handle src_or_index = src; + // PyPy: 7.3.7's 3.8 does not implement PyLong_*'s __index__ calls. +#if defined(PYPY_VERSION) + object index; + if (!PYBIND11_LONG_CHECK(src.ptr())) { // So: index_check(src.ptr()) + index = reinterpret_steal(PyNumber_Index(src.ptr())); + if (!index) { + PyErr_Clear(); + if (!convert) + return false; + } else { + src_or_index = index; + } + } +#endif + if (std::is_unsigned::value) { + py_value = as_unsigned(src_or_index.ptr()); + } else { // signed integer: + py_value = sizeof(T) <= sizeof(long) + ? (py_type) PyLong_AsLong(src_or_index.ptr()) + : (py_type) PYBIND11_LONG_AS_LONGLONG(src_or_index.ptr()); + } + } + + // Python API reported an error + bool py_err = py_value == (py_type) -1 && PyErr_Occurred(); + + // Check to see if the conversion is valid (integers should match exactly) + // Signed/unsigned checks happen elsewhere + if (py_err + || (std::is_integral::value && sizeof(py_type) != sizeof(T) + && py_value != (py_type) (T) py_value)) { + PyErr_Clear(); + if (py_err && convert && (PyNumber_Check(src.ptr()) != 0)) { + auto tmp = reinterpret_steal(std::is_floating_point::value + ? PyNumber_Float(src.ptr()) + : PyNumber_Long(src.ptr())); + PyErr_Clear(); + return load(tmp, false); + } + return false; + } + + value = (T) py_value; + return true; + } + + template + static typename std::enable_if::value, handle>::type + cast(U src, return_value_policy /* policy */, handle /* parent */) { + return PyFloat_FromDouble((double) src); + } + + template + static typename std::enable_if::value && std::is_signed::value + && (sizeof(U) <= sizeof(long)), + handle>::type + cast(U src, return_value_policy /* policy */, handle /* parent */) { + return PYBIND11_LONG_FROM_SIGNED((long) src); + } + + template + static typename std::enable_if::value && std::is_unsigned::value + && (sizeof(U) <= sizeof(unsigned long)), + handle>::type + cast(U src, return_value_policy /* policy */, handle /* parent */) { + return PYBIND11_LONG_FROM_UNSIGNED((unsigned long) src); + } + + template + static typename std::enable_if::value && std::is_signed::value + && (sizeof(U) > sizeof(long)), + handle>::type + cast(U src, return_value_policy /* policy */, handle /* parent */) { + return PyLong_FromLongLong((long long) src); + } + + template + static typename std::enable_if::value && std::is_unsigned::value + && (sizeof(U) > sizeof(unsigned long)), + handle>::type + cast(U src, return_value_policy /* policy */, handle /* parent */) { + return PyLong_FromUnsignedLongLong((unsigned long long) src); + } + + PYBIND11_TYPE_CASTER(T, + io_name::value>( + "typing.SupportsInt", "int", "typing.SupportsFloat", "float")); +}; + +template +struct void_caster { +public: + bool load(handle src, bool) { + if (src && src.is_none()) { + return true; + } + return false; + } + static handle cast(T, return_value_policy /* policy */, handle /* parent */) { + return none().release(); + } + PYBIND11_TYPE_CASTER(T, const_name("None")); +}; + +template <> +class type_caster : public void_caster {}; + +template <> +class type_caster : public type_caster { +public: + using type_caster::cast; + + bool load(handle h, bool) { + if (!h) { + return false; + } + if (h.is_none()) { + value = nullptr; + return true; + } + + /* Check if this is a capsule */ + if (isinstance(h)) { + value = reinterpret_borrow(h); + return true; + } + + /* Check if this is a C++ type */ + const auto &bases = all_type_info((PyTypeObject *) type::handle_of(h).ptr()); + if (bases.size() == 1) { // Only allowing loading from a single-value type + value = values_and_holders(reinterpret_cast(h.ptr())).begin()->value_ptr(); + return true; + } + + /* Fail */ + return false; + } + + static handle cast(const void *ptr, return_value_policy /* policy */, handle /* parent */) { + if (ptr) { + return capsule(ptr).release(); + } + return none().release(); + } + + template + using cast_op_type = void *&; + explicit operator void *&() { return value; } + static constexpr auto name = const_name(PYBIND11_CAPSULE_TYPE_TYPE_HINT); + +private: + void *value = nullptr; +}; + +template <> +class type_caster : public void_caster {}; + +template <> +class type_caster { +public: + bool load(handle src, bool convert) { + if (!src) { + return false; + } + if (src.ptr() == Py_True) { + value = true; + return true; + } + if (src.ptr() == Py_False) { + value = false; + return true; + } + if (convert || is_numpy_bool(src)) { + // (allow non-implicit conversion for numpy booleans), use strncmp + // since NumPy 1.x had an additional trailing underscore. + + Py_ssize_t res = -1; + if (src.is_none()) { + res = 0; // None is implicitly converted to False + } +#if defined(PYPY_VERSION) + // On PyPy, check that "__bool__" attr exists + else if (hasattr(src, PYBIND11_BOOL_ATTR)) { + res = PyObject_IsTrue(src.ptr()); + } +#else + // Alternate approach for CPython: this does the same as the above, but optimized + // using the CPython API so as to avoid an unneeded attribute lookup. + else if (auto *tp_as_number = Py_TYPE(src.ptr())->tp_as_number) { + if (PYBIND11_NB_BOOL(tp_as_number)) { + res = (*PYBIND11_NB_BOOL(tp_as_number))(src.ptr()); + } + } +#endif + if (res == 0 || res == 1) { + value = (res != 0); + return true; + } + PyErr_Clear(); + } + return false; + } + static handle cast(bool src, return_value_policy /* policy */, handle /* parent */) { + return handle(src ? Py_True : Py_False).inc_ref(); + } + PYBIND11_TYPE_CASTER(bool, const_name("bool")); + +private: + // Test if an object is a NumPy boolean (without fetching the type). + static inline bool is_numpy_bool(handle object) { + const char *type_name = Py_TYPE(object.ptr())->tp_name; + // Name changed to `numpy.bool` in NumPy 2, `numpy.bool_` is needed for 1.x support + return std::strcmp("numpy.bool", type_name) == 0 + || std::strcmp("numpy.bool_", type_name) == 0; + } +}; + +// Helper class for UTF-{8,16,32} C++ stl strings: +template +struct string_caster { + using CharT = typename StringType::value_type; + + // Simplify life by being able to assume standard char sizes (the standard only guarantees + // minimums, but Python requires exact sizes) + static_assert(!std::is_same::value || sizeof(CharT) == 1, + "Unsupported char size != 1"); +#if defined(PYBIND11_HAS_U8STRING) + static_assert(!std::is_same::value || sizeof(CharT) == 1, + "Unsupported char8_t size != 1"); +#endif + static_assert(!std::is_same::value || sizeof(CharT) == 2, + "Unsupported char16_t size != 2"); + static_assert(!std::is_same::value || sizeof(CharT) == 4, + "Unsupported char32_t size != 4"); + // wchar_t can be either 16 bits (Windows) or 32 (everywhere else) + static_assert(!std::is_same::value || sizeof(CharT) == 2 || sizeof(CharT) == 4, + "Unsupported wchar_t size != 2/4"); + static constexpr size_t UTF_N = 8 * sizeof(CharT); + + bool load(handle src, bool) { + handle load_src = src; + if (!src) { + return false; + } + if (!PyUnicode_Check(load_src.ptr())) { + return load_raw(load_src); + } + + // For UTF-8 we avoid the need for a temporary `bytes` object by using + // `PyUnicode_AsUTF8AndSize`. + if (UTF_N == 8) { + Py_ssize_t size = -1; + const auto *buffer + = reinterpret_cast(PyUnicode_AsUTF8AndSize(load_src.ptr(), &size)); + if (!buffer) { + PyErr_Clear(); + return false; + } + value = StringType(buffer, static_cast(size)); + return true; + } + + auto utfNbytes + = reinterpret_steal(PyUnicode_AsEncodedString(load_src.ptr(), + UTF_N == 8 ? "utf-8" + : UTF_N == 16 ? "utf-16" + : "utf-32", + nullptr)); + if (!utfNbytes) { + PyErr_Clear(); + return false; + } + + const auto *buffer + = reinterpret_cast(PYBIND11_BYTES_AS_STRING(utfNbytes.ptr())); + size_t length = (size_t) PYBIND11_BYTES_SIZE(utfNbytes.ptr()) / sizeof(CharT); + // Skip BOM for UTF-16/32 + if (UTF_N > 8) { + buffer++; + length--; + } + value = StringType(buffer, length); + + // If we're loading a string_view we need to keep the encoded Python object alive: + if (IsView) { + loader_life_support::add_patient(utfNbytes); + } + + return true; + } + + static handle + cast(const StringType &src, return_value_policy /* policy */, handle /* parent */) { + const char *buffer = reinterpret_cast(src.data()); + auto nbytes = ssize_t(src.size() * sizeof(CharT)); + handle s = decode_utfN(buffer, nbytes); + if (!s) { + throw error_already_set(); + } + return s; + } + + PYBIND11_TYPE_CASTER(StringType, const_name(PYBIND11_STRING_NAME)); + +private: + static handle decode_utfN(const char *buffer, ssize_t nbytes) { +#if !defined(PYPY_VERSION) + return UTF_N == 8 ? PyUnicode_DecodeUTF8(buffer, nbytes, nullptr) + : UTF_N == 16 ? PyUnicode_DecodeUTF16(buffer, nbytes, nullptr, nullptr) + : PyUnicode_DecodeUTF32(buffer, nbytes, nullptr, nullptr); +#else + // PyPy segfaults when on PyUnicode_DecodeUTF16 (and possibly on PyUnicode_DecodeUTF32 as + // well), so bypass the whole thing by just passing the encoding as a string value, which + // works properly: + return PyUnicode_Decode(buffer, + nbytes, + UTF_N == 8 ? "utf-8" + : UTF_N == 16 ? "utf-16" + : "utf-32", + nullptr); +#endif + } + + // When loading into a std::string or char*, accept a bytes/bytearray object as-is (i.e. + // without any encoding/decoding attempt). For other C++ char sizes this is a no-op. + // which supports loading a unicode from a str, doesn't take this path. + template + bool load_raw(enable_if_t::value, handle> src) { + if (PYBIND11_BYTES_CHECK(src.ptr())) { + // We were passed raw bytes; accept it into a std::string or char* + // without any encoding attempt. + const char *bytes = PYBIND11_BYTES_AS_STRING(src.ptr()); + if (!bytes) { + pybind11_fail("Unexpected PYBIND11_BYTES_AS_STRING() failure."); + } + value = StringType(bytes, (size_t) PYBIND11_BYTES_SIZE(src.ptr())); + return true; + } + if (PyByteArray_Check(src.ptr())) { + // We were passed a bytearray; accept it into a std::string or char* + // without any encoding attempt. + const char *bytearray = PyByteArray_AsString(src.ptr()); + if (!bytearray) { + pybind11_fail("Unexpected PyByteArray_AsString() failure."); + } + value = StringType(bytearray, (size_t) PyByteArray_Size(src.ptr())); + return true; + } + + return false; + } + + template + bool load_raw(enable_if_t::value, handle>) { + return false; + } +}; + +template +struct type_caster, + enable_if_t::value>> + : string_caster> {}; + +#ifdef PYBIND11_HAS_STRING_VIEW +template +struct type_caster, + enable_if_t::value>> + : string_caster, true> {}; +#endif + +// Type caster for C-style strings. We basically use a std::string type caster, but also add the +// ability to use None as a nullptr char* (which the string caster doesn't allow). +template +struct type_caster::value>> { + using StringType = std::basic_string; + using StringCaster = make_caster; + StringCaster str_caster; + bool none = false; + CharT one_char = 0; + +public: + bool load(handle src, bool convert) { + if (!src) { + return false; + } + if (src.is_none()) { + // Defer accepting None to other overloads (if we aren't in convert mode): + if (!convert) { + return false; + } + none = true; + return true; + } + return str_caster.load(src, convert); + } + + static handle cast(const CharT *src, return_value_policy policy, handle parent) { + if (src == nullptr) { + return pybind11::none().release(); + } + return StringCaster::cast(StringType(src), policy, parent); + } + + static handle cast(CharT src, return_value_policy policy, handle parent) { + if (std::is_same::value) { + handle s = PyUnicode_DecodeLatin1((const char *) &src, 1, nullptr); + if (!s) { + throw error_already_set(); + } + return s; + } + return StringCaster::cast(StringType(1, src), policy, parent); + } + + explicit operator CharT *() { + return none ? nullptr : const_cast(static_cast(str_caster).c_str()); + } + explicit operator CharT &() { + if (none) { + throw value_error("Cannot convert None to a character"); + } + + auto &value = static_cast(str_caster); + size_t str_len = value.size(); + if (str_len == 0) { + throw value_error("Cannot convert empty string to a character"); + } + + // If we're in UTF-8 mode, we have two possible failures: one for a unicode character that + // is too high, and one for multiple unicode characters (caught later), so we need to + // figure out how long the first encoded character is in bytes to distinguish between these + // two errors. We also allow want to allow unicode characters U+0080 through U+00FF, as + // those can fit into a single char value. + if (StringCaster::UTF_N == 8 && str_len > 1 && str_len <= 4) { + auto v0 = static_cast(value[0]); + // low bits only: 0-127 + // 0b110xxxxx - start of 2-byte sequence + // 0b1110xxxx - start of 3-byte sequence + // 0b11110xxx - start of 4-byte sequence + size_t char0_bytes = (v0 & 0x80) == 0 ? 1 + : (v0 & 0xE0) == 0xC0 ? 2 + : (v0 & 0xF0) == 0xE0 ? 3 + : 4; + + if (char0_bytes == str_len) { + // If we have a 128-255 value, we can decode it into a single char: + if (char0_bytes == 2 && (v0 & 0xFC) == 0xC0) { // 0x110000xx 0x10xxxxxx + one_char = static_cast(((v0 & 3) << 6) + + (static_cast(value[1]) & 0x3F)); + return one_char; + } + // Otherwise we have a single character, but it's > U+00FF + throw value_error("Character code point not in range(0x100)"); + } + } + + // UTF-16 is much easier: we can only have a surrogate pair for values above U+FFFF, thus a + // surrogate pair with total length 2 instantly indicates a range error (but not a "your + // string was too long" error). + else if (StringCaster::UTF_N == 16 && str_len == 2) { + one_char = static_cast(value[0]); + if (one_char >= 0xD800 && one_char < 0xE000) { + throw value_error("Character code point not in range(0x10000)"); + } + } + + if (str_len != 1) { + throw value_error("Expected a character, but multi-character string found"); + } + + one_char = value[0]; + return one_char; + } + + static constexpr auto name = const_name(PYBIND11_STRING_NAME); + template + using cast_op_type = pybind11::detail::cast_op_type<_T>; +}; + +// Base implementation for std::tuple and std::pair +template