diff --git a/MavenArtifacts.md b/MavenArtifacts.md index 9870028183..aaceb9a0e8 100644 --- a/MavenArtifacts.md +++ b/MavenArtifacts.md @@ -14,54 +14,27 @@ The development repository is where development releases of every commit to [mai ## Artifact classifiers We provide two base types of artifacts. -The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier. - -The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifier combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those. - -## Artifact Names - -WPILib builds four different types of artifacts. - -##### C++ Only Libraries -When we publish C++ only libraries, they are published with the base artifact name as their artifact name, with a `-cpp` extension. All dependencies for the library are linked as shared libraries to the binary. - - -Example: -``` -edu.wpi.first.wpilibc:wpilibc-cpp:version:classifier@zip -``` - -#### Java Only Libraries -When we publish Java only libraries, they are published with the base artifact name as their artifact name, with a `-java` extension. +The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier. These artifacts are published with the base artifact name as their artifact ID, with a `-java` extension. Example: ``` edu.wpi.first.wpilibj:wpilibj-java:version ``` -#### C++/Java Libraries without JNI -For libraries that are both C++ and Java, but without a JNI component, the C++ component is published with the `basename-cpp` artifact name, and the Java component is published with the `basename-java` artifact name. +The second types are native artifacts. These are usually published as `zip` files. The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The full list of supported platforms can be found in [native-utils](https://github.com/wpilibsuite/native-utils/blob/main/src/main/java/edu/wpi/first/nativeutils/WPINativeUtilsExtension.java#L94). If the library is built statically, it will have `static` appended to the classifier. Additionally, if the library was built in debug mode, `debug` will be appended to the classifier. The platform artifact only contains the binaries for a specific platform. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those. + +If the library is Java and C++ and has a JNI component, the native artifact will have a shared library containing JNI entrypoints alongside the C++ shared library. This JNI shared library will have a `jni` suffix in the file name. + +Native artifacts are published with the base artifact name as their artifact ID, with a `-cpp` extension. Example: ``` -edu.wpi.first.wpiutil:wpiutil-cpp:version:classifier@zip (C++) -edu.wpi.first.wpiutil:wpiutil-java:version (Java) -``` - -#### C++/Java Libraries with JNI -For libraries that are both C++ and Java with a JNI component there are three different artifact names. For Java, the component is published as `basename-java`. For C++, the `basename-cpp` artifact contains the C++ artifacts with all dependencies linked as shared libraries to the binary. These binaries DO contain the JNI entry points. The `basename-jni` artifact contains identical C++ binaries to the `-cpp` artifact, however all of its dependencies are statically linked, and only the JNI and C entry points are exported. - -The `-jni` artifact should only be used in cases where you want to create a self contained Java application where the native artifacts are embedded in the jar. Note in an extraction scenario, extending off of the library is never supported, which is why the C++ entry points are not exposed. The name of the library is randomly generated during extraction. For pretty much all cases, and if you ever want to extend from a native library, you should use the `-cpp` artifacts. GradleRIO uses the `-cpp` artifacts for all platforms, even desktop, for this reason. - -Example: -``` -edu.wpi.first.ntcore:ntcore-cpp:version:classifier@zip (C++) -edu.wpi.first.ntcore:ntcore-jni:version:classifier (JNI jar library) -edu.wpi.first.ntcore:ntcore-java:version (Java) +edu.wpi.first.wpimath:wpimath-cpp:version:classifier@zip +edu.wpi.first.wpimath:wpimath-cpp:version:windowsx86-64staticdebug@zip ``` ## Provided Artifacts -This repository provides the following artifacts. Below each artifact is its dependencies. Note if ever using the `-jni` artifacts, no dependencies are needed for native binaries. +This repository provides the following artifacts. Below each artifact is its dependencies. For C++, if building with static dependencies, the listed order should be the link order in your linker. diff --git a/apriltag/build.gradle b/apriltag/build.gradle index ffdf094ede..311779f842 100644 --- a/apriltag/build.gradle +++ b/apriltag/build.gradle @@ -103,14 +103,8 @@ model { return } it.cppCompiler.define 'WPILIB_EXPORTS' - - if (it.component.name == "${nativeName}JNI") { - lib project: ':wpimath', library: 'wpimath', linkage: 'static' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' - } else { - lib project: ':wpimath', library: 'wpimath', linkage: 'shared' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' - } + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } } tasks { diff --git a/cscore/.styleguide b/cscore/.styleguide index e82e06430e..2737ace2a7 100644 --- a/cscore/.styleguide +++ b/cscore/.styleguide @@ -38,8 +38,10 @@ includeOtherLibs { ^fmt/ ^gtest/ ^opencv2/ + ^imgui ^support/ ^tcpsockets/ ^wpi/ + ^wpigui ^wpinet/ } diff --git a/cscore/BUILD.bazel b/cscore/BUILD.bazel index a01afe2542..0af974eab6 100644 --- a/cscore/BUILD.bazel +++ b/cscore/BUILD.bazel @@ -48,6 +48,7 @@ objc_library( "Foundation", "CoreMedia", "CoreVideo", + "IOKit", ], tags = ["manual"], deps = [ diff --git a/cscore/CMakeLists.txt b/cscore/CMakeLists.txt index 3e96d42719..af5d17aa5b 100644 --- a/cscore/CMakeLists.txt +++ b/cscore/CMakeLists.txt @@ -21,7 +21,7 @@ if(APPLE) cscore PROPERTIES LINK_FLAGS - "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo" + "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo -framework IOKit" ) elseif(MSVC) target_sources(cscore PRIVATE ${cscore_windows_src}) diff --git a/cscore/build.gradle b/cscore/build.gradle index 44a1c37447..ffbdb9d063 100644 --- a/cscore/build.gradle +++ b/cscore/build.gradle @@ -60,13 +60,8 @@ model { if (!it.buildable || !(it instanceof NativeBinarySpec)) { return } - if (it.component.name == "${nativeName}JNI") { - lib project: ':wpinet', library: 'wpinet', linkage: 'static' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' - } else { - lib project: ':wpinet', library: 'wpinet', linkage: 'shared' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' - } + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } } } diff --git a/cscore/examples/usbviewer/usbviewer.cpp b/cscore/examples/usbviewer/usbviewer.cpp index 4653c516cd..0cb31dfb3f 100644 --- a/cscore/examples/usbviewer/usbviewer.cpp +++ b/cscore/examples/usbviewer/usbviewer.cpp @@ -3,14 +3,17 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include +#include #include -#define IMGUI_DEFINE_MATH_OPERATORS #include #include #include +#include #include +#include #include #include #include @@ -21,10 +24,10 @@ namespace gui = wpi::gui; int main() { - std::atomic latestFrame{nullptr}; - std::vector sharedFreeList; - wpi::spinlock sharedFreeListMutex; - std::vector sourceFreeList; + wpi::spinlock latestFrameMutex; + std::unique_ptr latestFrame; + wpi::mutex freeListMutex; + std::vector> freeList; std::atomic stopCamera{false}; cs::UsbCamera camera{"usbcam", 0}; @@ -42,36 +45,31 @@ int main() { continue; } - // get or create a mat, prefer sourceFreeList over sharedFreeList - cv::Mat* out; - if (!sourceFreeList.empty()) { - out = sourceFreeList.back(); - sourceFreeList.pop_back(); - } else { - { - std::scoped_lock lock(sharedFreeListMutex); - for (auto mat : sharedFreeList) { - sourceFreeList.emplace_back(mat); - } - sharedFreeList.clear(); - } - if (!sourceFreeList.empty()) { - out = sourceFreeList.back(); - sourceFreeList.pop_back(); + // get or create a mat + std::unique_ptr out; + { + std::scoped_lock lock{freeListMutex}; + if (!freeList.empty()) { + out = std::move(freeList.back()); + freeList.pop_back(); } else { - out = new cv::Mat; + out = std::make_unique(); } } // convert to RGBA cv::cvtColor(frame, *out, cv::COLOR_BGR2RGBA); - // make available - auto prev = latestFrame.exchange(out); + { + // make available + std::scoped_lock lock{latestFrameMutex}; + latestFrame.swap(out); + } - // put prev on free list - if (prev) { - sourceFreeList.emplace_back(prev); + // put the previous frame on free list + if (out) { + std::scoped_lock lock{freeListMutex}; + freeList.emplace_back(std::move(out)); } } }); @@ -80,7 +78,11 @@ int main() { gui::Initialize("Hello World", 1024, 768); gui::Texture tex; gui::AddEarlyExecute([&] { - auto frame = latestFrame.exchange(nullptr); + std::unique_ptr frame; + { + std::scoped_lock lock{latestFrameMutex}; + latestFrame.swap(frame); + } if (frame) { // create or update texture if (!tex || frame->cols != tex.GetWidth() || @@ -90,9 +92,10 @@ int main() { } else { tex.Update(frame->data); } - // put back on shared freelist - std::scoped_lock lock(sharedFreeListMutex); - sharedFreeList.emplace_back(frame); + { + std::scoped_lock lock{freeListMutex}; + freeList.emplace_back(std::move(frame)); + } } ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_FirstUseEver); diff --git a/cscore/src/main/native/objcpp/UsbCameraImpl.h b/cscore/src/main/native/objcpp/UsbCameraImpl.h index 86b1166b05..68c9078acc 100644 --- a/cscore/src/main/native/objcpp/UsbCameraImpl.h +++ b/cscore/src/main/native/objcpp/UsbCameraImpl.h @@ -12,6 +12,8 @@ #include #include +#include + #include "SourceImpl.h" namespace cs { @@ -88,8 +90,34 @@ class UsbCameraImpl : public SourceImpl { UsbCameraImplObjc* cppGetObjc() { return m_objc; } + int CreatePropertyPublic(std::string_view name, std::function()> newFunc) { + return CreateProperty(name, newFunc); + } + + PropertyImpl* GetPropertyPublic(int property) { + return GetProperty(property); + } + + void NotifyPropertyCreatedPublic(int propIndex, PropertyImpl& prop) { + NotifyPropertyCreated(propIndex, prop); + } + + void UpdatePropertyValuePublic(int property, bool setString, int value, std::string_view valueStr) { + UpdatePropertyValue(property, setString, value, valueStr); + } + + wpi::mutex& GetMutex() { return m_mutex; } + + // Property cache accessors + wpi::StringMap& GetPropertyCache() { return m_propertyCache; } + wpi::StringMap& GetPropertyAutoCache() { return m_propertyAutoCache; } + private: UsbCameraImplObjc* m_objc; std::vector m_platformModes; + + // Property caches + wpi::StringMap m_propertyCache; + wpi::StringMap m_propertyAutoCache; }; } // namespace cs diff --git a/cscore/src/main/native/objcpp/UsbCameraImplObjc.h b/cscore/src/main/native/objcpp/UsbCameraImplObjc.h index c173e80011..b4f7626ae9 100644 --- a/cscore/src/main/native/objcpp/UsbCameraImplObjc.h +++ b/cscore/src/main/native/objcpp/UsbCameraImplObjc.h @@ -5,11 +5,39 @@ #pragma once #import -#import "UsbCameraDelegate.h" + #include #include + +#import "UsbCameraDelegate.h" +#import "UvcControlImpl.h" + #include "cscore_cpp.h" +// Quirk: exposure auto is 3 for on, 1 for off +#define kPropertyAutoExposureOn 3 +#define kPropertyAutoExposureOff 1 + +// Property names +#define kPropertyBrightness "brightness" +#define kPropertyWhiteBalance "white_balance_temperature" +#define kPropertyExposure "raw_exposure_time_absolute" +#define kPropertyContrast "raw_contrast" +#define kPropertySaturation "raw_saturation" +#define kPropertySharpness "raw_sharpness" +#define kPropertyGain "gain" +#define kPropertyGamma "gamma" +#define kPropertyHue "raw_hue" +#define kPropertyFocus "focus_absolute" +#define kPropertyZoom "zoom" +#define kPropertyBackLightCompensation "backlight_compensation" +#define kPropertyPowerLineFrequency "power_line_frequency" + +// Auto property names +#define kPropertyAutoExposure "exposure_auto" +#define kPropertyAutoWhiteBalance "white_balance_automatic" +#define kPropertyAutoFocus "focus_auto" + namespace cs { class UsbCameraImpl; } @@ -30,6 +58,7 @@ class UsbCameraImpl; @property(nonatomic) AVCaptureDevice* videoDevice; @property(nonatomic) AVCaptureDeviceInput* videoInput; @property(nonatomic) UsbCameraDelegate* callback; +@property(nonatomic) UvcControlImpl* uvcControl; @property(nonatomic) AVCaptureVideoDataOutput* videoOutput; @property(nonatomic) AVCaptureSession* session; @@ -68,4 +97,8 @@ class UsbCameraImpl; - (void)getCameraName:(std::string*)name; - (void)setNewCameraPath:(std::string_view*)path; +- (void)deviceCacheProperties; +- (void)cacheProperty:(uint32_t)propID withName:(NSString *)name; +- (void)cacheAutoProperty:(uint32_t)propID withName:(NSString *)baseName; + @end diff --git a/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm b/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm index 5d43de7f32..58f5dde90c 100644 --- a/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm +++ b/cscore/src/main/native/objcpp/UsbCameraImplObjc.mm @@ -2,12 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#import "UsbCameraImplObjc.h" -#include "UsbCameraImpl.h" +#include #pragma GCC diagnostic ignored "-Wunused-parameter" +#import "UsbCameraImplObjc.h" + #include "Notifier.h" #include "Log.h" +#include "UsbCameraImpl.h" template inline void NamedLog(UsbCameraImplObjc* objc, unsigned int level, @@ -104,44 +106,300 @@ using namespace cs; name:AVCaptureDeviceWasDisconnectedNotification object:nil]; [self deviceConnect]; + [self deviceCacheProperties]; }); } +- (BOOL)getEnabledWithProperty:(int)property withValue:(int)value { + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + return false; + } + + // There is room for quirk handling improvement here, but I will leave it + // for now. + if (property == sharedThis->GetPropertyIndex(kPropertyAutoExposure)) { + return value == kPropertyAutoExposureOn; + } + + return value != 0; +} + + +- (int)clampToPercent:(int)value { + if (value < 0) { + return 0; + } + if (value > 100) { + return 100; + } + return value; +} + +- (int)percentageToRaw:(int)propID percentage:(int)percentage min:(int)min max:(int)max { + if (min == max) { + return min; + } + + return min + (max - min) * percentage / 100; +} + +- (BOOL)isPercentageProperty:(int)propID { + return propID == CAPPROPID_BRIGHTNESS || + propID == CAPPROPID_CONTRAST || + propID == CAPPROPID_SATURATION || + propID == CAPPROPID_HUE || + propID == CAPPROPID_SHARPNESS || + propID == CAPPROPID_GAIN; +} + // Property functions - (void)setProperty:(int)property withValue:(int)value status:(CS_Status*)status { + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // Get the property name from the property index + wpi::SmallString<128> nameBuf; + std::string_view propName = sharedThis->GetPropertyName(property, nameBuf, status); + if (*status != 0) { + OBJCERROR("Failed to get property name for index {}", property); + return; + } + + std::string nameStr(propName); + + // Check if it's an auto property + auto& propertyAutoCache = sharedThis->GetPropertyAutoCache(); + auto autoIt = propertyAutoCache.find(nameStr); + if (autoIt != propertyAutoCache.end()) { + uint32_t propID = autoIt->second; + bool enabled = [self getEnabledWithProperty:property withValue:value]; + dispatch_async_and_wait(self.sessionQueue, ^{ + if (self.uvcControl == nil) { + *status = CS_INVALID_PROPERTY; + return; + } + + if (![self.uvcControl setAutoProperty:propID enabled:enabled status:status]) { + OBJCERROR("Failed to set auto property {} to {}", + nameStr, enabled); + return; + } + + // Update property value + sharedThis->UpdatePropertyValuePublic(property, false, value, {}); + }); + return; + } + + // Handle regular property + auto& propertyCache = sharedThis->GetPropertyCache(); + auto it = propertyCache.find(nameStr); + if (it == propertyCache.end()) { + OBJCERROR("Property not found in cache: {}", nameStr); + *status = CS_INVALID_PROPERTY; + return; + } + + uint32_t propID = it->second; + + dispatch_async_and_wait(self.sessionQueue, ^{ + if (self.uvcControl == nil) { + *status = CS_INVALID_PROPERTY; + return; + } + + // Get the property implementation to access its limits + const PropertyImpl* prop = sharedThis->GetPropertyPublic(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return; + } + + + int32_t realValue = value; + if ([self isPercentageProperty:propID]) { + // Clamp to 0-100 + realValue = [self clampToPercent:realValue]; + + // Scale to min/max + realValue = [self percentageToRaw:propID percentage:realValue min:prop->minimum max:prop->maximum]; + } + + if (![self.uvcControl setProperty:propID withValue:realValue status:status]) { + OBJCERROR("Failed to set property {} to value {}", nameStr, realValue); + return; + } + + // Update property value in the container + sharedThis->UpdatePropertyValuePublic(property, false, value, {}); + }); } + - (void)setStringProperty:(int)property withValue:(std::string_view*)value status:(CS_Status*)status { + *status = CS_INVALID_PROPERTY; + return; } // Standard common camera properties - (void)setBrightness:(int)brightness status:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // Get the property index and set it + int prop = sharedThis->GetPropertyIndex(kPropertyBrightness); + sharedThis->SetProperty(prop, brightness, status); } + - (int)getBrightness:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; - return 0; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return 0; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // Get the property index and its value + int prop = sharedThis->GetPropertyIndex(kPropertyBrightness); + return sharedThis->GetProperty(prop, status); } + - (void)setWhiteBalanceAuto:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + int prop = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance); + sharedThis->SetProperty(prop, 1, status); } + - (void)setWhiteBalanceHoldCurrent:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + int prop = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance); + sharedThis->SetProperty(prop, 0, status); } + - (void)setWhiteBalanceManual:(int)value status:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // First disable auto white balance + int autoProp = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance); + sharedThis->SetProperty(autoProp, 0, status); + if (*status != 0) { + return; + } + + // Then set the white balance value + int prop = sharedThis->GetPropertyIndex(kPropertyWhiteBalance); + sharedThis->SetProperty(prop, value, status); } + - (void)setExposureAuto:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // Set the auto exposure property to enabled (1) + int prop = sharedThis->GetPropertyIndex(kPropertyAutoExposure); + sharedThis->SetProperty(prop, kPropertyAutoExposureOn, status); } + - (void)setExposureHoldCurrent:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // Set the auto exposure property to disabled (0) + int prop = sharedThis->GetPropertyIndex(kPropertyAutoExposure); + sharedThis->SetProperty(prop, kPropertyAutoExposureOff, status); } + - (void)setExposureManual:(int)value status:(CS_Status*)status { - *status = CS_INVALID_PROPERTY; + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + *status = CS_INVALID_HANDLE; + return; + } + + // Make sure properties are cached + if (!self.propertiesCached) { + [self deviceCacheProperties]; + } + + // First disable auto exposure + int autoProp = sharedThis->GetPropertyIndex(kPropertyAutoExposure); + sharedThis->SetProperty(autoProp, kPropertyAutoExposureOff, status); + if (*status != 0) { + return; + } + + // Then set the exposure value + int prop = sharedThis->GetPropertyIndex(kPropertyExposure); + sharedThis->SetProperty(prop, value, status); } - (bool)setVideoMode:(const cs::VideoMode&)mode status:(CS_Status*)status { @@ -295,10 +553,144 @@ using namespace cs; // All above are called from C++, must always dispatch to loop +// Property caching methods - (void)deviceCacheProperties { - if (self.session == nil) { - return; - } + if (self.uvcControl == nil) { + return; + } + + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + OBJCERROR("Cannot cache properties: UsbCameraImpl not available"); + return; + } + + // Cache basic properties + [self cacheProperty:CAPPROPID_BRIGHTNESS withName:@kPropertyBrightness]; + [self cacheProperty:CAPPROPID_WHITEBALANCE withName:@kPropertyWhiteBalance]; + [self cacheProperty:CAPPROPID_EXPOSURE withName:@kPropertyExposure]; + [self cacheProperty:CAPPROPID_CONTRAST withName:@kPropertyContrast]; + [self cacheProperty:CAPPROPID_SATURATION withName:@kPropertySaturation]; + [self cacheProperty:CAPPROPID_SHARPNESS withName:@kPropertySharpness]; + [self cacheProperty:CAPPROPID_GAIN withName:@kPropertyGain]; + [self cacheProperty:CAPPROPID_GAMMA withName:@kPropertyGamma]; + [self cacheProperty:CAPPROPID_HUE withName:@kPropertyHue]; + [self cacheProperty:CAPPROPID_FOCUS withName:@kPropertyFocus]; + [self cacheProperty:CAPPROPID_ZOOM withName:@kPropertyZoom]; + [self cacheProperty:CAPPROPID_BACKLIGHTCOMP withName:@kPropertyBackLightCompensation]; + [self cacheProperty:CAPPROPID_POWERLINEFREQ withName:@kPropertyPowerLineFrequency]; + + // Cache auto properties + [self cacheAutoProperty:CAPPROPID_EXPOSURE withName:@kPropertyAutoExposure]; + [self cacheAutoProperty:CAPPROPID_WHITEBALANCE withName:@kPropertyAutoWhiteBalance]; + [self cacheAutoProperty:CAPPROPID_FOCUS withName:@kPropertyAutoFocus]; + + self.propertiesCached = true; +} + +- (void)cacheProperty:(uint32_t)propID withName:(NSString *)name { + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + OBJCERROR("Cannot cache property: UsbCameraImpl not available"); + return; + } + + if (self.uvcControl == nil) { + OBJCWARNING("Cannot cache property {}: UVC control not initialized", [name UTF8String]); + return; + } + + // Get property limits + int32_t minimum = 0, maximum = 0, defaultValue = 0; + int32_t value = defaultValue; + CS_Status status; + + std::string nameStr = std::string([name UTF8String]); + + // Get the property limits + if (![self.uvcControl getPropertyLimits:propID + min:&minimum + max:&maximum + defValue:&defaultValue + status:&status]) { + OBJCWARNING("Failed to get property limits for {}", nameStr); + return; + } + + // Get current value + if (![self.uvcControl getProperty:propID withValue:&value status:&status]) { + value = defaultValue; + OBJCWARNING("Failed to get current value for {}: {}", + nameStr, value); + return; + } + + // Create property + auto& propertyCache = sharedThis->GetPropertyCache(); + propertyCache[nameStr] = propID; + + // Create the property implementation + std::unique_ptr prop; + prop = std::make_unique(nameStr); + prop->propKind = CS_PROP_INTEGER; + prop->value = value; + prop->minimum = minimum; + prop->maximum = maximum; + prop->step = 1; // Most camera properties use a step of 1 + prop->defaultValue = defaultValue; + + // Add the property to the container + std::scoped_lock lock(sharedThis->GetMutex()); + int ndx = sharedThis->CreatePropertyPublic(nameStr, [&] { return std::move(prop); }); + + // Notify that property has been created + sharedThis->NotifyPropertyCreatedPublic(ndx, *sharedThis->GetPropertyPublic(ndx)); +} + +- (void)cacheAutoProperty:(uint32_t)propID withName:(NSString *)baseName { + auto sharedThis = self.cppImpl.lock(); + if (!sharedThis) { + OBJCERROR("Cannot cache auto property: UsbCameraImpl not available"); + return; + } + + if (self.uvcControl == nil) { + OBJCWARNING("Cannot cache auto property {}: UVC control not initialized", [baseName UTF8String]); + return; + } + + // Build auto mode property name + std::string nameStr = std::string([baseName UTF8String]); + + // Get current auto mode status + bool enabled = false; + CS_Status status = 0; + + if(![self.uvcControl getAutoProperty:propID enabled:&enabled status:&status]) { + OBJCWARNING("Failed to get auto property {}", nameStr); + return; + } + + // Create property + std::unique_ptr prop; + prop = std::make_unique(nameStr); + prop->propKind = CS_PROP_BOOLEAN; + prop->value = enabled ? 1 : 0; + prop->minimum = 0; + prop->maximum = 1; + prop->step = 1; + prop->defaultValue = 0; // Default is manual mode + + // Add property to container + std::scoped_lock lock(sharedThis->GetMutex()); + int ndx = sharedThis->CreatePropertyPublic(nameStr, [&] { return std::move(prop); }); + + // Notify property created + sharedThis->NotifyPropertyCreatedPublic(ndx, *sharedThis->GetPropertyPublic(ndx)); + + // Map property name to ID + auto& propertyAutoCache = sharedThis->GetPropertyAutoCache(); + propertyAutoCache[nameStr] = propID; } static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) { @@ -459,6 +851,53 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) { self.deviceValid = true; } +- (CMTime)findNearestFrameDuration:(int)fps { + if (self.currentFormat == nil) { + return CMTimeMake(1, fps); + } + + NSArray* frameRates = self.currentFormat.videoSupportedFrameRateRanges; + if (frameRates.count == 0) { + return CMTimeMake(1, fps); + } + + // Find the nearest frame duration + CMTime nearestDuration = CMTimeMake(1, fps); + double minDiff = DBL_MAX; + + for (AVFrameRateRange* range in frameRates) { + CMTime minDuration = range.minFrameDuration; + CMTime maxDuration = range.maxFrameDuration; + + // Calculate frame duration for current fps + CMTime targetDuration = CMTimeMake(1, fps); + + // Check if within range + if (CMTimeCompare(targetDuration, minDuration) >= 0 && + CMTimeCompare(targetDuration, maxDuration) <= 0) { + return targetDuration; + } + + // Calculate difference with min value + double minDiffValue = fabs(CMTimeGetSeconds(targetDuration) - CMTimeGetSeconds(minDuration)); + if (minDiffValue < minDiff) { + minDiff = minDiffValue; + nearestDuration = minDuration; + } + + // Calculate difference with max value + double maxDiffValue = fabs(CMTimeGetSeconds(targetDuration) - CMTimeGetSeconds(maxDuration)); + if (maxDiffValue < minDiff) { + minDiff = maxDiffValue; + nearestDuration = maxDuration; + } + } + + OBJCDEBUG("Nearest fps: {}", nearestDuration.timescale / static_cast(nearestDuration.value)); + + return nearestDuration; +} + - (bool)deviceStreamOn { if (self.streaming) { return false; @@ -466,24 +905,32 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) { if (!self.deviceValid) { return false; } - self.streaming = true; + if (![self.videoDevice lockForConfiguration:nil]) { + OBJCERROR("Failed to lock for configuration"); + return false; + } + + [self.session beginConfiguration]; + + if (self.currentFormat != nil) { + self.videoDevice.activeFormat = self.currentFormat; + } + + if (self.currentFPS != 0) { + CMTime frameDuration = [self findNearestFrameDuration:self.currentFPS]; + self.videoDevice.activeVideoMinFrameDuration = frameDuration; + self.videoDevice.activeVideoMaxFrameDuration = frameDuration; + } + + [self.session commitConfiguration]; + + self.streaming = true; + // Start the capture session before device unlock to ensure + // the session preset settings are preserved [self.session startRunning]; - if ([self.videoDevice lockForConfiguration:nil]) { - if (self.currentFormat != nil) { - self.videoDevice.activeFormat = self.currentFormat; - } - if (self.currentFPS != 0) { - self.videoDevice.activeVideoMinFrameDuration = - CMTimeMake(1, self.currentFPS); - self.videoDevice.activeVideoMaxFrameDuration = - CMTimeMake(1, self.currentFPS); - } - [self.videoDevice unlockForConfiguration]; - } else { - OBJCERROR("Failed to lock for configuration"); - } + [self.videoDevice unlockForConfiguration]; return true; } @@ -574,6 +1021,16 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) { goto err; } + CS_Status status; + self.uvcControl = [UvcControlImpl createFromAVCaptureDevice:self.videoDevice status:&status]; + if (self.uvcControl == nil) { + OBJCWARNING("Failed to initialize UVC control for camera: {}", status); + } else { + OBJCINFO("UVC control initialized successfully"); + } + + self.uvcControl.cppImpl = self.cppImpl; + self.callback = [[UsbCameraDelegate alloc] init]; if (self.callback == nil) { OBJCWARNING("Creating Camera Callback failed"); diff --git a/cscore/src/main/native/objcpp/UvcControlImpl.h b/cscore/src/main/native/objcpp/UvcControlImpl.h new file mode 100644 index 0000000000..aea335775c --- /dev/null +++ b/cscore/src/main/native/objcpp/UvcControlImpl.h @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#import +#import +#import + +#include +#include + +#import "UsbCameraDelegate.h" + +#include "cscore_cpp.h" + +// Status code definition +#define CS_UVC_STATUS_ERROR -3001 +#define CS_UVC_STATUS_DEVICE_DISCONNECTED -3002 + +// UVC control selector definitions +#define UVC_INPUT_TERMINAL_ID 0x01 + +// Camera terminal control selectors +#define CT_AE_MODE_CONTROL 0x02 +#define CT_AE_PRIORITY_CONTROL 0x03 +#define CT_EXPOSURE_TIME_ABSOLUTE_CONTROL 0x04 +#define CT_EXPOSURE_TIME_RELATIVE_CONTROL 0x05 +#define CT_FOCUS_ABSOLUTE_CONTROL 0x06 +#define CT_FOCUS_RELATIVE_CONTROL 0x07 +#define CT_FOCUS_AUTO_CONTROL 0x08 +#define CT_ZOOM_ABSOLUTE_CONTROL 0x0B +#define CT_ZOOM_RELATIVE_CONTROL 0x0C + +// Processing unit control selectors +#define PU_BACKLIGHT_COMPENSATION_CONTROL 0x01 +#define PU_BRIGHTNESS_CONTROL 0x02 +#define PU_CONTRAST_CONTROL 0x03 +#define PU_GAIN_CONTROL 0x04 +#define PU_POWER_LINE_FREQUENCY_CONTROL 0x05 +#define PU_HUE_CONTROL 0x06 +#define PU_SATURATION_CONTROL 0x07 +#define PU_SHARPNESS_CONTROL 0x08 +#define PU_GAMMA_CONTROL 0x09 +#define PU_WHITE_BALANCE_TEMPERATURE_CONTROL 0x0A +#define PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL 0x0B +#define PU_WHITE_BALANCE_COMPONENT_CONTROL 0x0C +#define PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL 0x0D +#define PU_HUE_AUTO_CONTROL 0x10 +#define PU_CONTRAST_AUTO_CONTROL 0x13 + +// Camera request error code +#define VC_REQUEST_ERROR_CODE_CONTROL 0x02 + +// UVC control interface definitions +#define UVC_CONTROL_INTERFACE_CLASS 14 +#define UVC_CONTROL_INTERFACE_SUBCLASS 1 + +// UVC control request types +#define UVC_SET_CUR 0x01 +#define UVC_GET_CUR 0x81 +#define UVC_GET_MIN 0x82 +#define UVC_GET_MAX 0x83 +#define UVC_GET_RES 0x84 +#define UVC_GET_INFO 0x86 +#define UVC_GET_DEF 0x87 + +// Camera property ID definitions +#define CAPPROPID_EXPOSURE 1 +#define CAPPROPID_FOCUS 2 +#define CAPPROPID_ZOOM 3 +#define CAPPROPID_WHITEBALANCE 4 +#define CAPPROPID_GAIN 5 +#define CAPPROPID_BRIGHTNESS 6 +#define CAPPROPID_CONTRAST 7 +#define CAPPROPID_SATURATION 8 +#define CAPPROPID_GAMMA 9 +#define CAPPROPID_HUE 10 +#define CAPPROPID_SHARPNESS 11 +#define CAPPROPID_BACKLIGHTCOMP 12 +#define CAPPROPID_POWERLINEFREQ 13 +#define CAPPROPID_LAST 14 + +namespace cs { +class UsbCameraImpl; +} + +@interface UvcControlImpl : NSObject + +@property(nonatomic) IOUSBInterfaceInterface190** controlInterface; +@property(nonatomic) uint32_t processingUnitID; +@property(nonatomic) std::weak_ptr cppImpl; + +// Create from AVCaptureDevice ++ (instancetype)createFromAVCaptureDevice:(AVCaptureDevice*)device status:(CS_Status*)status; + +// Initialize with USB vendor/product/location +- (instancetype)initWithVendorId:(uint16_t)vid + productId:(uint16_t)pid + location:(uint32_t)location + status:(CS_Status*)status; + +- (void)dealloc; + +// Basic property control +- (bool)setProperty:(uint32_t)propID + withValue:(int32_t)value + status:(CS_Status*)status; +- (bool)getProperty:(uint32_t)propID + withValue:(int32_t*)value + status:(CS_Status*)status; + +// Auto mode control +- (bool)setAutoProperty:(uint32_t)propID + enabled:(bool)enabled + status:(CS_Status*)status; +- (bool)getAutoProperty:(uint32_t)propID + enabled:(bool*)enabled + status:(CS_Status*)status; + +// Property range query +- (bool)getPropertyLimits:(uint32_t)propID + min:(int32_t*)min + max:(int32_t*)max + defValue:(int32_t*)defValue + status:(CS_Status*)status; + +@end \ No newline at end of file diff --git a/cscore/src/main/native/objcpp/UvcControlImpl.mm b/cscore/src/main/native/objcpp/UvcControlImpl.mm new file mode 100644 index 0000000000..05b7229a78 --- /dev/null +++ b/cscore/src/main/native/objcpp/UvcControlImpl.mm @@ -0,0 +1,773 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +// Copyright (c) 2017 Jason von Nieda, Niels Moseley +// +// The MIT License (MIT) +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#import + +#import "UvcControlImpl.h" + +#include "Log.h" +#include "UsbCameraImpl.h" + +template +inline void NamedLog(UvcControlImpl* objc, unsigned int level, + const char* file, unsigned int line, const S& format, + Args&&... args) { + auto sharedThis = objc.cppImpl.lock(); + if (!sharedThis) { + return; + } + + wpi::Logger& logger = sharedThis->objcGetLogger(); + std::string_view name = sharedThis->GetName(); + + if (logger.HasLogger() && level >= logger.min_level()) { + cs::NamedLogV(logger, level, file, line, name, format, + fmt::make_format_args(args...)); + } +} + +#define UVCLOG(level, format, ...) \ + NamedLog(self, level, __FILE__, __LINE__, \ + format __VA_OPT__(, ) __VA_ARGS__) + +#define UVCERROR(format, ...) \ + UVCLOG(::wpi::WPI_LOG_ERROR, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCWARNING(format, ...) \ + UVCLOG(::wpi::WPI_LOG_WARNING, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCINFO(format, ...) \ + UVCLOG(::wpi::WPI_LOG_INFO, format __VA_OPT__(, ) __VA_ARGS__) + +#ifdef NDEBUG +#define UVCDEBUG(format, ...) \ + do { \ + } while (0) +#define UVCDEBUG1(format, ...) \ + do { \ + } while (0) +#define UVCDEBUG2(format, ...) \ + do { \ + } while (0) +#define UVCDEBUG3(format, ...) \ + do { \ + } while (0) +#define UVCDEBUG4(format, ...) \ + do { \ + } while (0) +#else +#define UVCDEBUG(format, ...) \ + UVCLOG(::wpi::WPI_LOG_DEBUG, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCDEBUG1(format, ...) \ + UVCLOG(::wpi::WPI_LOG_DEBUG1, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCDEBUG2(format, ...) \ + UVCLOG(::wpi::WPI_LOG_DEBUG2, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCDEBUG3(format, ...) \ + UVCLOG(::wpi::WPI_LOG_DEBUG3, format __VA_OPT__(, ) __VA_ARGS__) +#define UVCDEBUG4(format, ...) \ + UVCLOG(::wpi::WPI_LOG_DEBUG4, format __VA_OPT__(, ) __VA_ARGS__) +#endif + +// USB descriptor for UVC processing unit +struct ProcessingUnitDescriptor +{ + uint8_t bLength; + uint8_t bDescriptorType; // CS_INTERFACE 0x24 + uint8_t bDescriptorSubtype; // VC_PROCESSING_UNIT 0x05 + uint8_t bUnitID; +}; + +struct propertyInfo_t +{ + uint32_t selector; // selector ID + uint32_t unit; // unit (==0 for INPUT TERMINA:, ==1 for PROCESSING UNIT) + uint32_t length; // length (bytes) +}; + +/** The order of the propertyInfo structure must + be the same as the PROPID numbers in the + openpnp-capture.h header */ +const propertyInfo_t propertyInfo[] = +{ + {0,0,0}, + {CT_EXPOSURE_TIME_ABSOLUTE_CONTROL , 0, 4}, + {CT_FOCUS_ABSOLUTE_CONTROL , 0, 2}, + {CT_ZOOM_ABSOLUTE_CONTROL , 0, 2}, + {PU_WHITE_BALANCE_TEMPERATURE_CONTROL, 1, 2}, + {PU_GAIN_CONTROL , 1, 2}, + {PU_BRIGHTNESS_CONTROL , 1, 2}, + {PU_CONTRAST_CONTROL , 1, 2}, + {PU_SATURATION_CONTROL , 1, 2}, + {PU_GAMMA_CONTROL , 1, 2}, + {PU_HUE_CONTROL , 1, 2}, + {PU_SHARPNESS_CONTROL , 1, 2}, + {PU_BACKLIGHT_COMPENSATION_CONTROL , 1, 2}, + {PU_POWER_LINE_FREQUENCY_CONTROL , 1, 1} +}; + +@implementation UvcControlImpl { + IOUSBDeviceInterface** _deviceInterface; +} + + ++ (instancetype)createFromAVCaptureDevice:(AVCaptureDevice*)device status:(CS_Status*)status { + if (!device) { + NSLog(@"UVC: device is nil"); + *status = CS_UVC_STATUS_ERROR; + return nil; + } + + NSError* error = nil; + NSRegularExpression* regex = [NSRegularExpression regularExpressionWithPattern:@"^UVC\\s+Camera\\s+VendorID\\_([0-9]+)\\s+ProductID\\_([0-9]+)$" + options:0 + error:&error]; + if (error) { + NSLog(@"UVC: failed to create regex: %@", error); + *status = CS_UVC_STATUS_ERROR; + return nil; + } + + NSString* modelID = [device valueForKey:@"modelID"]; + if (!modelID) { + NSLog(@"UVC: modelID is nil"); + *status = CS_UVC_STATUS_ERROR; + return nil; + } + + NSTextCheckingResult* match = [regex firstMatchInString:modelID + options:0 + range:NSMakeRange(0, modelID.length)]; + if (!match || match.numberOfRanges != 3) { + NSLog(@"UVC: modelID regex match failed"); + *status = CS_UVC_STATUS_ERROR; + return nil; + } + + NSString* vendorIDStr = [modelID substringWithRange:[match rangeAtIndex:1]]; + NSString* productIDStr = [modelID substringWithRange:[match rangeAtIndex:2]]; + uint16_t vendorID = (uint16_t)strtoul([vendorIDStr UTF8String], NULL, 10); + uint16_t productID = (uint16_t)strtoul([productIDStr UTF8String], NULL, 10); + + uint32_t locationID = 0; + CFMutableDictionaryRef dict = IOServiceMatching(kIOUSBDeviceClassName); + CFDictionarySetValue(dict, CFSTR("idVendor"), (__bridge CFNumberRef)@(vendorID)); + CFDictionarySetValue(dict, CFSTR("idProduct"), (__bridge CFNumberRef)@(productID)); + + io_iterator_t iter = 0; + kern_return_t ioResult = IOServiceGetMatchingServices(kIOMainPortDefault, dict, &iter); + if (ioResult == kIOReturnSuccess) { + io_service_t usbDevice = IOIteratorNext(iter); + while (usbDevice != 0) { + CFTypeRef locationIDRef = IORegistryEntryCreateCFProperty(usbDevice, + CFSTR("locationID"), + kCFAllocatorDefault, + 0); + if (locationIDRef) { + locationID = [(__bridge NSNumber*)locationIDRef unsignedIntValue]; + CFRelease(locationIDRef); + NSString* uniqueID = [device valueForKey:@"uniqueID"]; + NSString* locationIDHex = [NSString stringWithFormat:@"0x%x", locationID]; + if ([uniqueID hasPrefix:locationIDHex]) { + IOObjectRelease(usbDevice); + break; + } + } + IOObjectRelease(usbDevice); + usbDevice = IOIteratorNext(iter); + } + IOObjectRelease(iter); + } + + UvcControlImpl *instance = [[UvcControlImpl alloc] initWithVendorId:vendorID + productId:productID + location:locationID + status:status]; + if (!instance) { + NSLog(@"UVC: failed to create UvcControlImpl, status=%d", *status); + } + return instance; +} + +- (instancetype)initWithVendorId:(uint16_t)vid + productId:(uint16_t)pid + location:(uint32_t)location + status:(CS_Status*)status { + self = [super init]; + if (self) { + // UVCINFO("Initializing with VID: 0x{:04X}, PID: 0x{:04X}, Location: 0x{:08X}", vid, pid, location); + _deviceInterface = [self findDevice:vid productId:pid location:location]; + if (_deviceInterface == nullptr) { + // UVCWARNING("Failed to find device"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return nil; + } + + _processingUnitID = [self getProcessingUnitID:_deviceInterface]; + + _controlInterface = [self createControlInterface:_deviceInterface]; + + if (_controlInterface == nullptr) { + // UVCWARNING("Failed to create control interface"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return nil; + } + } + return self; +} + +- (void)dealloc { + if (_controlInterface != nullptr) { + (*_controlInterface)->USBInterfaceClose(_controlInterface); + (*_controlInterface)->Release(_controlInterface); + } + if (_deviceInterface != nullptr) { + (*_deviceInterface)->Release(_deviceInterface); + } +} + +- (IOUSBDeviceInterface**)findDevice:(uint16_t)vid + productId:(uint16_t)pid + location:(uint32_t)location { + + CFMutableDictionaryRef dict = IOServiceMatching(kIOUSBDeviceClassName); + + io_iterator_t serviceIterator; + kern_return_t result = IOServiceGetMatchingServices((mach_port_t)NULL, dict, &serviceIterator); + if (result != kIOReturnSuccess) { + UVCERROR("findDevice: IOServiceGetMatchingServices failed: {}", result); + return nullptr; + } + + io_service_t device; + while((device = IOIteratorNext(serviceIterator)) != 0) { + IOUSBDeviceInterface **deviceInterface = nullptr; + IOCFPlugInInterface **plugInInterface = nullptr; + SInt32 score; + + kern_return_t result = IOCreatePlugInInterfaceForService( + device, kIOUSBDeviceUserClientTypeID, + kIOCFPlugInInterfaceID, &plugInInterface, &score); + + if ((result != kIOReturnSuccess) || (plugInInterface == nullptr)) { + UVCERROR("findDevice: Camera control error: {}", result); + IOObjectRelease(device); + continue; + } + + HRESULT hr = (*plugInInterface)->QueryInterface(plugInInterface, + CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID), + (LPVOID*)&deviceInterface); + + if (hr || (deviceInterface == nullptr)) { + (*plugInInterface)->Release(plugInInterface); + IOObjectRelease(device); + UVCERROR("findDevice: QueryInterface failed"); + continue; + } + + uint16_t vendorID, productID; + uint32_t locationID; + result = (*deviceInterface)->GetDeviceVendor(deviceInterface, &vendorID); + result = (*deviceInterface)->GetDeviceProduct(deviceInterface, &productID); + result = (*deviceInterface)->GetLocationID(deviceInterface, &locationID); + + // if 'location' is zero, we won't match on location + // to achieve this, we simply set locationID to zero. + if (location == 0) { + locationID = 0; + } + + if ((vendorID == vid) && (productID == pid) && (locationID == location)) { + (*plugInInterface)->Release(plugInInterface); + IOObjectRelease(device); + IOObjectRelease(serviceIterator); + return deviceInterface; + } + + (*deviceInterface)->Release(deviceInterface); + (*plugInInterface)->Release(plugInInterface); + IOObjectRelease(device); + } + + IOObjectRelease(serviceIterator); + return nullptr; +} + +- (uint32_t)getProcessingUnitID:(IOUSBDeviceInterface**)dev { + IOReturn kr; + IOUSBConfigurationDescriptorPtr configDesc; + + kr = (*dev)->GetConfigurationDescriptorPtr(dev, 0, &configDesc); + if (kr) { + return 0; + } + + UVCDEBUG4("USB descriptor:"); + UVCDEBUG4(" length = 0x{:08X}", configDesc->bLength); + UVCDEBUG4(" type = 0x{:08X}", configDesc->bDescriptorType); + UVCDEBUG4(" totalLen = 0x{:08X}", configDesc->wTotalLength); + UVCDEBUG4(" interfaces = 0x{:08X}", configDesc->bNumInterfaces); + + uint32_t idx = 0; + uint8_t *ptr = (uint8_t*)configDesc; + + // Search for VIDEO/CONTROL interface descriptor + // Class=14, Subclass=1, Protocol=0 + // and find the processing unit, if available.. + // DescriptorType 0x24, DescriptorSubType 0x5 + + IOUSBInterfaceDescriptor *iface = NULL; + ProcessingUnitDescriptor *pud = NULL; + bool inVideoControlInterfaceDescriptor = false; + while(idx < configDesc->wTotalLength) { + IOUSBDescriptorHeader *hdr = (IOUSBDescriptorHeader *)&ptr[idx]; + switch(hdr->bDescriptorType) + { + case 0x05: // Endpoint descriptor ID + break; + case 0x02: // Configuration descriptor ID + break; + case 0x04: // Interface descriptor ID + iface = (IOUSBInterfaceDescriptor*)&ptr[idx]; + if ((iface->bInterfaceClass == 14) && + (iface->bInterfaceSubClass == 1) && + (iface->bInterfaceProtocol == 0)) + { + inVideoControlInterfaceDescriptor = true; + } + else + { + inVideoControlInterfaceDescriptor = false; + } + break; + case 0x24: // class-specific ID + pud = (ProcessingUnitDescriptor*)&ptr[idx]; + if (inVideoControlInterfaceDescriptor) + { + if (pud->bDescriptorSubtype == 0x05) + { + return pud->bUnitID; + } + } + break; + default: + break; + } + idx += hdr->bLength; + } + + return 0; +} + +- (IOUSBInterfaceInterface190**)createControlInterface:(IOUSBDeviceInterface**)deviceInterface { + IOUSBInterfaceInterface190 **controlInterface; + + io_iterator_t interfaceIterator; + IOUSBFindInterfaceRequest interfaceRequest; + interfaceRequest.bInterfaceClass = UVC_CONTROL_INTERFACE_CLASS; + interfaceRequest.bInterfaceSubClass = UVC_CONTROL_INTERFACE_SUBCLASS; + interfaceRequest.bInterfaceProtocol = kIOUSBFindInterfaceDontCare; + interfaceRequest.bAlternateSetting = kIOUSBFindInterfaceDontCare; + + IOReturn result = (*deviceInterface)->CreateInterfaceIterator(deviceInterface, + &interfaceRequest, &interfaceIterator); + + if (result != kIOReturnSuccess) { + return nullptr; + } + + io_service_t usbInterface; + if ((usbInterface = IOIteratorNext(interfaceIterator)) != 0) { + IOCFPlugInInterface **plugInInterface = nullptr; + SInt32 score; + + kern_return_t kr = IOCreatePlugInInterfaceForService(usbInterface, + kIOUSBInterfaceUserClientTypeID, + kIOCFPlugInInterfaceID, + &plugInInterface, + &score); + + kr = IOObjectRelease(usbInterface); + if ((kr != kIOReturnSuccess) || !plugInInterface) { + UVCERROR("createControlInterface: cannot create plug-in {:08X}", + kr); + return nullptr; + } + + HRESULT hr = (*plugInInterface)->QueryInterface(plugInInterface, + CFUUIDGetUUIDBytes(kIOUSBInterfaceInterfaceID), + (LPVOID*) &controlInterface); + + (*plugInInterface)->Release(plugInInterface); + + if (hr || !controlInterface) { + UVCERROR("createControlInterface: cannot create device interface {:08X}", + result); + return nullptr; + } + + UVCDEBUG3("createControlInterface: created control interface"); + return controlInterface; + } + return nullptr; +} + +- (bool)sendControlRequest:(IOUSBDevRequest)req { + if (_controlInterface == nullptr) { + UVCERROR("control interface is NULL"); + return false; + } + + kern_return_t kr; + if (@available(macOS 12.0, *)) { + // macOS 12 doesn't like if we're trying to open USB interface here... + } else { + kr = (*_controlInterface)->USBInterfaceOpen(_controlInterface); + if (kr != kIOReturnSuccess) { + UVCERROR("USBInterfaceOpen failed with error: 0x{:08X}", kr); + return false; + } + } + + kr = (*_controlInterface)->ControlRequest(_controlInterface, 0, &req); + if (kr != kIOReturnSuccess) { + // IOKIT error code + #define err_get_system(err) (((err)>>26)&0x3f) + #define err_get_sub(err) (((err)>>14)&0xfff) + #define err_get_code(err) ((err)&0x3fff) + + uint32_t code = err_get_code(kr); + uint32_t sys = err_get_system(kr); + uint32_t sub = err_get_sub(kr); + + switch(kr) + { + case kIOUSBUnknownPipeErr: + UVCERROR("Pipe ref not recognised"); + break; + case kIOUSBTooManyPipesErr: + UVCERROR("Too many pipes"); + break; + case kIOUSBEndpointNotFound: + UVCERROR("Endpoint not found"); + break; + case kIOUSBConfigNotFound: + UVCERROR("USB configuration not found"); + break; + case kIOUSBPipeStalled: + //Note: we don't report this as an error as this happens when + // an unsupported or locked property is set. + UVCDEBUG("Pipe has stalled, error needs to be cleared"); + break; + case kIOUSBInterfaceNotFound: + UVCERROR("USB control interface not found"); + break; + default: + UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02Xh}:{:03Xh}:{:04Xh}", + sys, sub, code); + break; + } + + if (@available(macOS 12.0, *)) { + // macOS 12 doesn't like if we're trying to close USB interface here... + } else { + kr = (*_controlInterface)->USBInterfaceClose(_controlInterface); + if (kr != kIOReturnSuccess) { + UVCERROR("USBInterfaceClose failed"); + } + } + return false; + } + + if (@available(macOS 12.0, *)) { + // macOS 12 doesn't like if we're trying to close USB interface here either... + } else { + kr = (*_controlInterface)->USBInterfaceClose(_controlInterface); + + if (kr != kIOReturnSuccess) { + UVCERROR("USBInterfaceClose failed"); + } + } + + return true; +} + +- (bool)setData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t)data { + IOUSBDevRequest req; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBOut, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_SET_CUR; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = length; + req.wLenDone = 0; + req.pData = &data; + return [self sendControlRequest:req]; +} + +- (bool)getData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data { + IOUSBDevRequest req; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_GET_CUR; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = length; + req.wLenDone = 0; + req.pData = data; + return [self sendControlRequest:req]; +} + +- (bool)getMaxData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data { + IOUSBDevRequest req; + *data = 0; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_GET_MAX; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = length; + req.wLenDone = 0; + req.pData = data; + return [self sendControlRequest:req]; +} + +- (bool)getMinData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data { + IOUSBDevRequest req; + *data = 0; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_GET_MIN; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = length; + req.wLenDone = 0; + req.pData = data; + return [self sendControlRequest:req]; +} + +- (bool)getDefault:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data { + IOUSBDevRequest req; + *data = 0; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_GET_DEF; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = length; + req.wLenDone = 0; + req.pData = data; + return [self sendControlRequest:req]; +} + +- (bool)getInfo:(uint32_t)selector unit:(uint32_t)unit data:(uint32_t*)data { + IOUSBDevRequest req; + *data = 0; + req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface); + req.bRequest = UVC_GET_INFO; + req.wValue = (selector << 8); + req.wIndex = (unit << 8); + req.wLength = 1; + req.wLenDone = 0; + req.pData = data; + return [self sendControlRequest:req]; +} + +- (bool)setProperty:(uint32_t)propID withValue:(int32_t)value status:(CS_Status*)status { + if (_controlInterface == nullptr) { + UVCERROR("control interface is NULL"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return false; + } + + bool ok = false; + if (propID < CAPPROPID_LAST) { + uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID; + ok = [self setData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:value]; + if (!ok) { + UVCWARNING("Failed to set property {}", propID); + } + } else { + UVCWARNING("Invalid property ID: {}", propID); + } + return ok; +} + +- (bool)getProperty:(uint32_t)propID withValue:(int32_t*)value status:(CS_Status*)status { + if (_controlInterface == nullptr) { + UVCERROR("control interface is NULL"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return false; + } + + bool ok = false; + if (propID < CAPPROPID_LAST) { + uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID; + ok = [self getData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:value]; + + switch(propertyInfo[propID].length) { + case 2: + *value = static_cast(*value); + break; + case 1: + *value = static_cast(*value); + break; + default: + break; + } + if (!ok) { + UVCWARNING("Failed to get property {}", propID); + } + } else { + UVCWARNING("Invalid property ID: {}", propID); + } + return ok; +} + +- (bool)setAutoProperty:(uint32_t)propID enabled:(bool)enabled status:(CS_Status*)status { + if (_controlInterface == nullptr) { + UVCERROR("control interface is NULL"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return false; + } + + int32_t value = enabled ? 1 : 0; + switch(propID) { + case CAPPROPID_EXPOSURE: + return [self setData:CT_AE_MODE_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:enabled ? 0x8 : 0x1]; + case CAPPROPID_WHITEBALANCE: + return [self setData:PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL unit:_processingUnitID length:1 data:value]; + case CAPPROPID_FOCUS: + return [self setData:CT_FOCUS_AUTO_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:value]; + default: + return false; + } +} + +- (bool)getAutoProperty:(uint32_t)propID enabled:(bool*)enabled status:(CS_Status*)status { + if (_controlInterface == nullptr) { + UVCERROR("control interface is NULL"); + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return false; + } + + int32_t value; + + switch(propID) { + case CAPPROPID_EXPOSURE: + if ([self getData:CT_AE_MODE_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:&value]) { + // value = 1 -> manual mode + // 2 -> auto mode (I haven't seen this in the wild) + // 4 -> shutter priority mode (haven't seen this) + // 8 -> aperature prioritry mode (seen this used) + value &= 0xFF; + *enabled = (value==1) ? false : true; + return true; + } + return false; + case CAPPROPID_WHITEBALANCE: + if ([self getData:PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL unit:_processingUnitID length:1 data:&value]) { + value &= 0xFF; + *enabled = (value==1) ? true : false; + UVCDEBUG3("White balance auto mode: {}", *enabled ? "enabled" : "disabled"); + return true; + } + return false; + case CAPPROPID_FOCUS: + if ([self getData:CT_FOCUS_AUTO_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:&value]) { + value &= 0xFF; + *enabled = (value==1) ? true : false; + UVCDEBUG3("Focus auto mode: {}", *enabled ? "enabled" : "disabled"); + return true; + } + return false; + default: + UVCWARNING("Unsupported auto property ID: {}", propID); + return false; + } +} + +- (bool)getPropertyLimits:(uint32_t)propID min:(int32_t*)min max:(int32_t*)max defValue:(int32_t*)defValue status:(CS_Status*)status { + if (_controlInterface == nullptr) { + *status = CS_UVC_STATUS_DEVICE_DISCONNECTED; + return false; + } + + bool ok = true; + if (propID < CAPPROPID_LAST) { + uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID; + + if (![self getMinData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:min]) { + ok = false; + } + + if (![self getMaxData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:max]) { + ok = false; + } + + if (![self getDefault:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:defValue]) { + ok = false; + } + + switch(propertyInfo[propID].length) { + case 2: + *min = static_cast(*min); + *max = static_cast(*max); + *defValue = static_cast(*defValue); + break; + case 1: + *min = static_cast(*min); + *max = static_cast(*max); + *defValue = static_cast(*defValue); + break; + default: + break; + } + } else { + UVCWARNING("getPropertyLimits: property ID out of bounds"); + ok = false; + } + return ok; +} + +- (void)reportCapabilities:(uint32_t)selector unit:(uint32_t)unit { + uint32_t info; + [self getInfo:selector unit:unit data:&info]; + if (info & 0x01) { + UVCDEBUG4("GET "); + } + if (info & 0x02) { + UVCDEBUG4("SET "); + } + if (info & 0x04) { + UVCDEBUG4("DISABLED "); + } + if (info & 0x08) { + UVCDEBUG4("AUTO-UPD "); + } + if (info & 0x10) { + UVCDEBUG4("ASYNC "); + } + if (info & 0x20) { + UVCDEBUG4("DISCOMMIT"); + } + UVCDEBUG4(""); +} + +@end \ No newline at end of file diff --git a/datalog/src/main/java/edu/wpi/first/datalog/DataLog.java b/datalog/src/main/java/edu/wpi/first/datalog/DataLog.java index fed02a0a49..42985210a5 100644 --- a/datalog/src/main/java/edu/wpi/first/datalog/DataLog.java +++ b/datalog/src/main/java/edu/wpi/first/datalog/DataLog.java @@ -451,10 +451,10 @@ public class DataLog implements AutoCloseable { if (!seen.add(typeString)) { throw new UnsupportedOperationException(typeString + ": circular reference with " + seen); } - addSchema(typeString, "structschema", struct.getSchema(), timestamp); for (Struct inner : struct.getNested()) { addSchemaImpl(inner, timestamp, seen); } + addSchema(typeString, "structschema", struct.getSchema(), timestamp); seen.remove(typeString); } diff --git a/datalogtool/src/main/native/cpp/App.cpp b/datalogtool/src/main/native/cpp/App.cpp index 31512a0bd1..ee0d893d45 100644 --- a/datalogtool/src/main/native/cpp/App.cpp +++ b/datalogtool/src/main/native/cpp/App.cpp @@ -10,11 +10,9 @@ #include #include #include -#include - -#define IMGUI_DEFINE_MATH_OPERATORS #include #include +#include #include #include diff --git a/datalogtool/src/main/native/cpp/App.h b/datalogtool/src/main/native/cpp/App.h index a9978daf30..9d1520c8a0 100644 --- a/datalogtool/src/main/native/cpp/App.h +++ b/datalogtool/src/main/native/cpp/App.h @@ -4,7 +4,6 @@ #pragma once -#define IMGUI_DEFINE_MATH_OPERATORS #include void SetNextWindowPos(const ImVec2& pos, ImGuiCond cond = 0, diff --git a/glass/.styleguide b/glass/.styleguide index 148c63451e..e1943a80e7 100644 --- a/glass/.styleguide +++ b/glass/.styleguide @@ -26,8 +26,10 @@ includeOtherLibs { ^frc/ ^google/ ^imgui + portable-file-dialogs.h ^networktables/ ^ntcore + ^units ^wpi/ ^wpigui ^wpimath/ diff --git a/glass/src/lib/native/cpp/hardware/Gyro.cpp b/glass/src/lib/native/cpp/hardware/Gyro.cpp index 8ba5d5e1dd..cfc6e79855 100644 --- a/glass/src/lib/native/cpp/hardware/Gyro.cpp +++ b/glass/src/lib/native/cpp/hardware/Gyro.cpp @@ -7,8 +7,6 @@ #include #include -#define IMGUI_DEFINE_MATH_OPERATORS - #include #include #include diff --git a/glass/src/lib/native/cpp/other/Drive.cpp b/glass/src/lib/native/cpp/other/Drive.cpp index 60b6130d90..9ac1f78e14 100644 --- a/glass/src/lib/native/cpp/other/Drive.cpp +++ b/glass/src/lib/native/cpp/other/Drive.cpp @@ -6,12 +6,10 @@ #include #include - -#define IMGUI_DEFINE_MATH_OPERATORS +#include #include #include -#include #include "glass/DataSource.h" diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index cb29c6d0ee..4a3264001b 100644 --- a/glass/src/lib/native/cpp/other/Field2D.cpp +++ b/glass/src/lib/native/cpp/other/Field2D.cpp @@ -17,8 +17,6 @@ #include #include #include - -#define IMGUI_DEFINE_MATH_OPERATORS #include #include #include diff --git a/glass/src/lib/native/cpp/other/Mechanism2D.cpp b/glass/src/lib/native/cpp/other/Mechanism2D.cpp index b09acf81d3..ce4384f79a 100644 --- a/glass/src/lib/native/cpp/other/Mechanism2D.cpp +++ b/glass/src/lib/native/cpp/other/Mechanism2D.cpp @@ -17,8 +17,6 @@ #include #include #include - -#define IMGUI_DEFINE_MATH_OPERATORS #include #include #include diff --git a/glass/src/lib/native/cpp/other/Plot.cpp b/glass/src/lib/native/cpp/other/Plot.cpp index d17bd82f8d..0c65d97d54 100644 --- a/glass/src/lib/native/cpp/other/Plot.cpp +++ b/glass/src/lib/native/cpp/other/Plot.cpp @@ -22,7 +22,6 @@ #pragma GCC diagnostic ignored "-Wformat-nonliteral" #endif -#define IMGUI_DEFINE_MATH_OPERATORS #include #include #include diff --git a/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp b/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp index 6d81ac682a..b77678ee92 100644 --- a/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp +++ b/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp @@ -4,14 +4,12 @@ #include "glass/support/ExtraGuiWidgets.h" -#define IMGUI_DEFINE_MATH_OPERATORS -#include -#include - #include #include #include +#include +#include #include #include "glass/DataSource.h" diff --git a/glass/src/lib/native/include/glass/MainMenuBar.h b/glass/src/lib/native/include/glass/MainMenuBar.h index 914153636e..048ab9b9ef 100644 --- a/glass/src/lib/native/include/glass/MainMenuBar.h +++ b/glass/src/lib/native/include/glass/MainMenuBar.h @@ -4,12 +4,12 @@ #pragma once -#include - #include #include #include +#include + namespace glass { /** diff --git a/glass/src/lib/native/include/glass/Window.h b/glass/src/lib/native/include/glass/Window.h index 62b369cdd4..0a37f9ae88 100644 --- a/glass/src/lib/native/include/glass/Window.h +++ b/glass/src/lib/native/include/glass/Window.h @@ -9,7 +9,6 @@ #include #include -#define IMGUI_DEFINE_MATH_OPERATORS #include #include "glass/View.h" diff --git a/glass/src/lib/native/include/glass/other/Field2D.h b/glass/src/lib/native/include/glass/other/Field2D.h index 2b0f9a8ef0..9c9f72ae6f 100644 --- a/glass/src/lib/native/include/glass/other/Field2D.h +++ b/glass/src/lib/native/include/glass/other/Field2D.h @@ -10,8 +10,6 @@ #include #include #include - -#define IMGUI_DEFINE_MATH_OPERATORS #include #include diff --git a/glass/src/lib/native/include/glass/other/Mechanism2D.h b/glass/src/lib/native/include/glass/other/Mechanism2D.h index 440fed38b0..ab5ccdce54 100644 --- a/glass/src/lib/native/include/glass/other/Mechanism2D.h +++ b/glass/src/lib/native/include/glass/other/Mechanism2D.h @@ -6,8 +6,6 @@ #include #include - -#define IMGUI_DEFINE_MATH_OPERATORS #include #include diff --git a/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h b/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h index 6e6d76408f..dcf0223445 100644 --- a/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h +++ b/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h @@ -6,7 +6,6 @@ #include -#define IMGUI_DEFINE_MATH_OPERATORS #include namespace glass { diff --git a/ntcore/build.gradle b/ntcore/build.gradle index e4c3da5673..299afdd951 100644 --- a/ntcore/build.gradle +++ b/ntcore/build.gradle @@ -33,15 +33,9 @@ model { if (!it.buildable || !(it instanceof NativeBinarySpec)) { return } - if (it.component.name == "${nativeName}JNI") { - lib project: ':wpinet', library: 'wpinet', linkage: 'static' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' - lib project: ':datalog', library: 'datalog', linkage: 'static' - } else { - lib project: ':wpinet', library: 'wpinet', linkage: 'shared' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' - lib project: ':datalog', library: 'datalog', linkage: 'shared' - } + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib project: ':datalog', library: 'datalog', linkage: 'shared' } } } diff --git a/shared/config.gradle b/shared/config.gradle index 7732acac07..e819970603 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -31,6 +31,13 @@ nativeUtils.enableSourceLink() nativeUtils.wpi.addMacMinimumVersionArg() +nativeUtils.platformConfigs.each { + if (it.name.contains('osx')) { + it.linker.getArgs().add('-framework') + it.linker.getArgs().add('IOKit') + } +} + // Compress debug info on Linux nativeUtils.platformConfigs.each { if (it.name.contains('linux')) { diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 43519b1545..dce4b797bb 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -74,7 +74,7 @@ def tagList = [ /* --- Hardware --- */ "Analog", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP", "AddressableLEDs", "HAL", "Encoder", "Smart Motor Controller", "Digital Input", - "Digital Output", + "Digital Output", "Accelerometer", /* --- HID --- */ "XboxController", "PS4Controller", "PS5Controller", "Joystick", diff --git a/shared/jni/publish.gradle b/shared/jni/publish.gradle index a33ca82686..dd1ec1cb07 100644 --- a/shared/jni/publish.gradle +++ b/shared/jni/publish.gradle @@ -7,7 +7,6 @@ def baseArtifactId = nativeName def artifactGroupId = "edu.wpi.first.${nativeName}" def zipBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-cpp_CLS" ext.zipBaseName = zipBaseName -def jniBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jni_CLS" def jniCvStaticBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jnicvstatic_CLS" def licenseFile = file("$rootDir/license.md") @@ -77,28 +76,6 @@ model { "${nativeName}JNIShared" ], zipBaseName, Zip, project, includeStandardZipFormat) - def jniTaskList = createComponentZipTasks($.components, ["${nativeName}JNI"], jniBaseName, Jar, project, { task, value -> - value.each { binary -> - if (binary.buildable) { - if (binary instanceof SharedLibraryBinarySpec) { - task.dependsOn binary.tasks.link - def hashFile = new File(binary.sharedLibraryFile.parentFile.absolutePath, "${binary.component.baseName}.hash") - task.outputs.file(hashFile) - task.inputs.file(binary.sharedLibraryFile) - task.from(hashFile) { - into nativeUtils.getPlatformPath(binary) - } - task.doFirst { - hashFile.text = MessageDigest.getInstance("MD5").digest(binary.sharedLibraryFile.bytes).encodeHex().toString() - } - task.from(binary.sharedLibraryFile) { - into nativeUtils.getPlatformPath(binary) - } - } - } - } - }) - publications { cpp(MavenPublication) { taskList.each { @@ -111,15 +88,6 @@ model { groupId artifactGroupId version wpilibVersioning.version.get() } - jni(MavenPublication) { - jniTaskList.each { - artifact it - } - - artifactId = "${baseArtifactId}-jni" - groupId artifactGroupId - version wpilibVersioning.version.get() - } } if (project.hasProperty('cvStaticBuild') && project.getProperty('cvStaticBuild') == true) { diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 3265f3a020..57b9ff3dcd 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -145,53 +145,6 @@ model { } } } - "${nativeName}JNI"(JniNativeLibrarySpec) { - if (project.hasProperty('setBaseName')) { - baseName = setBaseName + 'jni' - } else { - baseName = nativeName + 'jni' - } - - if (project.hasProperty('skipJniSymbols')) { - checkSkipSymbols = skipJniSymbols - } - - enableCheckTask !project.hasProperty('skipJniCheck') - javaCompileTasks << compileJava - jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore) - jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32) - jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64) - sources { - cpp { - source { - srcDirs 'src/main/native/cpp' - if (project.hasProperty('generatedSources')) { - srcDir generatedSources - } - include '**/jni/**/*.cpp' - } - exportedHeaders { - srcDir 'src/main/native/include' - if (project.hasProperty('generatedHeaders')) { - srcDir generatedHeaders - } - include '**/*.h' - } - } - } - binaries.all { - if (it instanceof StaticLibraryBinarySpec) { - it.buildable = false - return - } - if (!project.hasProperty('noWpiutil')) { - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' - } - if (project.hasProperty('jniSplitSetup')) { - jniSplitSetup(it) - } - } - } // By default, a development executable will be generated. This is to help the case of // testing specific functionality of the library. "${nativeName}Dev"(NativeExecutableSpec) { diff --git a/thirdparty/imgui_suite/imgui/include/imconfig.h b/thirdparty/imgui_suite/imgui/include/imconfig.h index b56ba49467..291ce19954 100644 --- a/thirdparty/imgui_suite/imgui/include/imconfig.h +++ b/thirdparty/imgui_suite/imgui/include/imconfig.h @@ -98,7 +98,7 @@ operator MyVec4() const { return MyVec4(x,y,z,w); } */ //---- ...Or use Dear ImGui's own very basic math operators. -//#define IMGUI_DEFINE_MATH_OPERATORS +#define IMGUI_DEFINE_MATH_OPERATORS //---- Use 32-bit vertex indices (default is 16-bit) is one way to allow large meshes with more than 64K vertices. // Your renderer backend will need to support it (most example renderer backends support both 16/32-bit indices). diff --git a/upstream_utils/apriltag.py b/upstream_utils/apriltag.py index ea95919417..1c3d643848 100755 --- a/upstream_utils/apriltag.py +++ b/upstream_utils/apriltag.py @@ -1,9 +1,14 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if +from upstream_utils import ( + Lib, + comment_out_invalid_includes, + has_prefix, + walk_cwd_and_copy_if, +) def remove_tag(f: str): @@ -18,47 +23,45 @@ def remove_tag(f: str): return False -def copy_upstream_src(wpilib_root): - apriltag = os.path.join(wpilib_root, "apriltag") +def copy_upstream_src(wpilib_root: Path): + apriltag = wpilib_root / "apriltag" # Delete old install shutil.rmtree( - os.path.join(apriltag, "src/main/native/thirdparty/apriltag"), + apriltag / "src/main/native/thirdparty/apriltag", ignore_errors=True, ) shutil.rmtree( - os.path.join(apriltag, "src/main/include/thirdparty/apriltag"), + apriltag / "src/main/include/thirdparty/apriltag", ignore_errors=True, ) # Copy apriltag source files into allwpilib src_files = walk_cwd_and_copy_if( lambda dp, f: (f.endswith(".c") or f.endswith(".cpp")) - and not dp.startswith(os.path.join(".", "example")) - and not dp.startswith(os.path.join(".", "test")) - and not f.endswith("getopt.c") + and not has_prefix(dp, Path("example")) + and not has_prefix(dp, Path("test")) + and not f == "getopt.c" and not "py" in f and not remove_tag(f), - os.path.join(apriltag, "src/main/native/thirdparty/apriltag/src"), + apriltag / "src/main/native/thirdparty/apriltag/src", ) # Copy apriltag header files into allwpilib walk_cwd_and_copy_if( lambda dp, f: f.endswith(".h") - and not f.endswith("getopt.h") - and not f.endswith("postscript_utils.h") + and not f == "getopt.h" + and not f == "postscript_utils.h" and not remove_tag(f), - os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"), + apriltag / "src/main/native/thirdparty/apriltag/include", ) for f in src_files: comment_out_invalid_includes( f, [ - os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"), - os.path.join( - apriltag, "src/main/native/thirdparty/apriltag/include/common" - ), + apriltag / "src/main/native/thirdparty/apriltag/include", + apriltag / "src/main/native/thirdparty/apriltag/include/common", ], ) diff --git a/upstream_utils/argparse_lib.py b/upstream_utils/argparse_lib.py index 644193701c..2dd3363d0a 100755 --- a/upstream_utils/argparse_lib.py +++ b/upstream_utils/argparse_lib.py @@ -1,18 +1,17 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Copy header into allwpilib - dest_filename = os.path.join( - wpiutil, - f"src/main/native/thirdparty/argparse/include/wpi/argparse.h", + dest_filename = ( + wpiutil / f"src/main/native/thirdparty/argparse/include/wpi/argparse.h" ) shutil.copyfile("include/argparse/argparse.hpp", dest_filename) # Rename namespace from argparse to wpi diff --git a/upstream_utils/debugging.py b/upstream_utils/debugging.py index 117cf90048..a56348682c 100755 --- a/upstream_utils/debugging.py +++ b/upstream_utils/debugging.py @@ -1,26 +1,25 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/debugging/src", "src/main/native/thirdparty/debugging/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Copy debugging files into allwpilib filenames = walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "src")) - or dp.startswith(os.path.join(".", "include")), - os.path.join(wpiutil, "src/main/native/thirdparty/debugging"), + lambda dp, f: has_prefix(dp, Path("src")) or has_prefix(dp, Path("include")), + wpiutil / "src/main/native/thirdparty/debugging", ) for filename in filenames: diff --git a/upstream_utils/eigen.py b/upstream_utils/eigen.py index 875c6dfbba..93f68f79d5 100755 --- a/upstream_utils/eigen.py +++ b/upstream_utils/eigen.py @@ -1,13 +1,17 @@ #!/usr/bin/env python3 -import os -import re import shutil +from pathlib import Path -from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if +from upstream_utils import ( + Lib, + comment_out_invalid_includes, + has_prefix, + walk_cwd_and_copy_if, +) -def eigen_inclusions(dp, f): +def eigen_inclusions(dp: Path, f: str): """Returns true if the given file in the "Eigen" include directory of the Eigen git repo should be copied into allwpilib @@ -15,11 +19,9 @@ def eigen_inclusions(dp, f): dp -- directory path f -- filename """ - if not dp.startswith(os.path.join(".", "Eigen")): + if not has_prefix(dp, Path("Eigen")): return False - abspath = os.path.join(dp, f) - # Exclude NonMPL2.h since all non-MPL2 code will be excluded anyway if f == "NonMPL2.h": return False @@ -36,13 +38,13 @@ def eigen_inclusions(dp, f): if "MKL" in f: return False - # Include architectures we care about - if "Core/arch/" in abspath: + # Include architectures we care about by filtering for Core/arch + if "Core" in dp.parts and "arch" in dp.parts: return ( - "arch/AVX/" in abspath - or "arch/Default" in abspath - or "arch/NEON" in abspath - or "arch/SSE" in abspath + "AVX" in dp.parts + or "Default" in dp.parts + or "NEON" in dp.parts + or "SSE" in dp.parts ) # Include the following modules @@ -65,10 +67,13 @@ def eigen_inclusions(dp, f): "misc", "plugins", ] - return bool(re.search(r"|".join("/" + m for m in modules), abspath)) + for m in modules: + if m in dp.parts or f == m: + return True + return False -def unsupported_inclusions(dp, f): +def unsupported_inclusions(dp: Path, f: str): """Returns true if the given file in the "unsupported" include directory of the Eigen git repo should be copied into allwpilib @@ -76,58 +81,58 @@ def unsupported_inclusions(dp, f): dp -- directory path f -- filename """ - if not dp.startswith(os.path.join(".", "unsupported")): + if not has_prefix(dp, Path("unsupported")): return False - abspath = os.path.join(dp, f) + abspath = dp / f # Exclude build system and READMEs if f == "CMakeLists.txt" or "README" in f: return False # Include the MatrixFunctions module - return "MatrixFunctions" in abspath + return "MatrixFunctions" in abspath.parts -def copy_upstream_src(wpilib_root): - wpimath = os.path.join(wpilib_root, "wpimath") +def copy_upstream_src(wpilib_root: Path): + wpimath = wpilib_root / "wpimath" # Delete old install for d in ["src/main/native/thirdparty/eigen/include"]: - shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True) + shutil.rmtree(wpimath / d, ignore_errors=True) # Copy Eigen headers into allwpilib eigen_files = walk_cwd_and_copy_if( eigen_inclusions, - os.path.join(wpimath, "src/main/native/thirdparty/eigen/include"), + wpimath / "src/main/native/thirdparty/eigen/include", ) # Copy unsupported headers into allwpilib unsupported_files = walk_cwd_and_copy_if( unsupported_inclusions, - os.path.join(wpimath, "src/main/native/thirdparty/eigen/include"), + wpimath / "src/main/native/thirdparty/eigen/include", ) for f in eigen_files: comment_out_invalid_includes( - f, [os.path.join(wpimath, "src/main/native/thirdparty/eigen/include")] + f, [wpimath / "src/main/native/thirdparty/eigen/include"] ) for f in unsupported_files: comment_out_invalid_includes( - f, [os.path.join(wpimath, "src/main/native/thirdparty/eigen/include")] + f, [wpimath / "src/main/native/thirdparty/eigen/include"] ) shutil.copyfile( ".clang-format", - os.path.join(wpimath, "src/main/native/thirdparty/eigen/include/.clang-format"), + wpimath / "src/main/native/thirdparty/eigen/include/.clang-format", ) def main(): name = "eigen" url = "https://gitlab.com/libeigen/eigen.git" - # master on 2024-11-14 - tag = "0fb2ed140d4fc0108553ecfb25f2d7fc1a9319a1" + # master on 2025-05-18 + tag = "d81aa18f4dc56264b2cd7e2f230807d776a2d385" eigen = Lib(name, url, tag, copy_upstream_src) eigen.main() diff --git a/upstream_utils/eigen_patches/0001-Disable-warnings.patch b/upstream_utils/eigen_patches/0001-Disable-warnings.patch index eceda4a347..1e497db27f 100644 --- a/upstream_utils/eigen_patches/0001-Disable-warnings.patch +++ b/upstream_utils/eigen_patches/0001-Disable-warnings.patch @@ -1,17 +1,17 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 09:14:24 -0700 -Subject: [PATCH 1/3] Disable warnings +Subject: [PATCH 1/2] Disable warnings --- - Eigen/src/Core/util/DisableStupidWarnings.h | 6 ++++++ - 1 file changed, 6 insertions(+) + Eigen/src/Core/util/DisableStupidWarnings.h | 9 +++++++++ + 1 file changed, 9 insertions(+) diff --git a/Eigen/src/Core/util/DisableStupidWarnings.h b/Eigen/src/Core/util/DisableStupidWarnings.h -index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..7ecd7bf8cc927d07a28c9da4ebbe1ea4d4d2b97b 100644 +index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..031d75a4c8b715dafe7158fdc5c42cd4fb069235 100644 --- a/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/Eigen/src/Core/util/DisableStupidWarnings.h -@@ -81,6 +81,12 @@ +@@ -81,6 +81,15 @@ // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325 #pragma GCC diagnostic ignored "-Wattributes" #endif @@ -20,6 +20,9 @@ index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..7ecd7bf8cc927d07a28c9da4ebbe1ea4 +#endif +#if __GNUC__ >= 12 +#pragma GCC diagnostic ignored "-Warray-bounds" ++#endif ++#if __GNUC__ >= 13 ++#pragma GCC diagnostic ignored "-Wstringop-overflow" +#endif #endif diff --git a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch index 73a956d5ca..5522d69109 100644 --- a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch +++ b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch @@ -1,17 +1,17 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 20 Jan 2023 23:41:56 -0800 -Subject: [PATCH 2/3] Intellisense fix +Subject: [PATCH 2/2] Intellisense fix --- Eigen/src/Core/util/ConfigureVectorization.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Eigen/src/Core/util/ConfigureVectorization.h b/Eigen/src/Core/util/ConfigureVectorization.h -index 47ddd4f8ae33405f2a6600dc33bd3d07a668e63f..d0fd181ecfdeb9a80e065b1d81e8860d6a6ad57c 100644 +index 49f307c734e937f013e659e931286a17ef6756f9..a9430716a320327aed81ea0cdffabc051aeb0ce2 100644 --- a/Eigen/src/Core/util/ConfigureVectorization.h +++ b/Eigen/src/Core/util/ConfigureVectorization.h -@@ -174,6 +174,13 @@ +@@ -178,6 +178,13 @@ //---------------------------------------------------------------------- diff --git a/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch b/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch deleted file mode 100644 index a5c7dc83ff..0000000000 --- a/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch +++ /dev/null @@ -1,305 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Sun, 12 Jan 2025 21:04:07 -0800 -Subject: [PATCH 3/3] Make assignment constexpr - ---- - Eigen/src/Core/AssignEvaluator.h | 165 +++++++++++-------- - Eigen/src/Core/EigenBase.h | 2 +- - Eigen/src/Core/functors/AssignmentFunctors.h | 2 +- - 3 files changed, 102 insertions(+), 67 deletions(-) - -diff --git a/Eigen/src/Core/AssignEvaluator.h b/Eigen/src/Core/AssignEvaluator.h -index f7f0b238b8ca70bbc9100262479cc1dbebab9979..9c2436afa7fe98692a036e6ef255ed104a5bf388 100644 ---- a/Eigen/src/Core/AssignEvaluator.h -+++ b/Eigen/src/Core/AssignEvaluator.h -@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling { - DstAlignment = Kernel::AssignmentTraits::DstAlignment - }; - -- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { -+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { - kernel.template assignPacketByOuterInner(outer, inner); - enum { NextIndex = Index + unpacket_traits::size }; - copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); -@@ -431,17 +431,25 @@ struct dense_assignment_loop { - template - struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { -- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -- typedef typename Kernel::PacketType PacketType; -- -- enum { -- size = DstXprType::SizeAtCompileTime, -- packetSize = unpacket_traits::size, -- alignedSize = (int(size) / packetSize) * packetSize -- }; -- -- copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); -- copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); -+ if (internal::is_constant_evaluated()) { -+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { -+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { -+ kernel.assignCoeffByOuterInner(outer, inner); -+ } -+ } -+ } else { -+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -+ typedef typename Kernel::PacketType PacketType; -+ -+ enum { -+ size = DstXprType::SizeAtCompileTime, -+ packetSize = unpacket_traits::size, -+ alignedSize = (int(size) / packetSize) * packetSize -+ }; -+ -+ copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); -+ copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); -+ } - } - }; - -@@ -465,9 +473,17 @@ struct dense_assignment_loop { - - template - struct dense_assignment_loop { -- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { -- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -- copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); -+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { -+ if (internal::is_constant_evaluated()) { -+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { -+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { -+ kernel.assignCoeffByOuterInner(outer, inner); -+ } -+ } -+ } else { -+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -+ copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); -+ } - } - }; - -@@ -498,8 +514,16 @@ struct dense_assignment_loop { - template - struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { -- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -- copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); -+ if (internal::is_constant_evaluated()) { -+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { -+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { -+ kernel.assignCoeffByOuterInner(outer, inner); -+ } -+ } -+ } else { -+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType; -+ copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); -+ } - } - }; - -@@ -510,41 +534,49 @@ struct dense_assignment_loop { - template - struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { -- typedef typename Kernel::Scalar Scalar; -- typedef typename Kernel::PacketType PacketType; -- enum { -- packetSize = unpacket_traits::size, -- requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), -- alignable = -- packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), -- dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), -- dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) -- }; -- const Scalar* dst_ptr = kernel.dstDataPtr(); -- if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { -- // the pointer is not aligned-on scalar, so alignment is not possible -- return dense_assignment_loop::run(kernel); -- } -- const Index packetAlignedMask = packetSize - 1; -- const Index innerSize = kernel.innerSize(); -- const Index outerSize = kernel.outerSize(); -- const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; -- Index alignedStart = -- ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); -- -- for (Index outer = 0; outer < outerSize; ++outer) { -- const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); -- // do the non-vectorizable part of the assignment -- for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); -- -- // do the vectorizable part of the assignment -- for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) -- kernel.template assignPacketByOuterInner(outer, inner); -- -- // do the non-vectorizable part of the assignment -- for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); -- -- alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); -+ if (internal::is_constant_evaluated()) { -+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) { -+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) { -+ kernel.assignCoeffByOuterInner(outer, inner); -+ } -+ } -+ } else { -+ typedef typename Kernel::Scalar Scalar; -+ typedef typename Kernel::PacketType PacketType; -+ enum { -+ packetSize = unpacket_traits::size, -+ requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), -+ alignable = -+ packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), -+ dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), -+ dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) -+ }; -+ const Scalar* dst_ptr = kernel.dstDataPtr(); -+ if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { -+ // the pointer is not aligned-on scalar, so alignment is not possible -+ return dense_assignment_loop::run(kernel); -+ } -+ const Index packetAlignedMask = packetSize - 1; -+ const Index innerSize = kernel.innerSize(); -+ const Index outerSize = kernel.outerSize(); -+ const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; -+ Index alignedStart = -+ ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); -+ -+ for (Index outer = 0; outer < outerSize; ++outer) { -+ const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); -+ // do the non-vectorizable part of the assignment -+ for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); -+ -+ // do the vectorizable part of the assignment -+ for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) -+ kernel.template assignPacketByOuterInner(outer, inner); -+ -+ // do the non-vectorizable part of the assignment -+ for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); -+ -+ alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); -+ } - } - } - }; -@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel { - typedef copy_using_evaluator_traits AssignmentTraits; - typedef typename AssignmentTraits::PacketType PacketType; - -- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst, -- const SrcEvaluatorType& src, -- const Functor& func, DstXprType& dstExpr) -+ EIGEN_DEVICE_FUNC -+ EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src, -+ const Functor& func, DstXprType& dstExpr) - : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) { - #ifdef EIGEN_DEBUG_ASSIGN - AssignmentTraits::debug(); -@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel { - EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; } - - /// Assign src(row,col) to dst(row,col) through the assignment functor. -- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) { -+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) { - m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col)); - } - -@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel { - } - - /// \sa assignCoeff(Index,Index) -- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) { -+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) { - Index row = rowIndexByOuterInner(outer, inner); - Index col = colIndexByOuterInner(outer, inner); - assignCoeff(row, col); -@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel { - assignPacket(row, col); - } - -- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) { -+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) { - typedef typename DstEvaluatorType::ExpressionTraits Traits; - return int(Traits::RowsAtCompileTime) == 1 ? 0 - : int(Traits::ColsAtCompileTime) == 1 ? inner -@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel { - : inner; - } - -- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) { -+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) { - typedef typename DstEvaluatorType::ExpressionTraits Traits; - return int(Traits::ColsAtCompileTime) == 1 ? 0 - : int(Traits::RowsAtCompileTime) == 1 ? inner -@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co - } - - template --EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, -- const internal::assign_op& /*func*/) { -+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, -+ const internal::assign_op& /*func*/) { - Index dstRows = src.rows(); - Index dstCols = src.cols(); - if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols); -@@ -790,7 +822,7 @@ struct Assignment; - // not has to bother about these annoying details. - - template --EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) { -+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) { - call_assignment(dst, src, internal::assign_op()); - } - template -@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( - } - - template --EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment( -+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment( - Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { - call_assignment_no_alias(dst, src, func); - } -@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src); - // both partial specialization+SFINAE without ambiguous specialization - template - struct Assignment { -- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) { -+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src, -+ const Functor& func) { - #ifndef EIGEN_NO_DEBUG -- internal::check_for_aliasing(dst, src); -+ if (!internal::is_constant_evaluated()) { -+ internal::check_for_aliasing(dst, src); -+ } - #endif - - call_dense_assignment_loop(dst, src, func); -diff --git a/Eigen/src/Core/EigenBase.h b/Eigen/src/Core/EigenBase.h -index 6d167006a094181fa3693b19f6b9daeb6f2afb79..894bfc13b15eb994abd90f100da15de5bd8b22b7 100644 ---- a/Eigen/src/Core/EigenBase.h -+++ b/Eigen/src/Core/EigenBase.h -@@ -50,7 +50,7 @@ struct EigenBase { - /** \returns a const reference to the derived object */ - EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast(this); } - -- EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { -+ EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const { - return *static_cast(const_cast(this)); - } - EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } -diff --git a/Eigen/src/Core/functors/AssignmentFunctors.h b/Eigen/src/Core/functors/AssignmentFunctors.h -index 09d1da8ca2bcb41384520f46e2b793ba8b28a798..3687bb20db4dfe1a2f6cf1342b4fcbd8f91f1f68 100644 ---- a/Eigen/src/Core/functors/AssignmentFunctors.h -+++ b/Eigen/src/Core/functors/AssignmentFunctors.h -@@ -23,7 +23,7 @@ namespace internal { - */ - template - struct assign_op { -- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } -+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; } - - template - EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { diff --git a/upstream_utils/expected.py b/upstream_utils/expected.py index cfa323c44c..68d27f9ae6 100755 --- a/upstream_utils/expected.py +++ b/upstream_utils/expected.py @@ -1,18 +1,16 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Copy expected header into allwpilib - dest_filename = os.path.join( - wpiutil, "src/main/native/thirdparty/expected/include/wpi/expected" - ) + dest_filename = wpiutil / "src/main/native/thirdparty/expected/include/wpi/expected" shutil.copyfile("include/tl/expected.hpp", dest_filename) # Rename namespace from tl to wpi, and detail to detail_expected diff --git a/upstream_utils/fmt.py b/upstream_utils/fmt.py index e0bb883c88..daad0523fd 100755 --- a/upstream_utils/fmt.py +++ b/upstream_utils/fmt.py @@ -1,33 +1,33 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/fmtlib/src", "src/main/native/thirdparty/fmtlib/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Copy fmt source files into allwpilib walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "src")) + lambda dp, f: has_prefix(dp, Path("src")) and f.endswith(".cc") and f != "fmt.cc", - os.path.join(wpiutil, "src/main/native/thirdparty/fmtlib"), + wpiutil / "src/main/native/thirdparty/fmtlib", ) # Copy fmt header files into allwpilib walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "include", "fmt")), - os.path.join(wpiutil, "src/main/native/thirdparty/fmtlib"), + lambda dp, f: has_prefix(dp, Path("include/fmt")), + wpiutil / "src/main/native/thirdparty/fmtlib", ) diff --git a/upstream_utils/gcem.py b/upstream_utils/gcem.py index 1b4f2d1e44..0b9dc4fab6 100755 --- a/upstream_utils/gcem.py +++ b/upstream_utils/gcem.py @@ -1,24 +1,24 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpimath = os.path.join(wpilib_root, "wpimath") +def copy_upstream_src(wpilib_root: Path): + wpimath = wpilib_root / "wpimath" # Delete old install for d in [ "src/main/native/thirdparty/gcem/include", ]: - shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True) + shutil.rmtree(wpimath / d, ignore_errors=True) # Copy gcem include files into allwpilib walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "include")), - os.path.join(wpimath, "src/main/native/thirdparty/gcem"), + lambda dp, f: has_prefix(dp, Path("include")), + wpimath / "src/main/native/thirdparty/gcem", ) diff --git a/upstream_utils/gl3w.py b/upstream_utils/gl3w.py index 1858d675a6..9b9065877a 100755 --- a/upstream_utils/gl3w.py +++ b/upstream_utils/gl3w.py @@ -1,16 +1,16 @@ #!/usr/bin/env python3 -import os +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - gl3w = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "gl3w") +def copy_upstream_src(wpilib_root: Path): + gl3w = wpilib_root / "thirdparty/imgui_suite/gl3w" walk_cwd_and_copy_if( lambda dp, f: f == "gl3w_gen.py", - os.path.join(gl3w), + gl3w, ) diff --git a/upstream_utils/glfw.py b/upstream_utils/glfw.py index a2d7e84540..30cba25bf8 100755 --- a/upstream_utils/glfw.py +++ b/upstream_utils/glfw.py @@ -1,57 +1,47 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def matches(dp, f, allowed_files): - path = os.path.join(dp, f) - return path in allowed_files - - -def walk_and_copy_if_matches(allowed_files, output_directory): - walk_cwd_and_copy_if( - lambda dp, f: matches(dp, f, allowed_files), - output_directory, - ) - - -def copy_upstream_src(wpilib_root): - glfw = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "glfw") +def copy_upstream_src(wpilib_root: Path): + glfw = wpilib_root / "thirdparty/imgui_suite/glfw" # Delete old install for d in ["include", "src", "CMake"]: - shutil.rmtree(os.path.join(glfw, d), ignore_errors=True) + shutil.rmtree(glfw / d, ignore_errors=True) hdr_allow_list = [ - "./include/GLFW/glfw3.h", - "./include/GLFW/glfw3native.h", + Path("include/GLFW/glfw3.h"), + Path("include/GLFW/glfw3native.h"), ] - walk_and_copy_if_matches(hdr_allow_list, glfw) + walk_cwd_and_copy_if( + lambda dp, f: dp / f in hdr_allow_list, + glfw, + ) - def src_filter(dp, f): - if f.endswith("CMakeLists.txt"): + def src_filter(dp: Path, f: str): + if f == "CMakeLists.txt": return False - if dp.startswith(os.path.join(".", "src")): + if has_prefix(dp, Path("src")): return True return False src_files = walk_cwd_and_copy_if( src_filter, - os.path.join(glfw), + glfw, ) - def cmake_filter(dp, f): - if dp.startswith(os.path.join(".", "CMake")): + def cmake_filter(dp: Path, f: str): + if has_prefix(dp, Path("CMake")): return True - path = os.path.join(dp, f) - if path in ["./src/CMakeLists.txt", "./CMakeLists.txt"]: + if dp / f in [Path("src/CMakeLists.txt"), Path("CMakeLists.txt")]: return True return False @@ -59,7 +49,7 @@ def copy_upstream_src(wpilib_root): # Copy CMAKE files walk_cwd_and_copy_if( cmake_filter, - os.path.join(glfw), + glfw, ) diff --git a/upstream_utils/googletest.py b/upstream_utils/googletest.py index 60613ee535..a58583b17b 100755 --- a/upstream_utils/googletest.py +++ b/upstream_utils/googletest.py @@ -2,8 +2,9 @@ import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if, walk_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if EXCLUDED_FILES = [ "gtest_main.cc", @@ -14,49 +15,39 @@ EXCLUDED_FILES = [ ] -def walk_and_remap_copy(third_party_root, include_prefix): - gmock_files = walk_if( - ".", lambda dp, f: include_prefix in dp and f not in EXCLUDED_FILES - ) - - for f in gmock_files: - dst_file = os.path.join( - third_party_root, "include", f[len(include_prefix) + 1 :] - ) - dest_dir = os.path.dirname(dst_file) - if not os.path.exists(dest_dir): - os.makedirs(dest_dir) - shutil.copyfile(f, dst_file) - - -def copy_upstream_src(wpilib_root): - third_party_root = os.path.join(wpilib_root, "thirdparty/googletest") +def copy_upstream_src(wpilib_root: Path): + upstream_root = Path(".").absolute() + third_party_root = wpilib_root / "thirdparty/googletest" # Delete old install for d in [ "include", "src", ]: - shutil.rmtree(os.path.join(third_party_root, d), ignore_errors=True) + shutil.rmtree(third_party_root / d, ignore_errors=True) walk_cwd_and_copy_if( - lambda dp, f: "googlemock/src" in dp and f not in EXCLUDED_FILES, - os.path.join(third_party_root, "src"), + lambda dp, f: has_prefix(dp, Path("googlemock/src")) + and f not in EXCLUDED_FILES, + third_party_root / "src", ) walk_cwd_and_copy_if( - lambda dp, f: "googletest/src" in dp and f not in EXCLUDED_FILES, - os.path.join(third_party_root, "src"), + lambda dp, f: has_prefix(dp, Path("googletest/src")) + and f not in EXCLUDED_FILES, + third_party_root / "src", ) - walk_and_remap_copy( - third_party_root, - "./googlemock/include", + os.chdir(upstream_root / "googlemock/include") + walk_cwd_and_copy_if( + lambda dp, f: f not in EXCLUDED_FILES, + third_party_root / "include", ) - walk_and_remap_copy( - third_party_root, - "./googletest/include", + os.chdir(upstream_root / "googletest/include") + walk_cwd_and_copy_if( + lambda dp, f: f not in EXCLUDED_FILES, + third_party_root / "include", ) diff --git a/upstream_utils/imgui.py b/upstream_utils/imgui.py index 76dce5e94d..0fd28ef8b7 100755 --- a/upstream_utils/imgui.py +++ b/upstream_utils/imgui.py @@ -1,62 +1,57 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def matches(dp, f, allowed_files): - path = os.path.join(dp, f) - return path in allowed_files - - -def walk_and_copy_if_matches(allowed_files, output_directory): +def walk_and_copy_if_matches(allowed_files: list[Path], output_directory: Path): walk_cwd_and_copy_if( - lambda dp, f: matches(dp, f, allowed_files), + lambda dp, f: dp / f in allowed_files, output_directory, ) -def copy_upstream_src(wpilib_root): - imgui = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "imgui") +def copy_upstream_src(wpilib_root: Path): + imgui = wpilib_root / "thirdparty/imgui_suite/imgui" # Delete old install for d in ["include", "cpp"]: - shutil.rmtree(os.path.join(imgui, d), ignore_errors=True) + shutil.rmtree(imgui / d, ignore_errors=True) hdr_allow_list = [ - "./imgui.h", - "./imstb_truetype.h", - "./imgui_internal.h", - "./imstb_rectpack.h", - "./imconfig.h", - "./imstb_textedit.h", - "./backends/imgui_impl_glfw.h", - "./backends/imgui_impl_metal.h", - "./backends/imgui_impl_opengl3.h", - "./backends/imgui_impl_dx11.h", - "./backends/imgui_impl_opengl3_loader.h", - "./backends/imgui_impl_opengl2.h", - "./misc/cpp/imgui_stdlib.h", + Path("imgui.h"), + Path("imstb_truetype.h"), + Path("imgui_internal.h"), + Path("imstb_rectpack.h"), + Path("imconfig.h"), + Path("imstb_textedit.h"), + Path("backends/imgui_impl_glfw.h"), + Path("backends/imgui_impl_metal.h"), + Path("backends/imgui_impl_opengl3.h"), + Path("backends/imgui_impl_dx11.h"), + Path("backends/imgui_impl_opengl3_loader.h"), + Path("backends/imgui_impl_opengl2.h"), + Path("misc/cpp/imgui_stdlib.h"), ] src_allow_list = [ - "./backends/imgui_impl_dx11.cpp", - "./backends/imgui_impl_glfw.cpp", - "./backends/imgui_impl_metal.mm", - "./backends/imgui_impl_opengl2.cpp", - "./backends/imgui_impl_opengl3.cpp", - "./imgui.cpp", - "./imgui_demo.cpp", - "./imgui_draw.cpp", - "./imgui_tables.cpp", - "./imgui_widgets.cpp", - "./misc/cpp/imgui_stdlib.cpp", + Path("backends/imgui_impl_dx11.cpp"), + Path("backends/imgui_impl_glfw.cpp"), + Path("backends/imgui_impl_metal.mm"), + Path("backends/imgui_impl_opengl2.cpp"), + Path("backends/imgui_impl_opengl3.cpp"), + Path("imgui.cpp"), + Path("imgui_demo.cpp"), + Path("imgui_draw.cpp"), + Path("imgui_tables.cpp"), + Path("imgui_widgets.cpp"), + Path("misc/cpp/imgui_stdlib.cpp"), ] - walk_and_copy_if_matches(hdr_allow_list, os.path.join(imgui, "include")) - walk_and_copy_if_matches(src_allow_list, os.path.join(imgui, "cpp")) + walk_and_copy_if_matches(hdr_allow_list, imgui / "include") + walk_and_copy_if_matches(src_allow_list, imgui / "cpp") def main(): diff --git a/upstream_utils/imgui_patches/0001-Set-IMGUI_DEFINE_MATH_OPERATORS.patch b/upstream_utils/imgui_patches/0001-Set-IMGUI_DEFINE_MATH_OPERATORS.patch new file mode 100644 index 0000000000..fc05f16c48 --- /dev/null +++ b/upstream_utils/imgui_patches/0001-Set-IMGUI_DEFINE_MATH_OPERATORS.patch @@ -0,0 +1,23 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Jade Turner +Date: Fri, 11 Apr 2025 11:20:41 +0800 +Subject: [PATCH] Set IMGUI_DEFINE_MATH_OPERATORS + +Signed-off-by: Jade Turner +--- + imconfig.h | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/imconfig.h b/imconfig.h +index b56ba49467ebb388e038f0a23aed46044085bdff..291ce1995412f10e784beaf3f4c51e02d90b9813 100644 +--- a/imconfig.h ++++ b/imconfig.h +@@ -98,7 +98,7 @@ + operator MyVec4() const { return MyVec4(x,y,z,w); } + */ + //---- ...Or use Dear ImGui's own very basic math operators. +-//#define IMGUI_DEFINE_MATH_OPERATORS ++#define IMGUI_DEFINE_MATH_OPERATORS + + //---- Use 32-bit vertex indices (default is 16-bit) is one way to allow large meshes with more than 64K vertices. + // Your renderer backend will need to support it (most example renderer backends support both 16/32-bit indices). diff --git a/upstream_utils/implot.py b/upstream_utils/implot.py index 73101e84d9..c43d3dfef5 100755 --- a/upstream_utils/implot.py +++ b/upstream_utils/implot.py @@ -1,37 +1,27 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def matches(dp, f, allowed_files): - path = os.path.join(dp, f) - return path in allowed_files - - -def walk_and_copy_if_matches(allowed_files, output_directory): - walk_cwd_and_copy_if( - lambda dp, f: matches(dp, f, allowed_files), - output_directory, - ) - - -def copy_upstream_src(wpilib_root): - implot = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "implot") +def copy_upstream_src(wpilib_root: Path): + implot = wpilib_root / "thirdparty/imgui_suite/implot" # Delete old install for d in ["include", "cpp"]: - shutil.rmtree(os.path.join(implot, d), ignore_errors=True) + shutil.rmtree(implot / d, ignore_errors=True) # Copy files walk_cwd_and_copy_if( lambda dp, f: f.endswith(".h"), - os.path.join(implot, "include"), + implot / "include", ) - walk_and_copy_if_matches( - ["./implot_items.cpp", "./implot.cpp"], os.path.join(implot, "cpp") + implot_files = [Path("implot_items.cpp"), Path("implot.cpp")] + walk_cwd_and_copy_if( + lambda dp, f: dp / f in implot_files, + implot / "cpp", ) diff --git a/upstream_utils/json.py b/upstream_utils/json.py index 050e4dd66e..2b83d44486 100755 --- a/upstream_utils/json.py +++ b/upstream_utils/json.py @@ -2,35 +2,32 @@ import os import shutil +from pathlib import Path from upstream_utils import Lib, walk_if -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/json/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Create lists of source and destination files os.chdir("include/nlohmann") - files = walk_if(".", lambda dp, f: True) - src_include_files = [os.path.abspath(f) for f in files] - wpiutil_json_root = os.path.join( - wpiutil, "src/main/native/thirdparty/json/include/wpi" - ) - dest_include_files = [ - os.path.join(wpiutil_json_root, f.replace(".hpp", ".h")) for f in files - ] + files = walk_if(Path("."), lambda dp, f: True) + src_include_files = [f.absolute() for f in files] + wpiutil_json_root = wpiutil / "src/main/native/thirdparty/json/include/wpi" + dest_include_files = [wpiutil_json_root / f.with_suffix(".h") for f in files] # Copy json header files into allwpilib for i in range(len(src_include_files)): - dest_dir = os.path.dirname(dest_include_files[i]) - if not os.path.exists(dest_dir): - os.makedirs(dest_dir) + dest_dir = dest_include_files[i].parent + if not dest_dir.exists(): + dest_dir.mkdir(parents=True) shutil.copyfile(src_include_files[i], dest_include_files[i]) for include_file in dest_include_files: diff --git a/upstream_utils/libdogleg.py b/upstream_utils/libdogleg.py index 68ab38326a..590df443fa 100755 --- a/upstream_utils/libdogleg.py +++ b/upstream_utils/libdogleg.py @@ -1,24 +1,24 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpical = os.path.join(wpilib_root, "wpical") +def copy_upstream_src(wpilib_root: Path): + wpical = wpilib_root / "wpical" # Delete old install for d in [ "src/main/native/thirdparty/libdogleg/src", "src/main/native/thirdparty/libdogleg/include", ]: - shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + shutil.rmtree(wpical / d, ignore_errors=True) files = walk_cwd_and_copy_if( - lambda dp, f: f.endswith("dogleg.h"), - os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"), + lambda dp, f: f == "dogleg.h", + wpical / "src/main/native/thirdparty/libdogleg/include", ) for f in files: with open(f) as file: @@ -30,8 +30,8 @@ def copy_upstream_src(wpilib_root): file.write(content) files = walk_cwd_and_copy_if( - lambda dp, f: f.endswith("dogleg.cpp"), - os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"), + lambda dp, f: f == "dogleg.cpp", + wpical / "src/main/native/thirdparty/libdogleg/src", ) for f in files: with open(f) as file: diff --git a/upstream_utils/libuv.py b/upstream_utils/libuv.py index c837fe96be..469a996833 100755 --- a/upstream_utils/libuv.py +++ b/upstream_utils/libuv.py @@ -1,17 +1,17 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpinet = os.path.join(wpilib_root, "wpinet") +def copy_upstream_src(wpilib_root: Path): + wpinet = wpilib_root / "wpinet" # Delete old install for d in ["src/main/native/thirdparty/libuv"]: - shutil.rmtree(os.path.join(wpinet, d), ignore_errors=True) + shutil.rmtree(wpinet / d, ignore_errors=True) include_ignorelist = [ "aix.h", @@ -21,9 +21,8 @@ def copy_upstream_src(wpilib_root): ] walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "include")) - and f not in include_ignorelist, - os.path.join(wpinet, "src/main/native/thirdparty/libuv"), + lambda dp, f: has_prefix(dp, Path("include")) and f not in include_ignorelist, + wpinet / "src/main/native/thirdparty/libuv", ) src_ignorelist = [ @@ -43,9 +42,8 @@ def copy_upstream_src(wpilib_root): "sysinfo-memory.c", ] walk_cwd_and_copy_if( - lambda dp, f: dp.startswith(os.path.join(".", "src")) - and f not in src_ignorelist, - os.path.join(wpinet, "src/main/native/thirdparty/libuv"), + lambda dp, f: has_prefix(dp, Path("src")) and f not in src_ignorelist, + wpinet / "src/main/native/thirdparty/libuv", rename_c_to_cpp=True, ) diff --git a/upstream_utils/llvm.py b/upstream_utils/llvm.py index 0098c1be45..a3c198606f 100755 --- a/upstream_utils/llvm.py +++ b/upstream_utils/llvm.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path from upstream_utils import Lib -def run_global_replacements(wpiutil_llvm_files): +def run_global_replacements(wpiutil_llvm_files: list[Path]): for wpi_file in wpiutil_llvm_files: with open(wpi_file) as f: content = f.read() @@ -23,7 +23,7 @@ def run_global_replacements(wpiutil_llvm_files): # Fix uses of span content = content.replace("span", "std::span") content = content.replace("include ", "include ") - if wpi_file.endswith("ConvertUTFWrapper.cpp"): + if wpi_file.name == "ConvertUTFWrapper.cpp": content = content.replace( "const UTF16 *Src = reinterpret_cast(SrcBytes.begin());", "const UTF16 *Src = reinterpret_cast(&*SrcBytes.begin());", @@ -88,19 +88,19 @@ def run_global_replacements(wpiutil_llvm_files): f.write(content) -def flattened_llvm_files(llvm, dirs_to_keep): - file_lookup = {} +def flattened_llvm_files(llvm: Path, dirs_to_keep: list[Path]): + file_lookup: dict[str, Path] = {} for dir_to_keep in dirs_to_keep: - dir_to_crawl = os.path.join(llvm, dir_to_keep) - for root, _, files in os.walk(dir_to_crawl): + dir_to_crawl = llvm / dir_to_keep + for root, _, files in dir_to_crawl.walk(): for f in files: - file_lookup[f] = os.path.join(root, f) + file_lookup[f] = root / f return file_lookup -def find_wpiutil_llvm_files(wpiutil_root, subfolder): +def find_wpiutil_llvm_files(wpiutil_root: Path, subfolder: str): # These files have substantial changes, not worth managing with the patching process ignore_list = [ "StringExtras.h", @@ -110,22 +110,22 @@ def find_wpiutil_llvm_files(wpiutil_root, subfolder): "SmallVectorMemoryBuffer.h", ] - wpiutil_files = [] - for root, _, files in os.walk(os.path.join(wpiutil_root, subfolder)): + wpiutil_files: list[Path] = [] + for root, _, files in (wpiutil_root / subfolder).walk(): for f in files: if f not in ignore_list: - full_file = os.path.join(root, f) + full_file = root / f wpiutil_files.append(full_file) return wpiutil_files -def overwrite_files(wpiutil_files, llvm_files): +def overwrite_files(wpiutil_files: list[Path], llvm_files: dict[str, Path]): # Very sparse rips from LLVM sources. Not worth tyring to make match upstream unmatched_files_whitelist = ["fs.h", "fs.cpp", "function_ref.h"] for wpi_file in wpiutil_files: - wpi_base_name = os.path.basename(wpi_file) + wpi_base_name = wpi_file.name if wpi_base_name in llvm_files: shutil.copyfile(llvm_files[wpi_base_name], wpi_file) @@ -133,14 +133,14 @@ def overwrite_files(wpiutil_files, llvm_files): print(f"No file match for {wpi_file}, check if LLVM deleted it") -def overwrite_source(wpiutil_root, llvm_root): +def overwrite_source(wpiutil_root: Path, llvm_root: Path): llvm_files = flattened_llvm_files( llvm_root, [ - "llvm/include/llvm/ADT/", - "llvm/include/llvm/Config", - "llvm/include/llvm/Support/", - "llvm/lib/Support/", + Path("llvm/include/llvm/ADT/"), + Path("llvm/include/llvm/Config/"), + Path("llvm/include/llvm/Support/"), + Path("llvm/lib/Support/"), ], ) wpi_files = find_wpiutil_llvm_files( @@ -153,10 +153,14 @@ def overwrite_source(wpiutil_root, llvm_root): run_global_replacements(wpi_files) -def overwrite_tests(wpiutil_root, llvm_root): +def overwrite_tests(wpiutil_root: Path, llvm_root: Path): llvm_files = flattened_llvm_files( llvm_root, - ["llvm/unittests/ADT/", "llvm/unittests/Config", "llvm/unittests/Support/"], + [ + Path("llvm/unittests/ADT/"), + Path("llvm/unittests/Config/"), + Path("llvm/unittests/Support/"), + ], ) wpi_files = find_wpiutil_llvm_files(wpiutil_root, "src/test/native/cpp/llvm") @@ -164,9 +168,9 @@ def overwrite_tests(wpiutil_root, llvm_root): run_global_replacements(wpi_files) -def copy_upstream_src(wpilib_root): - upstream_root = os.path.abspath(".") - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + upstream_root = Path(".").absolute() + wpiutil = wpilib_root / "wpiutil" overwrite_source(wpiutil, upstream_root) overwrite_tests(wpiutil, upstream_root) diff --git a/upstream_utils/memory.py b/upstream_utils/memory.py index 0099de5ea9..99ecb11a89 100755 --- a/upstream_utils/memory.py +++ b/upstream_utils/memory.py @@ -2,11 +2,12 @@ import os import shutil +from pathlib import Path -from upstream_utils import Lib, copy_to, walk_if +from upstream_utils import Lib, walk_cwd_and_copy_if -def run_source_replacements(memory_files): +def run_source_replacements(memory_files: list[Path]): for wpi_file in memory_files: with open(wpi_file) as f: content = f.read() @@ -21,9 +22,9 @@ def run_source_replacements(memory_files): f.write(content) -def run_header_replacements(memory_files): +def run_header_replacements(memory_files: list[Path]): for wpi_file in memory_files: - if "detail" not in wpi_file: + if "detail" not in wpi_file.parts: continue with open(wpi_file) as f: content = f.read() @@ -35,7 +36,7 @@ def run_header_replacements(memory_files): f.write(content) -def run_global_replacements(memory_files): +def run_global_replacements(memory_files: list[Path]): for wpi_file in memory_files: with open(wpi_file) as f: content = f.read() @@ -52,42 +53,41 @@ def run_global_replacements(memory_files): f.write(content) -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + upstream_root = Path(".").absolute() + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/memory/src", "src/main/native/thirdparty/memory/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Copy sources - src_files = walk_if("src", lambda dp, f: f.endswith(".cpp") or f.endswith(".hpp")) - src_files = copy_to( - src_files, os.path.join(wpiutil, "src/main/native/thirdparty/memory") + os.chdir(upstream_root / "src") + src_files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith(".cpp") or f.endswith(".hpp"), + wpiutil / "src/main/native/thirdparty/memory/src", ) run_global_replacements(src_files) run_source_replacements(src_files) # Copy headers - os.chdir(os.path.join("include", "foonathan")) - include_files = walk_if(".", lambda dp, f: f.endswith(".hpp")) - include_files = copy_to( - include_files, - os.path.join(wpiutil, "src/main/native/thirdparty/memory/include/wpi"), + os.chdir(upstream_root / "include/foonathan") + include_files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith(".hpp"), + wpiutil / "src/main/native/thirdparty/memory/include/wpi", ) - os.chdir(os.path.join("..", "..")) + os.chdir(upstream_root) run_global_replacements(include_files) run_header_replacements(include_files) # Copy config_impl.hpp shutil.copyfile( - os.path.join(wpilib_root, "upstream_utils/memory_files/config_impl.hpp"), - os.path.join( - wpiutil, - "src/main/native/thirdparty/memory/include/wpi/memory/config_impl.hpp", - ), + wpilib_root / "upstream_utils/memory_files/config_impl.hpp", + wpiutil + / "src/main/native/thirdparty/memory/include/wpi/memory/config_impl.hpp", ) diff --git a/upstream_utils/mpack.py b/upstream_utils/mpack.py index 1acff50a37..e4fca5c39b 100755 --- a/upstream_utils/mpack.py +++ b/upstream_utils/mpack.py @@ -3,36 +3,34 @@ import os import shutil import subprocess +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/mpack/src", "src/main/native/thirdparty/mpack/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Run the amalgmation script subprocess.check_call(["bash", "tools/amalgamate.sh"]) # Copy the files - amalgamation_source_dir = os.path.join( - ".", ".build", "amalgamation", "src", "mpack" - ) - os.chdir(amalgamation_source_dir) + os.chdir(Path(".build/amalgamation/src/mpack")) walk_cwd_and_copy_if( lambda dp, f: f.endswith(".h"), - os.path.join(wpiutil, "src/main/native/thirdparty/mpack/include/wpi"), + wpiutil / "src/main/native/thirdparty/mpack/include/wpi", ) walk_cwd_and_copy_if( lambda dp, f: f.endswith(".c"), - os.path.join(wpiutil, "src/main/native/thirdparty/mpack/src"), + wpiutil / "src/main/native/thirdparty/mpack/src", rename_c_to_cpp=True, ) diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py index be6f57d7c6..4af10d908e 100755 --- a/upstream_utils/mrcal.py +++ b/upstream_utils/mrcal.py @@ -1,27 +1,27 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpical = os.path.join(wpilib_root, "wpical") +def copy_upstream_src(wpilib_root: Path): + wpical = wpilib_root / "wpical" # Delete old install for d in [ "src/main/native/thirdparty/mrcal/src", "src/main/native/thirdparty/mrcal/include", ]: - shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + shutil.rmtree(wpical / d, ignore_errors=True) files = walk_cwd_and_copy_if( lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) - and not f.endswith("heap.h") - and not f.endswith("stereo-matching-libelas.h") - and not dp.startswith(os.path.join(".", "test")), - os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + and not f == "heap.h" + and not f == "stereo-matching-libelas.h" + and not has_prefix(dp, Path("test")), + wpical / "src/main/native/thirdparty/mrcal/include", ) files = files + walk_cwd_and_copy_if( lambda dp, f: ( @@ -30,16 +30,16 @@ def copy_upstream_src(wpilib_root): or f.endswith(".cpp") or f.endswith(".pl") ) - and not f.endswith("heap.cc") - and not f.endswith("mrcal-pywrap.c") - and not f.endswith("image.c") - and not f.endswith("stereo.c") - and not f.endswith("stereo-matching-libelas.cc") - and not f.endswith("uncertainty.c") - and not f.endswith("traverse-sensor-links.c") - and not dp.startswith(os.path.join(".", "doc")) - and not dp.startswith(os.path.join(".", "test")), - os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"), + and not f == "heap.cc" + and not f == "mrcal-pywrap.c" + and not f == "image.c" + and not f == "stereo.c" + and not f == "stereo-matching-libelas.cc" + and not f == "uncertainty.c" + and not f == "traverse-sensor-links.c" + and not has_prefix(dp, Path("doc")) + and not has_prefix(dp, Path("test")), + wpical / "src/main/native/thirdparty/mrcal/src", ) for f in files: diff --git a/upstream_utils/mrcal_java.py b/upstream_utils/mrcal_java.py index 5fdd639b16..489df1f78c 100755 --- a/upstream_utils/mrcal_java.py +++ b/upstream_utils/mrcal_java.py @@ -2,46 +2,30 @@ import os import shutil +from pathlib import Path from upstream_utils import Lib, walk_cwd_and_copy_if -def delete_lines_by_range(file_path, start_line, end_line): - # Read all lines from the file - with open(file_path, "r") as file: - lines = file.readlines() - - # Filter out lines that are within the specified range - filtered_lines = [ - line - for i, line in enumerate(lines, start=1) - if not (start_line <= i <= end_line) - ] - - # Write the remaining lines back to the file - with open(file_path, "w") as file: - file.writelines(filtered_lines) - - -def copy_upstream_src(wpilib_root): - wpical = os.path.join(wpilib_root, "wpical") +def copy_upstream_src(wpilib_root: Path): + wpical = wpilib_root / "wpical" # Delete old install for d in [ "src/main/native/thirdparty/mrcal_java/src", "src/main/native/thirdparty/mrcal_java/include", ]: - shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + shutil.rmtree(wpical / d, ignore_errors=True) os.chdir("src") files = walk_cwd_and_copy_if( - lambda dp, f: f.endswith("mrcal_wrapper.h"), - os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/include"), + lambda dp, f: f == "mrcal_wrapper.h", + wpical / "src/main/native/thirdparty/mrcal_java/include", ) files = walk_cwd_and_copy_if( - lambda dp, f: f.endswith("mrcal_wrapper.cpp"), - os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/src"), + lambda dp, f: f == "mrcal_wrapper.cpp", + wpical / "src/main/native/thirdparty/mrcal_java/src", ) for f in files: diff --git a/upstream_utils/nanopb.py b/upstream_utils/nanopb.py index 1da46a8c19..543b19befa 100755 --- a/upstream_utils/nanopb.py +++ b/upstream_utils/nanopb.py @@ -1,39 +1,33 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, copy_to, walk_if +from upstream_utils import Lib, copy_to, has_prefix, walk_cwd_and_copy_if -nanopb_sources = set( - [ - "pb_encode.c", - "pb_decode.c", - "pb_common.c", - ] -) +nanopb_sources = [ + Path("pb_encode.c"), + Path("pb_decode.c"), + Path("pb_common.c"), +] -nanopb_headers = set( - [ - "pb.h", - "pb_encode.h", - "pb_decode.h", - "pb_common.h", - ] -) +nanopb_headers = [ + Path("pb.h"), + Path("pb_encode.h"), + Path("pb_decode.h"), + Path("pb_common.h"), +] -nanopb_generator = set( - [ - "pb.h", - "pb_encode.h", - "pb_decode.h", - "pb_common.h", - ] -) +nanopb_generator = [ + Path("pb.h"), + Path("pb_encode.h"), + Path("pb_decode.h"), + Path("pb_common.h"), +] -def copy_upstream_src(wpilib_root): - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ @@ -41,25 +35,24 @@ def copy_upstream_src(wpilib_root): "src/main/native/thirdparty/nanopb/include", "src/main/native/thirdparty/nanopb/generator", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Copy nanopb source files into allwpilib copy_to( nanopb_sources, - os.path.join(wpiutil, "src/main/native/thirdparty/nanopb/src"), + wpiutil / "src/main/native/thirdparty/nanopb/src", rename_c_to_cpp=True, ) # Copy nanopb header files into allwpilib copy_to( nanopb_headers, - os.path.join(wpiutil, "src/main/native/thirdparty/nanopb/include"), + wpiutil / "src/main/native/thirdparty/nanopb/include", ) - generator_files = walk_if("generator", lambda dp, f: True) - copy_to( - generator_files, - os.path.join(wpiutil, "src/main/native/thirdparty/nanopb"), + generator_files = walk_cwd_and_copy_if( + lambda dp, f: has_prefix(dp, Path("generator")), + wpiutil / "src/main/native/thirdparty/nanopb", ) diff --git a/upstream_utils/protobuf.py b/upstream_utils/protobuf.py index fc7055f524..9882c735b1 100755 --- a/upstream_utils/protobuf.py +++ b/upstream_utils/protobuf.py @@ -2,237 +2,227 @@ import os import shutil +from pathlib import Path -from upstream_utils import Lib, copy_to, walk_if +from upstream_utils import Lib, walk_cwd_and_copy_if -protobuf_lite_sources = set( - [ - "google/protobuf/any_lite.cc", - "google/protobuf/arena.cc", - "google/protobuf/arenastring.cc", - "google/protobuf/arenaz_sampler.cc", - "google/protobuf/extension_set.cc", - "google/protobuf/generated_enum_util.cc", - "google/protobuf/generated_message_tctable_lite.cc", - "google/protobuf/generated_message_util.cc", - "google/protobuf/implicit_weak_message.cc", - "google/protobuf/inlined_string_field.cc", - "google/protobuf/io/coded_stream.cc", - "google/protobuf/io/io_win32.cc", - "google/protobuf/io/strtod.cc", - "google/protobuf/io/zero_copy_stream.cc", - "google/protobuf/io/zero_copy_stream_impl.cc", - "google/protobuf/io/zero_copy_stream_impl_lite.cc", - "google/protobuf/map.cc", - "google/protobuf/message_lite.cc", - "google/protobuf/parse_context.cc", - "google/protobuf/repeated_field.cc", - "google/protobuf/repeated_ptr_field.cc", - "google/protobuf/stubs/bytestream.cc", - "google/protobuf/stubs/common.cc", - "google/protobuf/stubs/int128.cc", - "google/protobuf/stubs/status.cc", - "google/protobuf/stubs/statusor.cc", - "google/protobuf/stubs/stringpiece.cc", - "google/protobuf/stubs/stringprintf.cc", - "google/protobuf/stubs/structurally_valid.cc", - "google/protobuf/stubs/strutil.cc", - "google/protobuf/stubs/time.cc", - "google/protobuf/wire_format_lite.cc", - ] -) +protobuf_lite_sources = { + Path("any_lite.cc"), + Path("arena.cc"), + Path("arenastring.cc"), + Path("arenaz_sampler.cc"), + Path("extension_set.cc"), + Path("generated_enum_util.cc"), + Path("generated_message_tctable_lite.cc"), + Path("generated_message_util.cc"), + Path("implicit_weak_message.cc"), + Path("inlined_string_field.cc"), + Path("io/coded_stream.cc"), + Path("io/io_win32.cc"), + Path("io/strtod.cc"), + Path("io/zero_copy_stream.cc"), + Path("io/zero_copy_stream_impl.cc"), + Path("io/zero_copy_stream_impl_lite.cc"), + Path("map.cc"), + Path("message_lite.cc"), + Path("parse_context.cc"), + Path("repeated_field.cc"), + Path("repeated_ptr_field.cc"), + Path("stubs/bytestream.cc"), + Path("stubs/common.cc"), + Path("stubs/int128.cc"), + Path("stubs/status.cc"), + Path("stubs/statusor.cc"), + Path("stubs/stringpiece.cc"), + Path("stubs/stringprintf.cc"), + Path("stubs/structurally_valid.cc"), + Path("stubs/strutil.cc"), + Path("stubs/time.cc"), + Path("wire_format_lite.cc"), +} -protobuf_lite_includes = set( - [ - "google/protobuf/any.h", - "google/protobuf/arena.h", - "google/protobuf/arena_impl.h", - "google/protobuf/arenastring.h", - "google/protobuf/arenaz_sampler.h", - "google/protobuf/endian.h", - "google/protobuf/explicitly_constructed.h", - "google/protobuf/extension_set.h", - "google/protobuf/extension_set_inl.h", - "google/protobuf/generated_enum_util.h", - "google/protobuf/generated_message_tctable_decl.h", - "google/protobuf/generated_message_tctable_impl.h", - "google/protobuf/generated_message_util.h", - "google/protobuf/has_bits.h", - "google/protobuf/implicit_weak_message.h", - "google/protobuf/inlined_string_field.h", - "google/protobuf/io/coded_stream.h", - "google/protobuf/io/io_win32.h", - "google/protobuf/io/strtod.h", - "google/protobuf/io/zero_copy_stream.h", - "google/protobuf/io/zero_copy_stream_impl.h", - "google/protobuf/io/zero_copy_stream_impl_lite.h", - "google/protobuf/map.h", - "google/protobuf/map_entry_lite.h", - "google/protobuf/map_field_lite.h", - "google/protobuf/map_type_handler.h", - "google/protobuf/message_lite.h", - "google/protobuf/metadata_lite.h", - "google/protobuf/parse_context.h", - "google/protobuf/port.h", - "google/protobuf/repeated_field.h", - "google/protobuf/repeated_ptr_field.h", - "google/protobuf/stubs/bytestream.h", - "google/protobuf/stubs/callback.h", - "google/protobuf/stubs/casts.h", - "google/protobuf/stubs/common.h", - "google/protobuf/stubs/hash.h", - "google/protobuf/stubs/logging.h", - "google/protobuf/stubs/macros.h", - "google/protobuf/stubs/map_util.h", - "google/protobuf/stubs/mutex.h", - "google/protobuf/stubs/once.h", - "google/protobuf/stubs/platform_macros.h", - "google/protobuf/stubs/port.h", - "google/protobuf/stubs/status.h", - "google/protobuf/stubs/stl_util.h", - "google/protobuf/stubs/stringpiece.h", - "google/protobuf/stubs/strutil.h", - "google/protobuf/stubs/template_util.h", - "google/protobuf/wire_format_lite.h", - ] -) +protobuf_lite_includes = { + Path("google/protobuf/any.h"), + Path("google/protobuf/arena.h"), + Path("google/protobuf/arena_impl.h"), + Path("google/protobuf/arenastring.h"), + Path("google/protobuf/arenaz_sampler.h"), + Path("google/protobuf/endian.h"), + Path("google/protobuf/explicitly_constructed.h"), + Path("google/protobuf/extension_set.h"), + Path("google/protobuf/extension_set_inl.h"), + Path("google/protobuf/generated_enum_util.h"), + Path("google/protobuf/generated_message_tctable_decl.h"), + Path("google/protobuf/generated_message_tctable_impl.h"), + Path("google/protobuf/generated_message_util.h"), + Path("google/protobuf/has_bits.h"), + Path("google/protobuf/implicit_weak_message.h"), + Path("google/protobuf/inlined_string_field.h"), + Path("google/protobuf/io/coded_stream.h"), + Path("google/protobuf/io/io_win32.h"), + Path("google/protobuf/io/strtod.h"), + Path("google/protobuf/io/zero_copy_stream.h"), + Path("google/protobuf/io/zero_copy_stream_impl.h"), + Path("google/protobuf/io/zero_copy_stream_impl_lite.h"), + Path("google/protobuf/map.h"), + Path("google/protobuf/map_entry_lite.h"), + Path("google/protobuf/map_field_lite.h"), + Path("google/protobuf/map_type_handler.h"), + Path("google/protobuf/message_lite.h"), + Path("google/protobuf/metadata_lite.h"), + Path("google/protobuf/parse_context.h"), + Path("google/protobuf/port.h"), + Path("google/protobuf/repeated_field.h"), + Path("google/protobuf/repeated_ptr_field.h"), + Path("google/protobuf/stubs/bytestream.h"), + Path("google/protobuf/stubs/callback.h"), + Path("google/protobuf/stubs/casts.h"), + Path("google/protobuf/stubs/common.h"), + Path("google/protobuf/stubs/hash.h"), + Path("google/protobuf/stubs/logging.h"), + Path("google/protobuf/stubs/macros.h"), + Path("google/protobuf/stubs/map_util.h"), + Path("google/protobuf/stubs/mutex.h"), + Path("google/protobuf/stubs/once.h"), + Path("google/protobuf/stubs/platform_macros.h"), + Path("google/protobuf/stubs/port.h"), + Path("google/protobuf/stubs/status.h"), + Path("google/protobuf/stubs/stl_util.h"), + Path("google/protobuf/stubs/stringpiece.h"), + Path("google/protobuf/stubs/strutil.h"), + Path("google/protobuf/stubs/template_util.h"), + Path("google/protobuf/wire_format_lite.h"), +} +protobuf_sources = { + Path("any.cc"), + Path("any.pb.cc"), + Path("api.pb.cc"), + Path("compiler/importer.cc"), + Path("compiler/parser.cc"), + Path("descriptor.cc"), + Path("descriptor.pb.cc"), + Path("descriptor_database.cc"), + Path("duration.pb.cc"), + Path("dynamic_message.cc"), + Path("empty.pb.cc"), + Path("extension_set_heavy.cc"), + Path("field_mask.pb.cc"), + Path("generated_message_bases.cc"), + Path("generated_message_reflection.cc"), + Path("generated_message_tctable_full.cc"), + Path("io/gzip_stream.cc"), + Path("io/printer.cc"), + Path("io/tokenizer.cc"), + Path("map_field.cc"), + Path("message.cc"), + Path("reflection_ops.cc"), + Path("service.cc"), + Path("source_context.pb.cc"), + Path("struct.pb.cc"), + Path("stubs/substitute.cc"), + Path("text_format.cc"), + Path("timestamp.pb.cc"), + Path("type.pb.cc"), + Path("unknown_field_set.cc"), + Path("util/delimited_message_util.cc"), + Path("util/field_comparator.cc"), + Path("util/field_mask_util.cc"), + Path("util/internal/datapiece.cc"), + Path("util/internal/default_value_objectwriter.cc"), + Path("util/internal/error_listener.cc"), + Path("util/internal/field_mask_utility.cc"), + Path("util/internal/json_escaping.cc"), + Path("util/internal/json_objectwriter.cc"), + Path("util/internal/json_stream_parser.cc"), + Path("util/internal/object_writer.cc"), + Path("util/internal/proto_writer.cc"), + Path("util/internal/protostream_objectsource.cc"), + Path("util/internal/protostream_objectwriter.cc"), + Path("util/internal/type_info.cc"), + Path("util/internal/utility.cc"), + Path("util/json_util.cc"), + Path("util/message_differencer.cc"), + Path("util/time_util.cc"), + Path("util/type_resolver_util.cc"), + Path("wire_format.cc"), + Path("wrappers.pb.cc"), +} -protobuf_sources = set( - [ - "google/protobuf/any.cc", - "google/protobuf/any.pb.cc", - "google/protobuf/api.pb.cc", - "google/protobuf/compiler/importer.cc", - "google/protobuf/compiler/parser.cc", - "google/protobuf/descriptor.cc", - "google/protobuf/descriptor.pb.cc", - "google/protobuf/descriptor_database.cc", - "google/protobuf/duration.pb.cc", - "google/protobuf/dynamic_message.cc", - "google/protobuf/empty.pb.cc", - "google/protobuf/extension_set_heavy.cc", - "google/protobuf/field_mask.pb.cc", - "google/protobuf/generated_message_bases.cc", - "google/protobuf/generated_message_reflection.cc", - "google/protobuf/generated_message_tctable_full.cc", - "google/protobuf/io/gzip_stream.cc", - "google/protobuf/io/printer.cc", - "google/protobuf/io/tokenizer.cc", - "google/protobuf/map_field.cc", - "google/protobuf/message.cc", - "google/protobuf/reflection_ops.cc", - "google/protobuf/service.cc", - "google/protobuf/source_context.pb.cc", - "google/protobuf/struct.pb.cc", - "google/protobuf/stubs/substitute.cc", - "google/protobuf/text_format.cc", - "google/protobuf/timestamp.pb.cc", - "google/protobuf/type.pb.cc", - "google/protobuf/unknown_field_set.cc", - "google/protobuf/util/delimited_message_util.cc", - "google/protobuf/util/field_comparator.cc", - "google/protobuf/util/field_mask_util.cc", - "google/protobuf/util/internal/datapiece.cc", - "google/protobuf/util/internal/default_value_objectwriter.cc", - "google/protobuf/util/internal/error_listener.cc", - "google/protobuf/util/internal/field_mask_utility.cc", - "google/protobuf/util/internal/json_escaping.cc", - "google/protobuf/util/internal/json_objectwriter.cc", - "google/protobuf/util/internal/json_stream_parser.cc", - "google/protobuf/util/internal/object_writer.cc", - "google/protobuf/util/internal/proto_writer.cc", - "google/protobuf/util/internal/protostream_objectsource.cc", - "google/protobuf/util/internal/protostream_objectwriter.cc", - "google/protobuf/util/internal/type_info.cc", - "google/protobuf/util/internal/utility.cc", - "google/protobuf/util/json_util.cc", - "google/protobuf/util/message_differencer.cc", - "google/protobuf/util/time_util.cc", - "google/protobuf/util/type_resolver_util.cc", - "google/protobuf/wire_format.cc", - "google/protobuf/wrappers.pb.cc", - ] -) +protobuf_includes = { + Path("google/protobuf/any.pb.h"), + Path("google/protobuf/api.pb.h"), + Path("google/protobuf/compiler/importer.h"), + Path("google/protobuf/compiler/parser.h"), + Path("google/protobuf/descriptor.h"), + Path("google/protobuf/descriptor.pb.h"), + Path("google/protobuf/descriptor_database.h"), + Path("google/protobuf/duration.pb.h"), + Path("google/protobuf/dynamic_message.h"), + Path("google/protobuf/empty.pb.h"), + Path("google/protobuf/field_access_listener.h"), + Path("google/protobuf/field_mask.pb.h"), + Path("google/protobuf/generated_enum_reflection.h"), + Path("google/protobuf/generated_message_bases.h"), + Path("google/protobuf/generated_message_reflection.h"), + Path("google/protobuf/io/gzip_stream.h"), + Path("google/protobuf/io/printer.h"), + Path("google/protobuf/io/tokenizer.h"), + Path("google/protobuf/map_entry.h"), + Path("google/protobuf/map_field.h"), + Path("google/protobuf/map_field_inl.h"), + Path("google/protobuf/message.h"), + Path("google/protobuf/metadata.h"), + Path("google/protobuf/reflection.h"), + Path("google/protobuf/reflection_internal.h"), + Path("google/protobuf/reflection_ops.h"), + Path("google/protobuf/service.h"), + Path("google/protobuf/source_context.pb.h"), + Path("google/protobuf/struct.pb.h"), + Path("google/protobuf/text_format.h"), + Path("google/protobuf/timestamp.pb.h"), + Path("google/protobuf/type.pb.h"), + Path("google/protobuf/unknown_field_set.h"), + Path("google/protobuf/util/delimited_message_util.h"), + Path("google/protobuf/util/field_comparator.h"), + Path("google/protobuf/util/field_mask_util.h"), + Path("google/protobuf/util/json_util.h"), + Path("google/protobuf/util/message_differencer.h"), + Path("google/protobuf/util/time_util.h"), + Path("google/protobuf/util/type_resolver.h"), + Path("google/protobuf/util/type_resolver_util.h"), + Path("google/protobuf/wire_format.h"), + Path("google/protobuf/wrappers.pb.h"), +} -protobuf_includes = set( - [ - "google/protobuf/any.pb.h", - "google/protobuf/api.pb.h", - "google/protobuf/compiler/importer.h", - "google/protobuf/compiler/parser.h", - "google/protobuf/descriptor.h", - "google/protobuf/descriptor.pb.h", - "google/protobuf/descriptor_database.h", - "google/protobuf/duration.pb.h", - "google/protobuf/dynamic_message.h", - "google/protobuf/empty.pb.h", - "google/protobuf/field_access_listener.h", - "google/protobuf/field_mask.pb.h", - "google/protobuf/generated_enum_reflection.h", - "google/protobuf/generated_message_bases.h", - "google/protobuf/generated_message_reflection.h", - "google/protobuf/io/gzip_stream.h", - "google/protobuf/io/printer.h", - "google/protobuf/io/tokenizer.h", - "google/protobuf/map_entry.h", - "google/protobuf/map_field.h", - "google/protobuf/map_field_inl.h", - "google/protobuf/message.h", - "google/protobuf/metadata.h", - "google/protobuf/reflection.h", - "google/protobuf/reflection_internal.h", - "google/protobuf/reflection_ops.h", - "google/protobuf/service.h", - "google/protobuf/source_context.pb.h", - "google/protobuf/struct.pb.h", - "google/protobuf/text_format.h", - "google/protobuf/timestamp.pb.h", - "google/protobuf/type.pb.h", - "google/protobuf/unknown_field_set.h", - "google/protobuf/util/delimited_message_util.h", - "google/protobuf/util/field_comparator.h", - "google/protobuf/util/field_mask_util.h", - "google/protobuf/util/json_util.h", - "google/protobuf/util/message_differencer.h", - "google/protobuf/util/time_util.h", - "google/protobuf/util/type_resolver.h", - "google/protobuf/util/type_resolver_util.h", - "google/protobuf/wire_format.h", - "google/protobuf/wrappers.pb.h", - ] -) - -protobuf_internal_includes = set( - [ - "google/protobuf/port_def.inc", - "google/protobuf/port_undef.inc", - "google/protobuf/stubs/int128.h", - "google/protobuf/stubs/mathutil.h", - "google/protobuf/stubs/statusor.h", - "google/protobuf/stubs/status_macros.h", - "google/protobuf/stubs/stringprintf.h", - "google/protobuf/stubs/substitute.h", - "google/protobuf/stubs/time.h", - "google/protobuf/util/internal/constants.h", - "google/protobuf/util/internal/datapiece.h", - "google/protobuf/util/internal/default_value_objectwriter.h", - "google/protobuf/util/internal/error_listener.h", - "google/protobuf/util/internal/field_mask_utility.h", - "google/protobuf/util/internal/json_escaping.h", - "google/protobuf/util/internal/json_objectwriter.h", - "google/protobuf/util/internal/json_stream_parser.h", - "google/protobuf/util/internal/location_tracker.h", - "google/protobuf/util/internal/object_location_tracker.h", - "google/protobuf/util/internal/object_source.h", - "google/protobuf/util/internal/object_writer.h", - "google/protobuf/util/internal/proto_writer.h", - "google/protobuf/util/internal/protostream_objectsource.h", - "google/protobuf/util/internal/protostream_objectwriter.h", - "google/protobuf/util/internal/structured_objectwriter.h", - "google/protobuf/util/internal/type_info.h", - "google/protobuf/util/internal/utility.h", - ] -) +protobuf_internal_includes = { + Path("google/protobuf/port_def.inc"), + Path("google/protobuf/port_undef.inc"), + Path("google/protobuf/stubs/int128.h"), + Path("google/protobuf/stubs/mathutil.h"), + Path("google/protobuf/stubs/statusor.h"), + Path("google/protobuf/stubs/status_macros.h"), + Path("google/protobuf/stubs/stringprintf.h"), + Path("google/protobuf/stubs/substitute.h"), + Path("google/protobuf/stubs/time.h"), + Path("google/protobuf/util/internal/constants.h"), + Path("google/protobuf/util/internal/datapiece.h"), + Path("google/protobuf/util/internal/default_value_objectwriter.h"), + Path("google/protobuf/util/internal/error_listener.h"), + Path("google/protobuf/util/internal/field_mask_utility.h"), + Path("google/protobuf/util/internal/json_escaping.h"), + Path("google/protobuf/util/internal/json_objectwriter.h"), + Path("google/protobuf/util/internal/json_stream_parser.h"), + Path("google/protobuf/util/internal/location_tracker.h"), + Path("google/protobuf/util/internal/object_location_tracker.h"), + Path("google/protobuf/util/internal/object_source.h"), + Path("google/protobuf/util/internal/object_writer.h"), + Path("google/protobuf/util/internal/proto_writer.h"), + Path("google/protobuf/util/internal/protostream_objectsource.h"), + Path("google/protobuf/util/internal/protostream_objectwriter.h"), + Path("google/protobuf/util/internal/structured_objectwriter.h"), + Path("google/protobuf/util/internal/type_info.h"), + Path("google/protobuf/util/internal/utility.h"), +} use_src_files = protobuf_lite_sources | protobuf_sources use_include_files = ( @@ -240,38 +230,29 @@ use_include_files = ( ) -def matches(dp, f, files): - if not dp.startswith(os.path.join(".", "src")): - return False - p = dp[6:] + "/" + f - return p in files - - -def copy_upstream_src(wpilib_root): - upstream_root = os.path.abspath(".") - wpiutil = os.path.join(wpilib_root, "wpiutil") +def copy_upstream_src(wpilib_root: Path): + upstream_root = Path(".").absolute() + wpiutil = wpilib_root / "wpiutil" # Delete old install for d in [ "src/main/native/thirdparty/protobuf/src", "src/main/native/thirdparty/protobuf/include", ]: - shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True) + shutil.rmtree(wpiutil / d, ignore_errors=True) # Copy protobuf source files into allwpilib - src_files = walk_if(".", lambda dp, f: matches(dp, f, use_src_files)) - src_files = [f[22:] for f in src_files] - os.chdir(os.path.join(upstream_root, "src/google/protobuf")) - copy_to(src_files, os.path.join(wpiutil, "src/main/native/thirdparty/protobuf/src")) + os.chdir(upstream_root / "src/google/protobuf") + walk_cwd_and_copy_if( + lambda dp, f: dp / f in use_src_files, + wpiutil / "src/main/native/thirdparty/protobuf/src", + ) # Copy protobuf header files into allwpilib - os.chdir(upstream_root) - include_files = walk_if(".", lambda dp, f: matches(dp, f, use_include_files)) - include_files = [f[6:] for f in include_files] - os.chdir(os.path.join(upstream_root, "src")) - copy_to( - include_files, - os.path.join(wpiutil, "src/main/native/thirdparty/protobuf/include"), + os.chdir(upstream_root / "src") + walk_cwd_and_copy_if( + lambda dp, f: dp / f in use_include_files, + wpiutil / "src/main/native/thirdparty/protobuf/include", ) diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index ac25370ff8..c1b0de9a87 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -1,55 +1,59 @@ #!/usr/bin/env python3 -import os import shutil +from pathlib import Path -from upstream_utils import Lib, copy_to +from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if -def copy_upstream_src(wpilib_root): - wpimath = os.path.join(wpilib_root, "wpimath") +def copy_upstream_src(wpilib_root: Path): + wpimath = wpilib_root / "wpimath" # Delete old install for d in [ "src/main/native/thirdparty/sleipnir/src", "src/main/native/thirdparty/sleipnir/include", ]: - shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True) + shutil.rmtree(wpimath / d, ignore_errors=True) - # Copy Sleipnir source files into allwpilib - src_files = [os.path.join(dp, f) for dp, dn, fn in os.walk("src") for f in fn] - src_files = copy_to( - src_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") + # Copy Sleipnir files into allwpilib + walk_cwd_and_copy_if( + lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src"))) + or f == ".clang-format" + or f == ".clang-tidy" + or f == ".styleguide" + or f == ".styleguide-license", + wpimath / "src/main/native/thirdparty/sleipnir", ) - # Copy Sleipnir header files into allwpilib - include_files = [ - os.path.join(dp, f) - for dp, dn, fn in os.walk("include") - for f in fn - if not f.endswith("small_vector.hpp") - ] - include_files = copy_to( - include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") - ) + # Write shim for wpi::SmallVector + (wpimath / "src/main/native/thirdparty/sleipnir/include/gch").mkdir() + with open( + wpimath / "src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp", + "w", + ) as f: + f.write( + """// Copyright (c) Sleipnir contributors - for filename in [ - ".clang-format", - ".clang-tidy", - ".styleguide", - ".styleguide-license", - ]: - shutil.copyfile( - filename, - os.path.join(wpimath, "src/main/native/thirdparty/sleipnir", filename), +#pragma once + +#include + +namespace gch { + +template +using small_vector = wpi::SmallVector; + +} // namespace gch +""" ) def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2024-12-07 - tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c" + # main on 2025-05-18 + tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index deaaf7b5cb..2baae9b2ab 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,30 +1,32 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 1/3] Use fmtlib +Subject: [PATCH 1/8] Use fmtlib --- include/.styleguide | 1 + - include/sleipnir/util/Print.hpp | 31 ++++++++++++++++++------------- - 2 files changed, 19 insertions(+), 13 deletions(-) + include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- + src/optimization/problem.cpp | 2 +- + 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/include/.styleguide b/include/.styleguide -index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644 +index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 --- a/include/.styleguide +++ b/include/.styleguide -@@ -12,4 +12,5 @@ licenseUpdateExclude { +@@ -8,5 +8,6 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ + ^fmt/ + ^gch/ } -diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp -index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0984626fa 100644 ---- a/include/sleipnir/util/Print.hpp -+++ b/include/sleipnir/util/Print.hpp -@@ -3,52 +3,57 @@ - #pragma once +diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp +index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644 +--- a/include/sleipnir/util/print.hpp ++++ b/include/sleipnir/util/print.hpp +@@ -4,10 +4,15 @@ + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #include -#include #include @@ -36,7 +38,11 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0 +#include +#endif + - namespace sleipnir { + #endif + + namespace slp { +@@ -15,45 +20,45 @@ namespace slp { + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS /** - * Wrapper around std::print() that squelches write failure exceptions. @@ -93,3 +99,16 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0 } catch (const std::system_error&) { } } +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { + slp::println(" ↳ executed {} iterations", options.max_iterations); + } + if (std::isfinite(options.timeout.count())) { +- slp::println(" ↳ {} elapsed", options.timeout); ++ slp::println(" ↳ {} elapsed", options.timeout.count()); + } + } + diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 6b816784a7..df21b64a10 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,610 +1,77 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 16 Jun 2024 12:08:49 -0700 -Subject: [PATCH 2/3] Use wpi::SmallVector +Subject: [PATCH 2/8] Use wpi::SmallVector --- - include/.styleguide | 1 + - include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ - include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- - include/sleipnir/autodiff/Hessian.hpp | 4 ++-- - include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- - include/sleipnir/autodiff/Variable.hpp | 10 +++++----- - include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- - include/sleipnir/optimization/Multistart.hpp | 7 ++++--- - .../optimization/OptimizationProblem.hpp | 8 ++++---- - include/sleipnir/util/Pool.hpp | 7 ++++--- - include/sleipnir/util/Spy.hpp | 4 ++-- - src/.styleguide | 1 + - src/optimization/solver/InteriorPoint.cpp | 4 ++-- - src/optimization/solver/SQP.cpp | 4 ++-- - .../solver/util/FeasibilityRestoration.hpp | 18 +++++++++--------- - src/optimization/solver/util/Filter.hpp | 4 ++-- - 16 files changed, 60 insertions(+), 54 deletions(-) + include/sleipnir/autodiff/expression.hpp | 4 ++-- + include/sleipnir/autodiff/variable.hpp | 5 ++--- + include/sleipnir/autodiff/variable_matrix.hpp | 4 ++-- + 3 files changed, 6 insertions(+), 7 deletions(-) -diff --git a/include/.styleguide b/include/.styleguide -index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 ---- a/include/.styleguide -+++ b/include/.styleguide -@@ -13,4 +13,5 @@ licenseUpdateExclude { - includeOtherLibs { - ^Eigen/ - ^fmt/ -+ ^wpi/ - } -diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644 ---- a/include/sleipnir/autodiff/Expression.hpp -+++ b/include/sleipnir/autodiff/Expression.hpp -@@ -11,11 +11,12 @@ - #include - #include +diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp +index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644 +--- a/include/sleipnir/autodiff/expression.hpp ++++ b/include/sleipnir/autodiff/expression.hpp +@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true; + struct Expression; -+#include -+ - #include "sleipnir/autodiff/ExpressionType.hpp" - #include "sleipnir/util/IntrusiveSharedPtr.hpp" - #include "sleipnir/util/Pool.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir::detail { - -@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true; - - struct SLEIPNIR_DLLEXPORT Expression; - --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr); --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr); -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr); + inline constexpr void inc_ref_count(Expression* expr); +-inline constexpr void dec_ref_count(Expression* expr); ++inline void dec_ref_count(Expression* expr); /** * Typedef for intrusive shared pointer to Expression. -@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); +@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) { * * @param expr The shared pointer's managed object. */ --inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; - } - -@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { - * - * @param expr The shared pointer's managed object. - */ --inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { -+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { +-inline constexpr void dec_ref_count(Expression* expr) { ++inline void dec_ref_count(Expression* expr) { // If a deeply nested tree is being deallocated all at once, calling the // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their - // refcounts and deallocate them. -- small_vector stack; -+ wpi::SmallVector stack; - stack.emplace_back(expr); - - while (!stack.empty()) { -diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp -index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644 ---- a/include/sleipnir/autodiff/ExpressionGraph.hpp -+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp -@@ -4,10 +4,11 @@ - - #include - -+#include -+ - #include "sleipnir/autodiff/Expression.hpp" - #include "sleipnir/util/FunctionRef.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir::detail { - -@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - // https://en.wikipedia.org/wiki/Breadth-first_search - - // BFS list sorted from parent to child. -- small_vector stack; -+ wpi::SmallVector stack; - - stack.emplace_back(root.Get()); - -@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - * - * @param wrt Variables with respect to which to compute the gradient. +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable { + /** + * Constructs an empty Variable. */ -- small_vector GenerateGradientTree( -+ wpi::SmallVector GenerateGradientTree( - std::span wrt) const { - // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation - // for background on reverse accumulation automatic differentiation. -@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - wrt[row]->row = row; - } - -- small_vector grad; -+ wpi::SmallVector grad; - grad.reserve(wrt.size()); - for (size_t row = 0; row < wrt.size(); ++row) { - grad.emplace_back(MakeExpressionPtr()); -@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph { - - private: - // List that maps nodes to their respective row. -- small_vector m_rowList; -+ wpi::SmallVector m_rowList; - - // List for updating adjoints -- small_vector m_adjointList; -+ wpi::SmallVector m_adjointList; - - // List for updating values -- small_vector m_valueList; -+ wpi::SmallVector m_valueList; - }; - - } // namespace sleipnir::detail -diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp -index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644 ---- a/include/sleipnir/autodiff/Hessian.hpp -+++ b/include/sleipnir/autodiff/Hessian.hpp -@@ -6,6 +6,7 @@ - - #include - #include -+#include - - #include "sleipnir/autodiff/ExpressionGraph.hpp" - #include "sleipnir/autodiff/Jacobian.hpp" -@@ -13,7 +14,6 @@ - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian { - Hessian(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{ - [&] { -- small_vector wrtVec; -+ wpi::SmallVector wrtVec; - wrtVec.reserve(wrt.size()); - for (auto& elem : wrt) { - wrtVec.emplace_back(elem.expr); -diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp -index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644 ---- a/include/sleipnir/autodiff/Jacobian.hpp -+++ b/include/sleipnir/autodiff/Jacobian.hpp -@@ -5,13 +5,13 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/ExpressionGraph.hpp" - #include "sleipnir/autodiff/Profiler.hpp" - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian { - VariableMatrix Get() const { - VariableMatrix result{m_variables.Rows(), m_wrt.Rows()}; - -- small_vector wrtVec; -+ wpi::SmallVector wrtVec; - wrtVec.reserve(m_wrt.size()); - for (auto& elem : m_wrt) { - wrtVec.emplace_back(elem.expr); -@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian { - VariableMatrix m_variables; - VariableMatrix m_wrt; - -- small_vector m_graphs; -+ wpi::SmallVector m_graphs; - - Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; - - // Cached triplets for gradients of linear rows -- small_vector> m_cachedTriplets; -+ wpi::SmallVector> m_cachedTriplets; - - // List of row indices for nonlinear rows whose graients will be computed in - // Value() -- small_vector m_nonlinearRows; -+ wpi::SmallVector m_nonlinearRows; - - Profiler m_profiler; - }; -diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp -index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644 ---- a/include/sleipnir/autodiff/Variable.hpp -+++ b/include/sleipnir/autodiff/Variable.hpp -@@ -10,6 +10,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Expression.hpp" - #include "sleipnir/autodiff/ExpressionGraph.hpp" -@@ -17,7 +18,6 @@ - #include "sleipnir/util/Concepts.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -445,8 +445,8 @@ template - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) --small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { -- small_vector constraints; -+wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { -+ wpi::SmallVector constraints; - - if constexpr (ScalarLike> && - ScalarLike>) { -@@ -534,7 +534,7 @@ small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { - */ - struct SLEIPNIR_DLLEXPORT EqualityConstraints { - /// A vector of scalar equality constraints. -- small_vector constraints; -+ wpi::SmallVector constraints; +- explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {} ++ explicit Variable(std::nullptr_t) : expr{nullptr} {} /** - * Concatenates multiple equality constraints. -@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { - */ - struct SLEIPNIR_DLLEXPORT InequalityConstraints { - /// A vector of scalar inequality constraints. -- small_vector constraints; -+ wpi::SmallVector constraints; + * Constructs a Variable from a floating point type. +@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable { + * + * @param expr The autodiff variable. + */ +- explicit constexpr Variable(detail::ExpressionPtr&& expr) +- : expr{std::move(expr)} {} ++ explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {} /** - * Concatenates multiple inequality constraints. -diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp -index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644 ---- a/include/sleipnir/autodiff/VariableMatrix.hpp -+++ b/include/sleipnir/autodiff/VariableMatrix.hpp -@@ -11,6 +11,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Slice.hpp" - #include "sleipnir/autodiff/Variable.hpp" -@@ -18,7 +19,6 @@ - #include "sleipnir/util/Assert.hpp" - #include "sleipnir/util/FunctionRef.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { - } - - private: -- small_vector m_storage; -+ wpi::SmallVector m_storage; - int m_rows = 0; - int m_cols = 0; - }; -diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp -index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644 ---- a/include/sleipnir/optimization/Multistart.hpp -+++ b/include/sleipnir/optimization/Multistart.hpp -@@ -6,9 +6,10 @@ - #include - #include - -+#include -+ - #include "sleipnir/optimization/SolverStatus.hpp" - #include "sleipnir/util/FunctionRef.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -44,14 +45,14 @@ MultistartResult Multistart( - const DecisionVariables& initialGuess)> - solve, - std::span initialGuesses) { -- small_vector>> futures; -+ wpi::SmallVector>> futures; - futures.reserve(initialGuesses.size()); - - for (const auto& initialGuess : initialGuesses) { - futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); - } - -- small_vector> results; -+ wpi::SmallVector> results; - results.reserve(futures.size()); - - for (auto& future : futures) { -diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp -index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644 ---- a/include/sleipnir/optimization/OptimizationProblem.hpp -+++ b/include/sleipnir/optimization/OptimizationProblem.hpp -@@ -11,6 +11,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -22,7 +23,6 @@ - #include "sleipnir/optimization/solver/SQP.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { - private: - // The list of decision variables, which are the root of the problem's - // expression tree -- small_vector m_decisionVariables; -+ wpi::SmallVector m_decisionVariables; - - // The cost function: f(x) - std::optional m_f; - - // The list of equality constraints: cₑ(x) = 0 -- small_vector m_equalityConstraints; -+ wpi::SmallVector m_equalityConstraints; - - // The list of inequality constraints: cᵢ(x) ≥ 0 -- small_vector m_inequalityConstraints; -+ wpi::SmallVector m_inequalityConstraints; - - // The user callback - std::function m_callback = -diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp -index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644 ---- a/include/sleipnir/util/Pool.hpp -+++ b/include/sleipnir/util/Pool.hpp -@@ -5,8 +5,9 @@ - #include - #include - -+#include -+ - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -78,8 +79,8 @@ class SLEIPNIR_DLLEXPORT PoolResource { - } - - private: -- small_vector> m_buffer; -- small_vector m_freeList; -+ wpi::SmallVector> m_buffer; -+ wpi::SmallVector m_freeList; - size_t blocksPerChunk; + * Assignment operator for double. +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * + * @return Begin iterator. + */ +- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; } ++ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } /** -diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp -index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644 ---- a/include/sleipnir/util/Spy.hpp -+++ b/include/sleipnir/util/Spy.hpp -@@ -7,9 +7,9 @@ - #include + * Returns end iterator. + * + * @return End iterator. + */ +- const_iterator cend() const { return const_iterator{m_storage.cend()}; } ++ const_iterator cend() const { return const_iterator{m_storage.end()}; } - #include -+#include - - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file, - const int cells_width = mat.cols() + 1; - const int cells_height = mat.rows(); - -- small_vector cells; -+ wpi::SmallVector cells; - - // Allocate space for matrix of characters plus trailing newlines - cells.reserve(cells_width * cells_height); -diff --git a/src/.styleguide b/src/.styleguide -index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644 ---- a/src/.styleguide -+++ b/src/.styleguide -@@ -8,4 +8,5 @@ cppSrcFileInclude { - - includeOtherLibs { - ^Eigen/ -+ ^wpi/ - } -diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp -index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644 ---- a/src/optimization/solver/InteriorPoint.cpp -+++ b/src/optimization/solver/InteriorPoint.cpp -@@ -9,6 +9,7 @@ - #include - - #include -+#include - - #include "optimization/RegularizedLDLT.hpp" - #include "optimization/solver/util/ErrorEstimate.hpp" -@@ -23,7 +24,6 @@ - #include "sleipnir/optimization/SolverExitCondition.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/Spy.hpp" --#include "sleipnir/util/small_vector.hpp" - #include "util/ScopeExit.hpp" - #include "util/ToMilliseconds.hpp" - -@@ -226,7 +226,7 @@ void InteriorPoint(std::span decisionVariables, - }; - - // Kept outside the loop so its storage can be reused -- small_vector> triplets; -+ wpi::SmallVector> triplets; - - RegularizedLDLT solver; - -diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp -index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644 ---- a/src/optimization/solver/SQP.cpp -+++ b/src/optimization/solver/SQP.cpp -@@ -9,6 +9,7 @@ - #include - - #include -+#include - - #include "optimization/RegularizedLDLT.hpp" - #include "optimization/solver/util/ErrorEstimate.hpp" -@@ -22,7 +23,6 @@ - #include "sleipnir/optimization/SolverExitCondition.hpp" - #include "sleipnir/util/Print.hpp" - #include "sleipnir/util/Spy.hpp" --#include "sleipnir/util/small_vector.hpp" - #include "util/ScopeExit.hpp" - #include "util/ToMilliseconds.hpp" - -@@ -155,7 +155,7 @@ void SQP(std::span decisionVariables, - Filter filter{f}; - - // Kept outside the loop so its storage can be reused -- small_vector> triplets; -+ wpi::SmallVector> triplets; - - RegularizedLDLT solver; - -diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp -index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644 ---- a/src/optimization/solver/util/FeasibilityRestoration.hpp -+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp -@@ -8,6 +8,7 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -16,7 +17,6 @@ - #include "sleipnir/optimization/SolverStatus.hpp" - #include "sleipnir/optimization/solver/InteriorPoint.hpp" - #include "sleipnir/util/FunctionRef.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -57,7 +57,7 @@ inline void FeasibilityRestoration( - constexpr double ρ = 1000.0; - double μ = config.tolerance / 10.0; - -- small_vector fr_decisionVariables; -+ wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size()); - -@@ -70,7 +70,7 @@ inline void FeasibilityRestoration( - fr_decisionVariables.emplace_back(); - } - -- auto it = fr_decisionVariables.cbegin(); -+ auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); -@@ -128,7 +128,7 @@ inline void FeasibilityRestoration( - } - - // cₑ(x) - pₑ + nₑ = 0 -- small_vector fr_equalityConstraints; -+ wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -137,7 +137,7 @@ inline void FeasibilityRestoration( - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 -- small_vector fr_inequalityConstraints; -+ wpi::SmallVector fr_inequalityConstraints; - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), -@@ -227,7 +227,7 @@ inline void FeasibilityRestoration( - - constexpr double ρ = 1000.0; - -- small_vector fr_decisionVariables; -+ wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size() + - 2 * inequalityConstraints.size()); -@@ -243,7 +243,7 @@ inline void FeasibilityRestoration( - fr_decisionVariables.emplace_back(); - } - -- auto it = fr_decisionVariables.cbegin(); -+ auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); -@@ -319,7 +319,7 @@ inline void FeasibilityRestoration( - } - - // cₑ(x) - pₑ + nₑ = 0 -- small_vector fr_equalityConstraints; -+ wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -328,7 +328,7 @@ inline void FeasibilityRestoration( - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 -- small_vector fr_inequalityConstraints; -+ wpi::SmallVector fr_inequalityConstraints; - fr_inequalityConstraints.assign(inequalityConstraints.begin(), - inequalityConstraints.end()); - for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { -diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp -index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644 ---- a/src/optimization/solver/util/Filter.hpp -+++ b/src/optimization/solver/util/Filter.hpp -@@ -8,9 +8,9 @@ - #include - - #include -+#include - - #include "sleipnir/autodiff/Variable.hpp" --#include "sleipnir/util/small_vector.hpp" - - // See docs/algorithms.md#Works_cited for citation definitions. - -@@ -177,7 +177,7 @@ class Filter { - - private: - Variable* m_f = nullptr; -- small_vector m_filter; -+ wpi::SmallVector m_filter; - }; - - } // namespace sleipnir + /** + * Returns number of elements in matrix. diff --git a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch deleted file mode 100644 index 90ae6e261b..0000000000 --- a/upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch +++ /dev/null @@ -1,31 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 26 Jun 2024 12:13:33 -0700 -Subject: [PATCH 3/3] Suppress clang-tidy false positives - ---- - include/sleipnir/autodiff/Variable.hpp | 4 ++-- - 1 file changed, 2 insertions(+), 2 deletions(-) - -diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp -index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644 ---- a/include/sleipnir/autodiff/Variable.hpp -+++ b/include/sleipnir/autodiff/Variable.hpp -@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { - * - * @param equalityConstraints The list of EqualityConstraints to concatenate. - */ -- EqualityConstraints( -+ EqualityConstraints( // NOLINT - std::initializer_list equalityConstraints) { - for (const auto& elem : equalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), -@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { - * @param inequalityConstraints The list of InequalityConstraints to - * concatenate. - */ -- InequalityConstraints( -+ InequalityConstraints( // NOLINT - std::initializer_list inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch new file mode 100644 index 0000000000..02eb660bc0 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch @@ -0,0 +1,41 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 28 Jan 2025 22:19:14 -0800 +Subject: [PATCH 3/8] Use wpi::byteswap() + +--- + include/.styleguide | 1 + + include/sleipnir/util/spy.hpp | 3 ++- + 2 files changed, 3 insertions(+), 1 deletion(-) + +diff --git a/include/.styleguide b/include/.styleguide +index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644 +--- a/include/.styleguide ++++ b/include/.styleguide +@@ -10,4 +10,5 @@ includeOtherLibs { + ^Eigen/ + ^fmt/ + ^gch/ ++ ^wpi/ + } +diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp +index a2f94803e3744cee771669210d1af883160e9896..8cd7d4353aad20153af5cd7a818fa55889d35721 100644 +--- a/include/sleipnir/util/spy.hpp ++++ b/include/sleipnir/util/spy.hpp +@@ -12,6 +12,7 @@ + #include + + #include ++#include + + #include "sleipnir/util/symbol_exports.hpp" + +@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy { + */ + void write32le(int32_t num) { + if constexpr (std::endian::native != std::endian::little) { +- num = std::byteswap(num); ++ num = wpi::byteswap(num); + } + m_file.write(reinterpret_cast(&num), sizeof(num)); + } diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch new file mode 100644 index 0000000000..1b5f998cf8 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -0,0 +1,63 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Tue, 28 Jan 2025 22:19:31 -0800 +Subject: [PATCH 4/8] Replace std::to_underlying() + +--- + src/optimization/problem.cpp | 9 ++++----- + src/util/print_diagnostics.hpp | 6 +++--- + 2 files changed, 7 insertions(+), 8 deletions(-) + +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -7,7 +7,6 @@ + #include + #include + #include +-#include + + #include + #include +@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() { + // Print problem structure + slp::println("\nProblem structure:"); + slp::println(" ↳ {} cost function", +- types[std::to_underlying(cost_function_type())]); ++ types[static_cast(cost_function_type())]); + slp::println(" ↳ {} equality constraints", +- types[std::to_underlying(equality_constraint_type())]); ++ types[static_cast(equality_constraint_type())]); + slp::println(" ↳ {} inequality constraints", +- types[std::to_underlying(inequality_constraint_type())]); ++ types[static_cast(inequality_constraint_type())]); + + if (m_decision_variables.size() == 1) { + slp::print("\n1 decision variable\n"); +@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() { + [](const gch::small_vector& constraints) { + std::array counts{}; + for (const auto& constraint : constraints) { +- ++counts[std::to_underlying(constraint.type())]; ++ ++counts[static_cast(constraint.type())]; + } + for (const auto& [count, name] : + std::views::zip(counts, std::array{"empty", "constant", "linear", +diff --git a/src/util/print_diagnostics.hpp b/src/util/print_diagnostics.hpp +index fde36957c0258f6e3cd435ef6224d60407012ff7..82e0e082b0e40153dcb2fcd2c655a412a8a9540a 100644 +--- a/src/util/print_diagnostics.hpp ++++ b/src/util/print_diagnostics.hpp +@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type, + slp::println( + "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " + "{:.2e} {:2d}│", +- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, +- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, +- backtracks); ++ iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), ++ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, ++ dual_α, backtracks); + } + #else + #define print_iteration_diagnostics(...) diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch new file mode 100644 index 0000000000..2885d850de --- /dev/null +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -0,0 +1,53 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sat, 8 Feb 2025 13:42:36 -0800 +Subject: [PATCH 5/8] Replace std::views::zip() + +--- + include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++- + src/optimization/problem.cpp | 9 +++++---- + 2 files changed, 9 insertions(+), 5 deletions(-) + +diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp +index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644 +--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp ++++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp +@@ -155,7 +155,10 @@ class AdjointExpressionGraph { + } + } + } else { +- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) { ++ for (size_t i = 0; i < m_top_list.size(); ++i) { ++ const auto& col = m_col_list[i]; ++ const auto& node = m_top_list[i]; ++ + // Append adjoints of wrt to sparse matrix triplets + if (col != -1 && node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); +diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp +index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644 +--- a/src/optimization/problem.cpp ++++ b/src/optimization/problem.cpp +@@ -6,7 +6,6 @@ + #include + #include + #include +-#include + + #include + #include +@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() { + for (const auto& constraint : constraints) { + ++counts[static_cast(constraint.type())]; + } +- for (const auto& [count, name] : +- std::views::zip(counts, std::array{"empty", "constant", "linear", +- "quadratic", "nonlinear"})) { ++ for (size_t i = 0; i < counts.size(); ++i) { ++ constexpr std::array names{"empty", "constant", "linear", "quadratic", ++ "nonlinear"}; ++ const auto& count = counts[i]; ++ const auto& name = names[i]; + if (count > 0) { + slp::println(" ↳ {} {}", count, name); + } diff --git a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch new file mode 100644 index 0000000000..c71e612919 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch @@ -0,0 +1,22 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Mon, 10 Feb 2025 11:37:02 -0800 +Subject: [PATCH 6/8] Suppress clang-tidy false positives + +--- + include/sleipnir/autodiff/variable.hpp | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { + * @param inequality_constraints The list of InequalityConstraints to + * concatenate. + */ +- InequalityConstraints( ++ InequalityConstraints( // NOLINT + std::initializer_list inequality_constraints) { + for (const auto& elem : inequality_constraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch new file mode 100644 index 0000000000..a4841664ed --- /dev/null +++ b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch @@ -0,0 +1,34 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Mon, 24 Feb 2025 15:12:03 -0800 +Subject: [PATCH 7/8] Suppress GCC 12 warning false positive + +--- + include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++ + 1 file changed, 7 insertions(+) + +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols()); + ++#if __GNUC__ >= 12 ++#pragma GCC diagnostic push ++#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" ++#endif + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; +@@ -590,6 +594,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + result[i, j] = sum; + } + } ++#if __GNUC__ >= 12 ++#pragma GCC diagnostic pop ++#endif + + return result; + } diff --git a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch new file mode 100644 index 0000000000..d31ba4bcc7 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch @@ -0,0 +1,915 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sat, 12 Apr 2025 16:28:47 -0700 +Subject: [PATCH 8/8] Revert "Use multidimensional array subscript operator + (#843)" + +This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0. +--- + include/sleipnir/autodiff/hessian.hpp | 4 +- + include/sleipnir/autodiff/jacobian.hpp | 4 +- + include/sleipnir/autodiff/variable.hpp | 26 +---- + include/sleipnir/autodiff/variable_block.hpp | 70 +++++------ + include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------ + include/sleipnir/control/ocp.hpp | 14 +-- + include/sleipnir/optimization/problem.hpp | 6 +- + src/autodiff/variable_matrix.cpp | 66 +++++------ + 8 files changed, 118 insertions(+), 182 deletions(-) + +diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp +index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644 +--- a/include/sleipnir/autodiff/hessian.hpp ++++ b/include/sleipnir/autodiff/hessian.hpp +@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { +- result[row, col] = std::move(grad[col]); ++ result(row, col) = std::move(grad[col]); + } else { +- result[row, col] = Variable{0.0}; ++ result(row, col) = Variable{0.0}; + } + } + } +diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp +index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644 +--- a/include/sleipnir/autodiff/jacobian.hpp ++++ b/include/sleipnir/autodiff/jacobian.hpp +@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { +- result[row, col] = std::move(grad[col]); ++ result(row, col) = std::move(grad[col]); + } else { +- result[row, col] = Variable{0.0}; ++ result(row, col) = Variable{0.0}; + } + } + } +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -505,11 +505,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike>) { +- constraints.emplace_back(lhs - rhs(row, col)); +- } else { +- constraints.emplace_back(lhs - rhs[row, col]); +- } ++ constraints.emplace_back(lhs - rhs(row, col)); + } + } + } else if constexpr (MatrixLike && ScalarLike) { +@@ -518,11 +514,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs); +- } else { +- constraints.emplace_back(lhs[row, col] - rhs); +- } ++ constraints.emplace_back(lhs(row, col) - rhs); + } + } + } else if constexpr (MatrixLike && MatrixLike) { +@@ -532,19 +524,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + // Make right-hand side zero +- if constexpr (EigenMatrixLike> && +- EigenMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs(row, col)); +- } else if constexpr (EigenMatrixLike> && +- SleipnirMatrixLike>) { +- constraints.emplace_back(lhs(row, col) - rhs[row, col]); +- } else if constexpr (SleipnirMatrixLike> && +- EigenMatrixLike>) { +- constraints.emplace_back(lhs[row, col] - rhs(row, col)); +- } else if constexpr (SleipnirMatrixLike> && +- SleipnirMatrixLike>) { +- constraints.emplace_back(lhs[row, col] - rhs[row, col]); +- } ++ constraints.emplace_back(lhs(row, col) - rhs(row, col)); + } + } + } +diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp +index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fee39fe537 100644 +--- a/include/sleipnir/autodiff/variable_block.hpp ++++ b/include/sleipnir/autodiff/variable_block.hpp +@@ -50,7 +50,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + } +@@ -85,7 +85,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + } +@@ -152,7 +152,7 @@ class VariableBlock { + VariableBlock& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0] = value; ++ (*this)(0, 0) = value; + + return *this; + } +@@ -167,7 +167,7 @@ class VariableBlock { + void set_value(double value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0].set_value(value); ++ (*this)(0, 0).set_value(value); + } + + /** +@@ -182,7 +182,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values(row, col); ++ (*this)(row, col) = values(row, col); + } + } + +@@ -201,7 +201,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col].set_value(values(row, col)); ++ (*this)(row, col).set_value(values(row, col)); + } + } + } +@@ -217,7 +217,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)(row, col) = values(row, col); + } + } + return *this; +@@ -234,7 +234,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = std::move(values[row, col]); ++ (*this)(row, col) = std::move(values(row, col)); + } + } + return *this; +@@ -247,13 +247,13 @@ class VariableBlock { + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ +- Variable& operator[](int row, int col) ++ Variable& operator()(int row, int col) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step]; ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step); + } + + /** +@@ -263,11 +263,11 @@ class VariableBlock { + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ +- const Variable& operator[](int row, int col) const { ++ const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step]; ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step); + } + + /** +@@ -280,7 +280,7 @@ class VariableBlock { + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows() * cols()); +- return (*this)[row / cols(), row % cols()]; ++ return (*this)(row / cols(), row % cols()); + } + + /** +@@ -291,7 +291,7 @@ class VariableBlock { + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); +- return (*this)[row / cols(), row % cols()]; ++ return (*this)(row / cols(), row % cols()); + } + + /** +@@ -309,8 +309,8 @@ class VariableBlock { + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); +- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, +- Slice{col_offset, col_offset + block_cols, 1}]; ++ return (*this)({row_offset, row_offset + block_rows, 1}, ++ {col_offset, col_offset + block_cols, 1}); + } + + /** +@@ -328,8 +328,8 @@ class VariableBlock { + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); +- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, +- Slice{col_offset, col_offset + block_cols, 1}]; ++ return (*this)({row_offset, row_offset + block_rows, 1}, ++ {col_offset, col_offset + block_cols, 1}); + } + + /** +@@ -339,7 +339,7 @@ class VariableBlock { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, Slice col_slice) { ++ VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ +@@ -359,7 +359,7 @@ class VariableBlock { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); +@@ -385,7 +385,7 @@ class VariableBlock { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, int row_slice_length, ++ VariableBlock operator()(Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) { + return VariableBlock{ + *m_mat, +@@ -409,7 +409,7 @@ class VariableBlock { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) const { +@@ -524,7 +524,7 @@ class VariableBlock { + VariableBlock& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] *= rhs; ++ (*this)(row, col) *= rhs; + } + } + +@@ -542,7 +542,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs[0, 0]; ++ (*this)(row, col) /= rhs(0, 0); + } + } + +@@ -558,7 +558,7 @@ class VariableBlock { + VariableBlock& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs; ++ (*this)(row, col) /= rhs; + } + } + +@@ -576,7 +576,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs[row, col]; ++ (*this)(row, col) += rhs(row, col); + } + } + +@@ -594,7 +594,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs; ++ (*this)(row, col) += rhs; + } + } + +@@ -612,7 +612,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs[row, col]; ++ (*this)(row, col) -= rhs(row, col); + } + } + +@@ -630,7 +630,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs; ++ (*this)(row, col) -= rhs; + } + } + +@@ -655,7 +655,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[col, row] = (*this)[row, col]; ++ result(col, row) = (*this)(row, col); + } + } + +@@ -686,8 +686,8 @@ class VariableBlock { + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); +- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, +- m_col_slice.start + col * m_col_slice.step] ++ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, ++ m_col_slice.start + col * m_col_slice.step) + .value(); + } + +@@ -731,7 +731,7 @@ class VariableBlock { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = unary_op((*this)[row, col]); ++ result(row, col) = unary_op((*this)(row, col)); + } + } + +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col] = values(row, col); ++ (*this)(row, col) = values(row, col); + } + } + +@@ -229,7 +229,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + +- (*this)[0, 0] = value; ++ (*this)(0, 0) = value; + + return *this; + } +@@ -246,7 +246,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col].set_value(values(row, col)); ++ (*this)(row, col).set_value(values(row, col)); + } + } + } +@@ -280,7 +280,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- m_storage.emplace_back(values[row, col]); ++ m_storage.emplace_back(values(row, col)); + } + } + } +@@ -295,7 +295,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- m_storage.emplace_back(values[row, col]); ++ m_storage.emplace_back(values(row, col)); + } + } + } +@@ -340,7 +340,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col The block column. + * @return A block pointing to the given row and column. + */ +- Variable& operator[](int row, int col) { ++ Variable& operator()(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; +@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col The block column. + * @return A block pointing to the given row and column. + */ +- const Variable& operator[](int row, int col) const { ++ const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; +@@ -426,7 +426,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- VariableBlock operator[](Slice row_slice, Slice col_slice) { ++ VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, +@@ -440,7 +440,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[](Slice row_slice, ++ const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); +@@ -461,7 +461,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @return A slice of the variable matrix. + * + */ +- VariableBlock operator[](Slice row_slice, ++ VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) { +@@ -481,7 +481,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ +- const VariableBlock operator[]( ++ const VariableBlock operator()( + Slice row_slice, int row_slice_length, Slice col_slice, + int col_slice_length) const { + return VariableBlock{*this, std::move(row_slice), row_slice_length, +@@ -581,17 +581,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < lhs.cols(); ++k) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- sum += lhs[i, k] * rhs[k, j]; +- } else if constexpr (SleipnirMatrixLike && +- EigenMatrixLike) { +- sum += lhs[i, k] * rhs(k, j); +- } else if constexpr (EigenMatrixLike && +- SleipnirMatrixLike) { +- sum += lhs(i, k) * rhs[k, j]; +- } ++ sum += lhs(i, k) * rhs(k, j); + } +- result[i, j] = sum; ++ result(i, j) = sum; + } + } + #if __GNUC__ >= 12 +@@ -613,7 +605,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] * rhs; ++ result(row, col) = lhs(row, col) * rhs; + } + } + +@@ -632,11 +624,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] * rhs; +- } else { +- result[row, col] = lhs(row, col) * rhs; +- } ++ result(row, col) = lhs(row, col) * rhs; + } + } + +@@ -655,7 +643,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = rhs[row, col] * lhs; ++ result(row, col) = rhs(row, col) * lhs; + } + } + +@@ -674,11 +662,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = rhs[row, col] * lhs; +- } else { +- result[row, col] = rhs(row, col) * lhs; +- } ++ result(row, col) = rhs(row, col) * lhs; + } + } + +@@ -698,13 +682,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { +- if constexpr (SleipnirMatrixLike) { +- sum += (*this)[i, k] * rhs[k, j]; +- } else { +- sum += (*this)[i, k] * rhs(k, j); +- } ++ sum += (*this)(i, k) * rhs(k, j); + } +- (*this)[i, j] = sum; ++ (*this)(i, j) = sum; + } + } + +@@ -720,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { +- (*this)[row, col] *= rhs; ++ (*this)(row, col) *= rhs; + } + } + +@@ -740,11 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] / rhs; +- } else { +- result[row, col] = lhs(row, col) / rhs; +- } ++ result(row, col) = lhs(row, col) / rhs; + } + } + +@@ -760,7 +736,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + VariableMatrix& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs; ++ (*this)(row, col) /= rhs; + } + } + +@@ -784,13 +760,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] + rhs[row, col]; +- } else if constexpr (SleipnirMatrixLike && EigenMatrixLike) { +- result[row, col] = lhs[row, col] + rhs(row, col); +- } else if constexpr (EigenMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs(row, col) + rhs[row, col]; +- } ++ result(row, col) = lhs(row, col) + rhs(row, col); + } + } + +@@ -808,11 +778,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- (*this)[row, col] += rhs[row, col]; +- } else { +- (*this)[row, col] += rhs(row, col); +- } ++ (*this)(row, col) += rhs(row, col); + } + } + +@@ -830,7 +796,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs; ++ (*this)(row, col) += rhs; + } + } + +@@ -854,13 +820,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- if constexpr (SleipnirMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs[row, col] - rhs[row, col]; +- } else if constexpr (SleipnirMatrixLike && EigenMatrixLike) { +- result[row, col] = lhs[row, col] - rhs(row, col); +- } else if constexpr (EigenMatrixLike && SleipnirMatrixLike) { +- result[row, col] = lhs(row, col) - rhs[row, col]; +- } ++ result(row, col) = lhs(row, col) - rhs(row, col); + } + } + +@@ -878,11 +838,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- if constexpr (SleipnirMatrixLike) { +- (*this)[row, col] -= rhs[row, col]; +- } else { +- (*this)[row, col] -= rhs(row, col); +- } ++ (*this)(row, col) -= rhs(row, col); + } + } + +@@ -900,7 +856,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs; ++ (*this)(row, col) -= rhs; + } + } + +@@ -918,7 +874,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = -lhs[row, col]; ++ result(row, col) = -lhs(row, col); + } + } + +@@ -930,7 +886,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); +- return (*this)[0, 0]; ++ return (*this)(0, 0); + } + + /** +@@ -943,7 +899,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[col, row] = (*this)[row, col]; ++ result(col, row) = (*this)(row, col); + } + } + +@@ -1017,7 +973,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = unary_op((*this)[row, col]); ++ result(row, col) = unary_op((*this)(row, col)); + } + } + +@@ -1199,7 +1155,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( + + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { +- result[row, col] = binary_op(lhs[row, col], rhs[row, col]); ++ result(row, col) = binary_op(lhs(row, col), rhs(row, col)); + } + } + +diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp +index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644 +--- a/include/sleipnir/control/ocp.hpp ++++ b/include/sleipnir/control/ocp.hpp +@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + if (m_timestep_method == TimestepMethod::FIXED) { + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i] = m_dt.count(); ++ m_DT(0, i) = m_dt.count(); + } + } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable dt = decision_variable(); +@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + // Set the member variable matrix to track the decision variable + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i] = dt; ++ m_DT(0, i) = dt; + } + } else if (m_timestep_method == TimestepMethod::VARIABLE) { + m_DT = decision_variable(1, m_num_steps + 1); + for (int i = 0; i < num_steps + 1; ++i) { +- m_DT[0, i].set_value(m_dt.count()); ++ m_DT(0, i).set_value(m_dt.count()); + } + } + +@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); +- auto dt = this->dt()[0, i]; ++ auto dt = this->dt()(0, i); + callback(time, x, u, dt); + + time += dt; +@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + + // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ + for (int i = 0; i < m_num_steps; ++i) { +- Variable h = dt()[0, i]; ++ Variable h = dt()(0, i); + + auto& f = m_dynamics_function; + +@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); +- Variable dt = this->dt()[0, i]; ++ Variable dt = this->dt()(0, i); + + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + subject_to(x_end == rk4dt()[0, i]; ++ Variable dt = this->dt()(0, i); + + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + x_end = rk4= len(prefix.parts) and all( + p1 == p2 for p1, p2 in zip(path.parts, prefix.parts) + ) + + +def git_am(patch: Path, use_threeway=False, ignore_whitespace=False): """Apply patch to a Git repository in the current directory using "git am". Keyword arguments: @@ -152,7 +168,7 @@ def git_am(patch, use_threeway=False, ignore_whitespace=False): subprocess.check_output(args + [patch]) -def has_git_rev(rev): +def has_git_rev(rev: str): """Checks whether the Git repository in the current directory has the given revision. @@ -169,10 +185,10 @@ def has_git_rev(rev): class Lib: def __init__( self, - name, - url, - tag, - copy_upstream_src, + name: str, + url: str, + tag: str, + copy_upstream_src: Callable[[Path], None], patch_options={}, *, pre_patch_hook=None, @@ -207,7 +223,7 @@ class Lib: self.pre_patch_commits = pre_patch_commits self.wpilib_root = get_repo_root() - def get_repo_path(self, tempdir=None): + def get_repo_path(self, tempdir: str | None = None): """Returns the path to the clone of the upstream repository. Keyword argument: @@ -221,11 +237,11 @@ class Lib: if tempdir is None: tempdir = tempfile.gettempdir() repo = os.path.basename(self.url) - dest = os.path.join(tempdir, repo) - dest = dest.removesuffix(".git") + dest = Path(tempdir, repo) + dest = dest.with_suffix("") return dest - def open_repo(self, *, err_msg_if_absent): + def open_repo(self, *, err_msg_if_absent: str | None): """Changes the current working directory to the upstream repository. If err_msg_if_absent is not None and the upstream repository does not exist, the program exits with return code 1. @@ -241,7 +257,7 @@ class Lib: print(f"INFO: Opening repository at {dest}") - if not os.path.exists(dest): + if not dest.exists(): if err_msg_if_absent is None: subprocess.run(["git", "clone", "--filter=tree:0", self.url]) else: @@ -285,7 +301,7 @@ class Lib: exit(1) return root_tags[0] - def set_root_tag(self, tag): + def set_root_tag(self, tag: str): """Sets the root tag, deleting any potential candidates first. Keyword argument: @@ -307,19 +323,17 @@ class Lib: Returns: The absolute path to the directory containing the patch files. """ - return os.path.join(self.wpilib_root, f"upstream_utils/{self.name}_patches") + return self.wpilib_root / f"upstream_utils/{self.name}_patches" - def get_patch_list(self): + def get_patch_list(self) -> list[Path]: """Returns a list of the filenames of the patches to apply. Returns: A list of the filenames of the patches to apply, sorted in lexicographic order by the Unicode code points.""" - if not os.path.exists(self.get_patch_directory()): + if not self.get_patch_directory().exists(): return [] - return sorted( - f for f in os.listdir(self.get_patch_directory()) if f.endswith(".patch") - ) + return sorted(self.get_patch_directory().glob("*.patch")) def apply_patches(self): """Applies the patches listed in the patch list to the current @@ -330,17 +344,17 @@ class Lib: for f in self.get_patch_list(): git_am( - os.path.join(self.get_patch_directory(), f), + self.get_patch_directory() / f, **self.patch_options, ) - def replace_tag(self, tag): + def replace_tag(self, tag: str): """Replaces the tag in the script. Keyword argument: tag -- The tag to replace the script tag with. """ - path = os.path.join(self.wpilib_root, f"upstream_utils/{self.name}.py") + path = self.wpilib_root / f"upstream_utils/{self.name}.py" with open(path, "r") as file: lines = file.readlines() @@ -391,7 +405,7 @@ class Lib: self.set_root_tag(self.old_tag) - def rebase(self, new_tag): + def rebase(self, new_tag: str): """Rebases the patches. Keyword argument: @@ -449,7 +463,7 @@ class Lib: ] ) - if os.path.exists(self.get_patch_directory()): + if self.get_patch_directory().exists(): shutil.rmtree(self.get_patch_directory()) is_first = True diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 4dea3eb1a5..79b6d75968 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -192,7 +192,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param duration the timeout duration * @return the command with the timeout added */ - [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; /** @@ -203,7 +202,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param condition the interrupt condition * @return the command with the interrupt condition added */ - [[nodiscard]] CommandPtr Until(std::function condition) &&; /** @@ -214,7 +212,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param condition the run condition * @return the command with the run condition added */ - [[nodiscard]] CommandPtr OnlyWhile(std::function condition) &&; /** @@ -224,7 +221,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param requirements the required subsystems * @return the decorated command */ - [[nodiscard]] CommandPtr BeforeStarting(std::function toRun, Requirements requirements = {}) &&; @@ -235,7 +231,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param before the command to run before this one * @return the decorated command */ - [[nodiscard]] CommandPtr BeforeStarting(CommandPtr&& before) &&; /** @@ -245,7 +240,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param requirements the required subsystems * @return the decorated command */ - [[nodiscard]] CommandPtr AndThen(std::function toRun, Requirements requirements = {}) &&; @@ -257,7 +251,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param next the commands to run next * @return the decorated command */ - [[nodiscard]] CommandPtr AndThen(CommandPtr&& next) &&; /** @@ -266,7 +259,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * * @return the decorated command */ - [[nodiscard]] CommandPtr Repeatedly() &&; /** @@ -282,7 +274,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @return the decorated command * @see ProxyCommand */ - [[nodiscard]] CommandPtr AsProxy() &&; /** @@ -294,7 +285,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param condition the condition that will prevent the command from running * @return the decorated command */ - [[nodiscard]] CommandPtr Unless(std::function condition) &&; /** @@ -306,7 +296,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param condition the condition that will allow the command to run * @return the decorated command */ - [[nodiscard]] CommandPtr OnlyIf(std::function condition) &&; /** @@ -330,7 +319,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @return the decorated command * @see WithDeadline */ - [[nodiscard]] CommandPtr DeadlineFor(CommandPtr&& parallel) &&; /** @@ -341,7 +329,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param parallel the commands to run in parallel * @return the decorated command */ - [[nodiscard]] CommandPtr AlongWith(CommandPtr&& parallel) &&; /** @@ -352,7 +339,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param parallel the commands to run in parallel * @return the decorated command */ - [[nodiscard]] CommandPtr RaceWith(CommandPtr&& parallel) &&; /** @@ -361,7 +347,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param doesRunWhenDisabled true to run when disabled. * @return the decorated command */ - [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; /** @@ -370,7 +355,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param interruptBehavior the desired interrupt behavior * @return the decorated command */ - [[nodiscard]] CommandPtr WithInterruptBehavior( Command::InterruptionBehavior interruptBehavior) &&; @@ -382,7 +366,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * command was interrupted. * @return the decorated command */ - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; /** @@ -394,7 +377,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * interrupted. * @return the decorated command */ - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; /** @@ -404,7 +386,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param handler a lambda to run when the command is interrupted * @return the decorated command */ - [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; /** @@ -413,7 +394,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { * @param name name * @return the decorated Command */ - [[nodiscard]] CommandPtr WithName(std::string_view name) &&; /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h index 81a255a9a7..fdd53c6425 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -25,7 +25,8 @@ namespace frc2 { * std::unique_ptr, use CommandPtr::Unwrap to convert. * CommandPtr::UnwrapVector does the same for vectors. */ -class CommandPtr final { +class [[nodiscard]] +CommandPtr final { public: explicit CommandPtr(std::unique_ptr&& command); @@ -46,7 +47,6 @@ class CommandPtr final { * * @return the decorated command */ - [[nodiscard]] CommandPtr Repeatedly() &&; /** @@ -60,7 +60,6 @@ class CommandPtr final { * @return the decorated command * @see ProxyCommand */ - [[nodiscard]] CommandPtr AsProxy() &&; /** @@ -69,7 +68,6 @@ class CommandPtr final { * @param doesRunWhenDisabled true to run when disabled * @return the decorated command */ - [[nodiscard]] CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&; /** @@ -78,7 +76,6 @@ class CommandPtr final { * @param interruptBehavior the desired interrupt behavior * @return the decorated command */ - [[nodiscard]] CommandPtr WithInterruptBehavior( Command::InterruptionBehavior interruptBehavior) &&; @@ -89,7 +86,6 @@ class CommandPtr final { * @param requirements the required subsystems * @return the decorated command */ - [[nodiscard]] CommandPtr AndThen(std::function toRun, Requirements requirements = {}) &&; @@ -101,7 +97,6 @@ class CommandPtr final { * @param next the commands to run next * @return the decorated command */ - [[nodiscard]] CommandPtr AndThen(CommandPtr&& next) &&; /** @@ -111,7 +106,6 @@ class CommandPtr final { * @param requirements the required subsystems * @return the decorated command */ - [[nodiscard]] CommandPtr BeforeStarting(std::function toRun, Requirements requirements = {}) &&; @@ -122,7 +116,6 @@ class CommandPtr final { * @param before the command to run before this one * @return the decorated command */ - [[nodiscard]] CommandPtr BeforeStarting(CommandPtr&& before) &&; /** @@ -133,7 +126,6 @@ class CommandPtr final { * @param duration the timeout duration * @return the command with the timeout added */ - [[nodiscard]] CommandPtr WithTimeout(units::second_t duration) &&; /** @@ -144,7 +136,6 @@ class CommandPtr final { * @param condition the interrupt condition * @return the command with the interrupt condition added */ - [[nodiscard]] CommandPtr Until(std::function condition) &&; /** @@ -155,7 +146,6 @@ class CommandPtr final { * @param condition the run condition * @return the command with the run condition added */ - [[nodiscard]] CommandPtr OnlyWhile(std::function condition) &&; /** @@ -167,7 +157,6 @@ class CommandPtr final { * @param condition the condition that will prevent the command from running * @return the decorated command */ - [[nodiscard]] CommandPtr Unless(std::function condition) &&; /** @@ -179,7 +168,6 @@ class CommandPtr final { * @param condition the condition that will allow the command to run * @return the decorated command */ - [[nodiscard]] CommandPtr OnlyIf(std::function condition) &&; /** @@ -201,7 +189,7 @@ class CommandPtr final { * @param parallel the commands to run in parallel * @return the decorated command */ - [[nodiscard]] [[deprecated("Replace with DeadlineFor")]] + [[deprecated("Replace with DeadlineFor")]] CommandPtr DeadlineWith(CommandPtr&& parallel) &&; /** @@ -214,7 +202,6 @@ class CommandPtr final { * will be interupted when the deadline command ends * @return the decorated command */ - [[nodiscard]] CommandPtr DeadlineFor(CommandPtr&& parallel) &&; /** * Decorates this command with a set of commands to run parallel to it, ending @@ -224,7 +211,6 @@ class CommandPtr final { * @param parallel the commands to run in parallel * @return the decorated command */ - [[nodiscard]] CommandPtr AlongWith(CommandPtr&& parallel) &&; /** @@ -235,7 +221,6 @@ class CommandPtr final { * @param parallel the commands to run in parallel * @return the decorated command */ - [[nodiscard]] CommandPtr RaceWith(CommandPtr&& parallel) &&; /** @@ -246,7 +231,6 @@ class CommandPtr final { * command was interrupted * @return the decorated command */ - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; /** @@ -258,7 +242,6 @@ class CommandPtr final { * interrupted. * @return the decorated command */ - [[nodiscard]] CommandPtr FinallyDo(std::function end) &&; /** @@ -268,7 +251,6 @@ class CommandPtr final { * @param handler a lambda to run when the command is interrupted * @return the decorated command */ - [[nodiscard]] CommandPtr HandleInterrupt(std::function handler) &&; /** @@ -278,7 +260,6 @@ class CommandPtr final { * @param name name * @return the decorated Command */ - [[nodiscard]] CommandPtr WithName(std::string_view name) &&; /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index b0dcda022f..9907a247fc 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -29,7 +29,6 @@ namespace cmd { /** * Constructs a command that does nothing, finishing immediately. */ -[[nodiscard]] CommandPtr None(); /** @@ -38,7 +37,6 @@ CommandPtr None(); * @param requirements Subsystems to require * @return the command */ -[[nodiscard]] CommandPtr Idle(Requirements requirements = {}); // Action Commands @@ -49,7 +47,6 @@ CommandPtr Idle(Requirements requirements = {}); * @param action the action to run * @param requirements subsystems the action requires */ -[[nodiscard]] CommandPtr RunOnce(std::function action, Requirements requirements = {}); @@ -59,7 +56,6 @@ CommandPtr RunOnce(std::function action, * @param action the action to run * @param requirements subsystems the action requires */ -[[nodiscard]] CommandPtr Run(std::function action, Requirements requirements = {}); /** @@ -70,7 +66,6 @@ CommandPtr Run(std::function action, Requirements requirements = {}); * @param end the action to run on interrupt * @param requirements subsystems the action requires */ -[[nodiscard]] CommandPtr StartEnd(std::function start, std::function end, Requirements requirements = {}); @@ -82,7 +77,6 @@ CommandPtr StartEnd(std::function start, std::function end, * @param end the action to run on interrupt * @param requirements subsystems the action requires */ -[[nodiscard]] CommandPtr RunEnd(std::function run, std::function end, Requirements requirements = {}); @@ -94,7 +88,6 @@ CommandPtr RunEnd(std::function run, std::function end, * @param run the action to run every iteration * @param requirements subsystems the action requires */ -[[nodiscard]] CommandPtr StartRun(std::function start, std::function run, Requirements requirements = {}); @@ -103,7 +96,6 @@ CommandPtr StartRun(std::function start, std::function run, * * @param msg the message to print */ -[[nodiscard]] CommandPtr Print(std::string_view msg); // Idling Commands @@ -113,7 +105,6 @@ CommandPtr Print(std::string_view msg); * * @param duration after how long the command finishes */ -[[nodiscard]] CommandPtr Wait(units::second_t duration); /** @@ -122,7 +113,6 @@ CommandPtr Wait(units::second_t duration); * * @param condition the condition */ -[[nodiscard]] CommandPtr WaitUntil(std::function condition); // Selector Commands @@ -134,7 +124,6 @@ CommandPtr WaitUntil(std::function condition); * @param onFalse the command to run if the selector function returns false * @param selector the selector function */ -[[nodiscard]] CommandPtr Either(CommandPtr&& onTrue, CommandPtr&& onFalse, std::function selector); @@ -145,7 +134,6 @@ CommandPtr Either(CommandPtr&& onTrue, CommandPtr&& onFalse, * @param commands map of commands to select from */ template ... CommandPtrs> -[[nodiscard]] CommandPtr Select(std::function selector, std::pair&&... commands) { std::vector>> vec; @@ -162,7 +150,6 @@ CommandPtr Select(std::function selector, * @param supplier the command supplier * @param requirements the set of requirements for this command */ -[[nodiscard]] CommandPtr Defer(wpi::unique_function supplier, Requirements requirements); @@ -173,7 +160,6 @@ CommandPtr Defer(wpi::unique_function supplier, * * @param supplier the command supplier */ -[[nodiscard]] CommandPtr DeferredProxy(wpi::unique_function supplier); /** @@ -183,7 +169,6 @@ CommandPtr DeferredProxy(wpi::unique_function supplier); * * @param supplier the command supplier */ -[[nodiscard]] CommandPtr DeferredProxy(wpi::unique_function supplier); // Command Groups @@ -205,14 +190,12 @@ std::vector MakeVector(Args&&... args) { /** * Runs a group of commands in series, one after the other. */ -[[nodiscard]] CommandPtr Sequence(std::vector&& commands); /** * Runs a group of commands in series, one after the other. */ template ... CommandPtrs> -[[nodiscard]] CommandPtr Sequence(CommandPtrs&&... commands) { return Sequence(impl::MakeVector(std::forward(commands)...)); } @@ -221,7 +204,6 @@ CommandPtr Sequence(CommandPtrs&&... commands) { * Runs a group of commands in series, one after the other. Once the last * command ends, the group is restarted. */ -[[nodiscard]] CommandPtr RepeatingSequence(std::vector&& commands); /** @@ -229,7 +211,6 @@ CommandPtr RepeatingSequence(std::vector&& commands); * command ends, the group is restarted. */ template ... CommandPtrs> -[[nodiscard]] CommandPtr RepeatingSequence(CommandPtrs&&... commands) { return RepeatingSequence( impl::MakeVector(std::forward(commands)...)); @@ -239,7 +220,6 @@ CommandPtr RepeatingSequence(CommandPtrs&&... commands) { * Runs a group of commands at the same time. Ends once all commands in the * group finish. */ -[[nodiscard]] CommandPtr Parallel(std::vector&& commands); /** @@ -247,7 +227,6 @@ CommandPtr Parallel(std::vector&& commands); * group finish. */ template ... CommandPtrs> -[[nodiscard]] CommandPtr Parallel(CommandPtrs&&... commands) { return Parallel(impl::MakeVector(std::forward(commands)...)); } @@ -256,7 +235,6 @@ CommandPtr Parallel(CommandPtrs&&... commands) { * Runs a group of commands at the same time. Ends once any command in the group * finishes, and cancels the others. */ -[[nodiscard]] CommandPtr Race(std::vector&& commands); /** @@ -264,7 +242,6 @@ CommandPtr Race(std::vector&& commands); * finishes, and cancels the others. */ template ... CommandPtrs> -[[nodiscard]] CommandPtr Race(CommandPtrs&&... commands) { return Race(impl::MakeVector(std::forward(commands)...)); } @@ -273,7 +250,6 @@ CommandPtr Race(CommandPtrs&&... commands) { * Runs a group of commands at the same time. Ends once a specific command * finishes, and cancels the others. */ -[[nodiscard]] CommandPtr Deadline(CommandPtr&& deadline, std::vector&& others); /** @@ -281,7 +257,6 @@ CommandPtr Deadline(CommandPtr&& deadline, std::vector&& others); * finishes, and cancels the others. */ template ... CommandPtrs> -[[nodiscard]] CommandPtr Deadline(CommandPtr&& deadline, CommandPtrs&&... commands) { return Deadline(std::move(deadline), impl::MakeVector(std::forward(commands)...)); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index d62f69ca35..ea7fbb7591 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h @@ -127,7 +127,6 @@ class Subsystem { * * @param action the action to run */ - [[nodiscard]] CommandPtr RunOnce(std::function action); /** @@ -136,7 +135,6 @@ class Subsystem { * * @param action the action to run */ - [[nodiscard]] CommandPtr Run(std::function action); /** @@ -146,7 +144,6 @@ class Subsystem { * @param start the action to run on start * @param end the action to run on interrupt */ - [[nodiscard]] CommandPtr StartEnd(std::function start, std::function end); /** @@ -156,7 +153,6 @@ class Subsystem { * @param run the action to run every iteration * @param end the action to run on interrupt */ - [[nodiscard]] CommandPtr RunEnd(std::function run, std::function end); /** @@ -166,7 +162,6 @@ class Subsystem { * @param start the action to run on start * @param run the action to run every iteration */ - [[nodiscard]] CommandPtr StartRun(std::function start, std::function run); /** @@ -176,7 +171,6 @@ class Subsystem { * @param supplier the command supplier. * @return the command. */ - [[nodiscard]] CommandPtr Defer(wpi::unique_function supplier); }; } // namespace frc2 diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp index 1dae951bea..85853d24ed 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp @@ -38,5 +38,6 @@ TEST_F(CommandPtrTest, MovedFrom) { } TEST_F(CommandPtrTest, NullInitialization) { - EXPECT_THROW(CommandPtr{std::unique_ptr{}}, frc::RuntimeError); + EXPECT_THROW(auto cmd = CommandPtr{std::unique_ptr{}}, + frc::RuntimeError); } diff --git a/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h b/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h index b14ff7c179..cd4c04bc35 100644 --- a/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h +++ b/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h @@ -75,8 +75,7 @@ class AnalogAccelerometer : public wpi::Sendable, * Set the accelerometer sensitivity. * * This sets the sensitivity of the accelerometer used for calculating the - * acceleration. The sensitivity varies by accelerometer model. There are - * constants defined for various models. + * acceleration. The sensitivity varies by accelerometer model. * * @param sensitivity The sensitivity of accelerometer in Volts per G. */ @@ -85,8 +84,7 @@ class AnalogAccelerometer : public wpi::Sendable, /** * Set the voltage that corresponds to 0 G. * - * The zero G voltage varies by accelerometer model. There are constants - * defined for various models. + * The zero G voltage varies by accelerometer model. * * @param zero The zero G voltage. */ diff --git a/wpilibc/src/main/native/include/frc/event/BooleanEvent.h b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h index e06a931988..c17c89c8f4 100644 --- a/wpilibc/src/main/native/include/frc/event/BooleanEvent.h +++ b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h @@ -129,8 +129,12 @@ class BooleanEvent { frc::Debouncer::DebounceType::kRising); private: + /// Poller loop. EventLoop* m_loop; + std::function m_signal; - std::shared_ptr m_state; // A programmer's worst nightmare. + + /// The state of the condition in the current loop poll. + std::shared_ptr m_state; }; } // namespace frc diff --git a/wpilibcExamples/CMakeLists.txt b/wpilibcExamples/CMakeLists.txt index f79cd15d68..36212ee9ac 100644 --- a/wpilibcExamples/CMakeLists.txt +++ b/wpilibcExamples/CMakeLists.txt @@ -9,6 +9,7 @@ subdir_list(SNIPPETS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/snippets) add_custom_target(wpilibcExamples) add_custom_target(wpilibcExamples_test) +add_custom_target(wpilibcExamples_snippets) foreach(example ${EXAMPLES}) file( @@ -72,8 +73,8 @@ foreach(snippet ${SNIPPETS}) src/main/cpp/snippets/${snippet}/c/*.c ) add_executable(snippet${snippet} ${sources}) - wpilib_target_warnings(${snippet}) + wpilib_target_warnings(snippet${snippet}) target_include_directories(snippet${snippet} PUBLIC src/main/cpp/snippets/${snippet}/include) target_link_libraries(snippet${snippet} wpilibc wpilibNewCommands romiVendordep xrpVendordep) - add_dependencies(wpilibcExamples snippet${snippet}) + add_dependencies(wpilibcExamples_snippets snippet${snippet}) endforeach() diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h index 09e59dd83b..162a2abe4c 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h @@ -82,7 +82,6 @@ class DriveSubsystem : public frc2::SubsystemBase { * @param distance The distance to drive forward. * @return A command. */ - [[nodiscard]] frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance); /** @@ -92,7 +91,6 @@ class DriveSubsystem : public frc2::SubsystemBase { * @param distance The distance to drive forward. * @return A command. */ - [[nodiscard]] frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance); private: diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h index 633bf65a1e..798442de7d 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -28,7 +28,6 @@ class Drive : public frc2::SubsystemBase { * @param fwd the commanded forward movement * @param rot the commanded rotation */ - [[nodiscard]] frc2::CommandPtr ArcadeDriveCommand(std::function fwd, std::function rot); @@ -39,7 +38,6 @@ class Drive : public frc2::SubsystemBase { * @param distance The distance to drive forward in meters * @param speed The fraction of max speed at which to drive */ - [[nodiscard]] frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed); /** @@ -48,7 +46,6 @@ class Drive : public frc2::SubsystemBase { * * @param angle The angle to turn to */ - [[nodiscard]] frc2::CommandPtr TurnToAngleCommand(units::degree_t angle); private: diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h index 50b2304220..3fe30ce002 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h @@ -19,11 +19,9 @@ class Intake : public frc2::SubsystemBase { /** Returns a command that deploys the intake, and then runs the intake motor * indefinitely. */ - [[nodiscard]] frc2::CommandPtr IntakeCommand(); /** Returns a command that turns off and retracts the intake. */ - [[nodiscard]] frc2::CommandPtr RetractCommand(); private: diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.h index 80642c8a71..c3c54a7b16 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.h @@ -17,7 +17,6 @@ class Pneumatics : frc2::SubsystemBase { public: Pneumatics(); /** Returns a command that disables the compressor indefinitely. */ - [[nodiscard]] frc2::CommandPtr DisableCompressorCommand(); /** diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h index 2e9b580bb2..14fdc73612 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h @@ -28,7 +28,6 @@ class Shooter : public frc2::SubsystemBase { * * @param setpointRotationsPerSecond The desired shooter velocity */ - [[nodiscard]] frc2::CommandPtr ShootCommand(units::turns_per_second_t setpoint); private: diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h index ec99118d60..5ea8ac7779 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h @@ -16,7 +16,6 @@ class Storage : frc2::SubsystemBase { public: Storage(); /** Returns a command that runs the storage motor indefinitely. */ - [[nodiscard]] frc2::CommandPtr RunCommand(); /** Whether the ball storage is full. */ diff --git a/wpilibcExamples/src/main/cpp/snippets/AnalogAccelerometer/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/AnalogAccelerometer/cpp/Robot.cpp new file mode 100644 index 0000000000..752e7ce215 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/AnalogAccelerometer/cpp/Robot.cpp @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include + +/** + * AnalogAccelerometer snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() { + // Sets the sensitivity of the accelerometer to 1 volt per G + m_accelerometer.SetSensitivity(1); + // Sets the zero voltage of the accelerometer to 3 volts + m_accelerometer.SetZero(3); + } + + void TeleopPeriodic() override { + // Gets the current acceleration + m_accelerometer.GetAcceleration(); + } + + private: + // Creates an analog accelerometer on analog input 0 + frc::AnalogAccelerometer m_accelerometer{0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/AnalogEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/AnalogEncoder/cpp/Robot.cpp new file mode 100644 index 0000000000..d9bf670336 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/AnalogEncoder/cpp/Robot.cpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +/** + * AnalogEncoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() {} + + void TeleopPeriodic() override { + // Gets the rotation + m_encoder.Get(); + } + + private: + // Initializes an analog encoder on Analog Input pin 0 + frc::AnalogEncoder m_encoder{0}; + + // Initializes an analog encoder on DIO pins 0 to return a value of 4 for + // a full rotation, with the encoder reporting 0 half way through rotation (2 + // out of 4) + frc::AnalogEncoder m_encoderFR{0, 4.0, 2.0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/AnalogInput/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/AnalogInput/cpp/Robot.cpp new file mode 100644 index 0000000000..0cf7122da5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/AnalogInput/cpp/Robot.cpp @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +/** + * AnalogInput snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-input-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() { + // Sets the AnalogInput to 4-bit oversampling. 16 samples will be added + // together. + // Thus, the reported values will increase by about a factor of 16, and the + // update rate will decrease by a similar amount. + m_analog.SetOversampleBits(4); + + // Sets the AnalogInput to 4-bit averaging. 16 samples will be averaged + // together. The update rate will decrease by a factor of 16. + m_analog.SetAverageBits(4); + + // Gets the raw instantaneous measured value from the analog input, without + // applying any calibration and ignoring oversampling and averaging + // settings. + m_analog.GetValue(); + + // Gets the instantaneous measured voltage from the analog input. + // Oversampling and averaging settings are ignored + m_analog.GetVoltage(); + + // Gets the averaged value from the analog input. The value is not + // rescaled, but oversampling and averaging are both applied. + m_analog.GetAverageValue(); + + // Gets the averaged voltage from the analog input. Rescaling, + // oversampling, and averaging are all applied. + m_analog.GetAverageVoltage(); + } + + void TeleopPeriodic() override { + } + + private: + // Initializes an AnalogInput on port 0 + frc::AnalogInput m_analog{0}; + + // The count and value variables to fill + int64_t count; + int64_t value; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/AnalogPotentiometer/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/AnalogPotentiometer/cpp/Robot.cpp new file mode 100644 index 0000000000..7cf15bf55d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/AnalogPotentiometer/cpp/Robot.cpp @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include + +/** + * AnalogPotentiometer snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-potentiometers-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() { + // Set averaging bits to 2 + m_input.SetAverageBits(2); + } + + void TeleopPeriodic() override { + // Get the value of the potentiometer + m_pot.Get(); + } + + private: + // Initializes an AnalogPotentiometer on analog port 0 + // The full range of motion (in meaningful external units) is 0-180 (this + // could be degrees, for instance) The "starting point" of the motion, i.e. + // where the mechanism is located when the potentiometer reads 0v, is 30. + frc::AnalogPotentiometer m_pot{0, 180, 30}; + + // Initializes an AnalogInput on port 1 + frc::AnalogInput m_input{1}; + // Initializes an AnalogPotentiometer with the given AnalogInput + // The full range of motion (in meaningful external units) is 0-180 (this + // could be degrees, for instance) The "starting point" of the motion, i.e. + // where the mechanism is located when the potentiometer reads 0v, is 30. + frc::AnalogPotentiometer m_pot1{&m_input, 180, 30}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/DigitalInput/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/DigitalInput/cpp/Robot.cpp new file mode 100644 index 0000000000..4a3b648c63 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/DigitalInput/cpp/Robot.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +/** + * Digital Input snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/digital-input-software.html + */ +class Robot : public frc::TimedRobot { + public: + void TeleopPeriodic() override { + // Gets the value of the digital input. Returns true if the circuit is + // open. + m_input.Get(); + } + + private: + // Initializes a DigitalInput on DIO 0 + frc::DigitalInput m_input{0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp index 25593ec933..8655354ced 100644 --- a/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp @@ -2,8 +2,6 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include #include diff --git a/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp index 7f736d5c1b..deff20a1e6 100644 --- a/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp @@ -2,11 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include #include -#include #include /** diff --git a/wpilibcExamples/src/main/cpp/snippets/EncoderDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/EncoderDrive/cpp/Robot.cpp new file mode 100644 index 0000000000..d21a7f063d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/EncoderDrive/cpp/Robot.cpp @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +/** + * Encoder drive to distance snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +class Robot : public frc::TimedRobot { + public: + Robot() { + // Configures the encoder's distance-per-pulse + // The robot moves forward 1 foot per encoder rotation + // There are 256 pulses per encoder rotation + m_encoder.SetDistancePerPulse(1.0 / 256.0); + // Invert the right side of the drivetrain. You might have to invert the + // other side + rightLeader.SetInverted(true); + // Configure the followers to follow the leaders + leftLeader.AddFollower(leftFollower); + rightLeader.AddFollower(rightFollower); + } + void AutonomousPeriodic() override { + // Drives forward at half speed until the robot has moved 5 feet, then + // stops: + if (m_encoder.GetDistance() < 5) { + drive.TankDrive(0.5, 0.5); + } else { + drive.TankDrive(0, 0); + } + } + + private: + // Creates an encoder on DIO ports 0 and 1. + frc::Encoder m_encoder{0, 1}; + // Initialize motor controllers and drive + frc::Spark leftLeader{0}; + frc::Spark leftFollower{1}; + frc::Spark rightLeader{2}; + frc::Spark rightFollower{3}; + frc::DifferentialDrive drive{[&](double output) { leftLeader.Set(output); }, + [&](double output) { rightLeader.Set(output); }}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/EncoderHoming/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/EncoderHoming/cpp/Robot.cpp new file mode 100644 index 0000000000..a18914d545 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/EncoderHoming/cpp/Robot.cpp @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +/** + * Encoder mechanism homing snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + // Runs the motor backwards at half speed until the limit switch is pressed + // then turn off the motor and reset the encoder + if (!m_limit.Get()) { + m_spark.Set(-0.5); + } else { + m_spark.Set(0); + m_encoder.Reset(); + } + } + + private: + frc::Encoder m_encoder{0, 1}; + frc::Spark m_spark{0}; + // Limit switch on DIO 2 + frc::DigitalInput m_limit{2}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/LimitSwitch/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/LimitSwitch/cpp/Robot.cpp new file mode 100644 index 0000000000..7fc54ed6e1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/LimitSwitch/cpp/Robot.cpp @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include + +/** + * Limit Switch snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/limit-switch.html + */ +class Robot : public frc::TimedRobot { + public: + void TeleopPeriodic() override { SetMotorSpeed(m_joystick.GetRawAxis(2)); } + void SetMotorSpeed(double speed) { + if (speed > 0) { + if (m_toplimitSwitch.Get()) { + // We are going up and top limit is tripped so stop + m_motor.Set(0); + } else { + // We are going up but top limit is not tripped so go at commanded speed + m_motor.Set(speed); + } + } else { + if (m_bottomlimitSwitch.Get()) { + // We are going down and bottom limit is tripped so stop + m_motor.Set(0); + } else { + // We are going down but bottom limit is not tripped so go at commanded + // speed + m_motor.Set(speed); + } + } + } + + private: + frc::DigitalInput m_toplimitSwitch{0}; + frc::DigitalInput m_bottomlimitSwitch{1}; + frc::PWMVictorSPX m_motor{0}; + frc::Joystick m_joystick{0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index 5edd312bb9..437426ce0d 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -19,5 +19,89 @@ ], "foldername": "DutyCycleEncoder", "gradlebase": "cpp" + }, + { + "name": "AnalogEncoder", + "description": "Snippets of AnalogEncoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Analog" + ], + "foldername": "AnalogEncoder", + "gradlebase": "cpp" + }, + { + "name": "EncoderDrive", + "description": "Snippets of driving to a distance for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Differential Drive" + ], + "foldername": "EncoderDrive", + "gradlebase": "cpp" + }, + { + "name": "EncoderHoming", + "description": "Snippets of homing a mechanism for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Digital Input" + ], + "foldername": "EncoderHoming", + "gradlebase": "cpp" + }, + { + "name": "LimitSwitch", + "description": "Snippets of Limit Switch for frc-docs.", + "tags": [ + "Hardware", + "Digital Input" + ], + "foldername": "LimitSwitch", + "gradlebase": "cpp" + }, + { + "name": "DigitalInput", + "description": "Snippets of Digital Input for frc-docs.", + "tags": [ + "Hardware", + "Digital Input" + ], + "foldername": "DigitalInput", + "gradlebase": "cpp" + }, + { + "name": "AnalogInput", + "description": "Snippets of Analog Input for frc-docs.", + "tags": [ + "Hardware", + "Analog" + ], + "foldername": "AnalogInput", + "gradlebase": "cpp" + }, + { + "name": "AnalogPotentiometer", + "description": "Snippets of Analog Potentiometer for frc-docs.", + "tags": [ + "Hardware", + "Analog" + ], + "foldername": "AnalogPotentiometer", + "gradlebase": "cpp" + }, + { + "name": "AnalogAccelerometer", + "description": "Snippets of Analog Accelerometer for frc-docs.", + "tags": [ + "Hardware", + "Analog", + "Accelerometer" + ], + "foldername": "AnalogAccelerometer", + "gradlebase": "cpp" } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java index f2198e4a5c..e7c8f76429 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java @@ -90,7 +90,7 @@ public class AnalogAccelerometer implements Sendable, AutoCloseable { * Set the accelerometer sensitivity. * *

This sets the sensitivity of the accelerometer used for calculating the acceleration. The - * sensitivity varies by accelerometer model. There are constants defined for various models. + * sensitivity varies by accelerometer model. * * @param sensitivity The sensitivity of accelerometer in Volts per G. */ @@ -101,8 +101,7 @@ public class AnalogAccelerometer implements Sendable, AutoCloseable { /** * Set the voltage that corresponds to 0 G. * - *

The zero G voltage varies by accelerometer model. There are constants defined for various - * models. + *

The zero G voltage varies by accelerometer model. * * @param zero The zero G voltage. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java index 53d5d62153..9369b7811a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java @@ -28,7 +28,7 @@ public class BooleanEvent implements BooleanSupplier { private final BooleanSupplier m_signal; - /** The state of the condition in the current loop poll. Nightmare to manage. */ + /** The state of the condition in the current loop poll. */ private final AtomicBoolean m_state = new AtomicBoolean(false); /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Main.java new file mode 100644 index 0000000000..fdce111b55 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogaccelerometer; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Robot.java new file mode 100644 index 0000000000..adbb5bee35 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Robot.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogaccelerometer; + +import edu.wpi.first.wpilibj.AnalogAccelerometer; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * AnalogAccelerometer snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html + */ +public class Robot extends TimedRobot { + // Creates an analog accelerometer on analog input 0 + AnalogAccelerometer m_accelerometer = new AnalogAccelerometer(0); + + /** Called once at the beginning of the robot program. */ + public Robot() { + // Sets the sensitivity of the accelerometer to 1 volt per G + m_accelerometer.setSensitivity(1); + // Sets the zero voltage of the accelerometer to 3 volts + m_accelerometer.setZero(3); + } + + @Override + public void teleopPeriodic() { + // Gets the current acceleration + m_accelerometer.getAcceleration(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Main.java new file mode 100644 index 0000000000..0e074925d0 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogencoder; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Robot.java new file mode 100644 index 0000000000..aa09daf933 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogencoder/Robot.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogencoder; + +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * AnalogEncoder snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +public class Robot extends TimedRobot { + // Initializes an analog encoder on Analog Input pin 0 + AnalogEncoder m_encoder = new AnalogEncoder(0); + + // Initializes an analog encoder on DIO pins 0 to return a value of 4 for + // a full rotation, with the encoder reporting 0 half way through rotation (2 + // out of 4) + AnalogEncoder m_encoderFR = new AnalogEncoder(0, 4.0, 2.0); + + /** Called once at the beginning of the robot program. */ + public Robot() {} + + @Override + public void teleopPeriodic() { + // Gets the rotation + m_encoder.get(); + + m_encoderFR.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Main.java new file mode 100644 index 0000000000..767b0c3bce --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analoginput; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Robot.java new file mode 100644 index 0000000000..b59e103a56 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analoginput/Robot.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analoginput; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * AnalogInput snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-inputs-software.html + */ +public class Robot extends TimedRobot { + // Initializes an AnalogInput on port 0 + AnalogInput m_analog = new AnalogInput(0); + + /** Called once at the beginning of the robot program. */ + public Robot() { + // Sets the AnalogInput to 4-bit oversampling. 16 samples will be added together. + // Thus, the reported values will increase by about a factor of 16, and the update + // rate will decrease by a similar amount. + m_analog.setOversampleBits(4); + + // Sets the AnalogInput to 4-bit averaging. 16 samples will be averaged together. + // The update rate will decrease by a factor of 16. + m_analog.setAverageBits(4); + } + + @Override + public void teleopPeriodic() { + // Gets the raw instantaneous measured value from the analog input, without + // applying any calibration and ignoring oversampling and averaging + // settings. + m_analog.getValue(); + + // Gets the instantaneous measured voltage from the analog input. + // Oversampling and averaging settings are ignored + m_analog.getVoltage(); + + // Gets the averaged value from the analog input. The value is not + // rescaled, but oversampling and averaging are both applied. + m_analog.getAverageValue(); + + // Gets the averaged voltage from the analog input. Rescaling, + // oversampling, and averaging are all applied. + m_analog.getAverageVoltage(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Main.java new file mode 100644 index 0000000000..b64b880a95 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogpotentiometer; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Robot.java new file mode 100644 index 0000000000..cbb836cec7 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogpotentiometer/Robot.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.analogpotentiometer; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * AnalogPotentiometer snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-poteniometers-software.html + */ +public class Robot extends TimedRobot { + // Initializes an AnalogPotentiometer on analog port 0 + // The full range of motion (in meaningful external units) is 0-180 (this could be degrees, for + // instance) + // The "starting point" of the motion, i.e. where the mechanism is located when the potentiometer + // reads 0v, is 30. + AnalogPotentiometer m_pot = new AnalogPotentiometer(0, 180, 30); + + // Initializes an AnalogInput on port 1 + AnalogInput m_input = new AnalogInput(0); + // Initializes an AnalogPotentiometer with the given AnalogInput + // The full range of motion (in meaningful external units) is 0-180 (this could be degrees, for + // instance) + // The "starting point" of the motion, i.e. where the mechanism is located when the potentiometer + // reads 0v, is 30. + AnalogPotentiometer m_pot1 = new AnalogPotentiometer(m_input, 180, 30); + + /** Called once at the beginning of the robot program. */ + public Robot() { + // Set averaging bits to 2 + m_input.setAverageBits(2); + } + + @Override + public void teleopPeriodic() { + // Get the value of the potentiometer + m_pot.get(); + + m_pot1.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Main.java new file mode 100644 index 0000000000..fb52ad8ae6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.digitalinput; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Robot.java new file mode 100644 index 0000000000..999c72821f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/digitalinput/Robot.java @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.digitalinput; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.TimedRobot; + +/** + * DigitalInput snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/digital-inputs-software.html + */ +public class Robot extends TimedRobot { + // Initializes a DigitalInput on DIO 0 + DigitalInput m_input = new DigitalInput(0); + + @Override + public void teleopPeriodic() { + // Gets the value of the digital input. Returns true if the circuit is open. + m_input.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Main.java new file mode 100644 index 0000000000..e351e54948 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoderdrive; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Robot.java new file mode 100644 index 0000000000..de41a19102 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderdrive/Robot.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoderdrive; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.motorcontrol.Spark; + +/** + * Encoder drive to distance snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +public class Robot extends TimedRobot { + // Creates an encoder on DIO ports 0 and 1 + Encoder m_encoder = new Encoder(0, 1); + // Initialize motor controllers and drive + Spark m_leftLeader = new Spark(0); + Spark m_leftFollower = new Spark(1); + Spark m_rightLeader = new Spark(2); + Spark m_rightFollower = new Spark(3); + DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader::set, m_rightLeader::set); + + /** Called once at the beginning of the robot program. */ + public Robot() { + // Configures the encoder's distance-per-pulse + // The robot moves forward 1 foot per encoder rotation + // There are 256 pulses per encoder rotation + m_encoder.setDistancePerPulse(1.0 / 256.0); + // Invert the right side of the drivetrain. You might have to invert the other side + m_rightLeader.setInverted(true); + // Configure the followers to follow the leaders + m_leftLeader.addFollower(m_leftFollower); + m_rightLeader.addFollower(m_rightFollower); + } + + /** Drives forward at half speed until the robot has moved 5 feet, then stops. */ + @Override + public void autonomousPeriodic() { + if (m_encoder.getDistance() < 5.0) { + m_drive.tankDrive(0.5, 0.5); + } else { + m_drive.tankDrive(0.0, 0.0); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Main.java new file mode 100644 index 0000000000..f014009221 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoderhoming; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Robot.java new file mode 100644 index 0000000000..34e60d1b5b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/encoderhoming/Robot.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.encoderhoming; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.motorcontrol.Spark; + +/** + * Encoder mechanism homing snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html + */ +public class Robot extends TimedRobot { + Encoder m_encoder = new Encoder(0, 1); + Spark m_spark = new Spark(0); + // Limit switch on DIO 2 + DigitalInput m_limit = new DigitalInput(2); + + /** + * Runs the motor backwards at half speed until the limit switch is pressed then turn off the + * motor and reset the encoder. + */ + @Override + public void autonomousPeriodic() { + if (!m_limit.get()) { + m_spark.set(-0.5); + } else { + m_spark.set(0.0); + m_encoder.reset(); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Main.java new file mode 100644 index 0000000000..19c1da0b45 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.limitswitch; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Robot.java new file mode 100644 index 0000000000..73c2b6c398 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/limitswitch/Robot.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.snippets.limitswitch; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; + +/** + * Limit Switch snippets for frc-docs. + * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/limit-switch.html + */ +public class Robot extends TimedRobot { + DigitalInput m_toplimitSwitch = new DigitalInput(0); + DigitalInput m_bottomlimitSwitch = new DigitalInput(1); + PWMVictorSPX m_motor = new PWMVictorSPX(0); + Joystick m_joystick = new Joystick(0); + + @Override + public void teleopPeriodic() { + setMotorSpeed(m_joystick.getRawAxis(2)); + } + + /** + * Sets the motor speed based on joystick input while respecting limit switches. + * + * @param speed the desired speed of the motor, positive for up and negative for down + */ + public void setMotorSpeed(double speed) { + if (speed > 0) { + if (m_toplimitSwitch.get()) { + // We are going up and top limit is tripped so stop + m_motor.set(0); + } else { + // We are going up but top limit is not tripped so go at commanded speed + m_motor.set(speed); + } + } else { + if (m_bottomlimitSwitch.get()) { + // We are going down and bottom limit is tripped so stop + m_motor.set(0); + } else { + // We are going down but bottom limit is not tripped so go at commanded speed + m_motor.set(speed); + } + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json index abc1bd45e1..dc2d5b7d1e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -21,5 +21,97 @@ "foldername": "dutycycleencoder", "gradlebase": "java", "mainclass": "Main" + }, + { + "name": "AnalogEncoder", + "description": "Snippets of AnalogEncoder class usage for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Analog" + ], + "foldername": "analogencoder", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "EncoderDrive", + "description": "Snippets of driving to a distance for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Differential Drive" + ], + "foldername": "encoderdrive", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "EncoderHoming", + "description": "Snippets of homing a mechanism for frc-docs.", + "tags": [ + "Hardware", + "Encoder", + "Digital Input" + ], + "foldername": "encoderhoming", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "LimitSwitch", + "description": "Snippets of Limit Switch for frc-docs.", + "tags": [ + "Hardware", + "Digital Input" + ], + "foldername": "limitswitch", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "DigitalInput", + "description": "Snippets of Digital Input for frc-docs.", + "tags": [ + "Hardware", + "Digital Input" + ], + "foldername": "digitalinput", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "AnalogInput", + "description": "Snippets of Analog Input for frc-docs.", + "tags": [ + "Hardware", + "Analog" + ], + "foldername": "analoginput", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "AnalogPotentiometer", + "description": "Snippets of Analog Potentiometer for frc-docs.", + "tags": [ + "Hardware", + "Analog" + ], + "foldername": "analogpotentiometer", + "gradlebase": "java", + "mainclass": "Main" + }, + { + "name": "AnalogAccelerometer", + "description": "Snippets of Analog Accelerometer for frc-docs.", + "tags": [ + "Hardware", + "Analog", + "Accelerometer" + ], + "foldername": "analogaccelerometer", + "gradlebase": "java", + "mainclass": "Main" } ] diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index 9d3aaf3b9f..c18cfebedc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -13,11 +13,11 @@ import edu.wpi.first.math.util.Units; /** * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain * (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve - * compared to skid-steer style drivetrains because it is possible to individually control forward, - * sideways, and angular velocity. + * compared to skid-steer style drivetrains because it is possible to individually control + * field-relative x, y, and angular velocity. * - *

The holonomic drive controller takes in one PID controller for each direction, forward and - * sideways, and one profiled PID controller for the angular direction. Because the heading dynamics + *

The holonomic drive controller takes in one PID controller for each direction, field-relative + * x and y, and one profiled PID controller for the angular direction. Because the heading dynamics * are decoupled from translations, users can specify a custom heading that the drivetrain should * point toward. This heading reference is profiled for smoothness. */ diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index f0f6786e1a..01e987d5d4 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -4,6 +4,8 @@ #include "frc/StateSpaceUtil.h" +#include + namespace frc { template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, @@ -13,4 +15,57 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, template bool IsStabilizable( const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); +template bool IsDetectable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& C); + +template Eigen::VectorXd ClampInputMaxMagnitude( + const Eigen::VectorXd& u, const Eigen::VectorXd& umin, + const Eigen::VectorXd& umax); + +template Eigen::VectorXd DesaturateInputVector( + const Eigen::VectorXd& u, double maxMagnitude); + +Eigen::MatrixXd MakeCostMatrix(const std::span costs) { + Eigen::MatrixXd result{costs.size(), costs.size()}; + result.setZero(); + + for (size_t i = 0; i < costs.size(); ++i) { + if (costs[i] == std::numeric_limits::infinity()) { + result(i, i) = 0.0; + } else { + result(i, i) = 1.0 / (std::pow(costs[i], 2)); + } + } + return result; +} + +Eigen::VectorXd MakeWhiteNoiseVector(const std::span stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::VectorXd result{stdDevs.size()}; + for (size_t i = 0; i < stdDevs.size(); ++i) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDevs[i] == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDevs[i]}; + result(i) = distr(gen); + } + } + return result; +} + +Eigen::MatrixXd MakeCovMatrix(const std::span stdDevs) { + Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()}; + result.setZero(); + + for (size_t i = 0; i < stdDevs.size(); ++i) { + result(i, i) = std::pow(stdDevs[i], 2); + } + + return result; +} + } // namespace frc diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index fbfdde2227..9488ab19ff 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include #include "frc/EigenCore.h" #include "frc/system/NumericalIntegration.h" @@ -18,7 +18,7 @@ using namespace frc; units::volt_t ArmFeedforward::Calculate( units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity) const { - using VarMat = sleipnir::VariableMatrix; + using VarMat = slp::VariableMatrix; // Small kₐ values make the solver ill-conditioned if (kA < units::unit_t{1e-1}) { @@ -32,37 +32,37 @@ units::volt_t ArmFeedforward::Calculate( Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}}; const auto& f = [&](const VarMat& x, const VarMat& u) -> VarMat { VarMat c{{0.0}, - {-(kS / kA).value() * sleipnir::sign(x(1)) - - (kG / kA).value() * sleipnir::cos(x(0))}}; + {-(kS / kA).value() * slp::sign(x[1]) - + (kG / kA).value() * slp::cos(x[0])}}; return A * x + B * u + c; }; Vectord<2> r_k{currentAngle.value(), currentVelocity.value()}; - sleipnir::Variable u_k; + slp::Variable u_k; // Initial guess auto acceleration = (nextVelocity - currentVelocity) / m_dt; - u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + - kA * acceleration + kG * units::math::cos(currentAngle)) - .value()); + u_k.set_value((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + + kA * acceleration + kG * units::math::cos(currentAngle)) + .value()); auto r_k1 = RK4(f, r_k, u_k, m_dt); // Minimize difference between desired and actual next velocity auto cost = - (nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1)); + (nextVelocity.value() - r_k1[1]) * (nextVelocity.value() - r_k1[1]); // Refine solution via Newton's method { auto xAD = u_k; - double x = xAD.Value(); + double x = xAD.value(); - sleipnir::Gradient gradientF{cost, xAD}; - Eigen::SparseVector g = gradientF.Value(); + slp::Gradient gradientF{cost, xAD}; + Eigen::SparseVector g = gradientF.value(); - sleipnir::Hessian hessianF{cost, xAD}; - Eigen::SparseMatrix H = hessianF.Value(); + slp::Hessian hessianF{cost, xAD}; + Eigen::SparseMatrix H = hessianF.value(); double error_k = std::numeric_limits::infinity(); double error_k1 = std::abs(g.coeff(0)); @@ -81,31 +81,31 @@ units::volt_t ArmFeedforward::Calculate( // Shrink step until cost goes down { - double oldCost = cost.Value(); + double oldCost = cost.value(); double α = 1.0; double trial_x = x + α * p_x; - xAD.SetValue(trial_x); + xAD.set_value(trial_x); - while (cost.Value() > oldCost) { + while (cost.value() > oldCost) { α *= 0.5; trial_x = x + α * p_x; - xAD.SetValue(trial_x); + xAD.set_value(trial_x); } x = trial_x; } - xAD.SetValue(x); + xAD.set_value(x); - g = gradientF.Value(); - H = hessianF.Value(); + g = gradientF.value(); + H = hessianF.value(); error_k1 = std::abs(g.coeff(0)); } } - return units::volt_t{u_k.Value()}; + return units::volt_t{u_k.value()}; } diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 9c4fe4fc1e..ab53ecb5d1 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -4,7 +4,7 @@ #include "frc/geometry/Ellipse2d.h" -#include +#include using namespace frc; @@ -20,31 +20,29 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const { // Find nearest point { - namespace slp = sleipnir; - - sleipnir::OptimizationProblem problem; + slp::Problem problem; // Point on ellipse - auto x = problem.DecisionVariable(); - x.SetValue(rotPoint.X().value()); - auto y = problem.DecisionVariable(); - y.SetValue(rotPoint.Y().value()); + auto x = problem.decision_variable(); + x.set_value(rotPoint.X().value()); + auto y = problem.decision_variable(); + y.set_value(rotPoint.Y().value()); - problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) + + problem.minimize(slp::pow(x - rotPoint.X().value(), 2) + slp::pow(y - rotPoint.Y().value(), 2)); // (x − x_c)²/a² + (y − y_c)²/b² = 1 // b²(x − x_c)² + a²(y − y_c)² = a²b² double a2 = m_xSemiAxis.value() * m_xSemiAxis.value(); double b2 = m_ySemiAxis.value() * m_ySemiAxis.value(); - problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) + - a2 * slp::pow(y - m_center.Y().value(), 2) == - a2 * b2); + problem.subject_to(b2 * slp::pow(x - m_center.X().value(), 2) + + a2 * slp::pow(y - m_center.Y().value(), 2) == + a2 * b2); - problem.Solve(); + problem.solve(); - rotPoint = frc::Translation2d{units::meter_t{x.Value()}, - units::meter_t{y.Value()}}; + rotPoint = frc::Translation2d{units::meter_t{x.value()}, + units::meter_t{y.value()}}; } // Undo rotation diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 1e8ae2a108..6c58485d45 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -123,6 +123,22 @@ constexpr Matrixd MakeCostMatrix(const std::array& costs) { return result; } +/** + * Creates a cost matrix from the given vector for use with LQR. + * + * The cost matrix is constructed using Bryson's rule. The inverse square of + * each element in the input is placed on the cost matrix diagonal. If a + * tolerance is infinity, its cost matrix entry is set to zero. + * + * @param costs A possibly variable length container. For a Q matrix, its + * elements are the maximum allowed excursions of the states from + * the reference. For an R matrix, its elements are the maximum + * allowed excursions of the control inputs from no actuation. + * @return State excursion or control effort cost matrix. + */ +WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix( + const std::span costs); + /** * Creates a covariance matrix from the given vector for use with Kalman * filters. @@ -152,6 +168,21 @@ constexpr Matrixd MakeCovMatrix(const std::array& stdDevs) { return result; } +/** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + * Each element is squared and placed on the covariance matrix diagonal. + * + * @param stdDevs A possibly variable length container. For a Q matrix, its + * elements are the standard deviations of each state from how + * the model behaves. For an R matrix, its elements are the + * standard deviations for each output measurement. + * @return Process noise or measurement noise covariance matrix. + */ +WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix( + const std::span stdDevs); + template ... Ts> Vectord MakeWhiteNoiseVector(Ts... stdDevs) { std::random_device rd; @@ -200,6 +231,17 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { return result; } +/** + * Creates a vector of normally distributed white noise with the given noise + * intensities for each element. + * + * @param stdDevs A possibly variable length container whose elements are the + * standard deviations of each element of the noise vector. + * @return White noise vector. + */ +WPILIB_DLLEXPORT Eigen::VectorXd MakeWhiteNoiseVector( + const std::span stdDevs); + /** * Converts a Pose2d into a vector of [x, y, theta]. * @@ -311,6 +353,10 @@ bool IsDetectable(const Matrixd& A, return IsStabilizable(A.transpose(), C.transpose()); } +extern template WPILIB_DLLEXPORT bool +IsDetectable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& C); + /** * Converts a Pose2d into a vector of [x, y, theta]. * @@ -341,12 +387,17 @@ constexpr Vectord ClampInputMaxMagnitude(const Vectord& u, const Vectord& umin, const Vectord& umax) { Vectord result; - for (int i = 0; i < Inputs; ++i) { + for (int i = 0; i < u.rows(); ++i) { result(i) = std::clamp(u(i), umin(i), umax(i)); } return result; } +extern template WPILIB_DLLEXPORT Eigen::VectorXd +ClampInputMaxMagnitude(const Eigen::VectorXd& u, + const Eigen::VectorXd& umin, + const Eigen::VectorXd& umax); + /** * Renormalize all inputs if any exceeds the maximum magnitude. Useful for * systems such as differential drivetrains. @@ -366,4 +417,9 @@ Vectord DesaturateInputVector(const Vectord& u, } return u; } + +extern template WPILIB_DLLEXPORT Eigen::VectorXd +DesaturateInputVector(const Eigen::VectorXd& u, + double maxMagnitude); + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index 2bf42cc093..b823c48450 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -23,11 +23,11 @@ namespace frc { * This holonomic drive controller can be used to follow trajectories using a * holonomic drivetrain (i.e. swerve or mecanum). Holonomic trajectory following * is a much simpler problem to solve compared to skid-steer style drivetrains - * because it is possible to individually control forward, sideways, and angular - * velocity. + * because it is possible to individually control field-relative x, y, and + * angular velocity. * * The holonomic drive controller takes in one PID controller for each - * direction, forward and sideways, and one profiled PID controller for the + * direction, field-relative x and y, and one profiled PID controller for the * angular direction. Because the heading dynamics are decoupled from * translations, users can specify a custom heading that the drivetrain should * point toward. This heading reference is profiled for smoothness. diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h index c4cb6959dd..c6f5719a31 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h +++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h @@ -34,6 +34,36 @@ auto NumericalJacobian(F&& f, const Vectord& x) { return result; } +/** + * Returns numerical Jacobian with respect to x for f(x). + * + * @param f Vector-valued function from which to compute Jacobian. + * @param x Vector argument. + */ + +template +Eigen::MatrixXd NumericalJacobian(F&& f, const Eigen::VectorXd& x) { + constexpr double kEpsilon = 1e-5; + Eigen::MatrixXd result; + + // It's more expensive, but +- epsilon will be more accurate + for (int i = 0; i < x.rows(); ++i) { + Eigen::VectorXd dX_plus = x; + dX_plus(i) += kEpsilon; + Eigen::VectorXd dX_minus = x; + dX_minus(i) -= kEpsilon; + Eigen::VectorXd partialDerivative = + (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0); + if (i == 0) { + result.resize(partialDerivative.rows(), x.rows()); + result.setZero(); + } + result.col(i) = partialDerivative; + } + + return result; +} + /** * Returns numerical Jacobian with respect to x for f(x, u, ...). * @@ -54,6 +84,23 @@ auto NumericalJacobianX(F&& f, const Vectord& x, [&](const Vectord& x) { return f(x, u, args...); }, x); } +/** + * Returns numerical Jacobian with respect to x for f(x, u, ...). + * + * @tparam F Function object type. + * @tparam Args... Types of remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + * @param args Remaining arguments to f(x, u, ...). + */ +template +auto NumericalJacobianX(F&& f, const Eigen::VectorXd& x, + const Eigen::VectorXd& u, Args&&... args) { + return NumericalJacobian( + [&](const Eigen::VectorXd& x) { return f(x, u, args...); }, x); +} + /** * Returns numerical Jacobian with respect to u for f(x, u, ...). * @@ -74,4 +121,21 @@ auto NumericalJacobianU(F&& f, const Vectord& x, [&](const Vectord& u) { return f(x, u, args...); }, u); } +/** + * Returns numerical Jacobian with respect to u for f(x, u, ...). + * + * @tparam F Function object type. + * @tparam Args... Types of remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + * @param args Remaining arguments to f(x, u, ...). + */ +template +auto NumericalJacobianU(F&& f, const Eigen::VectorXd& x, + const Eigen::VectorXd& u, Args&&... args) { + return NumericalJacobian( + [&](const Eigen::VectorXd& u) { return f(x, u, args...); }, u); +} + } // namespace frc diff --git a/wpimath/src/main/native/include/units/length.h b/wpimath/src/main/native/include/units/length.h index 8b75c7c5a9..695bb57fe1 100644 --- a/wpimath/src/main/native/include/units/length.h +++ b/wpimath/src/main/native/include/units/length.h @@ -42,8 +42,8 @@ namespace units { UNIT_ADD_WITH_METRIC_PREFIXES(length, meter, meters, m, unit, units::category::length_unit>) UNIT_ADD(length, foot, feet, ft, unit, meters>) -UNIT_ADD(length, mil, mils, mil, unit, feet>) UNIT_ADD(length, inch, inches, in, unit, feet>) +UNIT_ADD(length, mil, mils, mil, unit, inches>) UNIT_ADD(length, mile, miles, mi, unit, feet>) UNIT_ADD(length, nauticalMile, nauticalMiles, nmi, unit, meters>) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core index a7f8dcadd3..69aa1f8e5c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core @@ -193,21 +193,27 @@ using std::ptrdiff_t; #include "src/Core/arch/Default/GenericPacketMathFunctionsFwd.h" #if defined EIGEN_VECTORIZE_AVX512 +#include "src/Core/arch/SSE/PacketMath.h" +#include "src/Core/arch/AVX/PacketMath.h" +// #include "src/Core/arch/AVX512/PacketMath.h" #if defined EIGEN_VECTORIZE_AVX512FP16 // #include "src/Core/arch/AVX512/PacketMathFP16.h" #endif -#include "src/Core/arch/SSE/PacketMath.h" #include "src/Core/arch/SSE/TypeCasting.h" -#include "src/Core/arch/SSE/Complex.h" -#include "src/Core/arch/AVX/PacketMath.h" #include "src/Core/arch/AVX/TypeCasting.h" -#include "src/Core/arch/AVX/Complex.h" -// #include "src/Core/arch/AVX512/PacketMath.h" // #include "src/Core/arch/AVX512/TypeCasting.h" +#if defined EIGEN_VECTORIZE_AVX512FP16 +// #include "src/Core/arch/AVX512/TypeCastingFP16.h" +#endif +#include "src/Core/arch/SSE/Complex.h" +#include "src/Core/arch/AVX/Complex.h" // #include "src/Core/arch/AVX512/Complex.h" #include "src/Core/arch/SSE/MathFunctions.h" #include "src/Core/arch/AVX/MathFunctions.h" // #include "src/Core/arch/AVX512/MathFunctions.h" +#if defined EIGEN_VECTORIZE_AVX512FP16 +// #include "src/Core/arch/AVX512/MathFunctionsFP16.h" +#endif // #include "src/Core/arch/AVX512/TrsmKernel.h" #elif defined EIGEN_VECTORIZE_AVX // Use AVX for floats and doubles, SSE for integers @@ -234,6 +240,11 @@ using std::ptrdiff_t; #include "src/Core/arch/NEON/TypeCasting.h" #include "src/Core/arch/NEON/MathFunctions.h" #include "src/Core/arch/NEON/Complex.h" +#elif defined EIGEN_VECTORIZE_LSX +// #include "src/Core/arch/LSX/PacketMath.h" +// #include "src/Core/arch/LSX/TypeCasting.h" +// #include "src/Core/arch/LSX/MathFunctions.h" +// #include "src/Core/arch/LSX/Complex.h" #elif defined EIGEN_VECTORIZE_SVE // #include "src/Core/arch/SVE/PacketMath.h" // #include "src/Core/arch/SVE/TypeCasting.h" @@ -300,11 +311,7 @@ using std::ptrdiff_t; #include "src/Core/Product.h" #include "src/Core/CoreEvaluators.h" #include "src/Core/AssignEvaluator.h" - -#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874 - // at least confirmed with Doxygen 1.5.5 and 1.5.6 #include "src/Core/Assign.h" -#endif #include "src/Core/ArrayBase.h" #include "src/Core/util/BlasUtil.h" @@ -318,6 +325,7 @@ using std::ptrdiff_t; #include "src/Core/PlainObjectBase.h" #include "src/Core/Matrix.h" #include "src/Core/Array.h" +#include "src/Core/Fill.h" #include "src/Core/CwiseTernaryOp.h" #include "src/Core/CwiseBinaryOp.h" #include "src/Core/CwiseUnaryOp.h" @@ -380,6 +388,8 @@ using std::ptrdiff_t; // #include "src/Core/arch/AltiVec/MatrixProduct.h" #elif defined EIGEN_VECTORIZE_NEON #include "src/Core/arch/NEON/GeneralBlockPanelKernel.h" +#elif defined EIGEN_VECTORIZE_LSX +// #include "src/Core/arch/LSX/GeneralBlockPanelKernel.h" #endif #if defined(EIGEN_VECTORIZE_AVX512) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry index 019c98b6ef..efe3e1fa33 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Geometry @@ -22,13 +22,11 @@ * - fixed-size homogeneous transformations * - translation, scaling, 2D and 3D rotations * - \link Quaternion quaternions \endlink - * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) - * - orthogonal vector generation (\ref MatrixBase::unitOrthogonal) - * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes - * \endlink + * - cross products (\ref MatrixBase::cross(), \ref MatrixBase::cross3()) + * - orthogonal vector generation (MatrixBase::unitOrthogonal) + * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink * - \link AlignedBox axis aligned bounding boxes \endlink - * - \link umeyama least-square transformation fitting \endlink - * + * - \link umeyama() least-square transformation fitting \endlink * \code * #include * \endcode diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h index 5d52ab20f6..b1d801d34d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h @@ -230,8 +230,8 @@ class LDLT : public SolverBase > { */ const LDLT& adjoint() const { return *this; } - EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } - EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols(); } /** \brief Reports whether previous computation was successful. * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h index 01b44769a2..7fa4fa2a0f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h @@ -182,10 +182,10 @@ class LLT : public SolverBase > { * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as: * \code x = decomposition.adjoint().solve(b) \endcode */ - const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; } + const LLT& adjoint() const noexcept { return *this; } - inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } - inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + constexpr Index rows() const noexcept { return m_matrix.rows(); } + constexpr Index cols() const noexcept { return m_matrix.cols(); } template LLT& rankUpdate(const VectorType& vec, const RealScalar& sigma = 1); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h index 2098749c0f..57f3186b09 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h @@ -102,8 +102,13 @@ class Array : public PlainObjectBase::value) { + EIGEN_DEVICE_FUNC Array& operator=(Array&& other) noexcept(std::is_nothrow_move_assignable::value) { Base::operator=(std::move(other)); return *this; } - /** \copydoc PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const - * ArgTypes&... args) + /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients. + * + * \only_for_vectors + * + * This constructor is for 1D array or vectors with more than 4 coefficients. + * + * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this + * constructor must match the the fixed number of rows (resp. columns) of \c *this. + * * * Example: \include Array_variadic_ctor_cxx11.cpp * Output: \verbinclude Array_variadic_ctor_cxx11.out @@ -240,8 +253,8 @@ class Array : public PlainObjectBaseinnerSize(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return 1; } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return this->innerSize(); } #ifdef EIGEN_ARRAY_PLUGIN #include EIGEN_ARRAY_PLUGIN diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h index 6237df454b..8465f54fed 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h @@ -81,9 +81,6 @@ class ArrayBase : public DenseBase { typedef typename Base::CoeffReturnType CoeffReturnType; -#endif // not EIGEN_PARSED_BY_DOXYGEN - -#ifndef EIGEN_PARSED_BY_DOXYGEN typedef typename Base::PlainObject PlainObject; /** \internal Represents a matrix with all coefficients equal to one another*/ @@ -118,19 +115,57 @@ class ArrayBase : public DenseBase { return derived(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const Scalar& scalar); - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const Scalar& scalar); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const Scalar& other) { + internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other), + internal::add_assign_op()); + return derived(); + } - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const ArrayBase& other); - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const ArrayBase& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const Scalar& other) { + internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other), + internal::sub_assign_op()); + return derived(); + } + /** replaces \c *this by \c *this + \a other. + * + * \returns a reference to \c *this + */ template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const ArrayBase& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator+=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::add_assign_op()); + return derived(); + } + /** replaces \c *this by \c *this - \a other. + * + * \returns a reference to \c *this + */ template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const ArrayBase& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator-=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::sub_assign_op()); + return derived(); + } + + /** replaces \c *this by \c *this * \a other coefficient wise. + * + * \returns a reference to \c *this + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::mul_assign_op()); + return derived(); + } + + /** replaces \c *this by \c *this / \a other coefficient wise. + * + * \returns a reference to \c *this + */ + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator/=(const ArrayBase& other) { + call_assignment(derived(), other.derived(), internal::div_assign_op()); + return derived(); + } public: EIGEN_DEVICE_FUNC ArrayBase& array() { return *this; } @@ -173,50 +208,6 @@ class ArrayBase : public DenseBase { } }; -/** replaces \c *this by \c *this - \a other. - * - * \returns a reference to \c *this - */ -template -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const ArrayBase& other) { - call_assignment(derived(), other.derived(), internal::sub_assign_op()); - return derived(); -} - -/** replaces \c *this by \c *this + \a other. - * - * \returns a reference to \c *this - */ -template -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const ArrayBase& other) { - call_assignment(derived(), other.derived(), internal::add_assign_op()); - return derived(); -} - -/** replaces \c *this by \c *this * \a other coefficient wise. - * - * \returns a reference to \c *this - */ -template -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator*=(const ArrayBase& other) { - call_assignment(derived(), other.derived(), internal::mul_assign_op()); - return derived(); -} - -/** replaces \c *this by \c *this / \a other coefficient wise. - * - * \returns a reference to \c *this - */ -template -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator/=(const ArrayBase& other) { - call_assignment(derived(), other.derived(), internal::div_assign_op()); - return derived(); -} - } // end namespace Eigen #endif // EIGEN_ARRAYBASE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h index b636d88ad1..c9a194e991 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h @@ -56,14 +56,10 @@ class ArrayWrapper : public ArrayBase > { EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { - return m_expression.outerStride(); - } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { - return m_expression.innerStride(); - } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_expression.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_expression.cols(); } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return m_expression.innerStride(); } EIGEN_DEVICE_FUNC constexpr ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } EIGEN_DEVICE_FUNC constexpr const Scalar* data() const { return m_expression.data(); } @@ -135,14 +131,10 @@ class MatrixWrapper : public MatrixBase > { EIGEN_DEVICE_FUNC explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { - return m_expression.outerStride(); - } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { - return m_expression.innerStride(); - } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_expression.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_expression.cols(); } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return m_expression.outerStride(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return m_expression.innerStride(); } EIGEN_DEVICE_FUNC constexpr ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } EIGEN_DEVICE_FUNC constexpr const Scalar* data() const { return m_expression.data(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h index 9c2436afa7..36f0a9d74d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h @@ -27,125 +27,116 @@ namespace internal { // copy_using_evaluator_traits is based on assign_traits -template +template struct copy_using_evaluator_traits { - typedef typename DstEvaluator::XprType Dst; - typedef typename Dst::Scalar DstScalar; + using Src = typename SrcEvaluator::XprType; + using Dst = typename DstEvaluator::XprType; + using DstScalar = typename Dst::Scalar; - enum { DstFlags = DstEvaluator::Flags, SrcFlags = SrcEvaluator::Flags }; + static constexpr int DstFlags = DstEvaluator::Flags; + static constexpr int SrcFlags = SrcEvaluator::Flags; public: - enum { - DstAlignment = DstEvaluator::Alignment, - SrcAlignment = SrcEvaluator::Alignment, - DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit, - JointAlignment = plain_enum_min(DstAlignment, SrcAlignment) - }; - - private: - enum { - InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime) - : int(DstFlags) & RowMajorBit ? int(Dst::ColsAtCompileTime) - : int(Dst::RowsAtCompileTime), - InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime) - : int(DstFlags) & RowMajorBit ? int(Dst::MaxColsAtCompileTime) - : int(Dst::MaxRowsAtCompileTime), - RestrictedInnerSize = min_size_prefer_fixed(InnerSize, MaxPacketSize), - RestrictedLinearSize = min_size_prefer_fixed(Dst::SizeAtCompileTime, MaxPacketSize), - OuterStride = int(outer_stride_at_compile_time::ret), - MaxSizeAtCompileTime = Dst::SizeAtCompileTime - }; + static constexpr int DstAlignment = DstEvaluator::Alignment; + static constexpr int SrcAlignment = SrcEvaluator::Alignment; + static constexpr int JointAlignment = plain_enum_min(DstAlignment, SrcAlignment); + static constexpr bool DstHasDirectAccess = bool(DstFlags & DirectAccessBit); + static constexpr bool SrcIsRowMajor = bool(SrcFlags & RowMajorBit); + static constexpr bool DstIsRowMajor = bool(DstFlags & RowMajorBit); + static constexpr bool IsVectorAtCompileTime = Dst::IsVectorAtCompileTime; + static constexpr int RowsAtCompileTime = size_prefer_fixed(Src::RowsAtCompileTime, Dst::RowsAtCompileTime); + static constexpr int ColsAtCompileTime = size_prefer_fixed(Src::ColsAtCompileTime, Dst::ColsAtCompileTime); + static constexpr int SizeAtCompileTime = size_at_compile_time(RowsAtCompileTime, ColsAtCompileTime); + static constexpr int MaxRowsAtCompileTime = + min_size_prefer_fixed(Src::MaxRowsAtCompileTime, Dst::MaxRowsAtCompileTime); + static constexpr int MaxColsAtCompileTime = + min_size_prefer_fixed(Src::MaxColsAtCompileTime, Dst::MaxColsAtCompileTime); + static constexpr int MaxSizeAtCompileTime = + min_size_prefer_fixed(Src::MaxSizeAtCompileTime, Dst::MaxSizeAtCompileTime); + static constexpr int InnerSizeAtCompileTime = IsVectorAtCompileTime ? SizeAtCompileTime + : DstIsRowMajor ? ColsAtCompileTime + : RowsAtCompileTime; + static constexpr int MaxInnerSizeAtCompileTime = IsVectorAtCompileTime ? MaxSizeAtCompileTime + : DstIsRowMajor ? MaxColsAtCompileTime + : MaxRowsAtCompileTime; + static constexpr int RestrictedInnerSize = min_size_prefer_fixed(MaxInnerSizeAtCompileTime, MaxPacketSize); + static constexpr int RestrictedLinearSize = min_size_prefer_fixed(MaxSizeAtCompileTime, MaxPacketSize); + static constexpr int OuterStride = outer_stride_at_compile_time::ret; // TODO distinguish between linear traversal and inner-traversals - typedef typename find_best_packet::type LinearPacketType; - typedef typename find_best_packet::type InnerPacketType; + using LinearPacketType = typename find_best_packet::type; + using InnerPacketType = typename find_best_packet::type; - enum { - LinearPacketSize = unpacket_traits::size, - InnerPacketSize = unpacket_traits::size - }; + static constexpr int LinearPacketSize = unpacket_traits::size; + static constexpr int InnerPacketSize = unpacket_traits::size; public: - enum { - LinearRequiredAlignment = unpacket_traits::alignment, - InnerRequiredAlignment = unpacket_traits::alignment - }; + static constexpr int LinearRequiredAlignment = unpacket_traits::alignment; + static constexpr int InnerRequiredAlignment = unpacket_traits::alignment; private: - enum { - DstIsRowMajor = DstFlags & RowMajorBit, - SrcIsRowMajor = SrcFlags & RowMajorBit, - StorageOrdersAgree = (int(DstIsRowMajor) == int(SrcIsRowMajor)), - MightVectorize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit) && - bool(functor_traits::PacketAccess), - MayInnerVectorize = MightVectorize && int(InnerSize) != Dynamic && int(InnerSize) % int(InnerPacketSize) == 0 && - int(OuterStride) != Dynamic && int(OuterStride) % int(InnerPacketSize) == 0 && - (EIGEN_UNALIGNED_VECTORIZE || int(JointAlignment) >= int(InnerRequiredAlignment)), - MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit), - MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess) && - (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment)) || - MaxSizeAtCompileTime == Dynamic), - /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, - so it's only good for large enough sizes. */ - MaySliceVectorize = bool(MightVectorize) && bool(DstHasDirectAccess) && - (int(InnerMaxSize) == Dynamic || - int(InnerMaxSize) >= (EIGEN_UNALIGNED_VECTORIZE ? InnerPacketSize : (3 * InnerPacketSize))) - /* slice vectorization can be slow, so we only want it if the slices are big, which is - indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block - in a fixed-size matrix - However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */ - }; + static constexpr bool StorageOrdersAgree = DstIsRowMajor == SrcIsRowMajor; + static constexpr bool MightVectorize = StorageOrdersAgree && bool(DstFlags & SrcFlags & ActualPacketAccessBit) && + bool(functor_traits::PacketAccess); + static constexpr bool MayInnerVectorize = MightVectorize && (InnerSizeAtCompileTime != Dynamic) && + (InnerSizeAtCompileTime % InnerPacketSize == 0) && + (OuterStride != Dynamic) && (OuterStride % InnerPacketSize == 0) && + (EIGEN_UNALIGNED_VECTORIZE || JointAlignment >= InnerRequiredAlignment); + static constexpr bool MayLinearize = StorageOrdersAgree && (DstFlags & SrcFlags & LinearAccessBit); + static constexpr bool MayLinearVectorize = + MightVectorize && MayLinearize && DstHasDirectAccess && + (EIGEN_UNALIGNED_VECTORIZE || (DstAlignment >= LinearRequiredAlignment) || MaxSizeAtCompileTime == Dynamic) && + (MaxSizeAtCompileTime == Dynamic || MaxSizeAtCompileTime >= LinearPacketSize); + /* If the destination isn't aligned, we have to do runtime checks and we don't unroll, + so it's only good for large enough sizes. */ + static constexpr int InnerSizeThreshold = (EIGEN_UNALIGNED_VECTORIZE ? 1 : 3) * InnerPacketSize; + static constexpr bool MaySliceVectorize = + MightVectorize && DstHasDirectAccess && + (MaxInnerSizeAtCompileTime == Dynamic || MaxInnerSizeAtCompileTime >= InnerSizeThreshold); + /* slice vectorization can be slow, so we only want it if the slices are big, which is + indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block + in a fixed-size matrix + However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */ public: - enum { - Traversal = int(Dst::SizeAtCompileTime) == 0 - ? int(AllAtOnceTraversal) // If compile-size is zero, traversing will fail at compile-time. - : (int(MayLinearVectorize) && (LinearPacketSize > InnerPacketSize)) ? int(LinearVectorizedTraversal) - : int(MayInnerVectorize) ? int(InnerVectorizedTraversal) - : int(MayLinearVectorize) ? int(LinearVectorizedTraversal) - : int(MaySliceVectorize) ? int(SliceVectorizedTraversal) - : int(MayLinearize) ? int(LinearTraversal) - : int(DefaultTraversal), - Vectorized = int(Traversal) == InnerVectorizedTraversal || int(Traversal) == LinearVectorizedTraversal || - int(Traversal) == SliceVectorizedTraversal - }; + static constexpr int Traversal = SizeAtCompileTime == 0 ? AllAtOnceTraversal + : (MayLinearVectorize && (LinearPacketSize > InnerPacketSize)) + ? LinearVectorizedTraversal + : MayInnerVectorize ? InnerVectorizedTraversal + : MayLinearVectorize ? LinearVectorizedTraversal + : MaySliceVectorize ? SliceVectorizedTraversal + : MayLinearize ? LinearTraversal + : DefaultTraversal; + static constexpr bool Vectorized = Traversal == InnerVectorizedTraversal || Traversal == LinearVectorizedTraversal || + Traversal == SliceVectorizedTraversal; - typedef std::conditional_t PacketType; + using PacketType = std::conditional_t; private: - enum { - ActualPacketSize = int(Traversal) == LinearVectorizedTraversal ? LinearPacketSize - : Vectorized ? InnerPacketSize - : 1, - UnrollingLimit = EIGEN_UNROLLING_LIMIT * ActualPacketSize, - MayUnrollCompletely = - int(Dst::SizeAtCompileTime) != Dynamic && - int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <= - int(UnrollingLimit), - MayUnrollInner = - int(InnerSize) != Dynamic && - int(InnerSize) * (int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit) - }; + static constexpr int ActualPacketSize = Vectorized ? unpacket_traits::size : 1; + static constexpr int UnrollingLimit = EIGEN_UNROLLING_LIMIT * ActualPacketSize; + static constexpr int CoeffReadCost = int(DstEvaluator::CoeffReadCost) + int(SrcEvaluator::CoeffReadCost); + static constexpr bool MayUnrollCompletely = + (SizeAtCompileTime != Dynamic) && (SizeAtCompileTime * CoeffReadCost <= UnrollingLimit); + static constexpr bool MayUnrollInner = + (InnerSizeAtCompileTime != Dynamic) && (InnerSizeAtCompileTime * CoeffReadCost <= UnrollingLimit); public: - enum { - Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal)) - ? (int(MayUnrollCompletely) ? int(CompleteUnrolling) - : int(MayUnrollInner) ? int(InnerUnrolling) - : int(NoUnrolling)) - : int(Traversal) == int(LinearVectorizedTraversal) - ? (bool(MayUnrollCompletely) && - (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment) >= int(LinearRequiredAlignment))) - ? int(CompleteUnrolling) - : int(NoUnrolling)) - : int(Traversal) == int(LinearTraversal) - ? (bool(MayUnrollCompletely) ? int(CompleteUnrolling) : int(NoUnrolling)) + static constexpr int Unrolling = + (Traversal == InnerVectorizedTraversal || Traversal == DefaultTraversal) + ? (MayUnrollCompletely ? CompleteUnrolling + : MayUnrollInner ? InnerUnrolling + : NoUnrolling) + : Traversal == LinearVectorizedTraversal + ? (MayUnrollCompletely && (EIGEN_UNALIGNED_VECTORIZE || (DstAlignment >= LinearRequiredAlignment)) + ? CompleteUnrolling + : NoUnrolling) + : Traversal == LinearTraversal ? (MayUnrollCompletely ? CompleteUnrolling : NoUnrolling) #if EIGEN_UNALIGNED_VECTORIZE - : int(Traversal) == int(SliceVectorizedTraversal) - ? (bool(MayUnrollInner) ? int(InnerUnrolling) : int(NoUnrolling)) + : Traversal == SliceVectorizedTraversal ? (MayUnrollInner ? InnerUnrolling : NoUnrolling) #endif - : int(NoUnrolling) - }; + : NoUnrolling; + static constexpr bool UsePacketSegment = has_packet_segment::value; #ifdef EIGEN_DEBUG_ASSIGN static void debug() { @@ -162,8 +153,8 @@ struct copy_using_evaluator_traits { EIGEN_DEBUG_VAR(LinearRequiredAlignment) EIGEN_DEBUG_VAR(InnerRequiredAlignment) EIGEN_DEBUG_VAR(JointAlignment) - EIGEN_DEBUG_VAR(InnerSize) - EIGEN_DEBUG_VAR(InnerMaxSize) + EIGEN_DEBUG_VAR(InnerSizeAtCompileTime) + EIGEN_DEBUG_VAR(MaxInnerSizeAtCompileTime) EIGEN_DEBUG_VAR(LinearPacketSize) EIGEN_DEBUG_VAR(InnerPacketSize) EIGEN_DEBUG_VAR(ActualPacketSize) @@ -196,28 +187,25 @@ struct copy_using_evaluator_traits { *** Default traversal *** ************************/ -template +template struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling { - // FIXME: this is not very clean, perhaps this information should be provided by the kernel? - typedef typename Kernel::DstEvaluatorType DstEvaluatorType; - typedef typename DstEvaluatorType::XprType DstXprType; + static constexpr int Outer = Index_ / Kernel::AssignmentTraits::InnerSizeAtCompileTime; + static constexpr int Inner = Index_ % Kernel::AssignmentTraits::InnerSizeAtCompileTime; - enum { outer = Index / DstXprType::InnerSizeAtCompileTime, inner = Index % DstXprType::InnerSizeAtCompileTime }; - - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - kernel.assignCoeffByOuterInner(outer, inner); - copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + kernel.assignCoeffByOuterInner(Outer, Inner); + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); } }; template struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} }; template struct copy_using_evaluator_DefaultTraversal_InnerUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Index outer) { kernel.assignCoeffByOuterInner(outer, Index_); copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); } @@ -225,62 +213,57 @@ struct copy_using_evaluator_DefaultTraversal_InnerUnrolling { template struct copy_using_evaluator_DefaultTraversal_InnerUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&, Index) {} }; /*********************** *** Linear traversal *** ***********************/ -template +template struct copy_using_evaluator_LinearTraversal_CompleteUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - kernel.assignCoeff(Index); - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + kernel.assignCoeff(Index_); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); } }; template struct copy_using_evaluator_LinearTraversal_CompleteUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} }; /************************** *** Inner vectorization *** **************************/ -template +template struct copy_using_evaluator_innervec_CompleteUnrolling { - // FIXME: this is not very clean, perhaps this information should be provided by the kernel? - typedef typename Kernel::DstEvaluatorType DstEvaluatorType; - typedef typename DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::PacketType PacketType; + using PacketType = typename Kernel::PacketType; + static constexpr int Outer = Index_ / Kernel::AssignmentTraits::InnerSizeAtCompileTime; + static constexpr int Inner = Index_ % Kernel::AssignmentTraits::InnerSizeAtCompileTime; + static constexpr int NextIndex = Index_ + unpacket_traits::size; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::SrcAlignment; + static constexpr int DstAlignment = Kernel::AssignmentTraits::DstAlignment; - enum { - outer = Index / DstXprType::InnerSizeAtCompileTime, - inner = Index % DstXprType::InnerSizeAtCompileTime, - SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, - DstAlignment = Kernel::AssignmentTraits::DstAlignment - }; - - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { - kernel.template assignPacketByOuterInner(outer, inner); - enum { NextIndex = Index + unpacket_traits::size }; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.template assignPacketByOuterInner(Outer, Inner); copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); } }; template struct copy_using_evaluator_innervec_CompleteUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} }; template struct copy_using_evaluator_innervec_InnerUnrolling { - typedef typename Kernel::PacketType PacketType; + using PacketType = typename Kernel::PacketType; + static constexpr int NextIndex = Index_ + unpacket_traits::size; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) { kernel.template assignPacketByOuterInner(outer, Index_); - enum { NextIndex = Index_ + unpacket_traits::size }; copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); } @@ -288,7 +271,34 @@ struct copy_using_evaluator_innervec_InnerUnrolling { template struct copy_using_evaluator_innervec_InnerUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&, Index) {} +}; + +template +struct copy_using_evaluator_innervec_segment { + using PacketType = typename Kernel::PacketType; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel, Index outer) { + kernel.template assignPacketSegmentByOuterInner(outer, Start, 0, + Stop - Start); + } +}; + +template +struct copy_using_evaluator_innervec_segment + : copy_using_evaluator_DefaultTraversal_InnerUnrolling {}; + +template +struct copy_using_evaluator_innervec_segment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&, Index) {} +}; + +template +struct copy_using_evaluator_innervec_segment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&, Index) {} }; /*************************************************************************** @@ -299,7 +309,21 @@ struct copy_using_evaluator_innervec_InnerUnrolling -struct dense_assignment_loop; +struct dense_assignment_loop_impl; + +template +struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { +#ifdef __cpp_lib_is_constant_evaluated + if (internal::is_constant_evaluated()) + dense_assignment_loop_impl::run(kernel); + else +#endif + dense_assignment_loop_impl::run(kernel); + } +}; /************************ ***** Special Cases ***** @@ -307,10 +331,11 @@ struct dense_assignment_loop; // Zero-sized assignment is a no-op. template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE EIGEN_CONSTEXPR run(Kernel& /*kernel*/) { - EIGEN_STATIC_ASSERT(int(Kernel::DstEvaluatorType::XprType::SizeAtCompileTime) == 0, - EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT) +struct dense_assignment_loop_impl { + static constexpr int SizeAtCompileTime = Kernel::AssignmentTraits::SizeAtCompileTime; + + EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE constexpr run(Kernel& /*kernel*/) { + EIGEN_STATIC_ASSERT(SizeAtCompileTime == 0, EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT) } }; @@ -319,8 +344,8 @@ struct dense_assignment_loop { ************************/ template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel& kernel) { +struct dense_assignment_loop_impl { + EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE constexpr run(Kernel& kernel) { for (Index outer = 0; outer < kernel.outerSize(); ++outer) { for (Index inner = 0; inner < kernel.innerSize(); ++inner) { kernel.assignCoeffByOuterInner(outer, inner); @@ -330,22 +355,22 @@ struct dense_assignment_loop { }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); +struct dense_assignment_loop_impl { + static constexpr int SizeAtCompileTime = Kernel::AssignmentTraits::SizeAtCompileTime; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + copy_using_evaluator_DefaultTraversal_CompleteUnrolling::run(kernel); } }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +struct dense_assignment_loop_impl { + static constexpr int InnerSizeAtCompileTime = Kernel::AssignmentTraits::InnerSizeAtCompileTime; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { const Index outerSize = kernel.outerSize(); for (Index outer = 0; outer < outerSize; ++outer) - copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, - outer); + copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); } }; @@ -356,100 +381,134 @@ struct dense_assignment_loop { // The goal of unaligned_dense_assignment_loop is simply to factorize the handling // of the non vectorizable beginning and ending parts -template +template struct unaligned_dense_assignment_loop { - // if IsAligned = true, then do nothing + // if Skip == true, then do nothing template - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&, Index, Index) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& /*kernel*/, Index /*start*/, Index /*end*/) {} + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& /*kernel*/, Index /*outer*/, + Index /*innerStart*/, Index /*innerEnd*/) {} }; -template <> -struct unaligned_dense_assignment_loop { - // MSVC must not inline this functions. If it does, it fails to optimize the - // packet access path. - // FIXME check which version exhibits this issue -#if EIGEN_COMP_MSVC +template +struct unaligned_dense_assignment_loop { template - static EIGEN_DONT_INLINE void run(Kernel& kernel, Index start, Index end) -#else + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Index start, Index end) { + Index count = end - start; + eigen_assert(count <= unpacket_traits::size); + if (count > 0) kernel.template assignPacketSegment(start, 0, count); + } template - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel, Index start, Index end) -#endif - { - for (Index index = start; index < end; ++index) kernel.assignCoeff(index); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Index outer, Index start, Index end) { + Index count = end - start; + eigen_assert(count <= unpacket_traits::size); + if (count > 0) + kernel.template assignPacketSegmentByOuterInner(outer, start, 0, count); } }; -template -struct copy_using_evaluator_linearvec_CompleteUnrolling { - // FIXME: this is not very clean, perhaps this information should be provided by the kernel? - typedef typename Kernel::DstEvaluatorType DstEvaluatorType; - typedef typename DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::PacketType PacketType; +template +struct unaligned_dense_assignment_loop { + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Index start, Index end) { + for (Index index = start; index < end; ++index) kernel.assignCoeff(index); + } + template + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Index outer, Index innerStart, + Index innerEnd) { + for (Index inner = innerStart; inner < innerEnd; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + } +}; - enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment }; +template +struct copy_using_evaluator_linearvec_CompleteUnrolling { + using PacketType = typename Kernel::PacketType; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::SrcAlignment; + static constexpr int DstAlignment = Kernel::AssignmentTraits::DstAlignment; + static constexpr int NextIndex = Index_ + unpacket_traits::size; EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - kernel.template assignPacket(Index); - enum { NextIndex = Index + unpacket_traits::size }; + kernel.template assignPacket(Index_); copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); } }; template struct copy_using_evaluator_linearvec_CompleteUnrolling { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel&) {} + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} +}; + +template +struct copy_using_evaluator_linearvec_segment { + using PacketType = typename Kernel::PacketType; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::SrcAlignment; + static constexpr int DstAlignment = Kernel::AssignmentTraits::DstAlignment; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + kernel.template assignPacketSegment(Index_, 0, Stop - Index_); + } +}; + +template +struct copy_using_evaluator_linearvec_segment + : copy_using_evaluator_LinearTraversal_CompleteUnrolling {}; + +template +struct copy_using_evaluator_linearvec_segment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} +}; + +template +struct copy_using_evaluator_linearvec_segment { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel&) {} }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +struct dense_assignment_loop_impl { + using Scalar = typename Kernel::Scalar; + using PacketType = typename Kernel::PacketType; + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::JointAlignment; + static constexpr int DstAlignment = plain_enum_max(Kernel::AssignmentTraits::DstAlignment, alignof(Scalar)); + static constexpr int RequestedAlignment = unpacket_traits::alignment; + static constexpr bool Alignable = + (DstAlignment >= RequestedAlignment) || ((RequestedAlignment - DstAlignment) % sizeof(Scalar) == 0); + static constexpr int Alignment = Alignable ? RequestedAlignment : DstAlignment; + static constexpr bool DstIsAligned = DstAlignment >= Alignment; + static constexpr bool UsePacketSegment = Kernel::AssignmentTraits::UsePacketSegment; + + using head_loop = + unaligned_dense_assignment_loop; + using tail_loop = unaligned_dense_assignment_loop; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { const Index size = kernel.size(); - typedef typename Kernel::Scalar Scalar; - typedef typename Kernel::PacketType PacketType; - enum { - requestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment, - packetSize = unpacket_traits::size, - dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), - dstAlignment = packet_traits::AlignedOnScalar ? int(requestedAlignment) - : int(Kernel::AssignmentTraits::DstAlignment), - srcAlignment = Kernel::AssignmentTraits::JointAlignment - }; - const Index alignedStart = - dstIsAligned ? 0 : internal::first_aligned(kernel.dstDataPtr(), size); - const Index alignedEnd = alignedStart + ((size - alignedStart) / packetSize) * packetSize; + const Index alignedStart = DstIsAligned ? 0 : first_aligned(kernel.dstDataPtr(), size); + const Index alignedEnd = alignedStart + numext::round_down(size - alignedStart, PacketSize); - unaligned_dense_assignment_loop::run(kernel, 0, alignedStart); + head_loop::run(kernel, 0, alignedStart); - for (Index index = alignedStart; index < alignedEnd; index += packetSize) - kernel.template assignPacket(index); + for (Index index = alignedStart; index < alignedEnd; index += PacketSize) + kernel.template assignPacket(index); - unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size); + tail_loop::run(kernel, alignedEnd, size); } }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - if (internal::is_constant_evaluated()) { - for (Index outer = 0; outer < kernel.outerSize(); ++outer) { - for (Index inner = 0; inner < kernel.innerSize(); ++inner) { - kernel.assignCoeffByOuterInner(outer, inner); - } - } - } else { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::PacketType PacketType; +struct dense_assignment_loop_impl { + using PacketType = typename Kernel::PacketType; + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int Size = Kernel::AssignmentTraits::SizeAtCompileTime; + static constexpr int AlignedSize = numext::round_down(Size, PacketSize); + static constexpr bool UsePacketSegment = Kernel::AssignmentTraits::UsePacketSegment; - enum { - size = DstXprType::SizeAtCompileTime, - packetSize = unpacket_traits::size, - alignedSize = (int(size) / packetSize) * packetSize - }; - - copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); - } + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); + copy_using_evaluator_linearvec_segment::run(kernel); } }; @@ -458,44 +517,41 @@ struct dense_assignment_loop -struct dense_assignment_loop { - typedef typename Kernel::PacketType PacketType; - enum { SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, DstAlignment = Kernel::AssignmentTraits::DstAlignment }; - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +struct dense_assignment_loop_impl { + using PacketType = typename Kernel::PacketType; + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::JointAlignment; + static constexpr int DstAlignment = Kernel::AssignmentTraits::DstAlignment; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { const Index innerSize = kernel.innerSize(); const Index outerSize = kernel.outerSize(); - const Index packetSize = unpacket_traits::size; for (Index outer = 0; outer < outerSize; ++outer) - for (Index inner = 0; inner < innerSize; inner += packetSize) + for (Index inner = 0; inner < innerSize; inner += PacketSize) kernel.template assignPacketByOuterInner(outer, inner); } }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { - if (internal::is_constant_evaluated()) { - for (Index outer = 0; outer < kernel.outerSize(); ++outer) { - for (Index inner = 0; inner < kernel.innerSize(); ++inner) { - kernel.assignCoeffByOuterInner(outer, inner); - } - } - } else { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); - } +struct dense_assignment_loop_impl { + static constexpr int SizeAtCompileTime = Kernel::AssignmentTraits::SizeAtCompileTime; + + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); } }; template -struct dense_assignment_loop { +struct dense_assignment_loop_impl { + static constexpr int InnerSize = Kernel::AssignmentTraits::InnerSizeAtCompileTime; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::SrcAlignment; + static constexpr int DstAlignment = Kernel::AssignmentTraits::DstAlignment; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::AssignmentTraits Traits; const Index outerSize = kernel.outerSize(); for (Index outer = 0; outer < outerSize; ++outer) - copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); + copy_using_evaluator_innervec_InnerUnrolling::run(kernel, + outer); } }; @@ -504,26 +560,18 @@ struct dense_assignment_loop { ***********************/ template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +struct dense_assignment_loop_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { const Index size = kernel.size(); for (Index i = 0; i < size; ++i) kernel.assignCoeff(i); } }; template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - if (internal::is_constant_evaluated()) { - for (Index outer = 0; outer < kernel.outerSize(); ++outer) { - for (Index inner = 0; inner < kernel.innerSize(); ++inner) { - kernel.assignCoeffByOuterInner(outer, inner); - } - } - } else { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); - } +struct dense_assignment_loop_impl { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run( + kernel); } }; @@ -532,72 +580,62 @@ struct dense_assignment_loop { ***************************/ template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - if (internal::is_constant_evaluated()) { - for (Index outer = 0; outer < kernel.outerSize(); ++outer) { - for (Index inner = 0; inner < kernel.innerSize(); ++inner) { - kernel.assignCoeffByOuterInner(outer, inner); - } - } - } else { - typedef typename Kernel::Scalar Scalar; - typedef typename Kernel::PacketType PacketType; - enum { - packetSize = unpacket_traits::size, - requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), - alignable = - packet_traits::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar), - dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment), - dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment) - }; - const Scalar* dst_ptr = kernel.dstDataPtr(); - if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) { - // the pointer is not aligned-on scalar, so alignment is not possible - return dense_assignment_loop::run(kernel); - } - const Index packetAlignedMask = packetSize - 1; - const Index innerSize = kernel.innerSize(); - const Index outerSize = kernel.outerSize(); - const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0; - Index alignedStart = - ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize); +struct dense_assignment_loop_impl { + using Scalar = typename Kernel::Scalar; + using PacketType = typename Kernel::PacketType; + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int SrcAlignment = Kernel::AssignmentTraits::JointAlignment; + static constexpr int DstAlignment = plain_enum_max(Kernel::AssignmentTraits::DstAlignment, alignof(Scalar)); + static constexpr int RequestedAlignment = unpacket_traits::alignment; + static constexpr bool Alignable = + (DstAlignment >= RequestedAlignment) || ((RequestedAlignment - DstAlignment) % sizeof(Scalar) == 0); + static constexpr int Alignment = Alignable ? RequestedAlignment : DstAlignment; + static constexpr bool DstIsAligned = DstAlignment >= Alignment; + static constexpr bool UsePacketSegment = Kernel::AssignmentTraits::UsePacketSegment; - for (Index outer = 0; outer < outerSize; ++outer) { - const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask); - // do the non-vectorizable part of the assignment - for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + using head_loop = unaligned_dense_assignment_loop; + using tail_loop = unaligned_dense_assignment_loop; - // do the vectorizable part of the assignment - for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) - kernel.template assignPacketByOuterInner(outer, inner); + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { + const Scalar* dst_ptr = kernel.dstDataPtr(); + const Index innerSize = kernel.innerSize(); + const Index outerSize = kernel.outerSize(); + const Index alignedStep = Alignable ? (PacketSize - kernel.outerStride() % PacketSize) % PacketSize : 0; + Index alignedStart = ((!Alignable) || DstIsAligned) ? 0 : internal::first_aligned(dst_ptr, innerSize); - // do the non-vectorizable part of the assignment - for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner); + for (Index outer = 0; outer < outerSize; ++outer) { + const Index alignedEnd = alignedStart + numext::round_down(innerSize - alignedStart, PacketSize); - alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize); - } + head_loop::run(kernel, outer, 0, alignedStart); + + // do the vectorizable part of the assignment + for (Index inner = alignedStart; inner < alignedEnd; inner += PacketSize) + kernel.template assignPacketByOuterInner(outer, inner); + + tail_loop::run(kernel, outer, alignedEnd, innerSize); + + alignedStart = numext::mini((alignedStart + alignedStep) % PacketSize, innerSize); } } }; #if EIGEN_UNALIGNED_VECTORIZE template -struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - typedef typename Kernel::PacketType PacketType; +struct dense_assignment_loop_impl { + using PacketType = typename Kernel::PacketType; + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int InnerSize = Kernel::AssignmentTraits::InnerSizeAtCompileTime; + static constexpr int VectorizableSize = numext::round_down(InnerSize, PacketSize); + static constexpr bool UsePacketSegment = Kernel::AssignmentTraits::UsePacketSegment; - enum { - innerSize = DstXprType::InnerSizeAtCompileTime, - packetSize = unpacket_traits::size, - vectorizableSize = (int(innerSize) / int(packetSize)) * int(packetSize), - size = DstXprType::SizeAtCompileTime - }; + using packet_loop = copy_using_evaluator_innervec_InnerUnrolling; + using packet_segment_loop = copy_using_evaluator_innervec_segment; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) { for (Index outer = 0; outer < kernel.outerSize(); ++outer) { - copy_using_evaluator_innervec_InnerUnrolling::run(kernel, outer); - copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(kernel, outer); + packet_loop::run(kernel, outer); + packet_segment_loop::run(kernel, outer); } } }; @@ -626,24 +664,25 @@ class generic_dense_assignment_kernel { typedef copy_using_evaluator_traits AssignmentTraits; typedef typename AssignmentTraits::PacketType PacketType; - EIGEN_DEVICE_FUNC - EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src, - const Functor& func, DstXprType& dstExpr) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, + const SrcEvaluatorType& src, + const Functor& func, + DstXprType& dstExpr) : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) { #ifdef EIGEN_DEBUG_ASSIGN AssignmentTraits::debug(); #endif } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_dstExpr.size(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const EIGEN_NOEXCEPT { return m_dstExpr.innerSize(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerSize() const EIGEN_NOEXCEPT { return m_dstExpr.outerSize(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dstExpr.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_dstExpr.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return m_dstExpr.outerStride(); } + EIGEN_DEVICE_FUNC constexpr Index size() const noexcept { return m_dstExpr.size(); } + EIGEN_DEVICE_FUNC constexpr Index innerSize() const noexcept { return m_dstExpr.innerSize(); } + EIGEN_DEVICE_FUNC constexpr Index outerSize() const noexcept { return m_dstExpr.outerSize(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_dstExpr.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_dstExpr.cols(); } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return m_dstExpr.outerStride(); } - EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() EIGEN_NOEXCEPT { return m_dst; } - EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; } + EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() noexcept { return m_dst; } + EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const noexcept { return m_src; } /// Assign src(row,col) to dst(row,col) through the assignment functor. EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) { @@ -680,6 +719,27 @@ class generic_dense_assignment_kernel { assignPacket(row, col); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketSegment(Index row, Index col, Index begin, Index count) { + m_functor.template assignPacketSegment( + &m_dst.coeffRef(row, col), m_src.template packetSegment(row, col, begin, count), begin, + count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketSegment(Index index, Index begin, Index count) { + m_functor.template assignPacketSegment( + &m_dst.coeffRef(index), m_src.template packetSegment(index, begin, count), begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketSegmentByOuterInner(Index outer, Index inner, Index begin, + Index count) { + Index row = rowIndexByOuterInner(outer, inner); + Index col = colIndexByOuterInner(outer, inner); + assignPacketSegment(row, col, begin, count); + } + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) { typedef typename DstEvaluatorType::ExpressionTraits Traits; return int(Traits::RowsAtCompileTime) == 1 ? 0 @@ -732,8 +792,8 @@ class restricted_packet_dense_assignment_kernel ***************************************************************************/ template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, - const Functor& /*func*/) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, + const Functor& /*func*/) { EIGEN_ONLY_USED_FOR_DEBUG(dst); EIGEN_ONLY_USED_FOR_DEBUG(src); eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); @@ -749,9 +809,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprTyp } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_dense_assignment_loop(DstXprType& dst, - const SrcXprType& src, - const Functor& func) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src, + const Functor& func) { typedef evaluator DstEvaluatorType; typedef evaluator SrcEvaluatorType; @@ -769,18 +828,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_dense_assignment dense_assignment_loop::run(kernel); } -// Specialization for filling the destination with a constant value. -#ifndef EIGEN_GPU_COMPILE_PHASE -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop( - DstXprType& dst, - const Eigen::CwiseNullaryOp, DstXprType>& src, - const internal::assign_op& func) { - resize_if_allowed(dst, src, func); - std::fill_n(dst.data(), dst.size(), src.functor()()); -} -#endif - template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src) { call_dense_assignment_loop(dst, src, internal::assign_op()); @@ -832,7 +879,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(const Dst& dst, const // Deal with "assume-aliasing" template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment( Dst& dst, const Src& src, const Func& func, std::enable_if_t::value, void*> = 0) { typename plain_matrix_type::type tmp(src); call_assignment_no_alias(dst, tmp, func); @@ -847,14 +894,14 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment( // by-pass "assume-aliasing" // When there is no aliasing, we require that 'dst' has been properly resized template class StorageBase, typename Src, typename Func> -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(NoAlias& dst, - const Src& src, const Func& func) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(NoAlias& dst, const Src& src, + const Func& func) { call_assignment_no_alias(dst.expression(), src, func); } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src, - const Func& func) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment_no_alias(Dst& dst, const Src& src, + const Func& func) { enum { NeedToTranspose = ((int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1) || (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)) && @@ -893,14 +940,13 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_restricted_packet_assignment_no_ } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(Dst& dst, const Src& src) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment_no_alias(Dst& dst, const Src& src) { call_assignment_no_alias(dst, src, internal::assign_op()); } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst, - const Src& src, - const Func& func) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src, + const Func& func) { // TODO check whether this is the right place to perform these checks: EIGEN_STATIC_ASSERT_LVALUE(Dst) EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst, Src) @@ -909,8 +955,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_al Assignment::run(dst, src, func); } template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias_no_transpose(Dst& dst, - const Src& src) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src) { call_assignment_no_alias_no_transpose(dst, src, internal::assign_op()); } @@ -935,6 +980,32 @@ struct Assignment { } }; +template +struct Assignment, SrcPlainObject>, + assign_op, Dense2Dense, Weak> { + using Scalar = typename DstXprType::Scalar; + using NullaryOp = scalar_constant_op; + using SrcXprType = CwiseNullaryOp; + using Functor = assign_op; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const Functor& /*func*/) { + eigen_fill_impl::run(dst, src); + } +}; + +template +struct Assignment, SrcPlainObject>, + assign_op, Dense2Dense, Weak> { + using Scalar = typename DstXprType::Scalar; + using NullaryOp = scalar_zero_op; + using SrcXprType = CwiseNullaryOp; + using Functor = assign_op; + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, + const Functor& /*func*/) { + eigen_zero_impl::run(dst, src); + } +}; + // Generic assignment through evalTo. // TODO: not sure we have to keep that one, but it helps porting current code to new evaluator mechanism. // Note that the last template argument "Weak" is needed to make it possible to perform diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h index ca991cad26..57b0322955 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h @@ -200,16 +200,16 @@ class BandMatrix : public BandMatrixBase * Adding an offset to nullptr is undefined behavior, so we must avoid it. */ template - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE static Scalar* add_to_nullable_pointer(Scalar* base, - Index offset) { + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE static Scalar* add_to_nullable_pointer(Scalar* base, Index offset) { return base != nullptr ? base + offset : nullptr; } @@ -378,30 +373,25 @@ class BlockImpl_dense init(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t& nestedExpression() const - EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::remove_all_t& nestedExpression() const noexcept { return m_xpr; } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE XprType& nestedExpression() { return m_xpr; } /** \sa MapBase::innerStride() */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index innerStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index innerStride() const noexcept { return internal::traits::HasSameStorageOrderAsXprType ? m_xpr.innerStride() : m_xpr.outerStride(); } /** \sa MapBase::outerStride() */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index outerStride() const noexcept { return internal::traits::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startRow() const EIGEN_NOEXCEPT { - return m_startRow.value(); - } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr StorageIndex startRow() const noexcept { return m_startRow.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR StorageIndex startCol() const EIGEN_NOEXCEPT { - return m_startCol.value(); - } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr StorageIndex startCol() const noexcept { return m_startCol.value(); } #ifndef __SUNPRO_CC // FIXME sunstudio is not friendly with the above friend... diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h index c6291234b1..c414117901 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h @@ -92,7 +92,7 @@ struct CommaInitializer { EIGEN_DEVICE_FUNC inline ~CommaInitializer() #if defined VERIFY_RAISES_ASSERT && (!defined EIGEN_NO_ASSERTION_CHECKING) && defined EIGEN_EXCEPTIONS - EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception) + noexcept(false) // Eigen::eigen_assert_exception #endif { finished(); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h index 156ca2b607..63f1895d2a 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h @@ -149,7 +149,7 @@ class plainobjectbase_evaluator_data { #endif eigen_internal_assert(outerStride == OuterStride); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return OuterStride; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index outerStride() const noexcept { return OuterStride; } const Scalar* data; }; @@ -198,19 +198,13 @@ struct evaluator> : evaluator_base { } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType coeff(Index row, Index col) const { - if (IsRowMajor) - return m_d.data[row * m_d.outerStride() + col]; - else - return m_d.data[row + col * m_d.outerStride()]; + return coeff(getIndex(row, col)); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType coeff(Index index) const { return m_d.data[index]; } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index row, Index col) { - if (IsRowMajor) - return const_cast(m_d.data)[row * m_d.outerStride() + col]; - else - return const_cast(m_d.data)[row + col * m_d.outerStride()]; + return coeffRef(getIndex(row, col)); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& coeffRef(Index index) { @@ -219,10 +213,7 @@ struct evaluator> : evaluator_base { template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { - if (IsRowMajor) - return ploadt(m_d.data + row * m_d.outerStride() + col); - else - return ploadt(m_d.data + row + col * m_d.outerStride()); + return packet(getIndex(row, col)); } template @@ -232,19 +223,43 @@ struct evaluator> : evaluator_base { template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { - if (IsRowMajor) - return pstoret(const_cast(m_d.data) + row * m_d.outerStride() + col, x); - else - return pstoret(const_cast(m_d.data) + row + col * m_d.outerStride(), x); + writePacket(getIndex(row, col), x); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { - return pstoret(const_cast(m_d.data) + index, x); + pstoret(const_cast(m_d.data) + index, x); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return packetSegment(getIndex(row, col), begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return ploadtSegment(m_d.data + index, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + writePacketSegment(getIndex(row, col), x, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + pstoretSegment(const_cast(m_d.data) + index, x, begin, count); } protected: plainobjectbase_evaluator_data m_d; + + private: + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index constexpr getIndex(Index row, Index col) const { + return IsRowMajor ? row * m_d.outerStride() + col : row + col * m_d.outerStride(); + } }; template @@ -318,6 +333,28 @@ struct unary_evaluator, IndexBased> : evaluator_base(index, x); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_argImpl.template packetSegment(col, row, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return m_argImpl.template packetSegment(index, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + m_argImpl.template writePacketSegment(col, row, x, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + m_argImpl.template writePacketSegment(index, x, begin, count); + } + protected: evaluator m_argImpl; }; @@ -464,10 +501,10 @@ template struct evaluator> : evaluator_base> { typedef CwiseNullaryOp XprType; - typedef internal::remove_all_t PlainObjectTypeCleaned; + typedef remove_all_t PlainObjectTypeCleaned; enum { - CoeffReadCost = internal::functor_traits::Cost, + CoeffReadCost = functor_traits::Cost, Flags = (evaluator::Flags & (HereditaryBits | (functor_has_linear_access::ret ? LinearAccessBit : 0) | @@ -502,9 +539,21 @@ struct evaluator> return m_wrapper.template packetOp(m_functor, index); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(IndexType row, IndexType col, Index /*begin*/, + Index /*count*/) const { + return packet(row, col); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(IndexType index, Index /*begin*/, + Index /*count*/) const { + return packet(index); + } + protected: const NullaryOp m_functor; - const internal::nullary_wrapper m_wrapper; + const nullary_wrapper m_wrapper; }; // -------------------- CwiseUnaryOp -------------------- @@ -546,6 +595,16 @@ struct unary_evaluator, IndexBased> : evaluator_b return m_d.func().packetOp(m_d.argImpl.template packet(index)); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_d.func().packetOp(m_d.argImpl.template packetSegment(row, col, begin, count)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return m_d.func().packetOp(m_d.argImpl.template packetSegment(index, begin, count)); + } + protected: // this helper permits to completely eliminate the functor if it is empty struct Data { @@ -600,16 +659,11 @@ struct unary_evaluator, ArgType>, In template using SrcPacketArgs8 = std::enable_if_t<(unpacket_traits::size) == (8 * SrcPacketSize), bool>; - template = true> - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index, Index col, Index packetSize) const { - return col + packetSize <= cols(); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index row, Index col, Index begin, Index count) const { + return IsRowMajor ? (col + count + begin <= cols()) : (row + count + begin <= rows()); } - template = true> - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index row, Index, Index packetSize) const { - return row + packetSize <= rows(); - } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index index, Index packetSize) const { - return index + packetSize <= size(); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool check_array_bounds(Index index, Index begin, Index count) const { + return index + count + begin <= size(); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE SrcType srcCoeff(Index row, Index col, Index offset) const { @@ -632,43 +686,88 @@ struct unary_evaluator, ArgType>, In template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType srcPacket(Index row, Index col, Index offset) const { constexpr int PacketSize = unpacket_traits::size; - Index actualRow = IsRowMajor ? row : row + (offset * PacketSize); - Index actualCol = IsRowMajor ? col + (offset * PacketSize) : col; - eigen_assert(check_array_bounds(actualRow, actualCol, PacketSize) && "Array index out of bounds"); + Index packetOffset = offset * PacketSize; + Index actualRow = IsRowMajor ? row : row + packetOffset; + Index actualCol = IsRowMajor ? col + packetOffset : col; + eigen_assert(check_array_bounds(actualRow, actualCol, 0, PacketSize) && "Array index out of bounds"); return m_argImpl.template packet(actualRow, actualCol); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType srcPacket(Index index, Index offset) const { constexpr int PacketSize = unpacket_traits::size; - Index actualIndex = index + (offset * PacketSize); - eigen_assert(check_array_bounds(actualIndex, PacketSize) && "Array index out of bounds"); + Index packetOffset = offset * PacketSize; + Index actualIndex = index + packetOffset; + eigen_assert(check_array_bounds(actualIndex, 0, PacketSize) && "Array index out of bounds"); return m_argImpl.template packet(actualIndex); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType srcPacketSegment(Index row, Index col, Index begin, Index count, + Index offset) const { + constexpr int PacketSize = unpacket_traits::size; + Index packetOffset = offset * PacketSize; + Index actualRow = IsRowMajor ? row : row + packetOffset; + Index actualCol = IsRowMajor ? col + packetOffset : col; + eigen_assert(check_array_bounds(actualRow, actualCol, 0, count) && "Array index out of bounds"); + return m_argImpl.template packetSegment(actualRow, actualCol, begin, count); + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType srcPacketSegment(Index index, Index begin, Index count, + Index offset) const { + constexpr int PacketSize = unpacket_traits::size; + Index packetOffset = offset * PacketSize; + Index actualIndex = index + packetOffset + begin; + eigen_assert(check_array_bounds(actualIndex, 0, count) && "Array index out of bounds"); + return m_argImpl.template packetSegment(actualIndex, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketBlock srcPacketSegmentHelper(Index row, Index col, + Index begin, + Index count) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets; + for (Index i = 0; i < NumPackets; i++) packets.packet[i] = pzero(PacketType()); + Index offset = begin / SrcPacketSize; + Index actualBegin = begin % SrcPacketSize; + for (; offset < NumPackets; offset++) { + Index actualCount = numext::mini(SrcPacketSize - actualBegin, count); + packets.packet[offset] = srcPacketSegment(row, col, actualBegin, actualCount, offset); + if (count == actualCount) break; + actualBegin = 0; + count -= actualCount; + } + return packets; + } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketBlock srcPacketSegmentHelper(Index index, + Index begin, + Index count) const { + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets; + for (Index i = 0; i < NumPackets; i++) packets.packet[i] = pzero(PacketType()); + Index offset = begin / SrcPacketSize; + Index actualBegin = begin % SrcPacketSize; + for (; offset < NumPackets; offset++) { + Index actualCount = numext::mini(SrcPacketSize - actualBegin, count); + packets.packet[offset] = srcPacketSegment(index, actualBegin, actualCount, offset); + if (count == actualCount) break; + actualBegin = 0; + count -= actualCount; + } + return packets; + } // There is no source packet type with equal or fewer elements than DstPacketType. // This is problematic as the evaluation loop may attempt to access data outside the bounds of the array. // For example, consider the cast utilizing pcast with an array of size 4: {0.0f,1.0f,2.0f,3.0f}. // The first iteration of the evaluation loop will load 16 bytes: {0.0f,1.0f,2.0f,3.0f} and cast to {0.0,1.0}, which // is acceptable. The second iteration will load 16 bytes: {2.0f,3.0f,?,?}, which is outside the bounds of the array. - - // Instead, perform runtime check to determine if the load would access data outside the bounds of the array. - // If not, perform full load. Otherwise, revert to a scalar loop to perform a partial load. - // In either case, perform a vectorized cast of the source packet. template = true> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packet(Index row, Index col) const { constexpr int DstPacketSize = unpacket_traits::size; constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); - SrcPacketType src; - if (EIGEN_PREDICT_TRUE(check_array_bounds(row, col, SrcPacketSize))) { - src = srcPacket(row, col, 0); - } else { - Array srcArray; - for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(row, col, k); - for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0); - src = pload(srcArray.data()); - } - return pcast(src); + return pcast(srcPacketSegment(row, col, 0, DstPacketSize, 0)); } // Use the source packet type with the same size as DstPacketType, if it exists template = true> @@ -704,22 +803,67 @@ struct unary_evaluator, ArgType>, In srcPacket(row, col, 6), srcPacket(row, col, 7)); } + // packetSegment variants + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + constexpr int DstPacketSize = unpacket_traits::size; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast(srcPacketSegment(row, col, begin, count, 0)); + } + // Use the source packet type with the same size as DstPacketType, if it exists + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + constexpr int DstPacketSize = unpacket_traits::size; + using SizedSrcPacketType = typename find_packet_by_size::type; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast( + srcPacketSegment(row, col, begin, count, 0)); + } + // unpacket_traits::size == 2 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + constexpr int NumPackets = 2; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(row, col, begin, count); + return pcast(packets.packet[0], packets.packet[1]); + } + // unpacket_traits::size == 4 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + constexpr int NumPackets = 4; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(row, col, begin, count); + return pcast(packets.packet[0], packets.packet[1], packets.packet[2], + packets.packet[3]); + } + // unpacket_traits::size == 8 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + constexpr int NumPackets = 8; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(row, col, begin, count); + return pcast(packets.packet[0], packets.packet[1], packets.packet[2], + packets.packet[3], packets.packet[4], packets.packet[5], + packets.packet[6], packets.packet[7]); + } + // Analogous routines for linear access. template = true> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { constexpr int DstPacketSize = unpacket_traits::size; constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); - SrcPacketType src; - if (EIGEN_PREDICT_TRUE(check_array_bounds(index, SrcPacketSize))) { - src = srcPacket(index, 0); - } else { - Array srcArray; - for (size_t k = 0; k < DstPacketSize; k++) srcArray[k] = srcCoeff(index, k); - for (size_t k = DstPacketSize; k < SrcPacketSize; k++) srcArray[k] = SrcType(0); - src = pload(srcArray.data()); - } - return pcast(src); + return pcast(srcPacketSegment(index, 0, DstPacketSize, 0)); } template = true> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packet(Index index) const { @@ -749,6 +893,55 @@ struct unary_evaluator, ArgType>, In srcPacket(index, 6), srcPacket(index, 7)); } + // packetSegment variants + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index index, Index begin, Index count) const { + constexpr int DstPacketSize = unpacket_traits::size; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast(srcPacketSegment(index, begin, count, 0)); + } + // Use the source packet type with the same size as DstPacketType, if it exists + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index index, Index begin, Index count) const { + constexpr int DstPacketSize = unpacket_traits::size; + using SizedSrcPacketType = typename find_packet_by_size::type; + constexpr int SrcBytesIncrement = DstPacketSize * sizeof(SrcType); + constexpr int SrcLoadMode = plain_enum_min(SrcBytesIncrement, LoadMode); + return pcast( + srcPacketSegment(index, begin, count, 0)); + } + // unpacket_traits::size == 2 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index index, Index begin, Index count) const { + constexpr int NumPackets = 2; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(index, begin, count); + return pcast(packets.packet[0], packets.packet[1]); + } + // unpacket_traits::size == 4 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index index, Index begin, Index count) const { + constexpr int NumPackets = 4; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(index, begin, count); + return pcast(packets.packet[0], packets.packet[1], packets.packet[2], + packets.packet[3]); + } + // unpacket_traits::size == 8 * SrcPacketSize + template = true> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DstPacketType packetSegment(Index index, Index begin, Index count) const { + constexpr int NumPackets = 8; + constexpr int SrcLoadMode = plain_enum_min(SrcPacketBytes, LoadMode); + PacketBlock packets = + srcPacketSegmentHelper(index, begin, count); + return pcast(packets.packet[0], packets.packet[1], packets.packet[2], + packets.packet[3], packets.packet[4], packets.packet[5], + packets.packet[6], packets.packet[7]); + } + constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_rows; } constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_cols; } constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const { return m_rows * m_cols; } @@ -826,6 +1019,20 @@ struct ternary_evaluator, IndexBased m_d.arg3Impl.template packet(index)); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_d.func().packetOp(m_d.arg1Impl.template packetSegment(row, col, begin, count), + m_d.arg2Impl.template packetSegment(row, col, begin, count), + m_d.arg3Impl.template packetSegment(row, col, begin, count)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return m_d.func().packetOp(m_d.arg1Impl.template packetSegment(index, begin, count), + m_d.arg2Impl.template packetSegment(index, begin, count), + m_d.arg3Impl.template packetSegment(index, begin, count)); + } + protected: // this helper permits to completely eliminate the functor if it is empty struct Data { @@ -922,6 +1129,18 @@ struct binary_evaluator, IndexBased, IndexBase m_d.rhsImpl.template packet(index)); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_d.func().packetOp(m_d.lhsImpl.template packetSegment(row, col, begin, count), + m_d.rhsImpl.template packetSegment(row, col, begin, count)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return m_d.func().packetOp(m_d.lhsImpl.template packetSegment(index, begin, count), + m_d.rhsImpl.template packetSegment(index, begin, count)); + } + protected: // this helper permits to completely eliminate the functor if it is empty struct Data { @@ -1013,7 +1232,7 @@ struct mapbase_evaluator : evaluator_base { m_innerStride(map.innerStride()), m_outerStride(map.outerStride()) { EIGEN_STATIC_ASSERT(check_implication((evaluator::Flags & PacketAccessBit) != 0, - internal::inner_stride_at_compile_time::ret == 1), + inner_stride_at_compile_time::ret == 1), PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); } @@ -1035,36 +1254,60 @@ struct mapbase_evaluator : evaluator_base { template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { PointerType ptr = m_data + row * rowStride() + col * colStride(); - return internal::ploadt(ptr); + return ploadt(ptr); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index index) const { - return internal::ploadt(m_data + index * m_innerStride.value()); + return ploadt(m_data + index * m_innerStride.value()); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { PointerType ptr = m_data + row * rowStride() + col * colStride(); - return internal::pstoret(ptr, x); + pstoret(ptr, x); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { - internal::pstoret(m_data + index * m_innerStride.value(), x); + pstoret(m_data + index * m_innerStride.value(), x); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + return ploadtSegment(ptr, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return ploadtSegment(m_data + index * m_innerStride.value(), begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + PointerType ptr = m_data + row * rowStride() + col * colStride(); + pstoretSegment(ptr, x, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + pstoretSegment(m_data + index * m_innerStride.value(), x, begin, count); } protected: - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rowStride() const noexcept { return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index colStride() const noexcept { return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value(); } PointerType m_data; - const internal::variable_if_dynamic m_innerStride; - const internal::variable_if_dynamic m_outerStride; + const variable_if_dynamic m_innerStride; + const variable_if_dynamic m_outerStride; }; template @@ -1117,7 +1360,7 @@ struct evaluator> // -------------------- Block -------------------- template ::ret> + bool HasDirectAccess = has_direct_access::ret> struct block_evaluator; template @@ -1246,6 +1489,39 @@ struct unary_evaluator, IndexBa x); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_argImpl.template packetSegment(m_startRow.value() + row, m_startCol.value() + col, + begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + if (ForwardLinearAccess) + return m_argImpl.template packetSegment(m_linear_offset.value() + index, begin, count); + else + return packetSegment(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0, + begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + return m_argImpl.template writePacketSegment(m_startRow.value() + row, + m_startCol.value() + col, x, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + if (ForwardLinearAccess) + return m_argImpl.template writePacketSegment(m_linear_offset.value() + index, x, begin, + count); + else + return writePacketSegment(RowsAtCompileTime == 1 ? 0 : index, + RowsAtCompileTime == 1 ? index : 0, x, begin, count); + } + protected: EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType linear_coeff_impl(Index index, internal::true_type /* ForwardLinearAccess */) const { @@ -1341,8 +1617,8 @@ struct unary_evaluator> typedef Replicate XprType; typedef typename XprType::CoeffReturnType CoeffReturnType; enum { Factor = (RowFactor == Dynamic || ColFactor == Dynamic) ? Dynamic : RowFactor * ColFactor }; - typedef typename internal::nested_eval::type ArgTypeNested; - typedef internal::remove_all_t ArgTypeNestedCleaned; + typedef typename nested_eval::type ArgTypeNested; + typedef remove_all_t ArgTypeNestedCleaned; enum { CoeffReadCost = evaluator::CoeffReadCost, @@ -1361,19 +1637,15 @@ struct unary_evaluator> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { // try to avoid using modulo; this is a pure optimization strategy - const Index actual_row = internal::traits::RowsAtCompileTime == 1 ? 0 - : RowFactor == 1 ? row - : row % m_rows.value(); - const Index actual_col = internal::traits::ColsAtCompileTime == 1 ? 0 - : ColFactor == 1 ? col - : col % m_cols.value(); + const Index actual_row = traits::RowsAtCompileTime == 1 ? 0 : RowFactor == 1 ? row : row % m_rows.value(); + const Index actual_col = traits::ColsAtCompileTime == 1 ? 0 : ColFactor == 1 ? col : col % m_cols.value(); return m_argImpl.coeff(actual_row, actual_col); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index index) const { // try to avoid using modulo; this is a pure optimization strategy - const Index actual_index = internal::traits::RowsAtCompileTime == 1 + const Index actual_index = traits::RowsAtCompileTime == 1 ? (ColFactor == 1 ? index : index % m_cols.value()) : (RowFactor == 1 ? index : index % m_rows.value()); @@ -1382,25 +1654,38 @@ struct unary_evaluator> template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { - const Index actual_row = internal::traits::RowsAtCompileTime == 1 ? 0 - : RowFactor == 1 ? row - : row % m_rows.value(); - const Index actual_col = internal::traits::ColsAtCompileTime == 1 ? 0 - : ColFactor == 1 ? col - : col % m_cols.value(); + const Index actual_row = traits::RowsAtCompileTime == 1 ? 0 : RowFactor == 1 ? row : row % m_rows.value(); + const Index actual_col = traits::ColsAtCompileTime == 1 ? 0 : ColFactor == 1 ? col : col % m_cols.value(); return m_argImpl.template packet(actual_row, actual_col); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index index) const { - const Index actual_index = internal::traits::RowsAtCompileTime == 1 + const Index actual_index = traits::RowsAtCompileTime == 1 ? (ColFactor == 1 ? index : index % m_cols.value()) : (RowFactor == 1 ? index : index % m_rows.value()); return m_argImpl.template packet(actual_index); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + const Index actual_row = traits::RowsAtCompileTime == 1 ? 0 : RowFactor == 1 ? row : row % m_rows.value(); + const Index actual_col = traits::ColsAtCompileTime == 1 ? 0 : ColFactor == 1 ? col : col % m_cols.value(); + + return m_argImpl.template packetSegment(actual_row, actual_col, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + const Index actual_index = traits::RowsAtCompileTime == 1 + ? (ColFactor == 1 ? index : index % m_cols.value()) + : (RowFactor == 1 ? index : index % m_rows.value()); + + return m_argImpl.template packetSegment(actual_index, begin, count); + } + protected: const ArgTypeNested m_arg; evaluator m_argImpl; @@ -1457,6 +1742,28 @@ struct evaluator_wrapper_base : evaluator_base { m_argImpl.template writePacket(index, x); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return m_argImpl.template packetSegment(row, col, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + return m_argImpl.template packetSegment(index, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + m_argImpl.template writePacketSegment(row, col, x, begin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + m_argImpl.template writePacketSegment(index, x, begin, count); + } + protected: evaluator m_argImpl; }; @@ -1536,41 +1843,97 @@ struct unary_evaluator> : evaluator_base EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { - enum { - PacketSize = unpacket_traits::size, - OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, - OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 - }; - typedef internal::reverse_packet_cond reverse_packet; - return reverse_packet::run(m_argImpl.template packet( - ReverseRow ? m_rows.value() - row - OffsetRow : row, ReverseCol ? m_cols.value() - col - OffsetCol : col)); + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1; + static constexpr int OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1; + using reverse_packet = reverse_packet_cond; + + Index actualRow = ReverseRow ? m_rows.value() - row - OffsetRow : row; + Index actualCol = ReverseCol ? m_cols.value() - col - OffsetCol : col; + + return reverse_packet::run(m_argImpl.template packet(actualRow, actualCol)); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packet(Index index) const { - enum { PacketSize = unpacket_traits::size }; - return preverse( - m_argImpl.template packet(m_rows.value() * m_cols.value() - index - PacketSize)); + static constexpr int PacketSize = unpacket_traits::size; + + Index actualIndex = m_rows.value() * m_cols.value() - index - PacketSize; + + return preverse(m_argImpl.template packet(actualIndex)); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketType& x) { - // FIXME we could factorize some code with packet(i,j) - enum { - PacketSize = unpacket_traits::size, - OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1, - OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1 - }; - typedef internal::reverse_packet_cond reverse_packet; - m_argImpl.template writePacket(ReverseRow ? m_rows.value() - row - OffsetRow : row, - ReverseCol ? m_cols.value() - col - OffsetCol : col, - reverse_packet::run(x)); + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1; + static constexpr int OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1; + using reverse_packet = reverse_packet_cond; + + Index actualRow = ReverseRow ? m_rows.value() - row - OffsetRow : row; + Index actualCol = ReverseCol ? m_cols.value() - col - OffsetCol : col; + + m_argImpl.template writePacket(actualRow, actualCol, reverse_packet::run(x)); } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacket(Index index, const PacketType& x) { - enum { PacketSize = unpacket_traits::size }; - m_argImpl.template writePacket(m_rows.value() * m_cols.value() - index - PacketSize, preverse(x)); + static constexpr int PacketSize = unpacket_traits::size; + + Index actualIndex = m_rows.value() * m_cols.value() - index - PacketSize; + + m_argImpl.template writePacket(actualIndex, preverse(x)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1; + static constexpr int OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1; + using reverse_packet = reverse_packet_cond; + + Index actualRow = ReverseRow ? m_rows.value() - row - OffsetRow : row; + Index actualCol = ReverseCol ? m_cols.value() - col - OffsetCol : col; + Index actualBegin = ReversePacket ? (PacketSize - count - begin) : begin; + + return reverse_packet::run( + m_argImpl.template packetSegment(actualRow, actualCol, actualBegin, count)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index index, Index begin, Index count) const { + static constexpr int PacketSize = unpacket_traits::size; + + Index actualIndex = m_rows.value() * m_cols.value() - index - PacketSize; + Index actualBegin = PacketSize - count - begin; + + return preverse(m_argImpl.template packetSegment(actualIndex, actualBegin, count)); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index row, Index col, const PacketType& x, Index begin, + Index count) { + static constexpr int PacketSize = unpacket_traits::size; + static constexpr int OffsetRow = ReverseRow && IsColMajor ? PacketSize : 1; + static constexpr int OffsetCol = ReverseCol && IsRowMajor ? PacketSize : 1; + using reverse_packet = reverse_packet_cond; + + Index actualRow = ReverseRow ? m_rows.value() - row - OffsetRow : row; + Index actualCol = ReverseCol ? m_cols.value() - col - OffsetCol : col; + Index actualBegin = ReversePacket ? (PacketSize - count - begin) : begin; + + m_argImpl.template writePacketSegment(actualRow, actualCol, reverse_packet::run(x), actualBegin, count); + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void writePacketSegment(Index index, const PacketType& x, Index begin, + Index count) { + static constexpr int PacketSize = unpacket_traits::size; + + Index actualIndex = m_rows.value() * m_cols.value() - index - PacketSize; + Index actualBegin = PacketSize - count - begin; + + m_argImpl.template writePacketSegment(actualIndex, preverse(x), actualBegin, count); } protected: @@ -1621,13 +1984,13 @@ struct evaluator> : evaluator_base m_argImpl; - const internal::variable_if_dynamicindex m_index; + const variable_if_dynamicindex m_index; private: - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index colOffset() const { return m_index.value() > 0 ? m_index.value() : 0; } }; @@ -1656,9 +2019,9 @@ class EvalToTemp : public dense_xpr_base>::type { const ArgType& arg() const { return m_arg; } - EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_arg.rows(); } + constexpr Index rows() const noexcept { return m_arg.rows(); } - EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_arg.cols(); } + constexpr Index cols() const noexcept { return m_arg.cols(); } private: const ArgType& m_arg; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h index aa79b60813..e2b2da5a64 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h @@ -108,12 +108,12 @@ class CwiseBinaryOp : public CwiseBinaryOpImpl>::RowsAtCompileTime == Dynamic ? m_rhs.rows() : m_lhs.rows(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index cols() const noexcept { // return the fixed size type if available to enable compile time optimizations return internal::traits>::ColsAtCompileTime == Dynamic ? m_rhs.cols() : m_lhs.cols(); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h index 39c33cf034..13a542a023 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h @@ -71,9 +71,13 @@ class CwiseNullaryOp : public internal::dense_xpr_base= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); } + EIGEN_DEVICE_FUNC CwiseNullaryOp(Index size, const NullaryOp& func = NullaryOp()) + : CwiseNullaryOp(RowsAtCompileTime == 1 ? 1 : size, RowsAtCompileTime == 1 ? size : 1, func) { + EIGEN_STATIC_ASSERT(CwiseNullaryOp::IsVectorAtCompileTime, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); + } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const { return m_cols.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rows() const { return m_rows.value(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index cols() const { return m_cols.value(); } /** \returns the functor representing the nullary operation */ EIGEN_DEVICE_FUNC const NullaryOp& functor() const { return m_functor; } @@ -283,7 +287,7 @@ DenseBase::LinSpaced(Index size, const Scalar& low, const Scalar& high) } /** - * \copydoc DenseBase::LinSpaced(Index, const Scalar&, const Scalar&) + * \copydoc DenseBase::LinSpaced(Index, const DenseBase::Scalar&, const DenseBase::Scalar&) * Special version for fixed size types which does not require the size parameter. */ template @@ -343,7 +347,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar */ template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& val) { - return derived() = Constant(rows(), cols(), val); + internal::eigen_fill_impl::run(derived(), val); + return derived(); } /** Resizes to the given \a size, and sets all coefficients in this expression to the given value \a val. @@ -479,9 +484,9 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setEqualSpace * \sa Zero(), Zero(Index) */ template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero( +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ZeroReturnType DenseBase::Zero( Index rows, Index cols) { - return Constant(rows, cols, Scalar(0)); + return ZeroReturnType(rows, cols); } /** \returns an expression of a zero vector. @@ -501,9 +506,9 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::Constan * \sa Zero(), Zero(Index,Index) */ template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero( +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ZeroReturnType DenseBase::Zero( Index size) { - return Constant(size, Scalar(0)); + return ZeroReturnType(size); } /** \returns an expression of a fixed-size zero matrix or vector. @@ -517,8 +522,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::Constan * \sa Zero(Index), Zero(Index,Index) */ template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType DenseBase::Zero() { - return Constant(Scalar(0)); +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase::ZeroReturnType DenseBase::Zero() { + return ZeroReturnType(RowsAtCompileTime, ColsAtCompileTime); } /** \returns true if *this is approximately equal to the zero matrix, @@ -547,7 +552,8 @@ EIGEN_DEVICE_FUNC bool DenseBase::isZero(const RealScalar& prec) const */ template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setZero() { - return setConstant(Scalar(0)); + internal::eigen_zero_impl::run(derived()); + return derived(); } /** Resizes to the given \a size, and sets all coefficients in this expression to zero. @@ -562,7 +568,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::setZero() { template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(Index newSize) { resize(newSize); - return setConstant(Scalar(0)); + return setZero(); } /** Resizes to the given size, and sets all coefficients in this expression to zero. @@ -578,7 +584,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& PlainObjectBase::setZero(Index rows, Index cols) { resize(rows, cols); - return setConstant(Scalar(0)); + return setZero(); } /** Resizes to the given size, changing only the number of columns, and sets all diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h index 42ed459a0f..94ec1a0fe2 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h @@ -60,8 +60,8 @@ class CwiseUnaryOp : public CwiseUnaryOpImpl EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeffRef(0)); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + EIGEN_DEVICE_FUNC constexpr Index innerStride() const { return StrideType::InnerStrideAtCompileTime != 0 ? int(StrideType::InnerStrideAtCompileTime) : derived().nestedExpression().innerStride() * sizeof(typename traits::Scalar) / sizeof(Scalar); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + EIGEN_DEVICE_FUNC constexpr Index outerStride() const { return StrideType::OuterStrideAtCompileTime != 0 ? int(StrideType::OuterStrideAtCompileTime) : derived().nestedExpression().outerStride() * sizeof(typename traits::Scalar) / sizeof(Scalar); @@ -145,8 +145,8 @@ class CwiseUnaryView : public internal::CwiseUnaryViewImplrows() : this->cols(); } @@ -217,7 +217,7 @@ class DenseBase * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a * column-major matrix, and the number of columns for a row-major matrix. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const { + EIGEN_DEVICE_FUNC constexpr Index innerSize() const { return IsVectorAtCompileTime ? this->size() : int(IsRowMajor) ? this->cols() : this->rows(); } @@ -243,6 +243,8 @@ class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal Represents a matrix with all coefficients equal to one another*/ typedef CwiseNullaryOp, PlainObject> ConstantReturnType; + /** \internal Represents a matrix with all coefficients equal to zero*/ + typedef CwiseNullaryOp, PlainObject> ZeroReturnType; /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */ EIGEN_DEPRECATED typedef CwiseNullaryOp, PlainObject> SequentialLinSpacedReturnType; /** \internal Represents a vector with linearly spaced coefficients that allows random access. */ @@ -328,9 +330,9 @@ class DenseBase template EIGEN_DEVICE_FUNC static const CwiseNullaryOp NullaryExpr(const CustomNullaryOp& func); - EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols); - EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size); - EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(); + EIGEN_DEVICE_FUNC static const ZeroReturnType Zero(Index rows, Index cols); + EIGEN_DEVICE_FUNC static const ZeroReturnType Zero(Index size); + EIGEN_DEVICE_FUNC static const ZeroReturnType Zero(); EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols); EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size); EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h index 97f9b50f34..cff104c366 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h @@ -89,13 +89,12 @@ class DenseCoeffsBase : public EigenBase { * * \sa operator()(Index,Index) const, coeffRef(Index,Index), coeff(Index) const */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType coeff(Index row, Index col) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType coeff(Index row, Index col) const { eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); return internal::evaluator(derived()).coeff(row, col); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType coeffByOuterInner(Index outer, - Index inner) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType coeffByOuterInner(Index outer, Index inner) const { return coeff(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner)); } @@ -103,7 +102,7 @@ class DenseCoeffsBase : public EigenBase { * * \sa operator()(Index,Index), operator[](Index) */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType operator()(Index row, Index col) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType operator()(Index row, Index col) const { eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); return coeff(row, col); } @@ -123,7 +122,7 @@ class DenseCoeffsBase : public EigenBase { * \sa operator[](Index) const, coeffRef(Index), coeff(Index,Index) const */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType coeff(Index index) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType coeff(Index index) const { EIGEN_STATIC_ASSERT(internal::evaluator::Flags & LinearAccessBit, THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS) eigen_internal_assert(index >= 0 && index < size()); @@ -138,7 +137,7 @@ class DenseCoeffsBase : public EigenBase { * z() const, w() const */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType operator[](Index index) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType operator[](Index index) const { EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD) eigen_assert(index >= 0 && index < size()); @@ -155,32 +154,32 @@ class DenseCoeffsBase : public EigenBase { * z() const, w() const */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType operator()(Index index) const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType operator()(Index index) const { eigen_assert(index >= 0 && index < size()); return coeff(index); } /** equivalent to operator[](0). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType x() const { return (*this)[0]; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType x() const { return (*this)[0]; } /** equivalent to operator[](1). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType y() const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType y() const { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS); return (*this)[1]; } /** equivalent to operator[](2). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType z() const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType z() const { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS); return (*this)[2]; } /** equivalent to operator[](3). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR CoeffReturnType w() const { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr CoeffReturnType w() const { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS); return (*this)[3]; } @@ -362,32 +361,32 @@ class DenseCoeffsBase : public DenseCoeffsBase= 0 && index < size()); return coeffRef(index); } /** equivalent to operator[](0). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Scalar& x() { return (*this)[0]; } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& x() { return (*this)[0]; } /** equivalent to operator[](1). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Scalar& y() { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& y() { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 2, OUT_OF_RANGE_ACCESS); return (*this)[1]; } /** equivalent to operator[](2). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Scalar& z() { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& z() { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 3, OUT_OF_RANGE_ACCESS); return (*this)[2]; } /** equivalent to operator[](3). */ - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Scalar& w() { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Scalar& w() { EIGEN_STATIC_ASSERT(Derived::SizeAtCompileTime == -1 || Derived::SizeAtCompileTime >= 4, OUT_OF_RANGE_ACCESS); return (*this)[3]; } @@ -421,33 +420,29 @@ class DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase : public DenseCoeffsBase struct first_aligned_impl { - static EIGEN_CONSTEXPR inline Index run(const Derived&) EIGEN_NOEXCEPT { return 0; } + static constexpr Index run(const Derived&) noexcept { return 0; } }; template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DeviceWrapper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DeviceWrapper.h index 3ae8256f57..012dce10d1 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DeviceWrapper.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DeviceWrapper.h @@ -66,7 +66,7 @@ struct AssignmentWithDevice, Functor, Dev static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func, Device&) { Base::run(dst, src, func); - }; + } }; // specialization for coeffcient-wise assignment @@ -87,13 +87,13 @@ template struct dense_assignment_loop_with_device { using Base = dense_assignment_loop; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel, Device&) { Base::run(kernel); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel, Device&) { Base::run(kernel); } }; // entry point for a generic expression with device template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_alias(DeviceWrapper dst, - const Src& src, const Func& func) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment_no_alias(DeviceWrapper dst, + const Src& src, const Func& func) { enum { NeedToTranspose = ((int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1) || (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)) && @@ -115,10 +115,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment_no_al // copy and pasted from AssignEvaluator except forward device to kernel template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_dense_assignment_loop(DstXprType& dst, - const SrcXprType& src, - const Functor& func, - Device& device) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src, + const Functor& func, Device& device) { using DstEvaluatorType = evaluator; using SrcEvaluatorType = evaluator; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h index 8d27857e05..ff8611c607 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h @@ -83,13 +83,11 @@ class Diagonal : public internal::dense_xpr_base(m_matrix.rows(), m_matrix.cols() - m_index.value()); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return 1; } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return 1; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { - return m_matrix.outerStride() + 1; - } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return m_matrix.outerStride() + 1; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return 0; } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return 0; } typedef std::conditional_t::value, Scalar, const Scalar> ScalarWithConstIfNotLvalue; @@ -134,13 +132,13 @@ class Diagonal : public internal::dense_xpr_base 0 ? m_index.value() : -m_index.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rowOffset() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rowOffset() const noexcept { return m_index.value() > 0 ? 0 : -m_index.value(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index colOffset() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index colOffset() const noexcept { return m_index.value() > 0 ? m_index.value() : 0; } // trigger a compile-time error if someone try to call packet diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h index fd61bb7931..52630d9297 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h @@ -76,9 +76,9 @@ class DiagonalBase : public EigenBase { } /** \returns the number of rows. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { return diagonal().size(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const { return diagonal().size(); } /** \returns the number of columns. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return diagonal().size(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const { return diagonal().size(); } /** \returns the diagonal matrix product of \c *this by the dense matrix, \a matrix */ template @@ -256,10 +256,13 @@ class DiagonalMatrix : public DiagonalBase, DiagonalVectorType>> InitializeReturnType; + typedef DiagonalWrapper, DiagonalVectorType>> + ZeroInitializeReturnType; + /** Initializes a diagonal matrix of size SizeAtCompileTime with coefficients set to zero */ - EIGEN_DEVICE_FUNC static const InitializeReturnType Zero() { return DiagonalVectorType::Zero().asDiagonal(); } + EIGEN_DEVICE_FUNC static const ZeroInitializeReturnType Zero() { return DiagonalVectorType::Zero().asDiagonal(); } /** Initializes a diagonal matrix of size dim with coefficients set to zero */ - EIGEN_DEVICE_FUNC static const InitializeReturnType Zero(Index size) { + EIGEN_DEVICE_FUNC static const ZeroInitializeReturnType Zero(Index size) { return DiagonalVectorType::Zero(size).asDiagonal(); } /** Initializes a identity matrix of size SizeAtCompileTime */ @@ -386,8 +389,9 @@ struct AssignmentKind { // Diagonal matrix to Dense assignment template struct Assignment { - static void run(DstXprType& dst, const SrcXprType& src, - const internal::assign_op& /*func*/) { + static EIGEN_DEVICE_FUNC void run( + DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*func*/) { Index dstRows = src.rows(); Index dstCols = src.cols(); if ((dst.rows() != dstRows) || (dst.cols() != dstCols)) dst.resize(dstRows, dstCols); @@ -396,13 +400,15 @@ struct Assignment { dst.diagonal() = src.diagonal(); } - static void run(DstXprType& dst, const SrcXprType& src, - const internal::add_assign_op& /*func*/) { + static EIGEN_DEVICE_FUNC void run( + DstXprType& dst, const SrcXprType& src, + const internal::add_assign_op& /*func*/) { dst.diagonal() += src.diagonal(); } - static void run(DstXprType& dst, const SrcXprType& src, - const internal::sub_assign_op& /*func*/) { + static EIGEN_DEVICE_FUNC void run( + DstXprType& dst, const SrcXprType& src, + const internal::sub_assign_op& /*func*/) { dst.diagonal() -= src.diagonal(); } }; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h index 894bfc13b1..c9a6e88e2f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h @@ -56,12 +56,12 @@ struct EigenBase { EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } /** \returns the number of rows. \sa cols(), RowsAtCompileTime */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return derived().rows(); } /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return derived().cols(); } /** \returns the number of coefficients, which is rows()*cols(). * \sa rows(), cols(), SizeAtCompileTime. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index size() const EIGEN_NOEXCEPT { return rows() * cols(); } + EIGEN_DEVICE_FUNC constexpr Index size() const noexcept { return rows() * cols(); } /** \internal Don't use it, but do the equivalent: \code dst = *this; \endcode */ template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h new file mode 100644 index 0000000000..9d4ecd445a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fill.h @@ -0,0 +1,139 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2024 Charles Schlosser +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_FILL_H +#define EIGEN_FILL_H + +// IWYU pragma: private +#include "./InternalHeaderCheck.h" + +namespace Eigen { + +namespace internal { + +template +struct eigen_fill_helper : std::false_type {}; + +template +struct eigen_fill_helper> : std::true_type {}; + +template +struct eigen_fill_helper> : std::true_type {}; + +template +struct eigen_fill_helper> : eigen_fill_helper {}; + +template +struct eigen_fill_helper> + : std::integral_constant::value && + (Xpr::IsRowMajor ? (BlockRows == 1) : (BlockCols == 1))> {}; + +template +struct eigen_fill_helper>> : eigen_fill_helper {}; + +template +struct eigen_fill_helper>> + : std::integral_constant::value && + enum_eq_not_dynamic(OuterStride_, Xpr::InnerSizeAtCompileTime)> {}; + +template +struct eigen_fill_helper>> + : eigen_fill_helper>> {}; + +template +struct eigen_fill_helper>> + : eigen_fill_helper>> {}; + +template +struct eigen_fill_helper>> + : eigen_fill_helper>> {}; + +template +struct eigen_fill_impl { + using Scalar = typename Xpr::Scalar; + using Func = scalar_constant_op; + using PlainObject = typename Xpr::PlainObject; + using Constant = typename PlainObject::ConstantReturnType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void run(Xpr& dst, const Scalar& val) { + const Constant src(dst.rows(), dst.cols(), val); + run(dst, src); + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void run(Xpr& dst, const SrcXpr& src) { + call_dense_assignment_loop(dst, src, assign_op()); + } +}; + +#if EIGEN_COMP_MSVC || defined(EIGEN_GPU_COMPILE_PHASE) +template +struct eigen_fill_impl : eigen_fill_impl {}; +#else +template +struct eigen_fill_impl { + using Scalar = typename Xpr::Scalar; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst, const Scalar& val) { + using std::fill_n; + fill_n(dst.data(), dst.size(), val); + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst, const SrcXpr& src) { + resize_if_allowed(dst, src, assign_op()); + const Scalar& val = src.functor()(); + run(dst, val); + } +}; +#endif + +template +struct eigen_memset_helper { + static constexpr bool value = + std::is_trivially_copyable::value && eigen_fill_helper::value; +}; + +template +struct eigen_zero_impl { + using Scalar = typename Xpr::Scalar; + using PlainObject = typename Xpr::PlainObject; + using Zero = typename PlainObject::ZeroReturnType; + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void run(Xpr& dst) { + const Zero src(dst.rows(), dst.cols()); + run(dst, src); + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void run(Xpr& dst, const SrcXpr& src) { + call_dense_assignment_loop(dst, src, assign_op()); + } +}; + +template +struct eigen_zero_impl { + using Scalar = typename Xpr::Scalar; + static constexpr size_t max_bytes = (std::numeric_limits::max)(); + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst) { + const size_t num_bytes = dst.size() * sizeof(Scalar); + if (num_bytes == 0) return; + void* dst_ptr = static_cast(dst.data()); +#ifndef EIGEN_NO_DEBUG + if (num_bytes > max_bytes) throw_std_bad_alloc(); + eigen_assert((dst_ptr != nullptr) && "null pointer dereference error!"); +#endif + EIGEN_USING_STD(memset); + memset(dst_ptr, 0, num_bytes); + } + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Xpr& dst, const SrcXpr& src) { + resize_if_allowed(dst, src, assign_op()); + run(dst); + } +}; + +} // namespace internal +} // namespace Eigen + +#endif // EIGEN_FILL_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h index a91b0da6a3..55beab35a1 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h @@ -41,14 +41,10 @@ class ForceAlignedAccess : public internal::dense_xpr_base::size; using DefaultPacket = typename packet_traits::type; static constexpr int DefaultSize = unpacket_traits::size; - static constexpr bool value = Size < DefaultSize; + static constexpr bool value = Size != 1 && Size < DefaultSize; }; template @@ -368,6 +369,11 @@ template EIGEN_DEVICE_FUNC inline Packet pdiv(const Packet& a, const Packet& b) { return a / b; } +// Avoid compiler warning for boolean algebra. +template <> +EIGEN_DEVICE_FUNC inline bool pdiv(const bool& a, const bool& b) { + return a && b; +} // In the generic case, memset to all one bits. template @@ -449,48 +455,42 @@ EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) template struct bit_and { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; } }; template struct bit_or { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a | b; } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a | b; } }; template struct bit_xor { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a ^ b; } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a ^ b; } }; template struct bit_not { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a) const { return ~a; } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a) const { return ~a; } }; template <> struct bit_and { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { - return a && b; - } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { return a && b; } }; template <> struct bit_or { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { - return a || b; - } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { return a || b; } }; template <> struct bit_xor { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { - return a != b; - } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE bool operator()(const bool& a, const bool& b) const { return a != b; } }; template <> struct bit_not { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE bool operator()(const bool& a) const { return !a; } + EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE bool operator()(const bool& a) const { return !a; } }; // Use operators &, |, ^, ~. @@ -580,7 +580,7 @@ EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) { } // In the general case, use bitwise select. -template +template ::value> struct pselect_impl { static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) { return por(pand(a, mask), pandnot(b, mask)); @@ -589,9 +589,9 @@ struct pselect_impl { // For scalars, use ternary select. template -struct pselect_impl::value>> { +struct pselect_impl { static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) { - return numext::equal_strict(mask, Packet(0)) ? b : a; + return numext::select(mask, a, b); } }; @@ -1294,29 +1294,61 @@ EIGEN_DEVICE_FUNC inline bool predux_any(const Packet& a) { * The following functions might not have to be overwritten for vectorized types ***************************************************************************/ +template +struct pmadd_impl { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pmadd(const Packet& a, const Packet& b, const Packet& c) { + return padd(pmul(a, b), c); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pmsub(const Packet& a, const Packet& b, const Packet& c) { + return psub(pmul(a, b), c); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pnmadd(const Packet& a, const Packet& b, const Packet& c) { + return psub(c, pmul(a, b)); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pnmsub(const Packet& a, const Packet& b, const Packet& c) { + return pnegate(pmadd(a, b, c)); + } +}; + +template +struct pmadd_impl::value && NumTraits::IsSigned>> { + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pmadd(const Scalar& a, const Scalar& b, const Scalar& c) { + return numext::fma(a, b, c); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pmsub(const Scalar& a, const Scalar& b, const Scalar& c) { + return numext::fma(a, b, Scalar(-c)); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pnmadd(const Scalar& a, const Scalar& b, const Scalar& c) { + return numext::fma(Scalar(-a), b, c); + } + static EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar pnmsub(const Scalar& a, const Scalar& b, const Scalar& c) { + return -Scalar(numext::fma(a, b, c)); + } +}; + // FMA instructions. /** \internal \returns a * b + c (coeff-wise) */ template EIGEN_DEVICE_FUNC inline Packet pmadd(const Packet& a, const Packet& b, const Packet& c) { - return padd(pmul(a, b), c); + return pmadd_impl::pmadd(a, b, c); } /** \internal \returns a * b - c (coeff-wise) */ template EIGEN_DEVICE_FUNC inline Packet pmsub(const Packet& a, const Packet& b, const Packet& c) { - return psub(pmul(a, b), c); + return pmadd_impl::pmsub(a, b, c); } /** \internal \returns -(a * b) + c (coeff-wise) */ template EIGEN_DEVICE_FUNC inline Packet pnmadd(const Packet& a, const Packet& b, const Packet& c) { - return psub(c, pmul(a, b)); + return pmadd_impl::pnmadd(a, b, c); } /** \internal \returns -((a * b + c) (coeff-wise) */ template EIGEN_DEVICE_FUNC inline Packet pnmsub(const Packet& a, const Packet& b, const Packet& c) { - return pnegate(pmadd(a, b, c)); + return pmadd_impl::pnmsub(a, b, c); } /** \internal copy a packet with constant coefficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned @@ -1525,6 +1557,104 @@ EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet pcarg(const Packet& a) { return (Packet)pand(result, peven_mask(result)); // atan2 0 atan2 0 ... } +/** \internal \returns a packet populated with values in the range [begin, begin + count). Elements + * outside this range are not defined. \a *from does not need to be aligned, and can be null if \a count is zero.*/ +template +EIGEN_DEVICE_FUNC inline Packet ploaduSegment(const typename unpacket_traits::type* from, Index begin, + Index count) { + using Scalar = typename unpacket_traits::type; + constexpr Index PacketSize = unpacket_traits::size; + eigen_assert((begin >= 0 && count >= 0 && begin + count <= PacketSize) && "invalid range"); + Scalar aux[PacketSize]; + memset(static_cast(aux), 0x00, sizeof(Scalar) * PacketSize); + smart_copy(from + begin, from + begin + count, aux + begin); + return ploadu(aux); +} + +/** \internal \returns a packet populated with values in the range [begin, begin + count). Elements + * outside this range are not defined. \a *from must be aligned, and cannot be null.*/ +template +EIGEN_DEVICE_FUNC inline Packet ploadSegment(const typename unpacket_traits::type* from, Index begin, + Index count) { + return ploaduSegment(from, begin, count); +} + +/** \internal copy the packet \a from in the range [begin, begin + count) to \a *to. +Elements outside of the range [begin, begin + count) are not defined. \a *to does not need to be aligned, and can be +null if \a count is zero.*/ +template +EIGEN_DEVICE_FUNC inline void pstoreuSegment(Scalar* to, const Packet& from, Index begin, Index count) { + constexpr Index PacketSize = unpacket_traits::size; + eigen_assert((begin >= 0 && count >= 0 && begin + count <= PacketSize) && "invalid range"); + Scalar aux[PacketSize]; + pstoreu(aux, from); + smart_copy(aux + begin, aux + begin + count, to + begin); +} + +/** \internal copy the packet \a from in the range [begin, begin + count) to \a *to. +Elements outside of the range [begin, begin + count) are not defined. \a *to must be aligned, and cannot be +null.*/ +template +EIGEN_DEVICE_FUNC inline void pstoreSegment(Scalar* to, const Packet& from, Index begin, Index count) { + return pstoreuSegment(to, from, begin, count); +} + +/** \internal \returns a packet populated with values in the range [begin, begin + count). Elements + * outside this range are not defined.*/ +template +EIGEN_DEVICE_FUNC inline Packet ploadtSegment(const typename unpacket_traits::type* from, Index begin, + Index count) { + constexpr int RequiredAlignment = unpacket_traits::alignment; + if (Alignment >= RequiredAlignment) { + return ploadSegment(from, begin, count); + } else { + return ploaduSegment(from, begin, count); + } +} + +/** \internal copy the packet \a from in the range [begin, begin + count) to \a *to. +Elements outside of the range [begin, begin + count) are not defined.*/ +template +EIGEN_DEVICE_FUNC inline void pstoretSegment(Scalar* to, const Packet& from, Index begin, Index count) { + constexpr int RequiredAlignment = unpacket_traits::alignment; + if (Alignment >= RequiredAlignment) { + pstoreSegment(to, from, begin, count); + } else { + pstoreuSegment(to, from, begin, count); + } +} + +#ifndef EIGEN_NO_IO + +template +class StreamablePacket { + public: + using Scalar = typename unpacket_traits::type; + StreamablePacket(const Packet& packet) { pstoreu(v_, packet); } + + friend std::ostream& operator<<(std::ostream& os, const StreamablePacket& packet) { + os << "{" << packet.v_[0]; + for (int i = 1; i < unpacket_traits::size; ++i) { + os << "," << packet.v_[i]; + } + os << "}"; + return os; + } + + private: + Scalar v_[unpacket_traits::size]; +}; + +/** + * \internal \returns an intermediary that can be used to ostream packets, e.g. for debugging. + */ +template +StreamablePacket postream(const Packet& packet) { + return StreamablePacket(packet); +} + +#endif // EIGEN_NO_IO + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h index ca5f247ede..0a1b583d6c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h @@ -68,8 +68,8 @@ struct IOFormat { // TODO check if rowPrefix, rowSuffix or rowSeparator contains a newline // don't add rowSpacer if columns are not to be aligned if ((flags & DontAlignCols)) return; - int i = int(matSuffix.length()) - 1; - while (i >= 0 && matSuffix[i] != '\n') { + int i = int(matPrefix.length()) - 1; + while (i >= 0 && matPrefix[i] != '\n') { rowSpacer += ' '; i--; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h index 454e560e4b..358239ca86 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h @@ -225,14 +225,14 @@ class IndexedViewImpl return this->nestedExpression().data() + row_offset + col_offset; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { if (traits::InnerStrideAtCompileTime != Dynamic) { return traits::InnerStrideAtCompileTime; } return innerIncrement() * this->nestedExpression().innerStride(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { if (traits::OuterStrideAtCompileTime != Dynamic) { return traits::OuterStrideAtCompileTime; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InnerProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InnerProduct.h index 1e16942c23..9849d9b1fe 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InnerProduct.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/InnerProduct.h @@ -57,16 +57,20 @@ struct inner_product_assert { template struct inner_product_evaluator { - static constexpr int LhsFlags = evaluator::Flags, RhsFlags = evaluator::Flags, - SizeAtCompileTime = min_size_prefer_fixed(Lhs::SizeAtCompileTime, Rhs::SizeAtCompileTime), - LhsAlignment = evaluator::Alignment, RhsAlignment = evaluator::Alignment; + static constexpr int LhsFlags = evaluator::Flags; + static constexpr int RhsFlags = evaluator::Flags; + static constexpr int SizeAtCompileTime = size_prefer_fixed(Lhs::SizeAtCompileTime, Rhs::SizeAtCompileTime); + static constexpr int MaxSizeAtCompileTime = + min_size_prefer_fixed(Lhs::MaxSizeAtCompileTime, Rhs::MaxSizeAtCompileTime); + static constexpr int LhsAlignment = evaluator::Alignment; + static constexpr int RhsAlignment = evaluator::Alignment; using Scalar = typename Func::result_type; using Packet = typename find_inner_product_packet::type; static constexpr bool Vectorize = bool(LhsFlags & RhsFlags & PacketAccessBit) && Func::PacketAccess && - ((SizeAtCompileTime == Dynamic) || (unpacket_traits::size <= SizeAtCompileTime)); + ((MaxSizeAtCompileTime == Dynamic) || (unpacket_traits::size <= MaxSizeAtCompileTime)); EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit inner_product_evaluator(const Lhs& lhs, const Rhs& rhs, Func func = Func()) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h index 013ad0a942..79fc3ab6a5 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h @@ -51,8 +51,8 @@ class Inverse : public InverseImpl:: explicit EIGEN_DEVICE_FUNC Inverse(const XprType& xpr) : m_xpr(xpr) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_xpr.rows(); } EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h index df7b7ca778..c740da7260 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h @@ -102,11 +102,11 @@ class Map : public MapBase > { typedef PointerType PointerArgType; EIGEN_DEVICE_FUNC inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + EIGEN_DEVICE_FUNC constexpr Index innerStride() const { return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + EIGEN_DEVICE_FUNC constexpr Index outerStride() const { return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() : internal::traits::OuterStrideAtCompileTime != Dynamic ? Index(internal::traits::OuterStrideAtCompileTime) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h index 1e83fdf70f..5e3d746bcd 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h @@ -84,9 +84,9 @@ class MapBase : public internal::dense_xpr_base struct imag_ref_default_impl { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Scalar run(Scalar&) { return Scalar(0); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline const Scalar run(const Scalar&) { return Scalar(0); } + EIGEN_DEVICE_FUNC constexpr static Scalar run(Scalar&) { return Scalar(0); } + EIGEN_DEVICE_FUNC constexpr static const Scalar run(const Scalar&) { return Scalar(0); } }; template @@ -182,6 +182,44 @@ struct imag_ref_retval { typedef typename NumTraits::Real& type; }; +// implementation in MathFunctionsImpl.h +template ::value> +struct scalar_select_mask; + +} // namespace internal + +namespace numext { + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t real_ref( + const Scalar& x) { + return internal::real_ref_impl::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) { + return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) { + return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); +} + +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar select(const Mask& mask, const Scalar& a, const Scalar& b) { + return internal::scalar_select_mask::run(mask) ? b : a; +} + +} // namespace numext + +namespace internal { + /**************************************************************************** * Implementation of conj * ****************************************************************************/ @@ -221,7 +259,9 @@ template struct abs2_impl_default // IsComplex { typedef typename NumTraits::Real RealScalar; - EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { return x.real() * x.real() + x.imag() * x.imag(); } + EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { + return numext::real(x) * numext::real(x) + numext::imag(x) * numext::imag(x); + } }; template @@ -250,16 +290,14 @@ struct sqrt_impl { }; // Complex sqrt defined in MathFunctionsImpl.h. -template -EIGEN_DEVICE_FUNC std::complex complex_sqrt(const std::complex& a_x); +template +EIGEN_DEVICE_FUNC ComplexT complex_sqrt(const ComplexT& a_x); // Custom implementation is faster than `std::sqrt`, works on // GPU, and correctly handles special cases (unlike MSVC). template struct sqrt_impl> { - EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex run(const std::complex& x) { - return complex_sqrt(x); - } + EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex run(const std::complex& x) { return complex_sqrt(x); } }; template @@ -272,13 +310,13 @@ template struct rsqrt_impl; // Complex rsqrt defined in MathFunctionsImpl.h. -template -EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& a_x); +template +EIGEN_DEVICE_FUNC ComplexT complex_rsqrt(const ComplexT& a_x); template struct rsqrt_impl> { EIGEN_DEVICE_FUNC static EIGEN_ALWAYS_INLINE std::complex run(const std::complex& x) { - return complex_rsqrt(x); + return complex_rsqrt(x); } }; @@ -299,7 +337,7 @@ struct norm1_default_impl { typedef typename NumTraits::Real RealScalar; EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { EIGEN_USING_STD(abs); - return abs(x.real()) + abs(x.imag()); + return abs(numext::real(x)) + abs(numext::imag(x)); } }; @@ -469,8 +507,8 @@ struct expm1_retval { ****************************************************************************/ // Complex log defined in MathFunctionsImpl.h. -template -EIGEN_DEVICE_FUNC std::complex complex_log(const std::complex& z); +template +EIGEN_DEVICE_FUNC ComplexT complex_log(const ComplexT& z); template struct log_impl { @@ -798,8 +836,8 @@ EIGEN_DEVICE_FUNC std::enable_if_t<(std::numeric_limits::has_infinity && !Num template EIGEN_DEVICE_FUNC - std::enable_if_t::has_quiet_NaN || std::numeric_limits::has_signaling_NaN), bool> - isnan_impl(const T&) { +std::enable_if_t::has_quiet_NaN || std::numeric_limits::has_signaling_NaN), bool> +isnan_impl(const T&) { return false; } @@ -846,7 +884,7 @@ struct sign_impl { real_type aa = abs(a); if (aa == real_type(0)) return Scalar(0); aa = real_type(1) / aa; - return Scalar(a.real() * aa, a.imag() * aa); + return Scalar(numext::real(a) * aa, numext::imag(a) * aa); } }; @@ -907,6 +945,38 @@ struct nearest_integer_impl { static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run_trunc(const Scalar& x) { return x; } }; +// Default implementation. +template +struct fma_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar run(const Scalar& a, const Scalar& b, const Scalar& c) { + return a * b + c; + } +}; + +// ADL version if it exists. +template +struct fma_impl< + T, + std::enable_if_t(), std::declval(), std::declval()))>::value>> { + static T run(const T& a, const T& b, const T& c) { return fma(a, b, c); } +}; + +#if defined(EIGEN_GPUCC) +template <> +struct fma_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float run(const float& a, const float& b, const float& c) { + return ::fmaf(a, b, c); + } +}; + +template <> +struct fma_impl { + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE double run(const double& a, const double& b, const double& c) { + return ::fma(a, b, c); + } +}; +#endif + } // end namespace internal /**************************************************************************** @@ -1042,27 +1112,6 @@ SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(maxi, fmax) #endif -template -EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) { - return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); -} - -template -EIGEN_DEVICE_FUNC inline internal::add_const_on_value_type_t real_ref( - const Scalar& x) { - return internal::real_ref_impl::run(x); -} - -template -EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) { - return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); -} - -template -EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) { - return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); -} - template EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(arg, Scalar) arg(const Scalar& x) { return EIGEN_MATHFUNC_IMPL(arg, Scalar)::run(x); @@ -1248,11 +1297,9 @@ EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double trunc(const double& x) { // Integer division with rounding up. // T is assumed to be an integer type with a>=0, and b>0 template -EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T div_ceil(T a, T b) { +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE constexpr T div_ceil(T a, T b) { using UnsignedT = typename internal::make_unsigned::type; EIGEN_STATIC_ASSERT((NumTraits::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES) - eigen_assert(a >= 0); - eigen_assert(b > 0); // Note: explicitly declaring a and b as non-negative values allows the compiler to use better optimizations const UnsignedT ua = UnsignedT(a); const UnsignedT ub = UnsignedT(b); @@ -1263,13 +1310,11 @@ EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T div_ceil(T a, T b) { // Integer round down to nearest power of b // T is assumed to be an integer type with a>=0, and b>0 template -EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T round_down(T a, U b) { +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE constexpr T round_down(T a, U b) { using UnsignedT = typename internal::make_unsigned::type; using UnsignedU = typename internal::make_unsigned::type; EIGEN_STATIC_ASSERT((NumTraits::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES) EIGEN_STATIC_ASSERT((NumTraits::IsInteger), THIS FUNCTION IS FOR INTEGER TYPES) - eigen_assert(a >= 0); - eigen_assert(b > 0); // Note: explicitly declaring a and b as non-negative values allows the compiler to use better optimizations const UnsignedT ua = UnsignedT(a); const UnsignedU ub = UnsignedU(b); @@ -1278,8 +1323,7 @@ EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE EIGEN_CONSTEXPR T round_down(T a, U b) { /** Log base 2 for 32 bits positive integers. * Conveniently returns 0 for x==0. */ -EIGEN_CONSTEXPR inline int log2(int x) { - eigen_assert(x >= 0); +constexpr int log2(int x) { unsigned int v(x); constexpr int table[32] = {0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31}; @@ -1317,11 +1361,17 @@ SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sqrt, sqrt) /** \returns the cube root of \a x. **/ template -EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T cbrt(const T& x) { +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::enable_if_t::IsComplex, T> cbrt(const T& x) { EIGEN_USING_STD(cbrt); return static_cast(cbrt(x)); } +template +EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE std::enable_if_t::IsComplex, T> cbrt(const T& x) { + EIGEN_USING_STD(pow); + return pow(x, typename NumTraits::Real(1.0 / 3.0)); +} + /** \returns the reciprocal square root of \a x. **/ template EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T rsqrt(const T& x) { @@ -1350,17 +1400,17 @@ EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE double log(const double& x) { #endif template -EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE - std::enable_if_t::IsSigned || NumTraits::IsComplex, typename NumTraits::Real> - abs(const T& x) { +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE std::enable_if_t::IsSigned || NumTraits::IsComplex, typename NumTraits::Real> +abs(const T& x) { EIGEN_USING_STD(abs); return abs(x); } template -EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE - std::enable_if_t::IsSigned || NumTraits::IsComplex), typename NumTraits::Real> - abs(const T& x) { +EIGEN_DEVICE_FUNC +EIGEN_ALWAYS_INLINE std::enable_if_t::IsSigned || NumTraits::IsComplex), typename NumTraits::Real> +abs(const T& x) { return x; } @@ -1840,6 +1890,15 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar arithmetic_shift_right(const Scalar return bit_cast(bit_cast(a) >> n); } +// Use std::fma if available. +using std::fma; + +// Otherwise, rely on template implementation. +template +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar fma(const Scalar& x, const Scalar& y, const Scalar& z) { + return internal::fma_impl::run(x, y, z); +} + } // end namespace numext namespace internal { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h index 10ddabd795..cbac1c2a4c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h @@ -76,7 +76,7 @@ struct generic_rsqrt_newton_step { static_assert(Steps > 0, "Steps must be at least 1."); using Scalar = typename unpacket_traits::type; EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Packet run(const Packet& a, const Packet& approx_rsqrt) { - constexpr Scalar kMinusHalf = Scalar(-1) / Scalar(2); + const Scalar kMinusHalf = Scalar(-1) / Scalar(2); const Packet cst_minus_half = pset1(kMinusHalf); const Packet cst_minus_one = pset1(Scalar(-1)); @@ -171,8 +171,8 @@ struct hypot_impl { // Generic complex sqrt implementation that correctly handles corner cases // according to https://en.cppreference.com/w/cpp/numeric/complex/sqrt -template -EIGEN_DEVICE_FUNC std::complex complex_sqrt(const std::complex& z) { +template +EIGEN_DEVICE_FUNC ComplexT complex_sqrt(const ComplexT& z) { // Computes the principal sqrt of the input. // // For a complex square root of the number x + i*y. We want to find real @@ -194,21 +194,21 @@ EIGEN_DEVICE_FUNC std::complex complex_sqrt(const std::complex& z) { // if x == 0: u = w, v = sign(y) * w // if x > 0: u = w, v = y / (2 * w) // if x < 0: u = |y| / (2 * w), v = sign(y) * w - + using T = typename NumTraits::Real; const T x = numext::real(z); const T y = numext::imag(z); const T zero = T(0); const T w = numext::sqrt(T(0.5) * (numext::abs(x) + numext::hypot(x, y))); - return (numext::isinf)(y) ? std::complex(NumTraits::infinity(), y) - : numext::is_exactly_zero(x) ? std::complex(w, y < zero ? -w : w) - : x > zero ? std::complex(w, y / (2 * w)) - : std::complex(numext::abs(y) / (2 * w), y < zero ? -w : w); + return (numext::isinf)(y) ? ComplexT(NumTraits::infinity(), y) + : numext::is_exactly_zero(x) ? ComplexT(w, y < zero ? -w : w) + : x > zero ? ComplexT(w, y / (2 * w)) + : ComplexT(numext::abs(y) / (2 * w), y < zero ? -w : w); } // Generic complex rsqrt implementation. -template -EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& z) { +template +EIGEN_DEVICE_FUNC ComplexT complex_rsqrt(const ComplexT& z) { // Computes the principal reciprocal sqrt of the input. // // For a complex reciprocal square root of the number z = x + i*y. We want to @@ -230,7 +230,7 @@ EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& z) { // if x == 0: u = w / |z|, v = -sign(y) * w / |z| // if x > 0: u = w / |z|, v = -y / (2 * w * |z|) // if x < 0: u = |y| / (2 * w * |z|), v = -sign(y) * w / |z| - + using T = typename NumTraits::Real; const T x = numext::real(z); const T y = numext::imag(z); const T zero = T(0); @@ -239,22 +239,65 @@ EIGEN_DEVICE_FUNC std::complex complex_rsqrt(const std::complex& z) { const T w = numext::sqrt(T(0.5) * (numext::abs(x) + abs_z)); const T woz = w / abs_z; // Corner cases consistent with 1/sqrt(z) on gcc/clang. - return numext::is_exactly_zero(abs_z) ? std::complex(NumTraits::infinity(), NumTraits::quiet_NaN()) - : ((numext::isinf)(x) || (numext::isinf)(y)) ? std::complex(zero, zero) - : numext::is_exactly_zero(x) ? std::complex(woz, y < zero ? woz : -woz) - : x > zero ? std::complex(woz, -y / (2 * w * abs_z)) - : std::complex(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz); + return numext::is_exactly_zero(abs_z) ? ComplexT(NumTraits::infinity(), NumTraits::quiet_NaN()) + : ((numext::isinf)(x) || (numext::isinf)(y)) ? ComplexT(zero, zero) + : numext::is_exactly_zero(x) ? ComplexT(woz, y < zero ? woz : -woz) + : x > zero ? ComplexT(woz, -y / (2 * w * abs_z)) + : ComplexT(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz); } -template -EIGEN_DEVICE_FUNC std::complex complex_log(const std::complex& z) { +template +EIGEN_DEVICE_FUNC ComplexT complex_log(const ComplexT& z) { // Computes complex log. + using T = typename NumTraits::Real; T a = numext::abs(z); EIGEN_USING_STD(atan2); T b = atan2(z.imag(), z.real()); - return std::complex(numext::log(a), b); + return ComplexT(numext::log(a), b); } +// For generic scalars, use ternary select. +template +struct scalar_select_mask { + static EIGEN_DEVICE_FUNC inline bool run(const Mask& mask) { return numext::is_exactly_zero(mask); } +}; + +// For built-in float mask, bitcast the mask to its integer counterpart and use ternary select. +template +struct scalar_select_mask { + using IntegerType = typename numext::get_integer_by_size::unsigned_type; + static EIGEN_DEVICE_FUNC inline bool run(const Mask& mask) { + return numext::is_exactly_zero(numext::bit_cast(std::abs(mask))); + } +}; + +template +struct ldbl_select_mask { + static constexpr int MantissaDigits = std::numeric_limits::digits; + static constexpr int NumBytes = (MantissaDigits == 64 ? 80 : 128) / CHAR_BIT; + static EIGEN_DEVICE_FUNC inline bool run(const long double& mask) { + const uint8_t* mask_bytes = reinterpret_cast(&mask); + for (Index i = 0; i < NumBytes; i++) { + if (mask_bytes[i] != 0) return false; + } + return true; + } +}; + +template <> +struct ldbl_select_mask : scalar_select_mask {}; + +template <> +struct scalar_select_mask : ldbl_select_mask<> {}; + +template +struct scalar_select_mask, false> { + using impl = scalar_select_mask; + static EIGEN_DEVICE_FUNC inline bool run(const std::complex& mask) { + return impl::run(numext::real(mask)) && impl::run(numext::imag(mask)); + } +}; + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h index 8b7f70c162..a2c8eba574 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h @@ -224,8 +224,6 @@ class Matrix : public PlainObjectBase &other) @@ -255,14 +253,26 @@ class Matrix : public PlainObjectBase::value) { + /** \brief Moves the matrix into the other one. + * + */ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Matrix& operator=(Matrix&& other) noexcept( + std::is_nothrow_move_assignable::value) { Base::operator=(std::move(other)); return *this; } - /** \copydoc PlainObjectBase(const Scalar&, const Scalar&, const Scalar&, const Scalar&, const ArgTypes&... args) + /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients. + * + * \only_for_vectors + * + * This constructor is for 1D array or vectors with more than 4 coefficients. + * + * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this + * constructor must match the the fixed number of rows (resp. columns) of \c *this. + * * * Example: \include Matrix_variadic_ctor_cxx11.cpp * Output: \verbinclude Matrix_variadic_ctor_cxx11.out @@ -276,6 +286,7 @@ class Matrix : public PlainObjectBase EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Matrix(const EigenBase& other) : Base(other.derived()) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return 1; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return 1; } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return this->innerSize(); } /////////// Geometry module /////////// diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h index 81d5a97ea2..8d5c47e472 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h @@ -112,7 +112,7 @@ class MatrixBase : public DenseBase { ConstTransposeReturnType> AdjointReturnType; /** \internal Return type of eigenvalues() */ - typedef Matrix, internal::traits::ColsAtCompileTime, 1, ColMajor> + typedef Matrix, internal::traits::ColsAtCompileTime, 1, ColMajor> EigenvaluesReturnType; /** \internal the return type of identity */ typedef CwiseNullaryOp, PlainObject> IdentityReturnType; @@ -280,7 +280,7 @@ class MatrixBase : public DenseBase { * \sa isApprox(), operator!= */ template EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase& other) const { - return cwiseEqual(other).all(); + return (this->rows() == other.rows()) && (this->cols() == other.cols()) && cwiseEqual(other).all(); } /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other. @@ -289,7 +289,7 @@ class MatrixBase : public DenseBase { * \sa isApprox(), operator== */ template EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase& other) const { - return cwiseNotEqual(other).any(); + return !(*this == other); } NoAlias EIGEN_DEVICE_FUNC noalias(); @@ -468,7 +468,7 @@ class MatrixBase : public DenseBase { EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root) EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm) EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue, pow, power to \c p, const RealScalar& p) - EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex& p) + EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const internal::make_complex_t& p) protected: EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h index ec360ebdea..2ce83a8c56 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h @@ -45,8 +45,8 @@ class NestByValue : public internal::dense_xpr_base EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_expression.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_expression.cols(); } EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h index 67a6c08ae4..5e4e5c2ff6 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h @@ -22,13 +22,13 @@ namespace internal { template ::is_specialized, bool is_integer = NumTraits::IsInteger> struct default_digits_impl { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::digits; } + EIGEN_DEVICE_FUNC constexpr static int run() { return std::numeric_limits::digits; } }; template struct default_digits_impl // Floating point { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + EIGEN_DEVICE_FUNC constexpr static int run() { using std::ceil; using std::log2; typedef typename NumTraits::Real Real; @@ -39,7 +39,7 @@ struct default_digits_impl // Floating point template struct default_digits_impl // Integer { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } + EIGEN_DEVICE_FUNC constexpr static int run() { return 0; } }; // default implementation of digits10(), based on numeric_limits if specialized, @@ -47,13 +47,13 @@ struct default_digits_impl // Integer template ::is_specialized, bool is_integer = NumTraits::IsInteger> struct default_digits10_impl { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::digits10; } + EIGEN_DEVICE_FUNC constexpr static int run() { return std::numeric_limits::digits10; } }; template struct default_digits10_impl // Floating point { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + EIGEN_DEVICE_FUNC constexpr static int run() { using std::floor; using std::log10; typedef typename NumTraits::Real Real; @@ -64,7 +64,7 @@ struct default_digits10_impl // Floating point template struct default_digits10_impl // Integer { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } + EIGEN_DEVICE_FUNC constexpr static int run() { return 0; } }; // default implementation of max_digits10(), based on numeric_limits if specialized, @@ -72,13 +72,13 @@ struct default_digits10_impl // Integer template ::is_specialized, bool is_integer = NumTraits::IsInteger> struct default_max_digits10_impl { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return std::numeric_limits::max_digits10; } + EIGEN_DEVICE_FUNC constexpr static int run() { return std::numeric_limits::max_digits10; } }; template struct default_max_digits10_impl // Floating point { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { + EIGEN_DEVICE_FUNC constexpr static int run() { using std::ceil; using std::log10; typedef typename NumTraits::Real Real; @@ -89,7 +89,7 @@ struct default_max_digits10_impl // Floating point template struct default_max_digits10_impl // Integer { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static int run() { return 0; } + EIGEN_DEVICE_FUNC constexpr static int run() { return 0; } }; } // end namespace internal @@ -115,6 +115,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) { } } // namespace numext +// clang-format off /** \class NumTraits * \ingroup Core_Module * @@ -126,48 +127,50 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) { * * The provided data consists of: * \li A typedef \c Real, giving the "real part" type of \a T. If \a T is already real, - * then \c Real is just a typedef to \a T. If \a T is \c std::complex then \c Real + * then \c Real is just a typedef to \a T. If \a T is `std::complex` then \c Real * is a typedef to \a U. * \li A typedef \c NonInteger, giving the type that should be used for operations producing non-integral values, * such as quotients, square roots, etc. If \a T is a floating-point type, then this typedef just gives * \a T again. Note however that many Eigen functions such as internal::sqrt simply refuse to * take integers. Outside of a few cases, Eigen doesn't do automatic type promotion. Thus, this typedef is * only intended as a helper for code that needs to explicitly promote types. - * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for \c - * std::complex, Literal is defined as \c U. Of course, this type must be fully compatible with \a T. In doubt, just - * use \a T here. \li A typedef \a Nested giving the type to use to nest a value inside of the expression tree. If you - * don't know what this means, just use \a T here. \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c - * std::complex type, and to 0 otherwise. \li An enum value \a IsInteger. It is equal to \c 1 if \a T is an integer type - * such as \c int, and to \c 0 otherwise. \li Enum values ReadCost, AddCost and MulCost representing a rough estimate of - * the number of CPU cycles needed to by move / add / mul instructions respectively, assuming the data is already stored - * in CPU registers. Stay vague here. No need to do architecture-specific stuff. If you don't know what this means, just - * use \c Eigen::HugeCost. \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T - * is unsigned. \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type - * \a T must be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 - * otherwise. \li An epsilon() function which, unlike std::numeric_limits::epsilon(), it returns a - * \a Real instead of a \a T. \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a - * default value by the fuzzy comparison operators. \li highest() and lowest() functions returning the highest and - * lowest possible values respectively. \li digits() function returning the number of radix digits (non-sign digits for - * integers, mantissa for floating-point). This is the analogue of std::numeric_limits::digits which is used - * as the default implementation if specialized. \li digits10() function returning the number of decimal digits that can - * be represented without change. This is the analogue of std::numeric_limits::digits10 which is - * used as the default implementation if specialized. \li max_digits10() function returning the number of decimal digits - * required to uniquely represent all distinct values of the type. This is the analogue of std::numeric_limits::max_digits10 + * \li A typedef \c Literal giving the type to use for numeric literals such as "2" or "0.5". For instance, for + * `std::complex`, Literal is defined as \a U. Of course, this type must be fully compatible with \a T. In doubt, + * just use \a T here. + * \li A typedef \c Nested giving the type to use to nest a value inside of the expression tree. If you don't know what + * this means, just use \a T here. + * \li An enum value \c IsComplex. It is equal to 1 if \a T is a \c std::complex type, and to 0 otherwise. + * \li An enum value \c IsInteger. It is equal to \c 1 if \a T is an integer type such as \c int, and to \c 0 otherwise. + * \li Enum values \c ReadCost, \c AddCost and \c MulCost representing a rough estimate of the number of CPU cycles needed to by + * move / add / mul instructions respectively, assuming the data is already stored in CPU registers. Stay vague here. + * No need to do architecture-specific stuff. If you don't know what this means, just use \c Eigen::HugeCost. + * \li An enum value \c IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T is unsigned. + * \li An enum value \c RequireInitialization. It is equal to \c 1 if the constructor of the numeric type \a T must be + * called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 otherwise. + * \li An epsilon() function which, unlike + * `std::numeric_limits::epsilon()`, it returns a \c Real instead of a \a T. + * \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a default value by the fuzzy + * comparison operators. + * \li highest() and lowest() functions returning the highest and lowest possible values respectively. + * \li digits() function returning the number of radix digits (non-sign digits for integers, mantissa for floating-point). + * This is the analogue of + * `std::numeric_limits::digits` which is used as the default implementation if specialized. + * \li digits10() function returning the number of decimal digits that can be represented without change. This is the + * analogue of + * `std::numeric_limits::digits10` which is used as the default implementation if specialized. + * \li max_digits10() function returning the number of decimal digits required to uniquely represent all distinct values + * of the type. This is the analogue of `std::numeric_limits::max_digits10` * which is used as the default implementation if specialized. * \li min_exponent() and max_exponent() functions returning the highest and lowest possible values, respectively, * such that the radix raised to the power exponent-1 is a normalized floating-point number. These are equivalent - * to std::numeric_limits::min_exponent/ - * std::numeric_limits::max_exponent. + * to + * `std::numeric_limits::min_exponent`/`std::numeric_limits::max_exponent`. * \li infinity() function returning a representation of positive infinity, if available. - * \li quiet_NaN function returning a non-signaling "not-a-number", if available. + * \li quiet_NaN() function returning a non-signaling "not-a-number", if available. */ - +// clang-format on template struct GenericNumTraits { enum { @@ -185,32 +188,30 @@ struct GenericNumTraits { typedef T Nested; typedef T Literal; - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return numext::numeric_limits::epsilon(); } + EIGEN_DEVICE_FUNC constexpr static Real epsilon() { return numext::numeric_limits::epsilon(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return internal::default_digits10_impl::run(); } + EIGEN_DEVICE_FUNC constexpr static int digits10() { return internal::default_digits10_impl::run(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() { - return internal::default_max_digits10_impl::run(); - } + EIGEN_DEVICE_FUNC constexpr static int max_digits10() { return internal::default_max_digits10_impl::run(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits() { return internal::default_digits_impl::run(); } + EIGEN_DEVICE_FUNC constexpr static int digits() { return internal::default_digits_impl::run(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int min_exponent() { return numext::numeric_limits::min_exponent; } + EIGEN_DEVICE_FUNC constexpr static int min_exponent() { return numext::numeric_limits::min_exponent; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_exponent() { return numext::numeric_limits::max_exponent; } + EIGEN_DEVICE_FUNC constexpr static int max_exponent() { return numext::numeric_limits::max_exponent; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() { + EIGEN_DEVICE_FUNC constexpr static Real dummy_precision() { // make sure to override this for floating-point types return Real(0); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T highest() { return (numext::numeric_limits::max)(); } + EIGEN_DEVICE_FUNC constexpr static T highest() { return (numext::numeric_limits::max)(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T lowest() { return (numext::numeric_limits::lowest)(); } + EIGEN_DEVICE_FUNC constexpr static T lowest() { return (numext::numeric_limits::lowest)(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T infinity() { return numext::numeric_limits::infinity(); } + EIGEN_DEVICE_FUNC constexpr static T infinity() { return numext::numeric_limits::infinity(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline T quiet_NaN() { return numext::numeric_limits::quiet_NaN(); } + EIGEN_DEVICE_FUNC constexpr static T quiet_NaN() { return numext::numeric_limits::quiet_NaN(); } }; template @@ -218,25 +219,23 @@ struct NumTraits : GenericNumTraits {}; template <> struct NumTraits : GenericNumTraits { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline float dummy_precision() { return 1e-5f; } + EIGEN_DEVICE_FUNC constexpr static float dummy_precision() { return 1e-5f; } }; template <> struct NumTraits : GenericNumTraits { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline double dummy_precision() { return 1e-12; } + EIGEN_DEVICE_FUNC constexpr static double dummy_precision() { return 1e-12; } }; // GPU devices treat `long double` as `double`. #ifndef EIGEN_GPU_COMPILE_PHASE template <> struct NumTraits : GenericNumTraits { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double dummy_precision() { - return static_cast(1e-15l); - } + EIGEN_DEVICE_FUNC constexpr static long double dummy_precision() { return static_cast(1e-15l); } #if defined(EIGEN_ARCH_PPC) && (__LDBL_MANT_DIG__ == 106) // PowerPC double double causes issues with some values - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline long double epsilon() { + EIGEN_DEVICE_FUNC constexpr static long double epsilon() { // 2^(-(__LDBL_MANT_DIG__)+1) return static_cast(2.4651903288156618919116517665087e-32l); } @@ -257,10 +256,10 @@ struct NumTraits > : GenericNumTraits > MulCost = 4 * NumTraits::MulCost + 2 * NumTraits::AddCost }; - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real epsilon() { return NumTraits::epsilon(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline Real dummy_precision() { return NumTraits::dummy_precision(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int digits10() { return NumTraits::digits10(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline int max_digits10() { return NumTraits::max_digits10(); } + EIGEN_DEVICE_FUNC constexpr static Real epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC constexpr static Real dummy_precision() { return NumTraits::dummy_precision(); } + EIGEN_DEVICE_FUNC constexpr static int digits10() { return NumTraits::digits10(); } + EIGEN_DEVICE_FUNC constexpr static int max_digits10() { return NumTraits::max_digits10(); } }; template @@ -287,25 +286,19 @@ struct NumTraits > { : ArrayType::SizeAtCompileTime * int(NumTraits::MulCost) }; - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar epsilon() { return NumTraits::epsilon(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static inline RealScalar dummy_precision() { - return NumTraits::dummy_precision(); - } + EIGEN_DEVICE_FUNC constexpr static RealScalar epsilon() { return NumTraits::epsilon(); } + EIGEN_DEVICE_FUNC constexpr static RealScalar dummy_precision() { return NumTraits::dummy_precision(); } - EIGEN_CONSTEXPR - static inline int digits10() { return NumTraits::digits10(); } - EIGEN_CONSTEXPR - static inline int max_digits10() { return NumTraits::max_digits10(); } + constexpr static int digits10() { return NumTraits::digits10(); } + constexpr static int max_digits10() { return NumTraits::max_digits10(); } }; template <> struct NumTraits : GenericNumTraits { enum { RequireInitialization = 1, ReadCost = HugeCost, AddCost = HugeCost, MulCost = HugeCost }; - EIGEN_CONSTEXPR - static inline int digits10() { return 0; } - EIGEN_CONSTEXPR - static inline int max_digits10() { return 0; } + constexpr static int digits10() { return 0; } + constexpr static int max_digits10() { return 0; } private: static inline std::string epsilon(); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h index 7b2c8dca38..1f638f9ac5 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h @@ -103,19 +103,36 @@ struct packetwise_redux_impl { EIGEN_DEVICE_FUNC static PacketType run(const Evaluator& eval, const Func& func, Index size) { if (size == 0) return packetwise_redux_empty_value(func); - const Index size4 = (size - 1) & (~3); + const Index size4 = 1 + numext::round_down(size - 1, 4); PacketType p = eval.template packetByOuterInner(0, 0); - Index i = 1; // This loop is optimized for instruction pipelining: // - each iteration generates two independent instructions // - thanks to branch prediction and out-of-order execution we have independent instructions across loops - for (; i < size4; i += 4) + for (Index i = 1; i < size4; i += 4) p = func.packetOp( p, func.packetOp(func.packetOp(eval.template packetByOuterInner(i + 0, 0), eval.template packetByOuterInner(i + 1, 0)), func.packetOp(eval.template packetByOuterInner(i + 2, 0), eval.template packetByOuterInner(i + 3, 0)))); - for (; i < size; ++i) p = func.packetOp(p, eval.template packetByOuterInner(i, 0)); + for (Index i = size4; i < size; ++i) + p = func.packetOp(p, eval.template packetByOuterInner(i, 0)); + return p; + } +}; + +template +struct packetwise_segment_redux_impl { + typedef typename Evaluator::Scalar Scalar; + typedef typename redux_traits::PacketType PacketScalar; + + template + EIGEN_DEVICE_FUNC static PacketType run(const Evaluator& eval, const Func& func, Index size, Index begin, + Index count) { + if (size == 0) return packetwise_redux_empty_value(func); + + PacketType p = eval.template packetSegmentByOuterInner(0, 0, begin, count); + for (Index i = 1; i < size; ++i) + p = func.packetOp(p, eval.template packetSegmentByOuterInner(i, 0, begin, count)); return p; } }; @@ -174,14 +191,13 @@ struct evaluator > template EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC PacketType packet(Index idx) const { - enum { PacketSize = internal::unpacket_traits::size }; - typedef Block - PanelType; - - PanelType panel(m_arg, Direction == Vertical ? 0 : idx, Direction == Vertical ? idx : 0, - Direction == Vertical ? m_arg.rows() : Index(PacketSize), - Direction == Vertical ? Index(PacketSize) : m_arg.cols()); + static constexpr int PacketSize = internal::unpacket_traits::size; + static constexpr int PanelRows = Direction == Vertical ? ArgType::RowsAtCompileTime : PacketSize; + static constexpr int PanelCols = Direction == Vertical ? PacketSize : ArgType::ColsAtCompileTime; + using PanelType = Block; + using PanelEvaluator = typename internal::redux_evaluator; + using BinaryOp = typename MemberOp::BinaryOp; + using Impl = internal::packetwise_redux_impl; // FIXME // See bug 1612, currently if PacketSize==1 (i.e. complex with 128bits registers) then the storage-order of @@ -189,11 +205,39 @@ struct evaluator > // by pass "vectorization" in this case: if (PacketSize == 1) return internal::pset1(coeff(idx)); - typedef typename internal::redux_evaluator PanelEvaluator; + Index startRow = Direction == Vertical ? 0 : idx; + Index startCol = Direction == Vertical ? idx : 0; + Index numRows = Direction == Vertical ? m_arg.rows() : PacketSize; + Index numCols = Direction == Vertical ? PacketSize : m_arg.cols(); + + PanelType panel(m_arg, startRow, startCol, numRows, numCols); PanelEvaluator panel_eval(panel); - typedef typename MemberOp::BinaryOp BinaryOp; - PacketType p = internal::packetwise_redux_impl::template run( - panel_eval, m_functor.binaryFunc(), m_arg.outerSize()); + PacketType p = Impl::template run(panel_eval, m_functor.binaryFunc(), m_arg.outerSize()); + return p; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegment(Index i, Index j, Index begin, Index count) const { + return packetSegment(Direction == Vertical ? j : i, begin, count); + } + + template + EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC PacketType packetSegment(Index idx, Index begin, Index count) const { + static constexpr int PanelRows = Direction == Vertical ? ArgType::RowsAtCompileTime : Dynamic; + static constexpr int PanelCols = Direction == Vertical ? Dynamic : ArgType::ColsAtCompileTime; + using PanelType = Block; + using PanelEvaluator = typename internal::redux_evaluator; + using BinaryOp = typename MemberOp::BinaryOp; + using Impl = internal::packetwise_segment_redux_impl; + + Index startRow = Direction == Vertical ? 0 : idx; + Index startCol = Direction == Vertical ? idx : 0; + Index numRows = Direction == Vertical ? m_arg.rows() : begin + count; + Index numCols = Direction == Vertical ? begin + count : m_arg.cols(); + + PanelType panel(m_arg, startRow, startCol, numRows, numCols); + PanelEvaluator panel_eval(panel); + PacketType p = Impl::template run(panel_eval, m_functor.binaryFunc(), m_arg.outerSize(), begin, count); return p; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h index 8720c4473e..a78305e259 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h @@ -80,27 +80,6 @@ struct matrix_swap_impl; } // end namespace internal -#ifdef EIGEN_PARSED_BY_DOXYGEN -namespace doxygen { - -// This is a workaround to doxygen not being able to understand the inheritance logic -// when it is hidden by the dense_xpr_base helper struct. -// Moreover, doxygen fails to include members that are not documented in the declaration body of -// MatrixBase if we inherits MatrixBase >, -// this is why we simply inherits MatrixBase, though this does not make sense. - -/** This class is just a workaround for Doxygen and it does not not actually exist. */ -template -struct dense_xpr_base_dispatcher; -/** This class is just a workaround for Doxygen and it does not not actually exist. */ -template -struct dense_xpr_base_dispatcher> : public MatrixBase {}; -/** This class is just a workaround for Doxygen and it does not not actually exist. */ -template -struct dense_xpr_base_dispatcher> : public ArrayBase {}; - -} // namespace doxygen - /** \class PlainObjectBase * \ingroup Core_Module * \brief %Dense storage base class for matrices and arrays. @@ -113,12 +92,7 @@ struct dense_xpr_base_dispatcher -class PlainObjectBase : public doxygen::dense_xpr_base_dispatcher -#else -template -class PlainObjectBase : public internal::dense_xpr_base::type -#endif -{ +class PlainObjectBase : public internal::dense_xpr_base::type { public: enum { Options = internal::traits::Options }; typedef typename internal::dense_xpr_base::type Base; @@ -188,8 +162,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_DEVICE_FUNC Base& base() { return *static_cast(this); } EIGEN_DEVICE_FUNC const Base& base() const { return *static_cast(this); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_storage.rows(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_storage.cols(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rows() const noexcept { return m_storage.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index cols() const noexcept { return m_storage.cols(); } /** This is an overloaded version of DenseCoeffsBase::coeff(Index,Index) const * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts. @@ -324,7 +298,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index), resize(NoChange_t, Index), resize(Index, NoChange_t) */ - EIGEN_DEVICE_FUNC inline constexpr void resize(Index size) { + EIGEN_DEVICE_FUNC constexpr void resize(Index size) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase) eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime == Dynamic || size <= MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && @@ -349,7 +323,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - EIGEN_DEVICE_FUNC inline constexpr void resize(NoChange_t, Index cols) { resize(rows(), cols); } + EIGEN_DEVICE_FUNC constexpr void resize(NoChange_t, Index cols) { resize(rows(), cols); } /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special * value \c NoChange as in the example below. @@ -359,7 +333,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - EIGEN_DEVICE_FUNC inline constexpr void resize(Index rows, NoChange_t) { resize(rows, cols()); } + EIGEN_DEVICE_FUNC constexpr void resize(Index rows, NoChange_t) { resize(rows, cols()); } /** Resizes \c *this to have the same dimensions as \a other. * Takes care of doing all the checking that's needed. @@ -473,8 +447,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type // by making all its constructor protected. See bug 1074. protected: EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr PlainObjectBase() = default; + /** \brief Move constructor */ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr PlainObjectBase(PlainObjectBase&&) = default; - EIGEN_DEVICE_FUNC constexpr PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT { + /** \brief Move assignment operator */ + EIGEN_DEVICE_FUNC constexpr PlainObjectBase& operator=(PlainObjectBase&& other) noexcept { m_storage = std::move(other.m_storage); return *this; } @@ -523,7 +499,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type eigen_assert(list_size == static_cast(RowsAtCompileTime) || RowsAtCompileTime == Dynamic); resize(list_size, ColsAtCompileTime); if (list.begin()->begin() != nullptr) { - std::copy(list.begin()->begin(), list.begin()->end(), m_storage.data()); + Index index = 0; + for (const Scalar& e : *list.begin()) { + coeffRef(index++) = e; + } } } else { eigen_assert(list.size() == static_cast(RowsAtCompileTime) || RowsAtCompileTime == Dynamic); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h index 37683e3c27..e16c7cc963 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h @@ -224,8 +224,8 @@ class Product "if you wanted a coeff-wise or a dot product use the respective explicit functions"); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index rows() const noexcept { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index cols() const noexcept { return m_rhs.cols(); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const LhsNestedCleaned& lhs() const { return m_lhs; } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const RhsNestedCleaned& rhs() const { return m_rhs; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h index 77a658a8ef..ce8d954bff 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h @@ -283,7 +283,7 @@ void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs& lhs, cons template struct generic_product_impl { template - struct is_row_major : std::conditional_t<(int(T::Flags) & RowMajorBit), internal::true_type, internal::false_type> {}; + struct is_row_major : bool_constant<(int(T::Flags) & RowMajorBit)> {}; typedef typename Product::Scalar Scalar; // TODO it would be nice to be able to exploit our *_assign_op functors for that purpose @@ -294,6 +294,7 @@ struct generic_product_impl { } }; struct add { + /** Add to dst. */ template EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; @@ -305,9 +306,12 @@ struct generic_product_impl { dst.const_cast_derived() -= src; } }; + /** Scaled add. */ struct adds { Scalar m_scale; + /** Constructor */ explicit adds(const Scalar& s) : m_scale(s) {} + /** Scaled add to dst. */ template void EIGEN_DEVICE_FUNC operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += m_scale * src; @@ -441,7 +445,7 @@ struct generic_product_impl::extract(lhs).template conjugateIf(), blas_traits::extract(rhs).template conjugateIf(), func, actualAlpha, - std::conditional_t()); + bool_constant()); } protected: @@ -631,6 +635,24 @@ struct product_evaluator, ProductTag, DenseShape, return packet(row, col); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packetSegment(Index row, Index col, Index begin, + Index count) const { + PacketType res; + typedef etor_product_packet_impl + PacketImpl; + PacketImpl::run_segment(row, col, m_lhsImpl, m_rhsImpl, m_innerDim, res, begin, count); + return res; + } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packetSegment(Index index, Index begin, Index count) const { + const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? 0 : index; + const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime == 1) ? index : 0; + return packetSegment(row, col, begin, count); + } + protected: add_const_on_value_type_t m_lhs; add_const_on_value_type_t m_rhs; @@ -666,6 +688,13 @@ struct etor_product_packet_impl(lhs.coeff(row, Index(UnrollingIndex - 1))), rhs.template packet(Index(UnrollingIndex - 1), col), res); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res, Index begin, Index count) { + etor_product_packet_impl::run_segment( + row, col, lhs, rhs, innerDim, res, begin, count); + res = pmadd(pset1(lhs.coeff(row, Index(UnrollingIndex - 1))), + rhs.template packetSegment(Index(UnrollingIndex - 1), col, begin, count), res); + } }; template @@ -677,6 +706,13 @@ struct etor_product_packet_impl(row, Index(UnrollingIndex - 1)), pset1(rhs.coeff(Index(UnrollingIndex - 1), col)), res); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res, Index begin, Index count) { + etor_product_packet_impl::run_segment( + row, col, lhs, rhs, innerDim, res, begin, count); + res = pmadd(lhs.template packetSegment(row, Index(UnrollingIndex - 1), begin, count), + pset1(rhs.coeff(Index(UnrollingIndex - 1), col)), res); + } }; template @@ -685,6 +721,12 @@ struct etor_product_packet_impl { Index /*innerDim*/, Packet& res) { res = pmul(pset1(lhs.coeff(row, Index(0))), rhs.template packet(Index(0), col)); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index /*innerDim*/, Packet& res, Index begin, + Index count) { + res = pmul(pset1(lhs.coeff(row, Index(0))), + rhs.template packetSegment(Index(0), col, begin, count)); + } }; template @@ -693,6 +735,12 @@ struct etor_product_packet_impl { Index /*innerDim*/, Packet& res) { res = pmul(lhs.template packet(row, Index(0)), pset1(rhs.coeff(Index(0), col))); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index /*innerDim*/, Packet& res, Index begin, + Index count) { + res = pmul(lhs.template packetSegment(row, Index(0), begin, count), + pset1(rhs.coeff(Index(0), col))); + } }; template @@ -701,6 +749,11 @@ struct etor_product_packet_impl { const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) { res = pset1(typename unpacket_traits::type(0)); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, + const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res, + Index /*begin*/, Index /*count*/) { + res = pset1(typename unpacket_traits::type(0)); + } }; template @@ -709,6 +762,11 @@ struct etor_product_packet_impl { const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res) { res = pset1(typename unpacket_traits::type(0)); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, + const Rhs& /*rhs*/, Index /*innerDim*/, Packet& res, + Index /*begin*/, Index /*count*/) { + res = pset1(typename unpacket_traits::type(0)); + } }; template @@ -719,6 +777,13 @@ struct etor_product_packet_impl { for (Index i = 0; i < innerDim; ++i) res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packet(i, col), res); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res, Index begin, Index count) { + res = pset1(typename unpacket_traits::type(0)); + for (Index i = 0; i < innerDim; ++i) + res = pmadd(pset1(lhs.coeff(row, i)), rhs.template packetSegment(i, col, begin, count), + res); + } }; template @@ -729,6 +794,13 @@ struct etor_product_packet_impl { for (Index i = 0; i < innerDim; ++i) res = pmadd(lhs.template packet(row, i), pset1(rhs.coeff(i, col)), res); } + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run_segment(Index row, Index col, const Lhs& lhs, const Rhs& rhs, + Index innerDim, Packet& res, Index begin, Index count) { + res = pset1(typename unpacket_traits::type(0)); + for (Index i = 0; i < innerDim; ++i) + res = pmadd(lhs.template packetSegment(row, i, begin, count), pset1(rhs.coeff(i, col)), + res); + } }; /*************************************************************************** @@ -867,6 +939,26 @@ struct diagonal_product_evaluator_base : evaluator_base { m_diagImpl.template packet(id)); } + template + EIGEN_STRONG_INLINE PacketType packet_segment_impl(Index row, Index col, Index id, Index begin, Index count, + internal::true_type) const { + return internal::pmul(m_matImpl.template packetSegment(row, col, begin, count), + internal::pset1(m_diagImpl.coeff(id))); + } + + template + EIGEN_STRONG_INLINE PacketType packet_segment_impl(Index row, Index col, Index id, Index begin, Index count, + internal::false_type) const { + enum { + InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, + DiagonalPacketLoadMode = plain_enum_min( + LoadMode, + ((InnerSize % 16) == 0) ? int(Aligned16) : int(evaluator::Alignment)) // FIXME hardcoded 16!! + }; + return internal::pmul(m_matImpl.template packetSegment(row, col, begin, count), + m_diagImpl.template packetSegment(id, begin, count)); + } + evaluator m_diagImpl; evaluator m_matImpl; }; @@ -888,7 +980,8 @@ struct product_evaluator, ProductTag, DiagonalSha typedef typename XprType::PlainObject PlainObject; typedef typename Lhs::DiagonalVectorType DiagonalType; - enum { StorageOrder = Base::StorageOrder_ }; + static constexpr int StorageOrder = Base::StorageOrder_; + using IsRowMajor_t = bool_constant; EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {} @@ -901,8 +994,7 @@ struct product_evaluator, ProductTag, DiagonalSha EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { // FIXME: NVCC used to complain about the template keyword, but we have to check whether this is still the case. // See also similar calls below. - return this->template packet_impl( - row, col, row, std::conditional_t()); + return this->template packet_impl(row, col, row, IsRowMajor_t()); } template @@ -910,6 +1002,19 @@ struct product_evaluator, ProductTag, DiagonalSha return packet(int(StorageOrder) == ColMajor ? idx : 0, int(StorageOrder) == ColMajor ? 0 : idx); } + + template + EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + // FIXME: NVCC used to complain about the template keyword, but we have to check whether this is still the case. + // See also similar calls below. + return this->template packet_segment_impl(row, col, row, begin, count, IsRowMajor_t()); + } + + template + EIGEN_STRONG_INLINE PacketType packetSegment(Index idx, Index begin, Index count) const { + return packetSegment(StorageOrder == ColMajor ? idx : 0, StorageOrder == ColMajor ? 0 : idx, + begin, count); + } #endif }; @@ -929,7 +1034,8 @@ struct product_evaluator, ProductTag, DenseShape, typedef Product XprType; typedef typename XprType::PlainObject PlainObject; - enum { StorageOrder = Base::StorageOrder_ }; + static constexpr int StorageOrder = Base::StorageOrder_; + using IsColMajor_t = bool_constant; EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal()) {} @@ -940,14 +1046,23 @@ struct product_evaluator, ProductTag, DenseShape, #ifndef EIGEN_GPUCC template EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const { - return this->template packet_impl( - row, col, col, std::conditional_t()); + return this->template packet_impl(row, col, col, IsColMajor_t()); } template EIGEN_STRONG_INLINE PacketType packet(Index idx) const { - return packet(int(StorageOrder) == ColMajor ? idx : 0, - int(StorageOrder) == ColMajor ? 0 : idx); + return packet(StorageOrder == ColMajor ? idx : 0, StorageOrder == ColMajor ? 0 : idx); + } + + template + EIGEN_STRONG_INLINE PacketType packetSegment(Index row, Index col, Index begin, Index count) const { + return this->template packet_segment_impl(row, col, col, begin, count, IsColMajor_t()); + } + + template + EIGEN_STRONG_INLINE PacketType packetSegment(Index idx, Index begin, Index count) const { + return packetSegment(StorageOrder == ColMajor ? idx : 0, StorageOrder == ColMajor ? 0 : idx, + begin, count); } #endif }; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h index 76e43f5dc5..efba33680d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/RandomImpl.h @@ -122,7 +122,7 @@ template ::digits != (2 * std::numeric_limits::digits)))> struct random_longdouble_impl { static constexpr int Size = sizeof(long double); - static constexpr EIGEN_DEVICE_FUNC inline int mantissaBits() { return NumTraits::digits() - 1; } + static constexpr EIGEN_DEVICE_FUNC int mantissaBits() { return NumTraits::digits() - 1; } static EIGEN_DEVICE_FUNC inline long double run(int numRandomBits) { eigen_assert(numRandomBits >= 0 && numRandomBits <= mantissaBits()); EIGEN_USING_STD(memcpy); @@ -140,7 +140,7 @@ struct random_longdouble_impl { }; template <> struct random_longdouble_impl { - static constexpr EIGEN_DEVICE_FUNC inline int mantissaBits() { return NumTraits::digits() - 1; } + static constexpr EIGEN_DEVICE_FUNC int mantissaBits() { return NumTraits::digits() - 1; } static EIGEN_DEVICE_FUNC inline long double run(int numRandomBits) { return static_cast(random_float_impl::run(numRandomBits)); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h index 0c5f2d9f6b..4e9ab0e4f8 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h @@ -414,6 +414,13 @@ class redux_evaluator : public internal::evaluator { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetByOuterInner(Index outer, Index inner) const { return Base::template packet(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); } + + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketType packetSegmentByOuterInner(Index outer, Index inner, Index begin, + Index count) const { + return Base::template packetSegment(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer, + begin, count); + } }; } // end namespace internal diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h index 129bc85f46..30ec277d06 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h @@ -73,11 +73,11 @@ class RefBase : public MapBase { typedef MapBase Base; EIGEN_DENSE_PUBLIC_INTERFACE(RefBase) - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const { + EIGEN_DEVICE_FUNC constexpr Index innerStride() const { return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const { + EIGEN_DEVICE_FUNC constexpr Index outerStride() const { return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() : IsVectorAtCompileTime ? this->size() : int(Flags) & RowMajorBit ? this->cols() @@ -97,11 +97,11 @@ class RefBase : public MapBase { typedef Stride StrideBase; // Resolves inner stride if default 0. - static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveInnerStride(Index inner) { return inner == 0 ? 1 : inner; } + static EIGEN_DEVICE_FUNC constexpr Index resolveInnerStride(Index inner) { return inner == 0 ? 1 : inner; } // Resolves outer stride if default 0. - static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols, - bool isVectorAtCompileTime, bool isRowMajor) { + static EIGEN_DEVICE_FUNC constexpr Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols, + bool isVectorAtCompileTime, bool isRowMajor) { return outer == 0 ? isVectorAtCompileTime ? inner * rows * cols : isRowMajor ? inner * cols : inner * rows : outer; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h index 11d7ad19e9..3415045227 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h @@ -85,8 +85,8 @@ class Replicate : public internal::dense_xpr_base : public MapBasecols() : this->rows()) * m_xpr.innerStride(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h index 3b5e470ce4..892c193bd3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h @@ -58,12 +58,8 @@ class ReturnByValue : public internal::dense_xpr_base >:: EIGEN_DEVICE_FUNC inline void evalTo(Dest& dst) const { static_cast(this)->evalTo(dst); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { - return static_cast(this)->rows(); - } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { - return static_cast(this)->cols(); - } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return static_cast(this)->rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return static_cast(this)->cols(); } #ifndef EIGEN_PARSED_BY_DOXYGEN #define Unusable \ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h index eb06fff57f..d11ba16708 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h @@ -87,8 +87,8 @@ class Reverse : public internal::dense_xpr_base > EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse) - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols(); } EIGEN_DEVICE_FUNC inline Index innerStride() const { return -m_matrix.innerStride(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h index 9f4612047a..0fa5f1e178 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h @@ -63,8 +63,8 @@ class Select : public internal::dense_xpr_base EIGEN_DEVICE_FUNC explicit inline SelfAdjointView(MatrixType& matrix) : m_matrix(matrix) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return m_matrix.outerStride(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return m_matrix.innerStride(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return m_matrix.outerStride(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return m_matrix.innerStride(); } /** \sa MatrixBase::coeff() * \warning the coordinates must fit into the referenced triangular part diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h index 4dc92f174e..f73ceb4007 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h @@ -24,20 +24,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(co return derived(); } -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) { - internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other), - internal::add_assign_op()); - return derived(); -} - -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) { - internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other), - internal::sub_assign_op()); - return derived(); -} - template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) { internal::call_assignment(this->derived(), PlainObject::Constant(rows(), cols(), other), diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h index b3fcc3a005..3545afc76c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SkewSymmetricMatrix3.h @@ -66,7 +66,7 @@ class SkewSymmetricBase : public EigenBase { EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const { return derived(); } /** Determinant vanishes */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar determinant() const { return 0; } + EIGEN_DEVICE_FUNC constexpr Scalar determinant() const { return 0; } /** A.transpose() = -A */ EIGEN_DEVICE_FUNC PlainObject transpose() const { return (-vector()).asSkewSymmetric(); } @@ -91,9 +91,9 @@ class SkewSymmetricBase : public EigenBase { EIGEN_DEVICE_FUNC inline SkewSymmetricVectorType& vector() { return derived().vector(); } /** \returns the number of rows. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const { return 3; } + EIGEN_DEVICE_FUNC constexpr Index rows() const { return 3; } /** \returns the number of columns. */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const { return 3; } + EIGEN_DEVICE_FUNC constexpr Index cols() const { return 3; } /** \returns the matrix product of \c *this by the dense matrix, \a matrix */ template @@ -321,7 +321,7 @@ bool MatrixBase::isSkewSymmetric(const RealScalar& prec) const { return (this->transpose() + *this).isZero(prec); } -/** \returns the matrix product of \c *this by the skew symmetric matrix \skew. +/** \returns the matrix product of \c *this by the skew symmetric matrix \a skew. */ template template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h index dfea9c6fb8..aa51410001 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h @@ -66,8 +66,8 @@ class Solve : public SolveImpl inline void evalTo(Dest& dst) const { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h index df2ac83710..5a6dfd425d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h @@ -78,6 +78,14 @@ class SolverBase : public EigenBase { template friend struct internal::solve_assertion; + ComputationInfo info() const { + // CRTP static dispatch: Calls the 'info()' method on the derived class. + // Derived must implement 'ComputationInfo info() const'. + // If not implemented, name lookup falls back to this base method, causing + // infinite recursion (detectable by -Winfinite-recursion). + return derived().info(); + } + enum { RowsAtCompileTime = internal::traits::RowsAtCompileTime, ColsAtCompileTime = internal::traits::ColsAtCompileTime, diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h index 25d4575306..a24d4c236c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h @@ -36,11 +36,11 @@ class indexed_based_stl_iterator_base { typedef Index difference_type; typedef std::random_access_iterator_tag iterator_category; - indexed_based_stl_iterator_base() EIGEN_NO_THROW : mp_xpr(0), m_index(0) {} - indexed_based_stl_iterator_base(XprType& xpr, Index index) EIGEN_NO_THROW : mp_xpr(&xpr), m_index(index) {} + indexed_based_stl_iterator_base() noexcept : mp_xpr(0), m_index(0) {} + indexed_based_stl_iterator_base(XprType& xpr, Index index) noexcept : mp_xpr(&xpr), m_index(index) {} - indexed_based_stl_iterator_base(const non_const_iterator& other) EIGEN_NO_THROW : mp_xpr(other.mp_xpr), - m_index(other.m_index) {} + indexed_based_stl_iterator_base(const non_const_iterator& other) noexcept + : mp_xpr(other.mp_xpr), m_index(other.m_index) {} indexed_based_stl_iterator_base& operator=(const non_const_iterator& other) { mp_xpr = other.mp_xpr; @@ -325,7 +325,7 @@ class pointer_based_stl_iterator { public: typedef Index difference_type; typedef typename XprType::Scalar value_type; -#if EIGEN_CPLUSPLUS >= 202002L +#if EIGEN_COMP_CXXVER >= 20 && defined(__cpp_lib_concepts) && __cpp_lib_concepts >= 202002L typedef std::conditional_t iterator_category; @@ -335,15 +335,14 @@ class pointer_based_stl_iterator { typedef std::conditional_t pointer; typedef std::conditional_t reference; - pointer_based_stl_iterator() EIGEN_NO_THROW : m_ptr(0) {} - pointer_based_stl_iterator(XprType& xpr, Index index) EIGEN_NO_THROW : m_incr(xpr.innerStride()) { + pointer_based_stl_iterator() noexcept : m_ptr(0) {} + pointer_based_stl_iterator(XprType& xpr, Index index) noexcept : m_incr(xpr.innerStride()) { m_ptr = xpr.data() + index * m_incr.value(); } - pointer_based_stl_iterator(const non_const_iterator& other) EIGEN_NO_THROW : m_ptr(other.m_ptr), - m_incr(other.m_incr) {} + pointer_based_stl_iterator(const non_const_iterator& other) noexcept : m_ptr(other.m_ptr), m_incr(other.m_incr) {} - pointer_based_stl_iterator& operator=(const non_const_iterator& other) EIGEN_NO_THROW { + pointer_based_stl_iterator& operator=(const non_const_iterator& other) noexcept { m_ptr = other.m_ptr; m_incr.setValue(other.m_incr); return *this; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h index 14b025c467..692f0a1cab 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h @@ -78,9 +78,9 @@ class Stride { } /** \returns the outer stride */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outer() const { return m_outer.value(); } + EIGEN_DEVICE_FUNC constexpr Index outer() const { return m_outer.value(); } /** \returns the inner stride */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index inner() const { return m_inner.value(); } + EIGEN_DEVICE_FUNC constexpr Index inner() const { return m_inner.value(); } protected: internal::variable_if_dynamic m_outer; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h index d417c1ad1e..dd825e9075 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h @@ -65,6 +65,31 @@ class generic_dense_assignment_kernel(row, col); } + + template + EIGEN_STRONG_INLINE void assignPacketSegment(Index row, Index col, Index begin, Index count) { + PacketType tmp = m_src.template packetSegment(row, col, begin, count); + const_cast(m_src).template writePacketSegment( + row, col, m_dst.template packetSegment(row, col, begin, count), begin, count); + m_dst.template writePacketSegment(row, col, tmp, begin, count); + } + + template + EIGEN_STRONG_INLINE void assignPacketSegment(Index index, Index begin, Index count) { + PacketType tmp = m_src.template packetSegment(index, begin, count); + const_cast(m_src).template writePacketSegment( + index, m_dst.template packetSegment(index, begin, count), begin, count); + m_dst.template writePacketSegment(index, tmp, begin, count); + } + + // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I + // mean no CRTP (Gael) + template + EIGEN_STRONG_INLINE void assignPacketSegmentByOuterInner(Index outer, Index inner, Index begin, Index count) { + Index row = Base::rowIndexByOuterInner(outer, inner); + Index col = Base::colIndexByOuterInner(outer, inner); + assignPacketSegment(row, col, begin, count); + } }; } // namespace internal diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h index 89e3d95fd7..0676a252af 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h @@ -65,8 +65,8 @@ class Transpose : public TransposeImpl& nestedExpression() const { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h index 6fbbbd81b1..f6dd258449 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h @@ -293,9 +293,9 @@ class Transpose > { public: explicit Transpose(const TranspositionType& t) : m_transpositions(t) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_transpositions.size(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_transpositions.size(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_transpositions.size(); } + EIGEN_DEVICE_FUNC constexpr Index size() const noexcept { return m_transpositions.size(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_transpositions.size(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_transpositions.size(); } /** \returns the \a matrix with the inverse transpositions applied to the columns. */ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h index 2b1683be95..27ad78ecaf 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h @@ -58,10 +58,10 @@ class TriangularBase : public EigenBase { eigen_assert(!((int(Mode) & int(UnitDiag)) && (int(Mode) & int(ZeroDiag)))); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return derived().rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return derived().cols(); } + EIGEN_DEVICE_FUNC constexpr Index outerStride() const noexcept { return derived().outerStride(); } + EIGEN_DEVICE_FUNC constexpr Index innerStride() const noexcept { return derived().innerStride(); } // dummy resize function EIGEN_DEVICE_FUNC void resize(Index rows, Index cols) { @@ -194,9 +194,9 @@ class TriangularView EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView) /** \copydoc EigenBase::rows() */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_matrix.rows(); } /** \copydoc EigenBase::cols() */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols(); } /** \returns a const reference to the nested expression */ EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h index 9887db67a4..9ccbf7d768 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h @@ -36,6 +36,7 @@ template class PartialReduxExpr; namespace internal { + template struct traits > : traits { typedef typename MemberOp::result_type Scalar; @@ -63,12 +64,8 @@ class PartialReduxExpr : public internal::dense_xpr_base - EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC - CwiseBinaryOp, const ExpressionTypeNestedCleaned, - const typename ExtendedType::Type> EIGEN_DEVICE_FUNC - operator*(const DenseBase& other) const { + EIGEN_DEVICE_FUNC CwiseBinaryOp, + const ExpressionTypeNestedCleaned, const typename ExtendedType::Type> + operator*(const DenseBase& other) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType) EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived) @@ -619,8 +615,8 @@ class VectorwiseOp { /** Returns the expression where each subvector is the quotient of the corresponding * subvector of \c *this by the vector \a other */ template - EIGEN_DEVICE_FUNC CwiseBinaryOp, const ExpressionTypeNestedCleaned, - const typename ExtendedType::Type> + EIGEN_DEVICE_FUNC CwiseBinaryOp, + const ExpressionTypeNestedCleaned, const typename ExtendedType::Type> operator/(const DenseBase& other) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h index 198ec952ad..0450e2d7f5 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h @@ -25,14 +25,12 @@ struct visitor_impl; template struct short_circuit_eval_impl { // if short circuit evaluation is not used, do nothing - static EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor&) { return false; } + static constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor&) { return false; } }; template struct short_circuit_eval_impl { // if short circuit evaluation is used, check the visitor - static EIGEN_CONSTEXPR EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor& visitor) { - return visitor.done(); - } + static constexpr EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool run(const Visitor& visitor) { return visitor.done(); } }; // unrolled inner-outer traversal @@ -296,9 +294,9 @@ class visitor_evaluator { EIGEN_DEVICE_FUNC explicit visitor_evaluator(const XprType& xpr) : m_evaluator(xpr), m_xpr(xpr) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_xpr.size(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_xpr.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_xpr.cols(); } + EIGEN_DEVICE_FUNC constexpr Index size() const noexcept { return m_xpr.size(); } // outer-inner access EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const { return m_evaluator.coeff(row, col); @@ -632,6 +630,17 @@ struct functor_traits> { }; }; +template ::Scalar>::IsInteger> +struct all_finite_impl { + static EIGEN_DEVICE_FUNC inline bool run(const Derived& /*derived*/) { return true; } +}; +#if !defined(__FINITE_MATH_ONLY__) || !(__FINITE_MATH_ONLY__) +template +struct all_finite_impl { + static EIGEN_DEVICE_FUNC inline bool run(const Derived& derived) { return derived.array().isFiniteTyped().all(); } +}; +#endif + } // end namespace internal /** \fn DenseBase::minCoeff(IndexType* rowId, IndexType* colId) const @@ -781,7 +790,7 @@ EIGEN_DEVICE_FUNC inline bool DenseBase::hasNaN() const { */ template EIGEN_DEVICE_FUNC inline bool DenseBase::allFinite() const { - return derived().array().isFiniteTyped().all(); + return internal::all_finite_impl::run(derived()); } } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h index d5506dae4d..a4a87c4fc6 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h @@ -475,19 +475,11 @@ EIGEN_STRONG_INLINE Packet4cf pmsub(const Packet4cf& a, const Packet4cf& b, cons } template <> EIGEN_STRONG_INLINE Packet4cf pnmadd(const Packet4cf& a, const Packet4cf& b, const Packet4cf& c) { - __m256 a_odd = _mm256_movehdup_ps(a.v); - __m256 a_even = _mm256_moveldup_ps(a.v); - __m256 b_swap = _mm256_permute_ps(b.v, _MM_SHUFFLE(2, 3, 0, 1)); - __m256 result = _mm256_fmaddsub_ps(a_odd, b_swap, _mm256_fmaddsub_ps(a_even, b.v, c.v)); - return Packet4cf(result); + return pnegate(pmsub(a, b, c)); } template <> EIGEN_STRONG_INLINE Packet4cf pnmsub(const Packet4cf& a, const Packet4cf& b, const Packet4cf& c) { - __m256 a_odd = _mm256_movehdup_ps(a.v); - __m256 a_even = _mm256_moveldup_ps(a.v); - __m256 b_swap = _mm256_permute_ps(b.v, _MM_SHUFFLE(2, 3, 0, 1)); - __m256 result = _mm256_fmaddsub_ps(a_odd, b_swap, _mm256_fmsubadd_ps(a_even, b.v, c.v)); - return Packet4cf(result); + return pnegate(pmadd(a, b, c)); } // std::complex template <> @@ -508,21 +500,64 @@ EIGEN_STRONG_INLINE Packet2cd pmsub(const Packet2cd& a, const Packet2cd& b, cons } template <> EIGEN_STRONG_INLINE Packet2cd pnmadd(const Packet2cd& a, const Packet2cd& b, const Packet2cd& c) { - __m256d a_odd = _mm256_permute_pd(a.v, 0xF); - __m256d a_even = _mm256_movedup_pd(a.v); - __m256d b_swap = _mm256_permute_pd(b.v, 0x5); - __m256d result = _mm256_fmaddsub_pd(a_odd, b_swap, _mm256_fmaddsub_pd(a_even, b.v, c.v)); - return Packet2cd(result); + return pnegate(pmsub(a, b, c)); } template <> EIGEN_STRONG_INLINE Packet2cd pnmsub(const Packet2cd& a, const Packet2cd& b, const Packet2cd& c) { - __m256d a_odd = _mm256_permute_pd(a.v, 0xF); - __m256d a_even = _mm256_movedup_pd(a.v); - __m256d b_swap = _mm256_permute_pd(b.v, 0x5); - __m256d result = _mm256_fmaddsub_pd(a_odd, b_swap, _mm256_fmsubadd_pd(a_even, b.v, c.v)); - return Packet2cd(result); + return pnegate(pmadd(a, b, c)); } #endif + +/*---------------- load/store segment support ----------------*/ + +/*---------------- std::complex ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet2cf ploaduSegment(const std::complex* from, Index begin, Index count) { + return (Packet2cf)_mm_maskload_ps(&numext::real_ref(*from), segment_mask_2x64(begin, count)); +} + +template <> +inline void pstoreuSegment, Packet2cf>(std::complex* to, const Packet2cf& from, Index begin, + Index count) { + _mm_maskstore_ps(&numext::real_ref(*to), segment_mask_2x64(begin, count), from.v); +} + +template <> +inline Packet4cf ploaduSegment(const std::complex* from, Index begin, Index count) { + return (Packet4cf)_mm256_maskload_ps(&numext::real_ref(*from), segment_mask_4x64(begin, count)); +} + +template <> +inline void pstoreuSegment, Packet4cf>(std::complex* to, const Packet4cf& from, Index begin, + Index count) { + _mm256_maskstore_ps(&numext::real_ref(*to), segment_mask_4x64(begin, count), from.v); +} + +/*---------------- std::complex ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet2cd ploaduSegment(const std::complex* from, Index begin, Index count) { + return (Packet2cd)_mm256_maskload_pd(&numext::real_ref(*from), segment_mask_4x64(2 * begin, 2 * count)); +} + +template <> +inline void pstoreuSegment, Packet2cd>(std::complex* to, const Packet2cd& from, + Index begin, Index count) { + _mm256_maskstore_pd(&numext::real_ref(*to), segment_mask_4x64(2 * begin, 2 * count), from.v); +} + +/*---------------- end load/store segment support ----------------*/ + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h index a5c38e7878..5b7285f99b 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h @@ -28,6 +28,7 @@ EIGEN_DOUBLE_PACKET_FUNCTION(log, Packet4d) EIGEN_DOUBLE_PACKET_FUNCTION(log2, Packet4d) EIGEN_DOUBLE_PACKET_FUNCTION(exp, Packet4d) EIGEN_DOUBLE_PACKET_FUNCTION(tanh, Packet4d) +EIGEN_DOUBLE_PACKET_FUNCTION(cbrt, Packet4d) #ifdef EIGEN_VECTORIZE_AVX2 EIGEN_DOUBLE_PACKET_FUNCTION(sin, Packet4d) EIGEN_DOUBLE_PACKET_FUNCTION(cos, Packet4d) @@ -106,6 +107,8 @@ BF16_PACKET_FUNCTION(Packet8f, Packet8bf, prsqrt) BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psin) BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psqrt) BF16_PACKET_FUNCTION(Packet8f, Packet8bf, ptanh) + +#ifndef EIGEN_VECTORIZE_AVX512FP16 F16_PACKET_FUNCTION(Packet8f, Packet8h, pcos) F16_PACKET_FUNCTION(Packet8f, Packet8h, pexp) F16_PACKET_FUNCTION(Packet8f, Packet8h, pexp2) @@ -118,6 +121,7 @@ F16_PACKET_FUNCTION(Packet8f, Packet8h, prsqrt) F16_PACKET_FUNCTION(Packet8f, Packet8h, psin) F16_PACKET_FUNCTION(Packet8f, Packet8h, psqrt) F16_PACKET_FUNCTION(Packet8f, Packet8h, ptanh) +#endif } // end namespace internal diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h index 1980e928be..470e36d8d9 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h @@ -122,6 +122,7 @@ struct packet_traits : default_packet_traits { HasBessel = 1, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasTanh = EIGEN_FAST_MATH, HasErf = EIGEN_FAST_MATH, HasErfc = EIGEN_FAST_MATH, @@ -145,10 +146,12 @@ struct packet_traits : default_packet_traits { #endif HasTanh = EIGEN_FAST_MATH, HasLog = 1, + HasErf = 1, HasErfc = 1, HasExp = 1, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasATan = 1, HasATanh = 1, HasBlend = 1 @@ -1838,10 +1841,13 @@ EIGEN_STRONG_INLINE Packet8ui pabs(const Packet8ui& a) { return a; } +#ifndef EIGEN_VECTORIZE_AVX512FP16 template <> EIGEN_STRONG_INLINE Packet8h psignbit(const Packet8h& a) { return _mm_cmpgt_epi16(_mm_setzero_si128(), a); } +#endif // EIGEN_VECTORIZE_AVX512FP16 + template <> EIGEN_STRONG_INLINE Packet8bf psignbit(const Packet8bf& a) { return _mm_cmpgt_epi16(_mm_setzero_si128(), a); @@ -1933,6 +1939,22 @@ EIGEN_STRONG_INLINE Packet4d pldexp(const Packet4d& a, const Packet4d& return out; } +template <> +EIGEN_STRONG_INLINE Packet4d pldexp_fast(const Packet4d& a, const Packet4d& exponent) { + // Clamp exponent to [-1024, 1024] + const Packet4d min_exponent = pset1(-1023.0); + const Packet4d max_exponent = pset1(1024.0); + const Packet4i e = _mm256_cvtpd_epi32(pmin(pmax(exponent, min_exponent), max_exponent)); + const Packet4i bias = pset1(1023); + + // 2^e + Packet4i hi = vec4i_swizzle1(padd(e, bias), 0, 2, 1, 3); + const Packet4i lo = _mm_slli_epi64(hi, 52); + hi = _mm_slli_epi64(_mm_srli_epi64(hi, 32), 52); + const Packet4d c = _mm256_castsi256_pd(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1)); + return pmul(a, c); // a * 2^e +} + template <> EIGEN_STRONG_INLINE float predux(const Packet8f& a) { return predux(Packet4f(_mm_add_ps(_mm256_castps256_ps128(a), _mm256_extractf128_ps(a, 1)))); @@ -2027,10 +2049,13 @@ EIGEN_STRONG_INLINE bool predux_any(const Packet8ui& x) { return _mm256_movemask_ps(_mm256_castsi256_ps(x)) != 0; } +#ifndef EIGEN_VECTORIZE_AVX512FP16 template <> EIGEN_STRONG_INLINE bool predux_any(const Packet8h& x) { return _mm_movemask_epi8(x) != 0; } +#endif // EIGEN_VECTORIZE_AVX512FP16 + template <> EIGEN_STRONG_INLINE bool predux_any(const Packet8bf& x) { return _mm_movemask_epi8(x) != 0; @@ -2194,7 +2219,6 @@ struct unpacket_traits { }; typedef Packet8h half; }; -#endif template <> EIGEN_STRONG_INLINE Packet8h pset1(const Eigen::half& from) { @@ -2393,6 +2417,26 @@ EIGEN_STRONG_INLINE Packet8h pmul(const Packet8h& a, const Packet8h& b return float2half(rf); } +template <> +EIGEN_STRONG_INLINE Packet8h pmadd(const Packet8h& a, const Packet8h& b, const Packet8h& c) { + return float2half(pmadd(half2float(a), half2float(b), half2float(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8h pmsub(const Packet8h& a, const Packet8h& b, const Packet8h& c) { + return float2half(pmsub(half2float(a), half2float(b), half2float(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8h pnmadd(const Packet8h& a, const Packet8h& b, const Packet8h& c) { + return float2half(pnmadd(half2float(a), half2float(b), half2float(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8h pnmsub(const Packet8h& a, const Packet8h& b, const Packet8h& c) { + return float2half(pnmsub(half2float(a), half2float(b), half2float(c))); +} + template <> EIGEN_STRONG_INLINE Packet8h pdiv(const Packet8h& a, const Packet8h& b) { Packet8f af = half2float(a); @@ -2429,14 +2473,12 @@ EIGEN_STRONG_INLINE void pscatter(Eigen::half* to, const to[stride * 7] = aux[7]; } -#ifndef EIGEN_VECTORIZE_AVX512FP16 template <> EIGEN_STRONG_INLINE Eigen::half predux(const Packet8h& a) { Packet8f af = half2float(a); float reduced = predux(af); return Eigen::half(reduced); } -#endif template <> EIGEN_STRONG_INLINE Eigen::half predux_max(const Packet8h& a) { @@ -2536,6 +2578,8 @@ EIGEN_STRONG_INLINE void ptranspose(PacketBlock& kernel) { kernel.packet[3] = pload(out[3]); } +#endif + // BFloat16 implementation. EIGEN_STRONG_INLINE Packet8f Bf16ToF32(const Packet8bf& a) { @@ -2763,6 +2807,26 @@ EIGEN_STRONG_INLINE Packet8bf pmul(const Packet8bf& a, const Packet8b return F32ToBf16(pmul(Bf16ToF32(a), Bf16ToF32(b))); } +template <> +EIGEN_STRONG_INLINE Packet8bf pmadd(const Packet8bf& a, const Packet8bf& b, const Packet8bf& c) { + return F32ToBf16(pmadd(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8bf pmsub(const Packet8bf& a, const Packet8bf& b, const Packet8bf& c) { + return F32ToBf16(pmsub(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8bf pnmadd(const Packet8bf& a, const Packet8bf& b, const Packet8bf& c) { + return F32ToBf16(pnmadd(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet8bf pnmsub(const Packet8bf& a, const Packet8bf& b, const Packet8bf& c) { + return F32ToBf16(pnmsub(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + template <> EIGEN_STRONG_INLINE Packet8bf pdiv(const Packet8bf& a, const Packet8bf& b) { return F32ToBf16(pdiv(Bf16ToF32(a), Bf16ToF32(b))); @@ -2876,6 +2940,258 @@ EIGEN_STRONG_INLINE void ptranspose(PacketBlock& kernel) { kernel.packet[3] = _mm_unpackhi_epi32(ab_47, cd_47); } +/*---------------- load/store segment support ----------------*/ + +// returns a mask of 8-bit elements (at most 4) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m128i segment_mask_4x8(Index begin, Index count) { + eigen_assert(begin >= 0 && begin + count <= 4); + long long mask = 1; + mask <<= CHAR_BIT * count; + mask--; + mask <<= CHAR_BIT * begin; +#if defined(_WIN32) && !defined(_WIN64) + return _mm_loadl_epi64(reinterpret_cast(&mask)); +#else + return _mm_cvtsi64_si128(mask); +#endif +} + +// returns a mask of 8-bit elements (at most 8) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m128i segment_mask_8x8(Index begin, Index count) { + eigen_assert(begin >= 0 && begin + count <= 8); + long long mask = 1; + // avoid UB when count == 8 + mask <<= (CHAR_BIT / 2) * count; + mask <<= (CHAR_BIT / 2) * count; + mask--; + mask <<= CHAR_BIT * begin; +#if defined(_WIN32) && !defined(_WIN64) + return _mm_loadl_epi64(reinterpret_cast(&mask)); +#else + return _mm_cvtsi64_si128(mask); +#endif +} + +// returns a mask of 32-bit elements (at most 4) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m128i segment_mask_4x32(Index begin, Index count) { + eigen_assert(begin >= 0 && begin + count <= 4); + return _mm_cvtepi8_epi32(segment_mask_4x8(begin, count)); +} + +// returns a mask of 64-bit elements (at most 2) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m128i segment_mask_2x64(Index begin, Index count) { + eigen_assert(begin >= 0 && begin + count <= 2); + return _mm_cvtepi8_epi64(segment_mask_4x8(begin, count)); +} + +// returns a mask of 32-bit elements (at most 8) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m256i segment_mask_8x32(Index begin, Index count) { + __m128i mask_epi8 = segment_mask_8x8(begin, count); +#ifdef EIGEN_VECTORIZE_AVX2 + __m256i mask_epi32 = _mm256_cvtepi8_epi32(mask_epi8); +#else + __m128i mask_epi32_lo = _mm_cvtepi8_epi32(mask_epi8); + __m128i mask_epi32_hi = _mm_cvtepi8_epi32(_mm_srli_epi64(mask_epi8, 32)); + __m256i mask_epi32 = _mm256_insertf128_si256(_mm256_castsi128_si256(mask_epi32_lo), mask_epi32_hi, 1); +#endif + return mask_epi32; +} + +// returns a mask of 64-bit elements (at most 4) that are all 1's in the range [begin, begin + count) and 0 elsewhere. +inline __m256i segment_mask_4x64(Index begin, Index count) { + __m128i mask_epi8 = segment_mask_4x8(begin, count); +#ifdef EIGEN_VECTORIZE_AVX2 + __m256i mask_epi64 = _mm256_cvtepi8_epi64(mask_epi8); +#else + __m128i mask_epi64_lo = _mm_cvtepi8_epi64(mask_epi8); + __m128i mask_epi64_hi = _mm_cvtepi8_epi64(_mm_srli_epi64(mask_epi8, 16)); + __m256i mask_epi64 = _mm256_insertf128_si256(_mm256_castsi128_si256(mask_epi64_lo), mask_epi64_hi, 1); +#endif + return mask_epi64; +} + +/*---------------- float ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet4f ploaduSegment(const float* from, Index begin, Index count) { + return _mm_maskload_ps(from, segment_mask_4x32(begin, count)); +} + +template <> +inline void pstoreuSegment(float* to, const Packet4f& from, Index begin, Index count) { + _mm_maskstore_ps(to, segment_mask_4x32(begin, count), from); +} + +template <> +inline Packet8f ploaduSegment(const float* from, Index begin, Index count) { + return _mm256_maskload_ps(from, segment_mask_8x32(begin, count)); +} + +template <> +inline void pstoreuSegment(float* to, const Packet8f& from, Index begin, Index count) { + _mm256_maskstore_ps(to, segment_mask_8x32(begin, count), from); +} + +/*---------------- int32 ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +#ifdef EIGEN_VECTORIZE_AVX2 + +template <> +inline Packet4i ploaduSegment(const int* from, Index begin, Index count) { + return _mm_maskload_epi32(from, segment_mask_4x32(begin, count)); +} + +template <> +inline void pstoreuSegment(int* to, const Packet4i& from, Index begin, Index count) { + _mm_maskstore_epi32(to, segment_mask_4x32(begin, count), from); +} + +template <> +inline Packet8i ploaduSegment(const int* from, Index begin, Index count) { + return _mm256_maskload_epi32(from, segment_mask_8x32(begin, count)); +} + +template <> +inline void pstoreuSegment(int* to, const Packet8i& from, Index begin, Index count) { + _mm256_maskstore_epi32(to, segment_mask_8x32(begin, count), from); +} + +#else + +template <> +inline Packet4i ploaduSegment(const int* from, Index begin, Index count) { + return _mm_castps_si128(ploaduSegment(reinterpret_cast(from), begin, count)); +} + +template <> +inline void pstoreuSegment(int* to, const Packet4i& from, Index begin, Index count) { + pstoreuSegment(reinterpret_cast(to), _mm_castsi128_ps(from), begin, count); +} + +template <> +inline Packet8i ploaduSegment(const int* from, Index begin, Index count) { + return _mm256_castps_si256(ploaduSegment(reinterpret_cast(from), begin, count)); +} + +template <> +inline void pstoreuSegment(int* to, const Packet8i& from, Index begin, Index count) { + pstoreuSegment(reinterpret_cast(to), _mm256_castsi256_ps(from), begin, count); +} + +#endif + +/*---------------- uint32 ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet4ui ploaduSegment(const uint32_t* from, Index begin, Index count) { + return Packet4ui(ploaduSegment(reinterpret_cast(from), begin, count)); +} + +template <> +inline void pstoreuSegment(uint32_t* to, const Packet4ui& from, Index begin, Index count) { + pstoreuSegment(reinterpret_cast(to), Packet4i(from), begin, count); +} + +template <> +inline Packet8ui ploaduSegment(const uint32_t* from, Index begin, Index count) { + return Packet8ui(ploaduSegment(reinterpret_cast(from), begin, count)); +} + +template <> +inline void pstoreuSegment(uint32_t* to, const Packet8ui& from, Index begin, Index count) { + pstoreuSegment(reinterpret_cast(to), Packet8i(from), begin, count); +} + +/*---------------- double ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet2d ploaduSegment(const double* from, Index begin, Index count) { + return _mm_maskload_pd(from, segment_mask_2x64(begin, count)); +} + +template <> +inline void pstoreuSegment(double* to, const Packet2d& from, Index begin, Index count) { + _mm_maskstore_pd(to, segment_mask_2x64(begin, count), from); +} + +template <> +inline Packet4d ploaduSegment(const double* from, Index begin, Index count) { + return _mm256_maskload_pd(from, segment_mask_4x64(begin, count)); +} + +template <> +inline void pstoreuSegment(double* to, const Packet4d& from, Index begin, Index count) { + _mm256_maskstore_pd(to, segment_mask_4x64(begin, count), from); +} + +#ifdef EIGEN_VECTORIZE_AVX2 + +/*---------------- int64_t ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet2l ploaduSegment(const int64_t* from, Index begin, Index count) { + return _mm_maskload_epi64(reinterpret_cast(from), segment_mask_2x64(begin, count)); +} +template <> +inline void pstoreuSegment(int64_t* to, const Packet2l& from, Index begin, Index count) { + _mm_maskstore_epi64(reinterpret_cast(to), segment_mask_2x64(begin, count), from); +} +template <> +inline Packet4l ploaduSegment(const int64_t* from, Index begin, Index count) { + return _mm256_maskload_epi64(reinterpret_cast(from), segment_mask_4x64(begin, count)); +} +template <> +inline void pstoreuSegment(int64_t* to, const Packet4l& from, Index begin, Index count) { + _mm256_maskstore_epi64(reinterpret_cast(to), segment_mask_4x64(begin, count), from); +} + +/*---------------- uint64_t ----------------*/ + +template <> +struct has_packet_segment : std::true_type {}; + +template <> +inline Packet4ul ploaduSegment(const uint64_t* from, Index begin, Index count) { + return Packet4ul(ploaduSegment(reinterpret_cast(from), begin, count)); +} +template <> +inline void pstoreuSegment(uint64_t* to, const Packet4ul& from, Index begin, Index count) { + pstoreuSegment(reinterpret_cast(to), Packet4l(from), begin, count); +} +#endif + +/*---------------- end load/store segment support ----------------*/ + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h index 9dcd6ef844..5b73ffe860 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h @@ -279,20 +279,22 @@ EIGEN_STRONG_INLINE Packet2l preinterpret(const Packet4l& a) } #endif +#ifndef EIGEN_VECTORIZE_AVX512FP16 template <> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8h& a) { return half2float(a); } -template <> -EIGEN_STRONG_INLINE Packet8f pcast(const Packet8bf& a) { - return Bf16ToF32(a); -} - template <> EIGEN_STRONG_INLINE Packet8h pcast(const Packet8f& a) { return float2half(a); } +#endif + +template <> +EIGEN_STRONG_INLINE Packet8f pcast(const Packet8bf& a) { + return Bf16ToF32(a); +} template <> EIGEN_STRONG_INLINE Packet8bf pcast(const Packet8f& a) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h index b8d3b4f69c..f2e55f3458 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h @@ -673,6 +673,11 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmax(const bfloat16& a, const bfl return bfloat16(::fmaxf(f1, f2)); } +EIGEN_DEVICE_FUNC inline bfloat16 fma(const bfloat16& a, const bfloat16& b, const bfloat16& c) { + // Emulate FMA via float. + return bfloat16(numext::fma(static_cast(a), static_cast(b), static_cast(c))); +} + #ifndef EIGEN_NO_IO EIGEN_ALWAYS_INLINE std::ostream& operator<<(std::ostream& os, const bfloat16& v) { os << static_cast(v); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h index 4e441b498d..e9f564b537 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h @@ -187,7 +187,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic(const Packet& a, Pac static constexpr int TotalBits = sizeof(Scalar) * CHAR_BIT, MantissaBits = numext::numeric_limits::digits - 1, ExponentBits = TotalBits - MantissaBits - 1; - EIGEN_CONSTEXPR ScalarUI scalar_sign_mantissa_mask = + constexpr ScalarUI scalar_sign_mantissa_mask = ~(((ScalarUI(1) << ExponentBits) - ScalarUI(1)) << MantissaBits); // ~0x7f800000 const Packet sign_mantissa_mask = pset1frombits(static_cast(scalar_sign_mantissa_mask)); const Packet half = pset1(Scalar(0.5)); @@ -196,7 +196,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic(const Packet& a, Pac // To handle denormals, normalize by multiplying by 2^(int(MantissaBits)+1). const Packet is_denormal = pcmp_lt(pabs(a), normal_min); - EIGEN_CONSTEXPR ScalarUI scalar_normalization_offset = ScalarUI(MantissaBits + 1); // 24 + constexpr ScalarUI scalar_normalization_offset = ScalarUI(MantissaBits + 1); // 24 // The following cannot be constexpr because bfloat16(uint16_t) is not constexpr. const Scalar scalar_normalization_factor = Scalar(ScalarUI(1) << int(scalar_normalization_offset)); // 2^24 const Packet normalization_factor = pset1(scalar_normalization_factor); @@ -274,22 +274,156 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_generic(const Packet& a, con // // Assumes IEEE floating point format template -struct pldexp_fast_impl { +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_fast(const Packet& a, const Packet& exponent) { typedef typename unpacket_traits::integer_packet PacketI; typedef typename unpacket_traits::type Scalar; typedef typename unpacket_traits::type ScalarI; static constexpr int TotalBits = sizeof(Scalar) * CHAR_BIT, MantissaBits = numext::numeric_limits::digits - 1, ExponentBits = TotalBits - MantissaBits - 1; - static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet run(const Packet& a, const Packet& exponent) { - const Packet bias = pset1(Scalar((ScalarI(1) << (ExponentBits - 1)) - ScalarI(1))); // 127 - const Packet limit = pset1(Scalar((ScalarI(1) << ExponentBits) - ScalarI(1))); // 255 - // restrict biased exponent between 0 and 255 for float. - const PacketI e = pcast(pmin(pmax(padd(exponent, bias), pzero(limit)), limit)); // exponent + 127 - // return a * (2^e) - return pmul(a, preinterpret(plogical_shift_left(e))); - } -}; + const Packet bias = pset1(Scalar((ScalarI(1) << (ExponentBits - 1)) - ScalarI(1))); // 127 + const Packet limit = pset1(Scalar((ScalarI(1) << ExponentBits) - ScalarI(1))); // 255 + // restrict biased exponent between 0 and 255 for float. + const PacketI e = pcast(pmin(pmax(padd(exponent, bias), pzero(limit)), limit)); // exponent + 127 + // return a * (2^e) + return pmul(a, preinterpret(plogical_shift_left(e))); +} + +// This function implements a single step of Halley's iteration for +// computing x = y^(1/3): +// x_{k+1} = x_k - (x_k^3 - y) x_k / (2x_k^3 + y) +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet cbrt_halley_iteration_step(const Packet& x_k, + const Packet& y) { + typedef typename unpacket_traits::type Scalar; + Packet x_k_cb = pmul(x_k, pmul(x_k, x_k)); + Packet denom = pmadd(pset1(Scalar(2)), x_k_cb, y); + Packet num = psub(x_k_cb, y); + Packet r = pdiv(num, denom); + return pnmadd(x_k, r, x_k); +} + +// Decompose the input such that x^(1/3) = y^(1/3) * 2^e_div3, and y is in the +// interval [0.125,1]. +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet cbrt_decompose(const Packet& x, Packet& e_div3) { + typedef typename unpacket_traits::type Scalar; + // Extract the significant s in the range [0.5,1) and exponent e, such that + // x = 2^e * s. + Packet e, s; + s = pfrexp(x, e); + + // Split the exponent into a part divisible by 3 and the remainder. + // e = 3*e_div3 + e_mod3. + constexpr Scalar kOneThird = Scalar(1) / 3; + e_div3 = pceil(pmul(e, pset1(kOneThird))); + Packet e_mod3 = pnmadd(pset1(Scalar(3)), e_div3, e); + + // Replace s by y = (s * 2^e_mod3). + return pldexp_fast(s, e_mod3); +} + +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet cbrt_special_cases_and_sign(const Packet& x, + const Packet& abs_root) { + typedef typename unpacket_traits::type Scalar; + + // Set sign. + const Packet sign_mask = pset1(Scalar(-0.0)); + const Packet x_sign = pand(sign_mask, x); + Packet root = por(x_sign, abs_root); + + // Pass non-finite and zero values of x straight through. + const Packet is_not_finite = por(pisinf(x), pisnan(x)); + const Packet is_zero = pcmp_eq(pzero(x), x); + const Packet use_x = por(is_not_finite, is_zero); + return pselect(use_x, x, root); +} + +// Generic implementation of cbrt(x) for float. +// +// The algorithm computes the cubic root of the input by first +// decomposing it into a exponent and significant +// x = s * 2^e. +// +// We can then write the cube root as +// +// x^(1/3) = 2^(e/3) * s^(1/3) +// = 2^((3*e_div3 + e_mod3)/3) * s^(1/3) +// = 2^(e_div3) * 2^(e_mod3/3) * s^(1/3) +// = 2^(e_div3) * (s * 2^e_mod3)^(1/3) +// +// where e_div3 = ceil(e/3) and e_mod3 = e - 3*e_div3. +// +// The cube root of the second term y = (s * 2^e_mod3)^(1/3) is coarsely +// approximated using a cubic polynomial and subsequently refined using a +// single step of Halley's iteration, and finally the two terms are combined +// using pldexp_fast. +// +// Note: Many alternatives exist for implementing cbrt. See, for example, +// the excellent discussion in Kahan's note: +// https://csclub.uwaterloo.ca/~pbarfuss/qbrt.pdf +// This particular implementation was found to be very fast and accurate +// among several alternatives tried, but is probably not "optimal" on all +// platforms. +// +// This is accurate to 2 ULP. +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt_float(const Packet& x) { + typedef typename unpacket_traits::type Scalar; + static_assert(std::is_same::value, "Scalar type must be float"); + + // Decompose the input such that x^(1/3) = y^(1/3) * 2^e_div3, and y is in the + // interval [0.125,1]. + Packet e_div3; + const Packet y = cbrt_decompose(pabs(x), e_div3); + + // Compute initial approximation accurate to 5.22e-3. + // The polynomial was computed using Rminimax. + constexpr float alpha[] = {5.9220016002655029296875e-01f, -1.3859539031982421875e+00f, 1.4581282138824462890625e+00f, + 3.408401906490325927734375e-01f}; + Packet r = ppolevl::run(y, alpha); + + // Take one step of Halley's iteration. + r = cbrt_halley_iteration_step(r, y); + + // Finally multiply by 2^(e_div3) + r = pldexp_fast(r, e_div3); + + return cbrt_special_cases_and_sign(x, r); +} + +// Generic implementation of cbrt(x) for double. +// +// The algorithm is identical to the one for float except that a different initial +// approximation is used for y^(1/3) and two Halley iteration steps are peformed. +// +// This is accurate to 1 ULP. +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt_double(const Packet& x) { + typedef typename unpacket_traits::type Scalar; + static_assert(std::is_same::value, "Scalar type must be double"); + + // Decompose the input such that x^(1/3) = y^(1/3) * 2^e_div3, and y is in the + // interval [0.125,1]. + Packet e_div3; + const Packet y = cbrt_decompose(pabs(x), e_div3); + + // Compute initial approximation accurate to 0.016. + // The polynomial was computed using Rminimax. + constexpr double alpha[] = {-4.69470621553356115551736138513660989701747894287109375e-01, + 1.072314636518546304699839311069808900356292724609375e+00, + 3.81249427609571867048288140722434036433696746826171875e-01}; + Packet r = ppolevl::run(y, alpha); + + // Take two steps of Halley's iteration. + r = cbrt_halley_iteration_step(r, y); + r = cbrt_halley_iteration_step(r, y); + + // Finally multiply by 2^(e_div3). + r = pldexp_fast(r, e_div3); + return cbrt_special_cases_and_sign(x, r); +} // Natural or base 2 logarithm. // Computes log(x) as log(2^e * m) = C*e + log(m), where the constant C =log(2) @@ -514,6 +648,7 @@ EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_float(const Pack const Packet cst_half = pset1(0.5f); const Packet cst_exp_hi = pset1(88.723f); const Packet cst_exp_lo = pset1(-104.f); + const Packet cst_pldexp_threshold = pset1(87.0); const Packet cst_cephes_LOG2EF = pset1(1.44269504088896341f); const Packet cst_p2 = pset1(0.49999988079071044921875f); @@ -549,7 +684,12 @@ EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_float(const Pack y = pmadd(r2, y, p_low); // Return 2^m * exp(r). - // TODO: replace pldexp with faster implementation since y in [-1, 1). + const Packet fast_pldexp_unsafe = pcmp_lt(cst_pldexp_threshold, pabs(x)); + if (!predux_any(fast_pldexp_unsafe)) { + // For |x| <= 87, we know the result is not zero or inf, and we can safely use + // the fast version of pldexp. + return pmax(pldexp_fast(y, m), _x); + } return pselect(zero_mask, cst_zero, pmax(pldexp(y, m), _x)); } @@ -562,8 +702,8 @@ EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_double(const Pac const Packet cst_half = pset1(0.5); const Packet cst_exp_hi = pset1(709.784); - const Packet cst_exp_lo = pset1(-709.784); - + const Packet cst_exp_lo = pset1(-745.519); + const Packet cst_pldexp_threshold = pset1(708.0); const Packet cst_cephes_LOG2EF = pset1(1.4426950408889634073599); const Packet cst_cephes_exp_p0 = pset1(1.26177193074810590878e-4); const Packet cst_cephes_exp_p1 = pset1(3.02994407707441961300e-2); @@ -616,7 +756,12 @@ EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pexp_double(const Pac // Construct the result 2^n * exp(g) = e * x. The max is used to catch // non-finite values in the input. - // TODO: replace pldexp with faster implementation since x in [-1, 1). + const Packet fast_pldexp_unsafe = pcmp_lt(cst_pldexp_threshold, pabs(_x)); + if (!predux_any(fast_pldexp_unsafe)) { + // For |x| <= 708, we know the result is not zero or inf, and we can safely use + // the fast version of pldexp. + return pmax(pldexp_fast(x, fx), _x); + } return pselect(zero_mask, cst_zero, pmax(pldexp(x, fx), _x)); } @@ -1114,7 +1259,7 @@ EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet generic_atan(const Pa constexpr Scalar kPiOverTwo = static_cast(EIGEN_PI / 2); - const Packet cst_signmask = pset1(-Scalar(0)); + const Packet cst_signmask = pset1(Scalar(-0.0)); const Packet cst_one = pset1(Scalar(1)); const Packet cst_pi_over_two = pset1(kPiOverTwo); @@ -1676,7 +1821,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet twoprod_low(const Packet& x, const template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void veltkamp_splitting(const Packet& x, Packet& x_hi, Packet& x_lo) { typedef typename unpacket_traits::type Scalar; - EIGEN_CONSTEXPR int shift = (NumTraits::digits() + 1) / 2; + constexpr int shift = (NumTraits::digits() + 1) / 2; const Scalar shift_scale = Scalar(uint64_t(1) << shift); // Scalar constructor not necessarily constexpr. const Packet gamma = pmul(pset1(shift_scale + Scalar(1)), x); Packet rho = psub(x, gamma); @@ -1828,81 +1973,53 @@ struct accurate_log2 { }; // This specialization uses a more accurate algorithm to compute log2(x) for -// floats in [1/sqrt(2);sqrt(2)] with a relative accuracy of ~6.42e-10. +// floats in [1/sqrt(2);sqrt(2)] with a relative accuracy of ~6.56508e-10. // This additional accuracy is needed to counter the error-magnification // inherent in multiplying by a potentially large exponent in pow(x,y). -// The minimax polynomial used was calculated using the Sollya tool. -// See sollya.org. +// The minimax polynomial used was calculated using the Rminimax tool, +// see https://gitlab.inria.fr/sfilip/rminimax. +// Command line: +// $ ratapprox --function="log2(1+x)/x" --dom='[-0.2929,0.41422]' +// --type=[10,0] +// --numF="[D,D,SG]" --denF="[SG]" --log --dispCoeff="dec" +// +// The resulting implementation of pow(x,y) is accurate to 3 ulps. template <> struct accurate_log2 { template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(const Packet& z, Packet& log2_x_hi, Packet& log2_x_lo) { - // The function log(1+x)/x is approximated in the interval - // [1/sqrt(2)-1;sqrt(2)-1] by a degree 10 polynomial of the form - // Q(x) = (C0 + x * (C1 + x * (C2 + x * (C3 + x * P(x))))), - // where the degree 6 polynomial P(x) is evaluated in single precision, - // while the remaining 4 terms of Q(x), as well as the final multiplication by x - // to reconstruct log(1+x) are evaluated in extra precision using - // double word arithmetic. C0 through C3 are extra precise constants - // stored as double words. - // - // The polynomial coefficients were calculated using Sollya commands: - // > n = 10; - // > f = log2(1+x)/x; - // > interval = [sqrt(0.5)-1;sqrt(2)-1]; - // > p = fpminimax(f,n,[|double,double,double,double,single...|],interval,relative,floating); + // Split the two lowest order constant coefficient into double-word representation. + constexpr double kC0 = 1.442695041742110273474963832995854318141937255859375e+00; + constexpr float kC0_hi = static_cast(kC0); + constexpr float kC0_lo = static_cast(kC0 - static_cast(kC0_hi)); + const Packet c0_hi = pset1(kC0_hi); + const Packet c0_lo = pset1(kC0_lo); - const Packet p6 = pset1(9.703654795885e-2f); - const Packet p5 = pset1(-0.1690667718648f); - const Packet p4 = pset1(0.1720575392246f); - const Packet p3 = pset1(-0.1789081543684f); - const Packet p2 = pset1(0.2050433009862f); - const Packet p1 = pset1(-0.2404672354459f); - const Packet p0 = pset1(0.2885761857032f); + constexpr double kC1 = -7.2134751588268664068692714863573201000690460205078125e-01; + constexpr float kC1_hi = static_cast(kC1); + constexpr float kC1_lo = static_cast(kC1 - static_cast(kC1_hi)); + const Packet c1_hi = pset1(kC1_hi); + const Packet c1_lo = pset1(kC1_lo); - const Packet C3_hi = pset1(-0.360674142838f); - const Packet C3_lo = pset1(-6.13283912543e-09f); - const Packet C2_hi = pset1(0.480897903442f); - const Packet C2_lo = pset1(-1.44861207474e-08f); - const Packet C1_hi = pset1(-0.721347510815f); - const Packet C1_lo = pset1(-4.84483164698e-09f); - const Packet C0_hi = pset1(1.44269502163f); - const Packet C0_lo = pset1(2.01711713999e-08f); + constexpr float c[] = { + 9.7010828554630279541015625e-02, -1.6896486282348632812500000e-01, 1.7200836539268493652343750e-01, + -1.7892272770404815673828125e-01, 2.0505344867706298828125000e-01, -2.4046677350997924804687500e-01, + 2.8857553005218505859375000e-01, -3.6067414283752441406250000e-01, 4.8089790344238281250000000e-01}; + + // Evaluate the higher order terms in the polynomial using + // standard arithmetic. const Packet one = pset1(1.0f); - const Packet x = psub(z, one); - // Evaluate P(x) in working precision. - // We evaluate it in multiple parts to improve instruction level - // parallelism. - Packet x2 = pmul(x, x); - Packet p_even = pmadd(p6, x2, p4); - p_even = pmadd(p_even, x2, p2); - p_even = pmadd(p_even, x2, p0); - Packet p_odd = pmadd(p5, x2, p3); - p_odd = pmadd(p_odd, x2, p1); - Packet p = pmadd(p_odd, x, p_even); - - // Now evaluate the low-order tems of Q(x) in double word precision. - // In the following, due to the alternating signs and the fact that - // |x| < sqrt(2)-1, we can assume that |C*_hi| >= q_i, and use - // fast_twosum instead of the slower twosum. - Packet q_hi, q_lo; - Packet t_hi, t_lo; - // C3 + x * p(x) - twoprod(p, x, t_hi, t_lo); - fast_twosum(C3_hi, C3_lo, t_hi, t_lo, q_hi, q_lo); - // C2 + x * p(x) - twoprod(q_hi, q_lo, x, t_hi, t_lo); - fast_twosum(C2_hi, C2_lo, t_hi, t_lo, q_hi, q_lo); - // C1 + x * p(x) - twoprod(q_hi, q_lo, x, t_hi, t_lo); - fast_twosum(C1_hi, C1_lo, t_hi, t_lo, q_hi, q_lo); - // C0 + x * p(x) - twoprod(q_hi, q_lo, x, t_hi, t_lo); - fast_twosum(C0_hi, C0_lo, t_hi, t_lo, q_hi, q_lo); - - // log(z) ~= x * Q(x) - twoprod(q_hi, q_lo, x, log2_x_hi, log2_x_lo); + Packet p = ppolevl::run(x, c); + // Evaluate the final two step in Horner's rule using double-word + // arithmetic. + Packet p_hi, p_lo; + twoprod(x, p, p_hi, p_lo); + fast_twosum(c1_hi, c1_lo, p_hi, p_lo, p_hi, p_lo); + twoprod(p_hi, p_lo, x, p_hi, p_lo); + fast_twosum(c0_hi, c0_lo, p_hi, p_lo, p_hi, p_lo); + // Multiply by x to recover log2(z). + twoprod(p_hi, p_lo, x, log2_x_hi, log2_x_lo); } }; @@ -1997,140 +2114,6 @@ struct accurate_log2 { } }; -// This function accurately computes exp2(x) for x in [-0.5:0.5], which is -// needed in pow(x,y). -template -struct fast_accurate_exp2 { - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet operator()(const Packet& x) { - return generic_exp2(x); - } -}; - -// This specialization uses a faster algorithm to compute exp2(x) for floats -// in [-0.5;0.5] with a relative accuracy of 1 ulp. -// The minimax polynomial used was calculated using the Sollya tool. -// See sollya.org. -template <> -struct fast_accurate_exp2 { - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet operator()(const Packet& x) { - // This function approximates exp2(x) by a degree 6 polynomial of the form - // Q(x) = 1 + x * (C + x * P(x)), where the degree 4 polynomial P(x) is evaluated in - // single precision, and the remaining steps are evaluated with extra precision using - // double word arithmetic. C is an extra precise constant stored as a double word. - // - // The polynomial coefficients were calculated using Sollya commands: - // > n = 6; - // > f = 2^x; - // > interval = [-0.5;0.5]; - // > p = fpminimax(f,n,[|1,double,single...|],interval,relative,floating); - - const Packet p4 = pset1(1.539513905e-4f); - const Packet p3 = pset1(1.340007293e-3f); - const Packet p2 = pset1(9.618283249e-3f); - const Packet p1 = pset1(5.550328270e-2f); - const Packet p0 = pset1(0.2402264923f); - - const Packet C_hi = pset1(0.6931471825f); - const Packet C_lo = pset1(2.36836577e-08f); - const Packet one = pset1(1.0f); - - // Evaluate P(x) in working precision. - // We evaluate even and odd parts of the polynomial separately - // to gain some instruction level parallelism. - Packet x2 = pmul(x, x); - Packet p_even = pmadd(p4, x2, p2); - Packet p_odd = pmadd(p3, x2, p1); - p_even = pmadd(p_even, x2, p0); - Packet p = pmadd(p_odd, x, p_even); - - // Evaluate the remaining terms of Q(x) with extra precision using - // double word arithmetic. - Packet p_hi, p_lo; - // x * p(x) - twoprod(p, x, p_hi, p_lo); - // C + x * p(x) - Packet q1_hi, q1_lo; - twosum(p_hi, p_lo, C_hi, C_lo, q1_hi, q1_lo); - // x * (C + x * p(x)) - Packet q2_hi, q2_lo; - twoprod(q1_hi, q1_lo, x, q2_hi, q2_lo); - // 1 + x * (C + x * p(x)) - Packet q3_hi, q3_lo; - // Since |q2_hi| <= sqrt(2)-1 < 1, we can use fast_twosum - // for adding it to unity here. - fast_twosum(one, q2_hi, q3_hi, q3_lo); - return padd(q3_hi, padd(q2_lo, q3_lo)); - } -}; - -// in [-0.5;0.5] with a relative accuracy of 1 ulp. -// The minimax polynomial used was calculated using the Sollya tool. -// See sollya.org. -template <> -struct fast_accurate_exp2 { - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet operator()(const Packet& x) { - // This function approximates exp2(x) by a degree 10 polynomial of the form - // Q(x) = 1 + x * (C + x * P(x)), where the degree 8 polynomial P(x) is evaluated in - // single precision, and the remaining steps are evaluated with extra precision using - // double word arithmetic. C is an extra precise constant stored as a double word. - // - // The polynomial coefficients were calculated using Sollya commands: - // > n = 11; - // > f = 2^x; - // > interval = [-0.5;0.5]; - // > p = fpminimax(f,n,[|1,DD,double...|],interval,relative,floating); - - const Packet p9 = pset1(4.431642109085495276e-10); - const Packet p8 = pset1(7.073829923303358410e-9); - const Packet p7 = pset1(1.017822306737031311e-7); - const Packet p6 = pset1(1.321543498017646657e-6); - const Packet p5 = pset1(1.525273342728892877e-5); - const Packet p4 = pset1(1.540353045780084423e-4); - const Packet p3 = pset1(1.333355814685869807e-3); - const Packet p2 = pset1(9.618129107593478832e-3); - const Packet p1 = pset1(5.550410866481961247e-2); - const Packet p0 = pset1(0.240226506959101332); - const Packet C_hi = pset1(0.693147180559945286); - const Packet C_lo = pset1(4.81927865669806721e-17); - const Packet one = pset1(1.0); - - // Evaluate P(x) in working precision. - // We evaluate even and odd parts of the polynomial separately - // to gain some instruction level parallelism. - Packet x2 = pmul(x, x); - Packet p_even = pmadd(p8, x2, p6); - Packet p_odd = pmadd(p9, x2, p7); - p_even = pmadd(p_even, x2, p4); - p_odd = pmadd(p_odd, x2, p5); - p_even = pmadd(p_even, x2, p2); - p_odd = pmadd(p_odd, x2, p3); - p_even = pmadd(p_even, x2, p0); - p_odd = pmadd(p_odd, x2, p1); - Packet p = pmadd(p_odd, x, p_even); - - // Evaluate the remaining terms of Q(x) with extra precision using - // double word arithmetic. - Packet p_hi, p_lo; - // x * p(x) - twoprod(p, x, p_hi, p_lo); - // C + x * p(x) - Packet q1_hi, q1_lo; - twosum(p_hi, p_lo, C_hi, C_lo, q1_hi, q1_lo); - // x * (C + x * p(x)) - Packet q2_hi, q2_lo; - twoprod(q1_hi, q1_lo, x, q2_hi, q2_lo); - // 1 + x * (C + x * p(x)) - Packet q3_hi, q3_lo; - // Since |q2_hi| <= sqrt(2)-1 < 1, we can use fast_twosum - // for adding it to unity here. - fast_twosum(one, q2_hi, q3_hi, q3_lo); - return padd(q3_hi, padd(q2_lo, q3_lo)); - } -}; - // This function implements the non-trivial case of pow(x,y) where x is // positive and y is (possibly) non-integer. // Formally, pow(x,y) = exp2(y * log2(x)), where exp2(x) is shorthand for 2^x. @@ -2144,7 +2127,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet generic_pow_impl(const Packet& x, c Packet m_x = pfrexp(x, e_x); // Adjust m_x to lie in [1/sqrt(2):sqrt(2)] to minimize absolute error in log2(m_x). - EIGEN_CONSTEXPR Scalar sqrt_half = Scalar(0.70710678118654752440); + constexpr Scalar sqrt_half = Scalar(0.70710678118654752440); const Packet m_x_scale_mask = pcmp_lt(m_x, pset1(sqrt_half)); m_x = pselect(m_x_scale_mask, pmul(pset1(Scalar(2)), m_x), m_x); e_x = pselect(m_x_scale_mask, psub(e_x, pset1(Scalar(1))), e_x); @@ -2177,11 +2160,18 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet generic_pow_impl(const Packet& x, c // We now have an accurate split of f = n_z + r_z and can compute // x^y = 2**{n_z + r_z) = exp2(r_z) * 2**{n_z}. - // Since r_z is in [-0.5;0.5], we compute the first factor to high accuracy - // using a specialized algorithm. Multiplication by the second factor can - // be done exactly using pldexp(), since it is an integer power of 2. - const Packet e_r = fast_accurate_exp2()(r_z); - return pldexp(e_r, n_z); + // Multiplication by the second factor can be done exactly using pldexp(), since + // it is an integer power of 2. + const Packet e_r = generic_exp2(r_z); + + // Since we know that e_r is in [1/sqrt(2); sqrt(2)], we can use the fast version + // of pldexp to multiply by 2**{n_z} when |n_z| is sufficiently small. + constexpr Scalar kPldExpThresh = std::numeric_limits::max_exponent - 2; + const Packet pldexp_fast_unsafe = pcmp_lt(pset1(kPldExpThresh), pabs(n_z)); + if (predux_any(pldexp_fast_unsafe)) { + return pldexp(e_r, n_z); + } + return pldexp_fast(e_r, n_z); } // Generic implementation of pow(x,y). @@ -2189,69 +2179,91 @@ template EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet generic_pow(const Packet& x, const Packet& y) { typedef typename unpacket_traits::type Scalar; - const Packet cst_pos_inf = pset1(NumTraits::infinity()); - const Packet cst_neg_inf = pset1(-NumTraits::infinity()); + const Packet cst_inf = pset1(NumTraits::infinity()); const Packet cst_zero = pset1(Scalar(0)); const Packet cst_one = pset1(Scalar(1)); const Packet cst_nan = pset1(NumTraits::quiet_NaN()); - const Packet abs_x = pabs(x); + const Packet x_abs = pabs(x); + Packet pow = generic_pow_impl(x_abs, y); + + // In the following we enforce the special case handling prescribed in + // https://en.cppreference.com/w/cpp/numeric/math/pow. + // Predicates for sign and magnitude of x. - const Packet abs_x_is_zero = pcmp_eq(abs_x, cst_zero); + const Packet x_is_negative = pcmp_lt(x, cst_zero); + const Packet x_is_zero = pcmp_eq(x, cst_zero); + const Packet x_is_one = pcmp_eq(x, cst_one); const Packet x_has_signbit = psignbit(x); - const Packet x_is_neg = pandnot(x_has_signbit, abs_x_is_zero); - const Packet x_is_neg_zero = pand(x_has_signbit, abs_x_is_zero); - const Packet abs_x_is_inf = pcmp_eq(abs_x, cst_pos_inf); - const Packet abs_x_is_one = pcmp_eq(abs_x, cst_one); - const Packet abs_x_is_gt_one = pcmp_lt(cst_one, abs_x); - const Packet abs_x_is_lt_one = pcmp_lt(abs_x, cst_one); - const Packet x_is_one = pandnot(abs_x_is_one, x_is_neg); - const Packet x_is_neg_one = pand(abs_x_is_one, x_is_neg); - const Packet x_is_nan = pisnan(x); + const Packet x_abs_gt_one = pcmp_lt(cst_one, x_abs); + const Packet x_abs_is_inf = pcmp_eq(x_abs, cst_inf); // Predicates for sign and magnitude of y. - const Packet abs_y = pabs(y); + const Packet y_abs = pabs(y); + const Packet y_abs_is_inf = pcmp_eq(y_abs, cst_inf); + const Packet y_is_negative = pcmp_lt(y, cst_zero); + const Packet y_is_zero = pcmp_eq(y, cst_zero); const Packet y_is_one = pcmp_eq(y, cst_one); - const Packet abs_y_is_zero = pcmp_eq(abs_y, cst_zero); - const Packet y_is_neg = pcmp_lt(y, cst_zero); - const Packet y_is_pos = pandnot(ptrue(y), por(abs_y_is_zero, y_is_neg)); - const Packet y_is_nan = pisnan(y); - const Packet abs_y_is_inf = pcmp_eq(abs_y, cst_pos_inf); - EIGEN_CONSTEXPR Scalar huge_exponent = - (NumTraits::max_exponent() * Scalar(EIGEN_LN2)) / NumTraits::epsilon(); - const Packet abs_y_is_huge = pcmp_le(pset1(huge_exponent), pabs(y)); - - // Predicates for whether y is integer and/or even. - const Packet y_is_int = pcmp_eq(pfloor(y), y); + // Predicates for whether y is integer and odd/even. + const Packet y_is_int = pandnot(pcmp_eq(pfloor(y), y), y_abs_is_inf); const Packet y_div_2 = pmul(y, pset1(Scalar(0.5))); const Packet y_is_even = pcmp_eq(pround(y_div_2), y_div_2); + const Packet y_is_odd_int = pandnot(y_is_int, y_is_even); + // Smallest exponent for which (1 + epsilon) overflows to infinity. + constexpr Scalar huge_exponent = + (NumTraits::max_exponent() * Scalar(EIGEN_LN2)) / NumTraits::epsilon(); + const Packet y_abs_is_huge = pcmp_le(pset1(huge_exponent), y_abs); - // Predicates encoding special cases for the value of pow(x,y) - const Packet invalid_negative_x = pandnot(pandnot(pandnot(x_is_neg, abs_x_is_inf), y_is_int), abs_y_is_inf); - const Packet pow_is_nan = por(invalid_negative_x, por(x_is_nan, y_is_nan)); - const Packet pow_is_one = - por(por(x_is_one, abs_y_is_zero), pand(x_is_neg_one, por(abs_y_is_inf, pandnot(y_is_even, invalid_negative_x)))); - const Packet pow_is_zero = por(por(por(pand(abs_x_is_zero, y_is_pos), pand(abs_x_is_inf, y_is_neg)), - pand(pand(abs_x_is_lt_one, abs_y_is_huge), y_is_pos)), - pand(pand(abs_x_is_gt_one, abs_y_is_huge), y_is_neg)); - const Packet pow_is_inf = por(por(por(pand(abs_x_is_zero, y_is_neg), pand(abs_x_is_inf, y_is_pos)), - pand(pand(abs_x_is_lt_one, abs_y_is_huge), y_is_neg)), - pand(pand(abs_x_is_gt_one, abs_y_is_huge), y_is_pos)); - const Packet pow_is_neg_zero = pand(pandnot(y_is_int, y_is_even), - por(pand(y_is_neg, pand(abs_x_is_inf, x_is_neg)), pand(y_is_pos, x_is_neg_zero))); - const Packet inf_val = - pselect(pandnot(pand(por(pand(abs_x_is_inf, x_is_neg), pand(x_is_neg_zero, y_is_neg)), y_is_int), y_is_even), - cst_neg_inf, cst_pos_inf); - // General computation of pow(x,y) for positive x or negative x and integer y. - const Packet negate_pow_abs = pandnot(x_is_neg, y_is_even); - const Packet pow_abs = generic_pow_impl(abs_x, y); - return pselect(y_is_one, x, - pselect(pow_is_one, cst_one, - pselect(pow_is_nan, cst_nan, - pselect(pow_is_inf, inf_val, - pselect(pow_is_neg_zero, pnegate(cst_zero), - pselect(pow_is_zero, cst_zero, - pselect(negate_pow_abs, pnegate(pow_abs), pow_abs))))))); + // * pow(base, exp) returns NaN if base is finite and negative + // and exp is finite and non-integer. + pow = pselect(pandnot(x_is_negative, y_is_int), cst_nan, pow); + + // * pow(±0, exp), where exp is negative, finite, and is an even integer or + // a non-integer, returns +∞ + // * pow(±0, exp), where exp is positive non-integer or a positive even + // integer, returns +0 + // * pow(+0, exp), where exp is a negative odd integer, returns +∞ + // * pow(-0, exp), where exp is a negative odd integer, returns -∞ + // * pow(+0, exp), where exp is a positive odd integer, returns +0 + // * pow(-0, exp), where exp is a positive odd integer, returns -0 + // Sign is flipped by the rule below. + pow = pselect(x_is_zero, pselect(y_is_negative, cst_inf, cst_zero), pow); + + // pow(base, exp) returns -pow(abs(base), exp) if base has the sign bit set, + // and exp is an odd integer exponent. + pow = pselect(pand(x_has_signbit, y_is_odd_int), pnegate(pow), pow); + + // * pow(base, -∞) returns +∞ for any |base|<1 + // * pow(base, -∞) returns +0 for any |base|>1 + // * pow(base, +∞) returns +0 for any |base|<1 + // * pow(base, +∞) returns +∞ for any |base|>1 + // * pow(±0, -∞) returns +∞ + // * pow(-1, +-∞) = 1 + Packet inf_y_val = pselect(por(pand(y_is_negative, x_is_zero), pxor(y_is_negative, x_abs_gt_one)), cst_inf, cst_zero); + inf_y_val = pselect(pcmp_eq(x, pset1(Scalar(-1.0))), cst_one, inf_y_val); + pow = pselect(y_abs_is_huge, inf_y_val, pow); + + // * pow(+∞, exp) returns +0 for any negative exp + // * pow(+∞, exp) returns +∞ for any positive exp + // * pow(-∞, exp) returns -0 if exp is a negative odd integer. + // * pow(-∞, exp) returns +0 if exp is a negative non-integer or negative + // even integer. + // * pow(-∞, exp) returns -∞ if exp is a positive odd integer. + // * pow(-∞, exp) returns +∞ if exp is a positive non-integer or positive + // even integer. + auto x_pos_inf_value = pselect(y_is_negative, cst_zero, cst_inf); + auto x_neg_inf_value = pselect(y_is_odd_int, pnegate(x_pos_inf_value), x_pos_inf_value); + pow = pselect(x_abs_is_inf, pselect(x_is_negative, x_neg_inf_value, x_pos_inf_value), pow); + + // All cases of NaN inputs return NaN, except the two below. + pow = pselect(por(pisnan(x), pisnan(y)), cst_nan, pow); + + // * pow(base, 1) returns base. + // * pow(base, +/-0) returns 1, regardless of base, even NaN. + // * pow(+1, exp) returns 1, regardless of exponent, even NaN. + pow = pselect(y_is_one, x, pselect(por(x_is_one, y_is_zero), cst_one, pow)); + + return pow; } namespace unary_pow { @@ -2451,7 +2463,12 @@ struct unary_pow_impl { static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const ScalarExponent& exponent) { const bool exponent_is_integer = (numext::isfinite)(exponent) && numext::round(exponent) == exponent; if (exponent_is_integer) { - return unary_pow::int_pow(x, exponent); + // The simple recursive doubling implementation is only accurate to 3 ulps + // for integer exponents in [-3:7]. Since this is a common case, we + // specialize it here. + bool use_repeated_squaring = + (exponent <= ScalarExponent(7) && (!ExponentIsSigned || exponent >= ScalarExponent(-3))); + return use_repeated_squaring ? unary_pow::int_pow(x, exponent) : generic_pow(x, pset1(exponent)); } else { Packet result = unary_pow::gen_pow(x, exponent); result = unary_pow::handle_nonint_nonint_errors(x, result, exponent); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h index 3b362f4f68..673954e92d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h @@ -42,6 +42,26 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pfrexp_generic_get_biased_exponent( template EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_generic(const Packet& a, const Packet& exponent); +// Explicitly multiplies +// a * (2^e) +// clamping e to the range +// [NumTraits::min_exponent()-2, NumTraits::max_exponent()] +// +// This is approx 7x faster than pldexp_impl, but will prematurely over/underflow +// if 2^e doesn't fit into a normal floating-point Scalar. +// +// Assumes IEEE floating point format +template +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Packet pldexp_fast(const Packet& a, const Packet& exponent); + +/** \internal \returns cbrt(x) for single precision float */ +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt_float(const Packet& x_in); + +/** \internal \returns cbrt(x) for double precision float */ +template +EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet pcbrt_double(const Packet& x_in); + /** \internal \returns log(x) for single precision float */ template EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS Packet plog_float(const Packet _x); @@ -183,6 +203,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet generic_round(const Packet& a); EIGEN_FLOAT_PACKET_FUNCTION(log, PACKET) \ EIGEN_FLOAT_PACKET_FUNCTION(log2, PACKET) \ EIGEN_FLOAT_PACKET_FUNCTION(exp, PACKET) \ + EIGEN_FLOAT_PACKET_FUNCTION(cbrt, PACKET) \ EIGEN_GENERIC_PACKET_FUNCTION(expm1, PACKET) \ EIGEN_GENERIC_PACKET_FUNCTION(exp2, PACKET) \ EIGEN_GENERIC_PACKET_FUNCTION(log1p, PACKET) \ @@ -196,6 +217,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet generic_round(const Packet& a); EIGEN_DOUBLE_PACKET_FUNCTION(log2, PACKET) \ EIGEN_DOUBLE_PACKET_FUNCTION(exp, PACKET) \ EIGEN_DOUBLE_PACKET_FUNCTION(tanh, PACKET) \ + EIGEN_DOUBLE_PACKET_FUNCTION(cbrt, PACKET) \ EIGEN_GENERIC_PACKET_FUNCTION(atan, PACKET) \ EIGEN_GENERIC_PACKET_FUNCTION(exp2, PACKET) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h index a8cb228c18..ba70d5fab2 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h @@ -37,21 +37,23 @@ // IWYU pragma: private #include "../../InternalHeaderCheck.h" -#if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) // When compiling with GPU support, the "__half_raw" base class as well as // some other routines are defined in the GPU compiler header files // (cuda_fp16.h, hip_fp16.h), and they are not tagged constexpr // As a consequence, we get compile failures when compiling Eigen with // GPU support. Hence the need to disable EIGEN_CONSTEXPR when building -// Eigen with GPU support -#pragma push_macro("EIGEN_CONSTEXPR") -#undef EIGEN_CONSTEXPR -#define EIGEN_CONSTEXPR +// Eigen with GPU support. +// Any functions that require `numext::bit_cast` may also not be constexpr, +// including any native types when setting via raw bit values. +#if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) || defined(EIGEN_HAS_BUILTIN_FLOAT16) +#define _EIGEN_MAYBE_CONSTEXPR +#else +#define _EIGEN_MAYBE_CONSTEXPR constexpr #endif #define F16_PACKET_FUNCTION(PACKET_F, PACKET_F16, METHOD) \ template <> \ - EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED PACKET_F16 METHOD(const PACKET_F16& _x) { \ + EIGEN_UNUSED EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC PACKET_F16 METHOD(const PACKET_F16& _x) { \ return float2half(METHOD(half2float(_x))); \ } @@ -81,8 +83,10 @@ namespace half_impl { // Making the host side compile phase of hipcc use the same Eigen::half impl, as the gcc compile, resolves // this error, and hence the following convoluted #if condition #if !defined(EIGEN_HAS_GPU_FP16) || !defined(EIGEN_GPU_COMPILE_PHASE) + // Make our own __half_raw definition that is similar to CUDA's. struct __half_raw { + struct construct_from_rep_tag {}; #if (defined(EIGEN_HAS_GPU_FP16) && !defined(EIGEN_GPU_COMPILE_PHASE)) // Eigen::half can be used as the datatype for shared memory declarations (in Eigen and TF) // The element type for shared memory cannot have non-trivial constructors @@ -91,43 +95,53 @@ struct __half_raw { // hence the need for this EIGEN_DEVICE_FUNC __half_raw() {} #else - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw() : x(0) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR __half_raw() : x(0) {} #endif + #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) - explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(numext::bit_cast<__fp16>(raw)) {} + explicit EIGEN_DEVICE_FUNC __half_raw(numext::uint16_t raw) : x(numext::bit_cast<__fp16>(raw)) {} + EIGEN_DEVICE_FUNC constexpr __half_raw(construct_from_rep_tag, __fp16 rep) : x{rep} {} __fp16 x; +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) + explicit EIGEN_DEVICE_FUNC __half_raw(numext::uint16_t raw) : x(numext::bit_cast<_Float16>(raw)) {} + EIGEN_DEVICE_FUNC constexpr __half_raw(construct_from_rep_tag, _Float16 rep) : x{rep} {} + _Float16 x; #else - explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(raw) {} + explicit EIGEN_DEVICE_FUNC constexpr __half_raw(numext::uint16_t raw) : x(raw) {} + EIGEN_DEVICE_FUNC constexpr __half_raw(construct_from_rep_tag, numext::uint16_t rep) : x{rep} {} numext::uint16_t x; #endif }; #elif defined(EIGEN_HAS_HIP_FP16) -// Nothing to do here +// HIP GPU compile phase: nothing to do here. // HIP fp16 header file has a definition for __half_raw #elif defined(EIGEN_HAS_CUDA_FP16) + +// CUDA GPU compile phase. #if EIGEN_CUDA_SDK_VER < 90000 // In CUDA < 9.0, __half is the equivalent of CUDA 9's __half_raw typedef __half __half_raw; #endif // defined(EIGEN_HAS_CUDA_FP16) + #elif defined(SYCL_DEVICE_ONLY) typedef cl::sycl::half __half_raw; #endif -EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x); +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x); EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff); EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h); struct half_base : public __half_raw { - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base() {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half_raw& h) : __half_raw(h) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half_base() {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half_base(const __half_raw& h) : __half_raw(h) {} #if defined(EIGEN_HAS_GPU_FP16) #if defined(EIGEN_HAS_HIP_FP16) - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) { x = __half_as_ushort(h); } + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half_base(const __half& h) { x = __half_as_ushort(h); } #elif defined(EIGEN_HAS_CUDA_FP16) #if EIGEN_CUDA_SDK_VER >= 90000 - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) : __half_raw(*(__half_raw*)&h) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half_base(const __half& h) : __half_raw(*(__half_raw*)&h) {} #endif #endif #endif @@ -156,21 +170,29 @@ struct half : public half_impl::half_base { #endif #endif - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half() {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half() {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half_raw& h) : half_impl::half_base(h) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(const __half_raw& h) : half_impl::half_base(h) {} #if defined(EIGEN_HAS_GPU_FP16) #if defined(EIGEN_HAS_HIP_FP16) - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {} #elif defined(EIGEN_HAS_CUDA_FP16) #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000 - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {} + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {} #endif #endif #endif - explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(bool b) +#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) + explicit EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(__fp16 b) + : half(__half_raw(__half_raw::construct_from_rep_tag(), b)) {} +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) + explicit EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(_Float16 b) + : half(__half_raw(__half_raw::construct_from_rep_tag(), b)) {} +#endif + + explicit EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR half(bool b) : half_impl::half_base(half_impl::raw_uint16_to_half(b ? 0x3c00 : 0)) {} template explicit EIGEN_DEVICE_FUNC half(T val) @@ -201,99 +223,99 @@ struct half : public half_impl::half_base { namespace half_impl { template struct numeric_limits_half_impl { - static EIGEN_CONSTEXPR const bool is_specialized = true; - static EIGEN_CONSTEXPR const bool is_signed = true; - static EIGEN_CONSTEXPR const bool is_integer = false; - static EIGEN_CONSTEXPR const bool is_exact = false; - static EIGEN_CONSTEXPR const bool has_infinity = true; - static EIGEN_CONSTEXPR const bool has_quiet_NaN = true; - static EIGEN_CONSTEXPR const bool has_signaling_NaN = true; + static constexpr const bool is_specialized = true; + static constexpr const bool is_signed = true; + static constexpr const bool is_integer = false; + static constexpr const bool is_exact = false; + static constexpr const bool has_infinity = true; + static constexpr const bool has_quiet_NaN = true; + static constexpr const bool has_signaling_NaN = true; EIGEN_DIAGNOSTICS(push) EIGEN_DISABLE_DEPRECATED_WARNING - static EIGEN_CONSTEXPR const std::float_denorm_style has_denorm = std::denorm_present; - static EIGEN_CONSTEXPR const bool has_denorm_loss = false; + static constexpr const std::float_denorm_style has_denorm = std::denorm_present; + static constexpr const bool has_denorm_loss = false; EIGEN_DIAGNOSTICS(pop) - static EIGEN_CONSTEXPR const std::float_round_style round_style = std::round_to_nearest; - static EIGEN_CONSTEXPR const bool is_iec559 = true; + static constexpr const std::float_round_style round_style = std::round_to_nearest; + static constexpr const bool is_iec559 = true; // The C++ standard defines this as "true if the set of values representable // by the type is finite." Half has finite precision. - static EIGEN_CONSTEXPR const bool is_bounded = true; - static EIGEN_CONSTEXPR const bool is_modulo = false; - static EIGEN_CONSTEXPR const int digits = 11; - static EIGEN_CONSTEXPR const int digits10 = + static constexpr const bool is_bounded = true; + static constexpr const bool is_modulo = false; + static constexpr const int digits = 11; + static constexpr const int digits10 = 3; // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html - static EIGEN_CONSTEXPR const int max_digits10 = + static constexpr const int max_digits10 = 5; // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html - static EIGEN_CONSTEXPR const int radix = std::numeric_limits::radix; - static EIGEN_CONSTEXPR const int min_exponent = -13; - static EIGEN_CONSTEXPR const int min_exponent10 = -4; - static EIGEN_CONSTEXPR const int max_exponent = 16; - static EIGEN_CONSTEXPR const int max_exponent10 = 4; - static EIGEN_CONSTEXPR const bool traps = std::numeric_limits::traps; + static constexpr const int radix = std::numeric_limits::radix; + static constexpr const int min_exponent = -13; + static constexpr const int min_exponent10 = -4; + static constexpr const int max_exponent = 16; + static constexpr const int max_exponent10 = 4; + static constexpr const bool traps = std::numeric_limits::traps; // IEEE754: "The implementer shall choose how tininess is detected, but shall // detect tininess in the same way for all operations in radix two" - static EIGEN_CONSTEXPR const bool tinyness_before = std::numeric_limits::tinyness_before; + static constexpr const bool tinyness_before = std::numeric_limits::tinyness_before; - static EIGEN_CONSTEXPR Eigen::half(min)() { return Eigen::half_impl::raw_uint16_to_half(0x0400); } - static EIGEN_CONSTEXPR Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); } - static EIGEN_CONSTEXPR Eigen::half(max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); } - static EIGEN_CONSTEXPR Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x1400); } - static EIGEN_CONSTEXPR Eigen::half round_error() { return Eigen::half_impl::raw_uint16_to_half(0x3800); } - static EIGEN_CONSTEXPR Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); } - static EIGEN_CONSTEXPR Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); } - static EIGEN_CONSTEXPR Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7d00); } - static EIGEN_CONSTEXPR Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x0001); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half(min)() { return Eigen::half_impl::raw_uint16_to_half(0x0400); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half(max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x1400); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half round_error() { return Eigen::half_impl::raw_uint16_to_half(0x3800); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7d00); } + static _EIGEN_MAYBE_CONSTEXPR Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x0001); } }; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_specialized; +constexpr const bool numeric_limits_half_impl::is_specialized; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_signed; +constexpr const bool numeric_limits_half_impl::is_signed; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_integer; +constexpr const bool numeric_limits_half_impl::is_integer; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_exact; +constexpr const bool numeric_limits_half_impl::is_exact; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::has_infinity; +constexpr const bool numeric_limits_half_impl::has_infinity; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::has_quiet_NaN; +constexpr const bool numeric_limits_half_impl::has_quiet_NaN; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::has_signaling_NaN; +constexpr const bool numeric_limits_half_impl::has_signaling_NaN; EIGEN_DIAGNOSTICS(push) EIGEN_DISABLE_DEPRECATED_WARNING template -EIGEN_CONSTEXPR const std::float_denorm_style numeric_limits_half_impl::has_denorm; +constexpr const std::float_denorm_style numeric_limits_half_impl::has_denorm; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::has_denorm_loss; +constexpr const bool numeric_limits_half_impl::has_denorm_loss; EIGEN_DIAGNOSTICS(pop) template -EIGEN_CONSTEXPR const std::float_round_style numeric_limits_half_impl::round_style; +constexpr const std::float_round_style numeric_limits_half_impl::round_style; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_iec559; +constexpr const bool numeric_limits_half_impl::is_iec559; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_bounded; +constexpr const bool numeric_limits_half_impl::is_bounded; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::is_modulo; +constexpr const bool numeric_limits_half_impl::is_modulo; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::digits; +constexpr const int numeric_limits_half_impl::digits; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::digits10; +constexpr const int numeric_limits_half_impl::digits10; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::max_digits10; +constexpr const int numeric_limits_half_impl::max_digits10; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::radix; +constexpr const int numeric_limits_half_impl::radix; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::min_exponent; +constexpr const int numeric_limits_half_impl::min_exponent; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::min_exponent10; +constexpr const int numeric_limits_half_impl::min_exponent10; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::max_exponent; +constexpr const int numeric_limits_half_impl::max_exponent; template -EIGEN_CONSTEXPR const int numeric_limits_half_impl::max_exponent10; +constexpr const int numeric_limits_half_impl::max_exponent10; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::traps; +constexpr const bool numeric_limits_half_impl::traps; template -EIGEN_CONSTEXPR const bool numeric_limits_half_impl::tinyness_before; +constexpr const bool numeric_limits_half_impl::tinyness_before; } // end namespace half_impl } // end namespace Eigen @@ -320,8 +342,7 @@ namespace half_impl { (defined(EIGEN_HAS_HIP_FP16) && defined(HIP_DEVICE_COMPILE)) // Note: We deliberately do *not* define this to 1 even if we have Arm's native // fp16 type since GPU half types are rather different from native CPU half types. -// TODO: Rename to something like EIGEN_HAS_NATIVE_GPU_FP16 -#define EIGEN_HAS_NATIVE_FP16 +#define EIGEN_HAS_NATIVE_GPU_FP16 #endif // Intrinsics for native fp16 support. Note that on current hardware, @@ -329,7 +350,7 @@ namespace half_impl { // versions to get the ALU speed increased), but you do save the // conversion steps back and forth. -#if defined(EIGEN_HAS_NATIVE_FP16) +#if defined(EIGEN_HAS_NATIVE_GPU_FP16) EIGEN_STRONG_INLINE __device__ half operator+(const half& a, const half& b) { #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000 return __hadd(::__half(a), ::__half(b)); @@ -371,7 +392,8 @@ EIGEN_STRONG_INLINE __device__ bool operator<(const half& a, const half& b) { re EIGEN_STRONG_INLINE __device__ bool operator<=(const half& a, const half& b) { return __hle(a, b); } EIGEN_STRONG_INLINE __device__ bool operator>(const half& a, const half& b) { return __hgt(a, b); } EIGEN_STRONG_INLINE __device__ bool operator>=(const half& a, const half& b) { return __hge(a, b); } -#endif + +#endif // EIGEN_HAS_NATIVE_GPU_FP16 #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) && !defined(EIGEN_GPU_COMPILE_PHASE) EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator+(const half& a, const half& b) { return half(vaddh_f16(a.x, b.x)); } @@ -401,16 +423,47 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<(const half& a, const half& EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<=(const half& a, const half& b) { return vcleh_f16(a.x, b.x); } EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>(const half& a, const half& b) { return vcgth_f16(a.x, b.x); } EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const half& a, const half& b) { return vcgeh_f16(a.x, b.x); } + +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) && !defined(EIGEN_GPU_COMPILE_PHASE) + +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator+(const half& a, const half& b) { return half(a.x + b.x); } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator*(const half& a, const half& b) { return half(a.x * b.x); } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a, const half& b) { return half(a.x - b.x); } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator/(const half& a, const half& b) { return half(a.x / b.x); } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator-(const half& a) { return half(-a.x); } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator+=(half& a, const half& b) { + a = a + b; + return a; +} +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator*=(half& a, const half& b) { + a = a * b; + return a; +} +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator-=(half& a, const half& b) { + a = a - b; + return a; +} +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator/=(half& a, const half& b) { + a = a / b; + return a; +} +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator==(const half& a, const half& b) { return a.x == b.x; } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator!=(const half& a, const half& b) { return a.x != b.x; } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<(const half& a, const half& b) { return a.x < b.x; } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator<=(const half& a, const half& b) { return a.x <= b.x; } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>(const half& a, const half& b) { return a.x > b.x; } +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const half& a, const half& b) { return a.x >= b.x; } + // We need to distinguish ‘clang as the CUDA compiler’ from ‘clang as the host compiler, // invoked by NVCC’ (e.g. on MacOS). The former needs to see both host and device implementation // of the functions, while the latter can only deal with one of them. -#elif !defined(EIGEN_HAS_NATIVE_FP16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for half floats +#elif !defined(EIGEN_HAS_NATIVE_GPU_FP16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for half floats #if EIGEN_COMP_CLANG && defined(EIGEN_GPUCC) // We need to provide emulated *host-side* FP16 operators for clang. #pragma push_macro("EIGEN_DEVICE_FUNC") #undef EIGEN_DEVICE_FUNC -#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_HAS_NATIVE_FP16) +#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_HAS_NATIVE_GPU_FP16) #define EIGEN_DEVICE_FUNC __host__ #else // both host and device need emulated ops. #define EIGEN_DEVICE_FUNC __host__ __device__ @@ -458,6 +511,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator>=(const half& a, const half& #if EIGEN_COMP_CLANG && defined(EIGEN_GPUCC) #pragma pop_macro("EIGEN_DEVICE_FUNC") #endif + #endif // Emulate support for half floats // Division by an index. Do it in full float precision to avoid accuracy @@ -493,7 +547,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator--(half& a, int) { // these in hardware. If we need more performance on older/other CPUs, they are // also possible to vectorize directly. -EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x) { +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x) { // We cannot simply do a "return __half_raw(x)" here, because __half_raw is union type // in the hip_fp16 header file, and that will trigger a compile error // On the other hand, having anything but a return statement also triggers a compile error @@ -515,6 +569,8 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC numext::uint16_t raw_half_as_uint16(const // For SYCL, cl::sycl::half is _Float16, so cast directly. #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) return numext::bit_cast(h.x); +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) + return numext::bit_cast(h.x); #elif defined(SYCL_DEVICE_ONLY) return numext::bit_cast(h); #else @@ -522,17 +578,22 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC numext::uint16_t raw_half_as_uint16(const #endif } -union float32_bits { - unsigned int u; - float f; -}; - EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff) { #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) __half tmp_ff = __float2half(ff); return *(__half_raw*)&tmp_ff; +#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) + __half_raw h; + h.x = static_cast<__fp16>(ff); + return h; + +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) + __half_raw h; + h.x = static_cast<_Float16>(ff); + return h; + #elif defined(EIGEN_HAS_FP16_C) __half_raw h; #if EIGEN_COMP_MSVC @@ -543,52 +604,46 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff) { #endif return h; -#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) - __half_raw h; - h.x = static_cast<__fp16>(ff); - return h; - #else - float32_bits f; - f.f = ff; - - const float32_bits f32infty = {255 << 23}; - const float32_bits f16max = {(127 + 16) << 23}; - const float32_bits denorm_magic = {((127 - 15) + (23 - 10) + 1) << 23}; - unsigned int sign_mask = 0x80000000u; + uint32_t f_bits = Eigen::numext::bit_cast(ff); + const uint32_t f32infty_bits = {255 << 23}; + const uint32_t f16max_bits = {(127 + 16) << 23}; + const uint32_t denorm_magic_bits = {((127 - 15) + (23 - 10) + 1) << 23}; + const uint32_t sign_mask = 0x80000000u; __half_raw o; - o.x = static_cast(0x0u); + o.x = static_cast(0x0u); - unsigned int sign = f.u & sign_mask; - f.u ^= sign; + const uint32_t sign = f_bits & sign_mask; + f_bits ^= sign; // NOTE all the integer compares in this function can be safely // compiled into signed compares since all operands are below // 0x80000000. Important if you want fast straight SSE2 code // (since there's no unsigned PCMPGTD). - if (f.u >= f16max.u) { // result is Inf or NaN (all exponent bits set) - o.x = (f.u > f32infty.u) ? 0x7e00 : 0x7c00; // NaN->qNaN and Inf->Inf - } else { // (De)normalized number or zero - if (f.u < (113 << 23)) { // resulting FP16 is subnormal or zero + if (f_bits >= f16max_bits) { // result is Inf or NaN (all exponent bits set) + o.x = (f_bits > f32infty_bits) ? 0x7e00 : 0x7c00; // NaN->qNaN and Inf->Inf + } else { // (De)normalized number or zero + if (f_bits < (113 << 23)) { // resulting FP16 is subnormal or zero // use a magic value to align our 10 mantissa bits at the bottom of // the float. as long as FP addition is round-to-nearest-even this // just works. - f.f += denorm_magic.f; + f_bits = Eigen::numext::bit_cast(Eigen::numext::bit_cast(f_bits) + + Eigen::numext::bit_cast(denorm_magic_bits)); // and one integer subtract of the bias later, we have our final float! - o.x = static_cast(f.u - denorm_magic.u); + o.x = static_cast(f_bits - denorm_magic_bits); } else { - unsigned int mant_odd = (f.u >> 13) & 1; // resulting mantissa is odd + const uint32_t mant_odd = (f_bits >> 13) & 1; // resulting mantissa is odd // update exponent, rounding bias part 1 // Equivalent to `f.u += ((unsigned int)(15 - 127) << 23) + 0xfff`, but // without arithmetic overflow. - f.u += 0xc8000fffU; + f_bits += 0xc8000fffU; // rounding bias part 2 - f.u += mant_odd; + f_bits += mant_odd; // take the bits! - o.x = static_cast(f.u >> 13); + o.x = static_cast(f_bits >> 13); } } @@ -601,6 +656,8 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h) { #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \ (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) return __half2float(h); +#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) || defined(EIGEN_HAS_BUILTIN_FLOAT16) + return static_cast(h.x); #elif defined(EIGEN_HAS_FP16_C) #if EIGEN_COMP_MSVC // MSVC does not have scalar instructions. @@ -608,34 +665,31 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h) { #else return _cvtsh_ss(h.x); #endif -#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) - return static_cast(h.x); #else - const float32_bits magic = {113 << 23}; - const unsigned int shifted_exp = 0x7c00 << 13; // exponent mask after shift - float32_bits o; - - o.u = (h.x & 0x7fff) << 13; // exponent/mantissa bits - unsigned int exp = shifted_exp & o.u; // just the exponent - o.u += (127 - 15) << 23; // exponent adjust + const float magic = Eigen::numext::bit_cast(static_cast(113 << 23)); + const uint32_t shifted_exp = 0x7c00 << 13; // exponent mask after shift + uint32_t o_bits = (h.x & 0x7fff) << 13; // exponent/mantissa bits + const uint32_t exp = shifted_exp & o_bits; // just the exponent + o_bits += (127 - 15) << 23; // exponent adjust // handle exponent special cases - if (exp == shifted_exp) { // Inf/NaN? - o.u += (128 - 16) << 23; // extra exp adjust - } else if (exp == 0) { // Zero/Denormal? - o.u += 1 << 23; // extra exp adjust - o.f -= magic.f; // renormalize + if (exp == shifted_exp) { // Inf/NaN? + o_bits += (128 - 16) << 23; // extra exp adjust + } else if (exp == 0) { // Zero/Denormal? + o_bits += 1 << 23; // extra exp adjust + // renormalize + o_bits = Eigen::numext::bit_cast(Eigen::numext::bit_cast(o_bits) - magic); } - o.u |= (h.x & 0x8000) << 16; // sign bit - return o.f; + o_bits |= (h.x & 0x8000) << 16; // sign bit + return Eigen::numext::bit_cast(o_bits); #endif } // --- standard functions --- EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isinf)(const half& a) { -#ifdef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC +#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) || defined(EIGEN_HAS_BUILTIN_FLOAT16) return (numext::bit_cast(a.x) & 0x7fff) == 0x7c00; #else return (a.x & 0x7fff) == 0x7c00; @@ -645,7 +699,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isnan)(const half& a) { #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \ (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) return __hisnan(a); -#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) +#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) || defined(EIGEN_HAS_BUILTIN_FLOAT16) return (numext::bit_cast(a.x) & 0x7fff) > 0x7c00; #else return (a.x & 0x7fff) > 0x7c00; @@ -658,6 +712,11 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool(isfinite)(const half& a) { EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half abs(const half& a) { #if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) return half(vabsh_f16(a.x)); +#elif defined(EIGEN_HAS_BUILTIN_FLOAT16) + half result; + result.x = + numext::bit_cast<_Float16>(static_cast(numext::bit_cast(a.x) & 0x7FFF)); + return result; #else half result; result.x = a.x & 0x7FFF; @@ -741,24 +800,19 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half fmod(const half& a, const half& b) { return half(::fmodf(float(a), float(b))); } -EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(min)(const half& a, const half& b) { -#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \ - (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) - return __hlt(b, a) ? b : a; +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(min)(const half& a, const half& b) { return b < a ? b : a; } + +EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(max)(const half& a, const half& b) { return a < b ? b : a; } + +EIGEN_DEVICE_FUNC inline half fma(const half& a, const half& b, const half& c) { +#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) + return half(vfmah_f16(c.x, a.x, b.x)); +#elif defined(EIGEN_VECTORIZE_AVX512FP16) + // Reduces to vfmadd213sh. + return half(_mm_cvtsh_h(_mm_fmadd_ph(_mm_set_sh(a.x), _mm_set_sh(b.x), _mm_set_sh(c.x)))); #else - const float f1 = static_cast(a); - const float f2 = static_cast(b); - return f2 < f1 ? b : a; -#endif -} -EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half(max)(const half& a, const half& b) { -#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \ - (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE)) - return __hlt(a, b) ? b : a; -#else - const float f1 = static_cast(a); - const float f2 = static_cast(b); - return f1 < f2 ? b : a; + // Emulate FMA via float. + return half(numext::fma(static_cast(a), static_cast(b), static_cast(c))); #endif } @@ -801,31 +855,29 @@ template <> struct NumTraits : GenericNumTraits { enum { IsSigned = true, IsInteger = false, IsComplex = false, RequireInitialization = false }; - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half epsilon() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half epsilon() { return half_impl::raw_uint16_to_half(0x0800); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half dummy_precision() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half dummy_precision() { return half_impl::raw_uint16_to_half(0x211f); // Eigen::half(1e-2f); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half highest() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half highest() { return half_impl::raw_uint16_to_half(0x7bff); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half lowest() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half lowest() { return half_impl::raw_uint16_to_half(0xfbff); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half infinity() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half infinity() { return half_impl::raw_uint16_to_half(0x7c00); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half quiet_NaN() { + EIGEN_DEVICE_FUNC _EIGEN_MAYBE_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half quiet_NaN() { return half_impl::raw_uint16_to_half(0x7e00); } }; } // end namespace Eigen -#if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC) -#pragma pop_macro("EIGEN_CONSTEXPR") -#endif +#undef _EIGEN_MAYBE_CONSTEXPR namespace Eigen { namespace numext { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h index 2f401fdff1..6d7f038ff8 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h @@ -207,6 +207,7 @@ struct packet_traits : default_packet_traits { HasExp = 1, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasTanh = EIGEN_FAST_MATH, HasErf = EIGEN_FAST_MATH, HasErfc = EIGEN_FAST_MATH, @@ -4964,6 +4965,26 @@ EIGEN_STRONG_INLINE Packet4bf pmul(const Packet4bf& a, const Packet4b return F32ToBf16(pmul(Bf16ToF32(a), Bf16ToF32(b))); } +template <> +EIGEN_STRONG_INLINE Packet4bf pmadd(const Packet4bf& a, const Packet4bf& b, const Packet4bf& c) { + return F32ToBf16(pmadd(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet4bf pmsub(const Packet4bf& a, const Packet4bf& b, const Packet4bf& c) { + return F32ToBf16(pmsub(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet4bf pnmadd(const Packet4bf& a, const Packet4bf& b, const Packet4bf& c) { + return F32ToBf16(pnmadd(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + +template <> +EIGEN_STRONG_INLINE Packet4bf pnmsub(const Packet4bf& a, const Packet4bf& b, const Packet4bf& c) { + return F32ToBf16(pnmsub(Bf16ToF32(a), Bf16ToF32(b), Bf16ToF32(c))); +} + template <> EIGEN_STRONG_INLINE Packet4bf pdiv(const Packet4bf& a, const Packet4bf& b) { return F32ToBf16(pdiv(Bf16ToF32(a), Bf16ToF32(b))); @@ -5140,8 +5161,9 @@ struct packet_traits : default_packet_traits { HasCos = EIGEN_FAST_MATH, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasTanh = EIGEN_FAST_MATH, - HasErf = 0, + HasErf = EIGEN_FAST_MATH, HasErfc = EIGEN_FAST_MATH }; }; @@ -5634,6 +5656,21 @@ EIGEN_STRONG_INLINE Packet4hf pmadd(const Packet4hf& a, const Packet4hf& b, cons return vfma_f16(c, a, b); } +template <> +EIGEN_STRONG_INLINE Packet8hf pmsub(const Packet8hf& a, const Packet8hf& b, const Packet8hf& c) { + return vfmaq_f16(pnegate(c), a, b); +} + +template <> +EIGEN_STRONG_INLINE Packet4hf pnmadd(const Packet4hf& a, const Packet4hf& b, const Packet4hf& c) { + return vfma_f16(c, pnegate(a), b); +} + +template <> +EIGEN_STRONG_INLINE Packet4hf pnmsub(const Packet4hf& a, const Packet4hf& b, const Packet4hf& c) { + return vfma_f16(pnegate(c), pnegate(a), b); +} + template <> EIGEN_STRONG_INLINE Packet8hf pmin(const Packet8hf& a, const Packet8hf& b) { return vminq_f16(a, b); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h index c69e3d4791..f79da7b8cd 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h @@ -465,19 +465,11 @@ EIGEN_STRONG_INLINE Packet2cf pmsub(const Packet2cf& a, const Packet2cf& b, cons } template <> EIGEN_STRONG_INLINE Packet2cf pnmadd(const Packet2cf& a, const Packet2cf& b, const Packet2cf& c) { - __m128 a_odd = _mm_movehdup_ps(a.v); - __m128 a_even = _mm_moveldup_ps(a.v); - __m128 b_swap = _mm_permute_ps(b.v, _MM_SHUFFLE(2, 3, 0, 1)); - __m128 result = _mm_fmaddsub_ps(a_odd, b_swap, _mm_fmaddsub_ps(a_even, b.v, c.v)); - return Packet2cf(result); + return pnegate(pmsub(a, b, c)); } template <> EIGEN_STRONG_INLINE Packet2cf pnmsub(const Packet2cf& a, const Packet2cf& b, const Packet2cf& c) { - __m128 a_odd = _mm_movehdup_ps(a.v); - __m128 a_even = _mm_moveldup_ps(a.v); - __m128 b_swap = _mm_permute_ps(b.v, _MM_SHUFFLE(2, 3, 0, 1)); - __m128 result = _mm_fmaddsub_ps(a_odd, b_swap, _mm_fmsubadd_ps(a_even, b.v, c.v)); - return Packet2cf(result); + return pnegate(pmadd(a, b, c)); } // std::complex template <> @@ -498,19 +490,11 @@ EIGEN_STRONG_INLINE Packet1cd pmsub(const Packet1cd& a, const Packet1cd& b, cons } template <> EIGEN_STRONG_INLINE Packet1cd pnmadd(const Packet1cd& a, const Packet1cd& b, const Packet1cd& c) { - __m128d a_odd = _mm_permute_pd(a.v, 0x3); - __m128d a_even = _mm_movedup_pd(a.v); - __m128d b_swap = _mm_permute_pd(b.v, 0x1); - __m128d result = _mm_fmaddsub_pd(a_odd, b_swap, _mm_fmaddsub_pd(a_even, b.v, c.v)); - return Packet1cd(result); + return pnegate(pmsub(a, b, c)); } template <> EIGEN_STRONG_INLINE Packet1cd pnmsub(const Packet1cd& a, const Packet1cd& b, const Packet1cd& c) { - __m128d a_odd = _mm_permute_pd(a.v, 0x3); - __m128d a_even = _mm_movedup_pd(a.v); - __m128d b_swap = _mm_permute_pd(b.v, 0x1); - __m128d result = _mm_fmaddsub_pd(a_odd, b_swap, _mm_fmsubadd_pd(a_even, b.v, c.v)); - return Packet1cd(result); + return pnegate(pmadd(a, b, c)); } #endif } // end namespace internal diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h index c749763df7..70d13d6af7 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h @@ -195,6 +195,7 @@ struct packet_traits : default_packet_traits { HasBessel = 1, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasTanh = EIGEN_FAST_MATH, HasErf = EIGEN_FAST_MATH, HasErfc = EIGEN_FAST_MATH, @@ -217,10 +218,12 @@ struct packet_traits : default_packet_traits { HasCos = EIGEN_FAST_MATH, HasTanh = EIGEN_FAST_MATH, HasLog = 1, + HasErf = EIGEN_FAST_MATH, HasErfc = EIGEN_FAST_MATH, HasExp = 1, HasSqrt = 1, HasRsqrt = 1, + HasCbrt = 1, HasATan = 1, HasATanh = 1, HasBlend = 1 @@ -1693,10 +1696,24 @@ EIGEN_STRONG_INLINE void pscatter(uint32_t* to, const Packe } template <> EIGEN_STRONG_INLINE void pscatter(bool* to, const Packet16b& from, Index stride) { - to[4 * stride * 0] = _mm_cvtsi128_si32(from); - to[4 * stride * 1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1)); - to[4 * stride * 2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2)); - to[4 * stride * 3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3)); + EIGEN_ALIGN16 bool tmp[16]; + pstore(tmp, from); + to[stride * 0] = tmp[0]; + to[stride * 1] = tmp[1]; + to[stride * 2] = tmp[2]; + to[stride * 3] = tmp[3]; + to[stride * 4] = tmp[4]; + to[stride * 5] = tmp[5]; + to[stride * 6] = tmp[6]; + to[stride * 7] = tmp[7]; + to[stride * 8] = tmp[8]; + to[stride * 9] = tmp[9]; + to[stride * 10] = tmp[10]; + to[stride * 11] = tmp[11]; + to[stride * 12] = tmp[12]; + to[stride * 13] = tmp[13]; + to[stride * 14] = tmp[14]; + to[stride * 15] = tmp[15]; } // some compilers might be tempted to perform multiple moves instead of using a vector path. @@ -1766,7 +1783,6 @@ EIGEN_STRONG_INLINE Packet4f pldexp(const Packet4f& a, const Packet4f& // We specialize pldexp here, since the generic implementation uses Packet2l, which is not well // supported by SSE, and has more range than is needed for exponents. -// TODO(rmlarsen): Remove this specialization once Packet2l has support or casting. template <> EIGEN_STRONG_INLINE Packet2d pldexp(const Packet2d& a, const Packet2d& exponent) { // Clamp exponent to [-2099, 2099] @@ -1787,6 +1803,24 @@ EIGEN_STRONG_INLINE Packet2d pldexp(const Packet2d& a, const Packet2d& return out; } +// We specialize pldexp here, since the generic implementation uses Packet2l, which is not well +// supported by SSE, and has more range than is needed for exponents. +template <> +EIGEN_STRONG_INLINE Packet2d pldexp_fast(const Packet2d& a, const Packet2d& exponent) { + // Clamp exponent to [-1023, 1024] + const Packet2d min_exponent = pset1(-1023.0); + const Packet2d max_exponent = pset1(1024.0); + const Packet2d e = pmin(pmax(exponent, min_exponent), max_exponent); + + // Convert e to integer and swizzle to low-order bits. + const Packet4i ei = vec4i_swizzle1(_mm_cvtpd_epi32(e), 0, 3, 1, 3); + + // Compute 2^e multiply: + const Packet4i bias = _mm_set_epi32(0, 1023, 0, 1023); + const Packet2d c = _mm_castsi128_pd(_mm_slli_epi64(padd(ei, bias), 52)); // 2^e + return pmul(a, c); +} + // with AVX, the default implementations based on pload1 are faster #ifndef __AVX__ template <> diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h index 3687bb20db..0239262ae3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h @@ -27,7 +27,12 @@ struct assign_op { template EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { - internal::pstoret(a, b); + pstoret(a, b); + } + + template + EIGEN_STRONG_INLINE void assignPacketSegment(DstScalar* a, const Packet& b, Index begin, Index count) const { + pstoretSegment(a, b, begin, count); } }; @@ -36,7 +41,7 @@ template struct assign_op {}; template -struct functor_traits > { +struct functor_traits> { enum { Cost = NumTraits::ReadCost, PacketAccess = is_same::value && packet_traits::Vectorizable && @@ -45,88 +50,76 @@ struct functor_traits > { }; /** \internal - * \brief Template functor for scalar/packet assignment with addition + * \brief Template functor for scalar/packet compound assignment * */ -template -struct add_assign_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a += b; } +template +struct compound_assign_op { + using traits = functor_traits>; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { + assign_op().assignCoeff(a, Func().operator()(a, b)); + } template EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { - internal::pstoret(a, internal::padd(internal::ploadt(a), b)); + assign_op().template assignPacket( + a, Func().packetOp(ploadt(a), b)); + } + + template + EIGEN_STRONG_INLINE void assignPacketSegment(DstScalar* a, const Packet& b, Index begin, Index count) const { + assign_op().template assignPacketSegment( + a, Func().packetOp(ploadtSegment(a, begin, count), b), begin, count); } }; -template -struct functor_traits > { + +template +struct functor_traits> { enum { - Cost = NumTraits::ReadCost + NumTraits::AddCost, - PacketAccess = is_same::value && packet_traits::HasAdd + Cost = int(functor_traits>::Cost) + int(functor_traits::Cost), + PacketAccess = functor_traits>::PacketAccess && functor_traits::PacketAccess }; }; +/** \internal + * \brief Template functor for scalar/packet assignment with addition + * + */ +template +struct add_assign_op : compound_assign_op> {}; + +template +struct functor_traits> : add_assign_op::traits {}; + /** \internal * \brief Template functor for scalar/packet assignment with subtraction * */ -template -struct sub_assign_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a -= b; } +template +struct sub_assign_op : compound_assign_op> {}; - template - EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { - internal::pstoret(a, internal::psub(internal::ploadt(a), b)); - } -}; template -struct functor_traits > { - enum { - Cost = NumTraits::ReadCost + NumTraits::AddCost, - PacketAccess = is_same::value && packet_traits::HasSub - }; -}; +struct functor_traits> : sub_assign_op::traits {}; /** \internal * \brief Template functor for scalar/packet assignment with multiplication * */ template -struct mul_assign_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a *= b; } +struct mul_assign_op : compound_assign_op> {}; - template - EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { - internal::pstoret(a, internal::pmul(internal::ploadt(a), b)); - } -}; template -struct functor_traits > { - enum { - Cost = NumTraits::ReadCost + NumTraits::MulCost, - PacketAccess = is_same::value && packet_traits::HasMul - }; -}; +struct functor_traits> : mul_assign_op::traits {}; /** \internal - * \brief Template functor for scalar/packet assignment with diviving + * \brief Template functor for scalar/packet assignment with dividing * */ template -struct div_assign_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a /= b; } +struct div_assign_op : compound_assign_op> {}; - template - EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { - internal::pstoret(a, internal::pdiv(internal::ploadt(a), b)); - } -}; template -struct functor_traits > { - enum { - Cost = NumTraits::ReadCost + NumTraits::MulCost, - PacketAccess = is_same::value && packet_traits::HasDiv - }; -}; +struct functor_traits> : div_assign_op::traits {}; /** \internal * \brief Template functor for scalar/packet assignment with swapping @@ -158,7 +151,7 @@ struct swap_assign_op { } }; template -struct functor_traits > { +struct functor_traits> { enum { Cost = 3 * NumTraits::ReadCost, PacketAccess = diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h index c91e6bb526..a93b998b95 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h @@ -438,7 +438,6 @@ struct scalar_quotient_op : binary_op_base { } template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const { - maybe_raise_div_by_zero::run(b); return internal::pdiv(a, b); } }; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h index c53bb9073b..35dc73869a 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h @@ -19,7 +19,6 @@ namespace internal { template struct scalar_constant_op { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const scalar_constant_op& other) : m_other(other.m_other) {} EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_constant_op(const Scalar& other) : m_other(other) {} EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()() const { return m_other; } template @@ -29,7 +28,7 @@ struct scalar_constant_op { const Scalar m_other; }; template -struct functor_traits > { +struct functor_traits> { enum { Cost = 0 /* as the constant value should be loaded in register only once for the whole expression */, PacketAccess = packet_traits::Vectorizable, @@ -37,6 +36,18 @@ struct functor_traits > { }; }; +template +struct scalar_zero_op { + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE scalar_zero_op() = default; + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator()() const { return Scalar(0); } + template + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const PacketType packetOp() const { + return internal::pzero(PacketType()); + } +}; +template +struct functor_traits> : functor_traits> {}; + template struct scalar_identity_op { template @@ -45,7 +56,7 @@ struct scalar_identity_op { } }; template -struct functor_traits > { +struct functor_traits> { enum { Cost = NumTraits::AddCost, PacketAccess = false, IsRepeatable = true }; }; @@ -75,18 +86,19 @@ struct linspaced_op_impl { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const { // Principle: // [low, ..., low] + ( [step, ..., step] * ( [i, ..., i] + [0, ..., size] ) ) + Packet low = pset1(m_low); + Packet high = pset1(m_high); + Packet step = pset1(m_step); if (m_flip) { Packet pi = plset(Scalar(i - m_size1)); - Packet res = padd(pset1(m_high), pmul(pset1(m_step), pi)); - if (EIGEN_PREDICT_TRUE(i != 0)) return res; - Packet mask = pcmp_lt(pset1(0), plset(0)); - return pselect(mask, res, pset1(m_low)); + Packet res = pmadd(step, pi, high); + Packet mask = pcmp_lt(pzero(res), plset(Scalar(i))); + return pselect(mask, res, low); } else { Packet pi = plset(Scalar(i)); - Packet res = padd(pset1(m_low), pmul(pset1(m_step), pi)); - if (EIGEN_PREDICT_TRUE(i != m_size1 - unpacket_traits::size + 1)) return res; - Packet mask = pcmp_lt(plset(0), pset1(unpacket_traits::size - 1)); - return pselect(mask, res, pset1(m_high)); + Packet res = pmadd(step, pi, low); + Packet mask = pcmp_lt(pi, pset1(Scalar(m_size1))); + return pselect(mask, res, high); } } @@ -128,11 +140,10 @@ struct linspaced_op_impl { template struct linspaced_op; template -struct functor_traits > { +struct functor_traits> { enum { Cost = 1, - PacketAccess = - (!NumTraits::IsInteger) && packet_traits::HasSetLinear && packet_traits::HasBlend, + PacketAccess = (!NumTraits::IsInteger) && packet_traits::HasSetLinear, /*&& ((!NumTraits::IsInteger) || packet_traits::HasDiv),*/ // <- vectorization for integer is // currently disabled IsRepeatable = true @@ -182,7 +193,7 @@ struct equalspaced_op { }; template -struct functor_traits > { +struct functor_traits> { enum { Cost = NumTraits::AddCost + NumTraits::MulCost, PacketAccess = diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h index defd3c2a18..ba7d97a038 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h @@ -558,11 +558,15 @@ struct functor_traits> { template struct scalar_cbrt_op { EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::cbrt(a); } + template + EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { + return internal::pcbrt(a); + } }; template struct functor_traits> { - enum { Cost = 5 * NumTraits::MulCost, PacketAccess = false }; + enum { Cost = 20 * NumTraits::MulCost, PacketAccess = packet_traits::HasCbrt }; }; /** \internal @@ -1292,7 +1296,7 @@ struct scalar_logistic_op { p = pmadd(r2, p, p_low); // 4. Undo subtractive range reduction exp(m*ln(2) + r) = 2^m * exp(r). - Packet e = pldexp_fast_impl::run(p, m); + Packet e = pldexp_fast(p, m); // 5. Undo multiplicative range reduction by using exp(r) = exp(r/2)^2. e = pmul(e, e); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h index b65c246e7d..e72c6b48e4 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -305,7 +305,8 @@ inline bool useSpecificBlockingSizes(Index& k, Index& m, Index& n) { * \param[in,out] k Input: the third dimension of the product. Output: the blocking size along the same dimension. * \param[in,out] m Input: the number of rows of the left hand side. Output: the blocking size along the same dimension. * \param[in,out] n Input: the number of columns of the right hand side. Output: the blocking size along the same - * dimension. + * dimension. + * \param[in] num_threads Input: the number of threads used for the computation. * * Given a m x k times k x n matrix product of scalar types \c LhsScalar and \c RhsScalar, * this function computes the blocking size parameters along the respective dimensions @@ -1117,7 +1118,7 @@ struct lhs_process_one_packet { // loops on each largest micro horizontal panel of lhs // (LhsProgress x depth) for (Index i = peelStart; i < peelEnd; i += LhsProgress) { -#if EIGEN_ARCH_ARM64 +#if EIGEN_ARCH_ARM64 || EIGEN_ARCH_LOONGARCH64 EIGEN_IF_CONSTEXPR(nr >= 8) { for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { const LhsScalar* blA = &blockA[i * strideA + offsetA * (LhsProgress)]; @@ -1467,7 +1468,7 @@ EIGEN_DONT_INLINE void gebp_kernel= 8) { for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { for (Index i = i1; i < actual_panel_end; i += 3 * LhsProgress) { @@ -1935,7 +1936,7 @@ EIGEN_DONT_INLINE void gebp_kernel= 8) { for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { for (Index i = i1; i < actual_panel_end; i += 2 * LhsProgress) { @@ -2326,7 +2327,7 @@ EIGEN_DONT_INLINE void gebp_kernel= 8) { // loop on each panel of the rhs for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { @@ -2852,7 +2853,7 @@ EIGEN_DONT_INLINE void gemm_pack_rhs= 8) { for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { // skip what we have before @@ -3035,7 +3036,7 @@ struct gemm_pack_rhs= 4 ? (cols / 4) * 4 : 0; Index count = 0; -#if EIGEN_ARCH_ARM64 +#if EIGEN_ARCH_ARM64 || EIGEN_ARCH_LOONGARCH64 EIGEN_IF_CONSTEXPR(nr >= 8) { for (Index j2 = 0; j2 < packet_cols8; j2 += 8) { // skip what we have before diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h index f47615f727..bf27567572 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h @@ -308,15 +308,19 @@ struct general_product_to_triangular_selector +template template -EIGEN_DEVICE_FUNC TriangularView& TriangularViewImpl::_assignProduct( - const ProductType& prod, const Scalar& alpha, bool beta) { - EIGEN_STATIC_ASSERT((UpLo & UnitDiag) == 0, WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED); +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename TriangularViewImpl::TriangularViewType& +TriangularViewImpl::_assignProduct( + const ProductType& prod, const typename TriangularViewImpl::Scalar& alpha, bool beta) { + EIGEN_STATIC_ASSERT((Mode_ & UnitDiag) == 0, WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED); eigen_assert(derived().nestedExpression().rows() == prod.rows() && derived().cols() == prod.cols()); - general_product_to_triangular_selector::InnerSize == 1>:: - run(derived().nestedExpression().const_cast_derived(), prod, alpha, beta); + general_product_to_triangular_selector::InnerSize == 1>::run(derived() + .nestedExpression() + .const_cast_derived(), + prod, alpha, beta); return derived(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h index 018efa64b2..4f3668944f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h @@ -153,7 +153,14 @@ inline void manage_multi_threading(Action action, int* v) { #endif } else if (action == GetAction) { eigen_internal_assert(v != nullptr); +#if defined(EIGEN_HAS_OPENMP) + if (m_maxThreads > 0) + *v = m_maxThreads; + else + *v = omp_get_max_threads(); +#else *v = m_maxThreads; +#endif } else { eigen_internal_assert(false); } @@ -210,7 +217,7 @@ EIGEN_STRONG_INLINE void parallelize_gemm(const Functor& func, Index rows, Index // Note that the actual number of threads might be lower than the number of // requested ones Index actual_threads = omp_get_num_threads(); - GemmParallelInfo info(i, static_cast(actual_threads), task_info); + GemmParallelInfo info(static_cast(i), static_cast(actual_threads), task_info); Index blockCols = (cols / actual_threads) & ~Index(0x3); Index blockRows = (rows / actual_threads); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h index d0fd181ecf..a9430716a3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h @@ -39,8 +39,12 @@ #endif // Align to the boundary that avoids false sharing. -// https://en.cppreference.com/w/cpp/thread/hardware_destructive_interference_size -#ifdef __cpp_lib_hardware_interference_size +// https://en.cppreference.com/w/cpp/thread/hardware_destructive_interference_size +// There is a bug in android NDK < r26 where the macro is defined but std::hardware_destructive_interference_size +// still does not exist. +#if defined(__cpp_lib_hardware_interference_size) && __cpp_lib_hardware_interference_size >= 201603 && \ + (!EIGEN_OS_ANDROID || __NDK_MAJOR__ + 0 >= 26) +#include #define EIGEN_ALIGN_TO_AVOID_FALSE_SHARING EIGEN_ALIGN_TO_BOUNDARY(std::hardware_destructive_interference_size) #else // Overalign for the cache line size of 128 bytes (Apple M1) @@ -99,8 +103,8 @@ // certain common platform (compiler+architecture combinations) to avoid these problems. // Only static alignment is really problematic (relies on nonstandard compiler extensions), // try to keep heap alignment even when we have to disable static alignment. -#if EIGEN_COMP_GNUC && \ - !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64 || EIGEN_ARCH_MIPS) +#if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64 || \ + EIGEN_ARCH_MIPS || EIGEN_ARCH_LOONGARCH64) #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1 #else #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0 @@ -288,6 +292,8 @@ #ifdef __AVX512FP16__ #ifdef __AVX512VL__ #define EIGEN_VECTORIZE_AVX512FP16 +// Built-in _Float16. +#define EIGEN_HAS_BUILTIN_FLOAT16 1 #else #if EIGEN_COMP_GNUC #error Please add -mavx512vl to your compiler flags: compiling with -mavx512fp16 alone without AVX512-VL is not supported. @@ -436,6 +442,12 @@ extern "C" { #include #endif +#elif (defined __loongarch64 && defined __loongarch_sx) + +#define EIGEN_VECTORIZE +#define EIGEN_VECTORIZE_LSX +#include + #elif defined __HVX__ && (__HVX_LENGTH__ == 128) #define EIGEN_VECTORIZE @@ -526,6 +538,8 @@ inline static const char *SimdInstructionSetsInUse(void) { return "S390X ZVECTOR"; #elif defined(EIGEN_VECTORIZE_MSA) return "MIPS MSA"; +#elif defined(EIGEN_VECTORIZE_LSX) + return "LOONGARCH64 LSX"; #else return "None"; #endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h index 4f0b273ce0..fcc2db8226 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h @@ -474,6 +474,7 @@ enum Type { MSA = 0x5, SVE = 0x6, HVX = 0x7, + LSX = 0x8, #if defined EIGEN_VECTORIZE_SSE Target = SSE #elif defined EIGEN_VECTORIZE_ALTIVEC @@ -488,6 +489,8 @@ enum Type { Target = MSA #elif defined EIGEN_VECTORIZE_HVX Target = HVX +#elif defined EIGEN_VECTORIZE_LSX + Target = LSX #else Target = Generic #endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h index 7ecd7bf8cc..031d75a4c8 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h @@ -87,6 +87,9 @@ #if __GNUC__ >= 12 #pragma GCC diagnostic ignored "-Warray-bounds" #endif +#if __GNUC__ >= 13 +#pragma GCC diagnostic ignored "-Wstringop-overflow" +#endif #endif #if defined __NVCC__ && defined __CUDACC__ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h index f2fd10bb16..6c4c22d413 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/EmulateArray.h @@ -248,15 +248,15 @@ namespace internal { #endif template -constexpr inline T& array_get(std::array& a) { +constexpr T& array_get(std::array& a) { return (T&)STD_GET_ARR_HACK; } template -constexpr inline T&& array_get(std::array&& a) { +constexpr T&& array_get(std::array&& a) { return (T&&)STD_GET_ARR_HACK; } template -constexpr inline T const& array_get(std::array const& a) { +constexpr T const& array_get(std::array const& a) { return (T const&)STD_GET_ARR_HACK; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h index cf25359287..3c0bc461e2 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h @@ -497,7 +497,7 @@ class MatrixComplexPowerReturnValue; namespace internal { template struct stem_function { - typedef std::complex::Real> ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef ComplexScalar type(ComplexScalar, int); }; } // namespace internal @@ -505,6 +505,20 @@ struct stem_function { template struct DeviceWrapper; +namespace internal { +template +struct eigen_fill_helper; +template ::value> +struct eigen_fill_impl; +template +struct eigen_memset_helper; +template ::value> +struct eigen_zero_impl; + +template +struct has_packet_segment : std::false_type {}; +} // namespace internal + } // end namespace Eigen #endif // EIGEN_FORWARDDECLARATIONS_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h index fb56051641..00d55577d9 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h @@ -376,6 +376,13 @@ #define EIGEN_ARCH_MIPS 0 #endif +/// \internal EIGEN_ARCH_LOONGARCH64 set to 1 if the architecture is LOONGARCH64 +#if defined(__loongarch64) +#define EIGEN_ARCH_LOONGARCH64 1 +#else +#define EIGEN_ARCH_LOONGARCH64 0 +#endif + /// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC #if defined(__sparc__) || defined(__sparc) #define EIGEN_ARCH_SPARC 1 @@ -419,6 +426,16 @@ // note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain. #if defined(__ANDROID__) || defined(ANDROID) #define EIGEN_OS_ANDROID 1 + +// Since NDK r16, `__NDK_MAJOR__` and `__NDK_MINOR__` are defined in +// . For NDK < r16, users should define these macros, +// e.g. `-D__NDK_MAJOR__=11 -D__NKD_MINOR__=0` for NDK r11. +#if defined __has_include +#if __has_include() +#include +#endif +#endif + #else #define EIGEN_OS_ANDROID 0 #endif @@ -1260,11 +1277,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void ignore_unused_variable(cons #define EIGEN_CATCH(X) else #endif -#define EIGEN_NOEXCEPT noexcept -#define EIGEN_NOEXCEPT_IF(x) noexcept(x) -#define EIGEN_NO_THROW noexcept(true) -#define EIGEN_EXCEPTION_SPEC(X) noexcept(false) - // The all function is used to enable a variadic version of eigen_assert which can take a parameter pack as its input. namespace Eigen { namespace internal { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h index 54b556dd01..db5bb8950e 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MaxSizeVector.h @@ -13,7 +13,7 @@ namespace Eigen { /** \class MaxSizeVector - * \ingroup Core + * \ingroup Core_Module * * \brief The MaxSizeVector class. * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h index a278c9129b..89b2fff000 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h @@ -141,23 +141,24 @@ EIGEN_DEVICE_FUNC inline void throw_std_bad_alloc() { */ EIGEN_DEVICE_FUNC inline void* handmade_aligned_malloc(std::size_t size, std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES) { - eigen_assert(alignment >= sizeof(void*) && alignment <= 128 && (alignment & (alignment - 1)) == 0 && - "Alignment must be at least sizeof(void*), less than or equal to 128, and a power of 2"); + eigen_assert(alignment >= sizeof(void*) && alignment <= 256 && (alignment & (alignment - 1)) == 0 && + "Alignment must be at least sizeof(void*), less than or equal to 256, and a power of 2"); check_that_malloc_is_allowed(); EIGEN_USING_STD(malloc) void* original = malloc(size + alignment); if (original == nullptr) return nullptr; - uint8_t offset = static_cast(alignment - (reinterpret_cast(original) & (alignment - 1))); + std::size_t offset = alignment - (reinterpret_cast(original) & (alignment - 1)); void* aligned = static_cast(static_cast(original) + offset); - *(static_cast(aligned) - 1) = offset; + // Store offset - 1, since it is guaranteed to be at least 1. + *(static_cast(aligned) - 1) = static_cast(offset - 1); return aligned; } /** \internal Frees memory allocated with handmade_aligned_malloc */ EIGEN_DEVICE_FUNC inline void handmade_aligned_free(void* ptr) { if (ptr != nullptr) { - uint8_t offset = static_cast(*(static_cast(ptr) - 1)); + std::size_t offset = static_cast(*(static_cast(ptr) - 1)) + 1; void* original = static_cast(static_cast(ptr) - offset); check_that_malloc_is_allowed(); @@ -174,7 +175,7 @@ EIGEN_DEVICE_FUNC inline void handmade_aligned_free(void* ptr) { EIGEN_DEVICE_FUNC inline void* handmade_aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size, std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES) { if (ptr == nullptr) return handmade_aligned_malloc(new_size, alignment); - uint8_t old_offset = *(static_cast(ptr) - 1); + std::size_t old_offset = static_cast(*(static_cast(ptr) - 1)) + 1; void* old_original = static_cast(ptr) - old_offset; check_that_malloc_is_allowed(); @@ -182,14 +183,15 @@ EIGEN_DEVICE_FUNC inline void* handmade_aligned_realloc(void* ptr, std::size_t n void* original = realloc(old_original, new_size + alignment); if (original == nullptr) return nullptr; if (original == old_original) return ptr; - uint8_t offset = static_cast(alignment - (reinterpret_cast(original) & (alignment - 1))); + std::size_t offset = alignment - (reinterpret_cast(original) & (alignment - 1)); void* aligned = static_cast(static_cast(original) + offset); if (offset != old_offset) { const void* src = static_cast(static_cast(original) + old_offset); std::size_t count = (std::min)(new_size, old_size); std::memmove(aligned, src, count); } - *(static_cast(aligned) - 1) = offset; + // Store offset - 1, since it is guaranteed to be at least 1. + *(static_cast(aligned) - 1) = static_cast(offset - 1); return aligned; } @@ -831,46 +833,44 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void* eigen_aligned_alloca_helper(void* pt // HIP does not support new/delete on device. #if EIGEN_MAX_ALIGN_BYTES != 0 && !defined(EIGEN_HIP_DEVICE_COMPILE) -#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ - EIGEN_DEVICE_FUNC void* operator new(std::size_t size, const std::nothrow_t&) EIGEN_NO_THROW { \ - EIGEN_TRY { return Eigen::internal::conditional_aligned_malloc(size); } \ - EIGEN_CATCH(...) { return 0; } \ +#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ + EIGEN_DEVICE_FUNC void* operator new(std::size_t size, const std::nothrow_t&) noexcept { \ + EIGEN_TRY { return Eigen::internal::conditional_aligned_malloc(size); } \ + EIGEN_CATCH(...) { return 0; } \ } -#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ - EIGEN_DEVICE_FUNC void* operator new(std::size_t size) { \ - return Eigen::internal::conditional_aligned_malloc(size); \ - } \ - EIGEN_DEVICE_FUNC void* operator new[](std::size_t size) { \ - return Eigen::internal::conditional_aligned_malloc(size); \ - } \ - EIGEN_DEVICE_FUNC void operator delete(void* ptr) EIGEN_NO_THROW { \ - Eigen::internal::conditional_aligned_free(ptr); \ - } \ - EIGEN_DEVICE_FUNC void operator delete[](void* ptr) EIGEN_NO_THROW { \ - Eigen::internal::conditional_aligned_free(ptr); \ - } \ - EIGEN_DEVICE_FUNC void operator delete(void* ptr, std::size_t /* sz */) EIGEN_NO_THROW { \ - Eigen::internal::conditional_aligned_free(ptr); \ - } \ - EIGEN_DEVICE_FUNC void operator delete[](void* ptr, std::size_t /* sz */) EIGEN_NO_THROW { \ - Eigen::internal::conditional_aligned_free(ptr); \ - } \ - /* in-place new and delete. since (at least afaik) there is no actual */ \ - /* memory allocated we can safely let the default implementation handle */ \ - /* this particular case. */ \ - EIGEN_DEVICE_FUNC static void* operator new(std::size_t size, void* ptr) { return ::operator new(size, ptr); } \ - EIGEN_DEVICE_FUNC static void* operator new[](std::size_t size, void* ptr) { return ::operator new[](size, ptr); } \ - EIGEN_DEVICE_FUNC void operator delete(void* memory, void* ptr) EIGEN_NO_THROW { \ - return ::operator delete(memory, ptr); \ - } \ - EIGEN_DEVICE_FUNC void operator delete[](void* memory, void* ptr) EIGEN_NO_THROW { \ - return ::operator delete[](memory, ptr); \ - } \ - /* nothrow-new (returns zero instead of std::bad_alloc) */ \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ - EIGEN_DEVICE_FUNC void operator delete(void* ptr, const std::nothrow_t&) EIGEN_NO_THROW { \ - Eigen::internal::conditional_aligned_free(ptr); \ - } \ +#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + EIGEN_DEVICE_FUNC void* operator new(std::size_t size) { \ + return Eigen::internal::conditional_aligned_malloc(size); \ + } \ + EIGEN_DEVICE_FUNC void* operator new[](std::size_t size) { \ + return Eigen::internal::conditional_aligned_malloc(size); \ + } \ + EIGEN_DEVICE_FUNC void operator delete(void* ptr) noexcept { \ + Eigen::internal::conditional_aligned_free(ptr); \ + } \ + EIGEN_DEVICE_FUNC void operator delete[](void* ptr) noexcept { \ + Eigen::internal::conditional_aligned_free(ptr); \ + } \ + EIGEN_DEVICE_FUNC void operator delete(void* ptr, std::size_t /* sz */) noexcept { \ + Eigen::internal::conditional_aligned_free(ptr); \ + } \ + EIGEN_DEVICE_FUNC void operator delete[](void* ptr, std::size_t /* sz */) noexcept { \ + Eigen::internal::conditional_aligned_free(ptr); \ + } \ + /* in-place new and delete. since (at least afaik) there is no actual */ \ + /* memory allocated we can safely let the default implementation handle */ \ + /* this particular case. */ \ + EIGEN_DEVICE_FUNC static void* operator new(std::size_t size, void* ptr) { return ::operator new(size, ptr); } \ + EIGEN_DEVICE_FUNC static void* operator new[](std::size_t size, void* ptr) { return ::operator new[](size, ptr); } \ + EIGEN_DEVICE_FUNC void operator delete(void* memory, void* ptr) noexcept { return ::operator delete(memory, ptr); } \ + EIGEN_DEVICE_FUNC void operator delete[](void* memory, void* ptr) noexcept { \ + return ::operator delete[](memory, ptr); \ + } \ + /* nothrow-new (returns zero instead of std::bad_alloc) */ \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ + EIGEN_DEVICE_FUNC void operator delete(void* ptr, const std::nothrow_t&) noexcept { \ + Eigen::internal::conditional_aligned_free(ptr); \ + } \ typedef void eigen_aligned_operator_new_marker_type; #else #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) @@ -913,7 +913,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void* eigen_aligned_alloca_helper(void* pt * \sa \blank \ref TopicStlContainers. */ template -class aligned_allocator : public std::allocator { +class aligned_allocator { public: typedef std::size_t size_type; typedef std::ptrdiff_t difference_type; @@ -928,14 +928,21 @@ class aligned_allocator : public std::allocator { typedef aligned_allocator other; }; - aligned_allocator() : std::allocator() {} + aligned_allocator() = default; - aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} + aligned_allocator(const aligned_allocator&) = default; template - aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} + aligned_allocator(const aligned_allocator&) {} - ~aligned_allocator() {} + template + constexpr bool operator==(const aligned_allocator&) const noexcept { + return true; + } + template + constexpr bool operator!=(const aligned_allocator&) const noexcept { + return false; + } #if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_STRICT_AT_LEAST(7, 0, 0) // In gcc std::allocator::max_size() is bugged making gcc triggers a warning: @@ -1307,7 +1314,8 @@ inline int queryTopLevelCacheSize() { * This wraps C++20's std::construct_at, using placement new instead if it is not available. */ -#if EIGEN_COMP_CXXVER >= 20 +#if EIGEN_COMP_CXXVER >= 20 && defined(__cpp_lib_constexpr_dynamic_alloc) && \ + __cpp_lib_constexpr_dynamic_alloc >= 201907L using std::construct_at; #else template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h index de325b7f02..4c56a2b445 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h @@ -79,7 +79,6 @@ typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex; * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. * \sa \blank \ref TopicPreprocessorDirectives, StorageIndex. */ - typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE Index; namespace internal { @@ -91,12 +90,8 @@ namespace internal { * we however don't want to add a dependency to Boost. */ -struct true_type { - enum { value = 1 }; -}; -struct false_type { - enum { value = 0 }; -}; +using std::false_type; +using std::true_type; template struct bool_constant; @@ -221,7 +216,7 @@ struct is_void : is_same> {}; * * Post C++17: Uses std::void_t */ -#if EIGEN_COMP_CXXVER >= 17 +#if EIGEN_COMP_CXXVER >= 17 && defined(__cpp_lib_void_t) && __cpp_lib_void_t >= 201411L using std::void_t; #else template @@ -339,24 +334,27 @@ struct array_size> { * * For C++20, this function just forwards to `std::ssize`, or any ADL discoverable `ssize` function. */ -#if EIGEN_COMP_CXXVER < 20 || EIGEN_GNUC_STRICT_LESS_THAN(10, 0, 0) +#if EIGEN_COMP_CXXVER >= 20 && defined(__cpp_lib_ssize) && __cpp_lib_ssize >= 201902L + template -EIGEN_CONSTEXPR auto index_list_size(const T& x) { +constexpr auto index_list_size(T&& x) { + using std::ssize; + return ssize(std::forward(x)); +} + +#else + +template +constexpr auto index_list_size(const T& x) { using R = std::common_type_t>; return static_cast(x.size()); } template -EIGEN_CONSTEXPR std::ptrdiff_t index_list_size(const T (&)[N]) { +constexpr std::ptrdiff_t index_list_size(const T (&)[N]) { return N; } -#else -template -EIGEN_CONSTEXPR auto index_list_size(T&& x) { - using std::ssize; - return ssize(std::forward(x)); -} -#endif // EIGEN_COMP_CXXVER +#endif /** \internal * Convenient struct to get the result type of a nullary, unary, binary, or @@ -639,21 +637,21 @@ template constexpr bool is_int_or_enum_v = std::is_enum::value || std::is_integral::value; template -inline constexpr void plain_enum_asserts(A, B) { +constexpr void plain_enum_asserts(A, B) { static_assert(is_int_or_enum_v, "Argument a must be an integer or enum"); static_assert(is_int_or_enum_v, "Argument b must be an integer or enum"); } /// \internal Gets the minimum of two values which may be integers or enums template -inline constexpr int plain_enum_min(A a, B b) { +constexpr int plain_enum_min(A a, B b) { plain_enum_asserts(a, b); return ((int)a <= (int)b) ? (int)a : (int)b; } /// \internal Gets the maximum of two values which may be integers or enums template -inline constexpr int plain_enum_max(A a, B b) { +constexpr int plain_enum_max(A a, B b) { plain_enum_asserts(a, b); return ((int)a >= (int)b) ? (int)a : (int)b; } @@ -665,7 +663,7 @@ inline constexpr int plain_enum_max(A a, B b) { * finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3. */ template -inline constexpr int min_size_prefer_dynamic(A a, B b) { +constexpr int min_size_prefer_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == 0 || (int)b == 0) return 0; if ((int)a == 1 || (int)b == 1) return 1; @@ -680,7 +678,7 @@ inline constexpr int min_size_prefer_dynamic(A a, B b) { * 0 and 3), it is not more than 3. */ template -inline constexpr int min_size_prefer_fixed(A a, B b) { +constexpr int min_size_prefer_fixed(A a, B b) { plain_enum_asserts(a, b); if ((int)a == 0 || (int)b == 0) return 0; if ((int)a == 1 || (int)b == 1) return 1; @@ -692,12 +690,18 @@ inline constexpr int min_size_prefer_fixed(A a, B b) { /// \internal see `min_size_prefer_fixed`. No need for a separate variant for MaxSizes here. template -inline constexpr int max_size_prefer_dynamic(A a, B b) { +constexpr int max_size_prefer_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == Dynamic || (int)b == Dynamic) return Dynamic; return plain_enum_max(a, b); } +template +inline constexpr int size_prefer_fixed(A a, B b) { + plain_enum_asserts(a, b); + return int(a) == Dynamic ? int(b) : int(a); +} + template inline constexpr bool enum_eq_not_dynamic(A a, B b) { plain_enum_asserts(a, b); @@ -706,46 +710,49 @@ inline constexpr bool enum_eq_not_dynamic(A a, B b) { } template -inline constexpr bool enum_lt_not_dynamic(A a, B b) { +constexpr bool enum_lt_not_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == Dynamic || (int)b == Dynamic) return false; return (int)a < (int)b; } template -inline constexpr bool enum_le_not_dynamic(A a, B b) { +constexpr bool enum_le_not_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == Dynamic || (int)b == Dynamic) return false; return (int)a <= (int)b; } template -inline constexpr bool enum_gt_not_dynamic(A a, B b) { +constexpr bool enum_gt_not_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == Dynamic || (int)b == Dynamic) return false; return (int)a > (int)b; } template -inline constexpr bool enum_ge_not_dynamic(A a, B b) { +constexpr bool enum_ge_not_dynamic(A a, B b) { plain_enum_asserts(a, b); if ((int)a == Dynamic || (int)b == Dynamic) return false; return (int)a >= (int)b; } /// \internal Calculate logical XOR at compile time -inline constexpr bool logical_xor(bool a, bool b) { return a != b; } +constexpr bool logical_xor(bool a, bool b) { return a != b; } /// \internal Calculate logical IMPLIES at compile time -inline constexpr bool check_implication(bool a, bool b) { return !a || b; } +constexpr bool check_implication(bool a, bool b) { return !a || b; } /// \internal Provide fallback for std::is_constant_evaluated for pre-C++20. -#if EIGEN_COMP_CXXVER >= 20 +#if EIGEN_COMP_CXXVER >= 20 && defined(__cpp_lib_is_constant_evaluated) && __cpp_lib_is_constant_evaluated >= 201811L using std::is_constant_evaluated; #else constexpr bool is_constant_evaluated() { return false; } #endif +template +using make_complex_t = std::conditional_t::IsComplex, Scalar, std::complex>; + } // end namespace internal } // end namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h index 2d4aeee23c..6823bca977 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/MoreMeta.h @@ -40,6 +40,7 @@ struct numeric_list { static constexpr T first_value = n; }; +// Ddoxygen doesn't like the recursive definition of gen_numeric_list. #ifndef EIGEN_PARSED_BY_DOXYGEN /* numeric list constructors * @@ -53,6 +54,7 @@ struct numeric_list { template struct gen_numeric_list : gen_numeric_list {}; + template struct gen_numeric_list { typedef numeric_list type; @@ -80,6 +82,10 @@ template struct gen_numeric_list_repeated { typedef numeric_list type; }; +#else +template +struct gen_numeric_list; +#endif // not EIGEN_PARSED_BY_DOXYGEN /* list manipulation: concatenate */ @@ -110,16 +116,20 @@ struct mconcat : concat::type> {}; template struct take; + template struct take> : concat, typename take>::type> {}; + template struct take> { typedef type_list<> type; }; + template struct take<0, type_list> { typedef type_list<> type; }; + template <> struct take<0, type_list<>> { typedef type_list<> type; @@ -128,13 +138,12 @@ struct take<0, type_list<>> { template struct take> : concat, typename take>::type> {}; -// XXX The following breaks in gcc-11, and is invalid anyways. -// template struct take> { typedef numeric_list type; -// }; + template struct take<0, numeric_list> { typedef numeric_list type; }; + template struct take<0, numeric_list> { typedef numeric_list type; @@ -173,7 +182,6 @@ template <> struct h_skip_helper_type<0> { typedef type_list<> type; }; -#endif // not EIGEN_PARSED_BY_DOXYGEN template struct h_skip { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h index e56940880f..17479505bd 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h @@ -40,7 +40,7 @@ struct get_compiletime_reshape_size { inline Index get_runtime_reshape_size(AutoSize_t /*size*/, Index other, Index total) { return total / other; } -constexpr inline int get_compiletime_reshape_order(int flags, int order) { +constexpr int get_compiletime_reshape_order(int flags, int order) { return order == AutoOrder ? flags & RowMajorBit : order; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h index cecbee8652..a0e160eba4 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h @@ -158,8 +158,8 @@ class variable_if_dynamic { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); } - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR T value() { return T(Value); } - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR operator T() const { return T(Value); } + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr T value() { return T(Value); } + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr operator T() const { return T(Value); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T v) const { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); @@ -171,7 +171,7 @@ class variable_if_dynamic { T m_value; public: - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value = 0) EIGEN_NO_THROW : m_value(value) {} + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value = 0) noexcept : m_value(value) {} EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator T() const { return m_value; } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; } @@ -186,7 +186,7 @@ class variable_if_dynamicindex { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); } - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR T value() { return T(Value); } + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr T value() { return T(Value); } EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {} }; @@ -315,7 +315,7 @@ struct find_packet_by_size { }; #if EIGEN_MAX_STATIC_ALIGN_BYTES > 0 -constexpr inline int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) { +constexpr int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) { if ((ArrayBytes % AlignmentBytes) == 0) { return AlignmentBytes; } else if (EIGEN_MIN_ALIGN_BYTES < AlignmentBytes) { @@ -327,7 +327,7 @@ constexpr inline int compute_default_alignment_helper(int ArrayBytes, int Alignm #else // If static alignment is disabled, no need to bother. // This also avoids a division by zero -constexpr inline int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) { +constexpr int compute_default_alignment_helper(int ArrayBytes, int AlignmentBytes) { EIGEN_UNUSED_VARIABLE(ArrayBytes); EIGEN_UNUSED_VARIABLE(AlignmentBytes); return 0; @@ -362,7 +362,7 @@ class make_proper_matrix_type { typedef Matrix type; }; -constexpr inline unsigned compute_matrix_flags(int Options) { +constexpr unsigned compute_matrix_flags(int Options) { unsigned row_major_bit = Options & RowMajor ? RowMajorBit : 0; // FIXME currently we still have to handle DirectAccessBit at the expression level to handle DenseCoeffsBase<> // and then propagate this information to the evaluator's flags. @@ -370,7 +370,7 @@ constexpr inline unsigned compute_matrix_flags(int Options) { return DirectAccessBit | LvalueBit | NestByRefBit | row_major_bit; } -constexpr inline int size_at_compile_time(int rows, int cols) { +constexpr int size_at_compile_time(int rows, int cols) { if (rows == 0 || cols == 0) return 0; if (rows == Dynamic || cols == Dynamic) return Dynamic; return rows * cols; @@ -885,8 +885,12 @@ struct scalar_div_cost { }; template -struct scalar_div_cost, Vectorized> { - enum { value = 2 * scalar_div_cost::value + 6 * NumTraits::MulCost + 3 * NumTraits::AddCost }; +struct scalar_div_cost::IsComplex>> { + using RealScalar = typename NumTraits::Real; + enum { + value = + 2 * scalar_div_cost::value + 6 * NumTraits::MulCost + 3 * NumTraits::AddCost + }; }; template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h index 60a24a899e..50fa3b8095 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -70,7 +70,7 @@ class ComplexEigenSolver { * \c float or \c double) and just \c Scalar if #Scalar is * complex. */ - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; /** \brief Type for vector of eigenvalues as returned by eigenvalues(). * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h index a33e46ee79..22433f2bde 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h @@ -75,7 +75,7 @@ class ComplexSchur { * \c float or \c double) and just \c Scalar if #Scalar is * complex. */ - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; /** \brief Type for the matrices in the Schur decomposition. * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h index f73d58f870..9dba7bd186 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h @@ -89,7 +89,7 @@ class EigenSolver { * \c float or \c double) and just \c Scalar if #Scalar is * complex. */ - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; /** \brief Type for vector of eigenvalues as returned by eigenvalues(). * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h index b114cfab55..c0a61dcd4e 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -83,7 +83,7 @@ class GeneralizedEigenSolver { * \c float or \c double) and just \c Scalar if #Scalar is * complex. */ - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; /** \brief Type for vector of real scalar values eigenvalues as returned by betas(). * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h index 3466f51c10..a54d82d4e3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h @@ -69,7 +69,7 @@ class RealQZ { MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime }; typedef typename MatrixType::Scalar Scalar; - typedef std::complex::Real> ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 typedef Matrix EigenvalueType; @@ -449,23 +449,23 @@ inline void RealQZ::step(Index f, Index l, Index iter) { Index lr = (std::min)(k + 4, dim); // last row to update Map > tmp(m_workspace.data(), lr); // S - tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2; + tmp.noalias() = m_S.template middleCols<2>(k).topRows(lr) * essential2; tmp += m_S.col(k + 2).head(lr); m_S.col(k + 2).head(lr) -= tau * tmp; - m_S.template middleCols<2>(k).topRows(lr) -= (tau * tmp) * essential2.adjoint(); + m_S.template middleCols<2>(k).topRows(lr).noalias() -= (tau * tmp) * essential2.adjoint(); // T tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2; tmp += m_T.col(k + 2).head(lr); m_T.col(k + 2).head(lr) -= tau * tmp; - m_T.template middleCols<2>(k).topRows(lr) -= (tau * tmp) * essential2.adjoint(); + m_T.template middleCols<2>(k).topRows(lr).noalias() -= (tau * tmp) * essential2.adjoint(); } if (m_computeQZ) { // Z Map > tmp(m_workspace.data(), dim); - tmp = essential2.adjoint() * (m_Z.template middleRows<2>(k)); + tmp.noalias() = essential2.adjoint() * (m_Z.template middleRows<2>(k)); tmp += m_Z.row(k + 2); m_Z.row(k + 2) -= tau * tmp; - m_Z.template middleRows<2>(k) -= essential2 * (tau * tmp); + m_Z.template middleRows<2>(k).noalias() -= essential2 * (tau * tmp); } m_T.coeffRef(k + 2, k) = m_T.coeffRef(k + 2, k + 1) = Scalar(0.0); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h index 5cef6587b3..54a74e2f59 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h @@ -66,7 +66,7 @@ class RealSchur { MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime }; typedef typename MatrixType::Scalar Scalar; - typedef std::complex::Real> ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 typedef Matrix EigenvalueType; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h index e49e9db5af..9cc92011f7 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -345,7 +345,7 @@ EIGEN_DEVICE_FUNC void tridiagonalization_inplace(MatrixType& matA, CoeffVectorT // Apply similarity transformation to remaining columns, // i.e., A = H A H' where H = I - h v v' and v = matA.col(i).tail(n-i-1) - matA.col(i).coeffRef(i + 1) = 1; + matA.col(i).coeffRef(i + 1) = Scalar(1); hCoeffs.tail(n - i - 1).noalias() = (matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView() * @@ -379,6 +379,8 @@ struct tridiagonalization_inplace_selector; * decomposition. * \param[out] subdiag The subdiagonal of the tridiagonal matrix T in * the decomposition. + * \param[out] hcoeffs + * \param[out] workspace * \param[in] extractQ If true, the orthogonal matrix Q in the * decomposition is computed and stored in \p mat. * @@ -513,8 +515,8 @@ struct TridiagonalizationMatrixTReturnType : public ReturnByValue() = m_matrix.template diagonal<-1>(); } - EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); } - EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + constexpr Index rows() const noexcept { return m_matrix.rows(); } + constexpr Index cols() const noexcept { return m_matrix.cols(); } protected: typename MatrixType::Nested m_matrix; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h index 64c1b651a6..795af0d8d6 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h @@ -69,10 +69,10 @@ class Homogeneous : public MatrixBase >, int EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) : m_matrix(matrix) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_matrix.rows() + (int(Direction) == Vertical ? 1 : 0); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols() + (int(Direction) == Horizontal ? 1 : 0); } @@ -244,8 +244,8 @@ struct homogeneous_left_product_impl, Lhs> EIGEN_DEVICE_FUNC homogeneous_left_product_impl(const Lhs& lhs, const MatrixType& rhs) : m_lhs(take_matrix_for_product::run(lhs)), m_rhs(rhs) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_rhs.cols(); } template EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { @@ -275,8 +275,8 @@ struct homogeneous_right_product_impl, Rhs> typedef remove_all_t RhsNested; EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {} - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_rhs.cols(); } template EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h index a8e050236e..fc708ee223 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/OrthoMethods.h @@ -78,27 +78,25 @@ struct cross_impl { * spanned by the two vectors. * * \note With complex numbers, the cross product is implemented as - * \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times - * \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} + \mathbf{b} \times \mathbf{c})\f$ + * \f[ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times + * \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} + \mathbf{b} \times \mathbf{c}).\f] + * This definition preserves the orthogonality condition that \f$\mathbf{u} \cdot (\mathbf{u} \times \mathbf{v}) = + * \mathbf{v} \cdot (\mathbf{u} \times \mathbf{v}) = 0\f$. * * \sa MatrixBase::cross3() */ template template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE -#ifndef EIGEN_PARSED_BY_DOXYGEN - typename internal::cross_impl::return_type -#else - inline std::conditional_t -#endif - MatrixBase::cross(const MatrixBase& other) const { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::cross_impl::return_type +MatrixBase::cross(const MatrixBase& other) const { return internal::cross_impl::run(*this, other); } namespace internal { template ::Flags & evaluator::Flags) & PacketAccessBit)> + bool Vectorizable = + bool((int(evaluator::Flags) & int(evaluator::Flags)) & PacketAccessBit)> struct cross3_impl { EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type::type run(const VectorLhs& lhs, const VectorRhs& rhs) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h index 1d8ded9bd6..147e6e3ae3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Quaternion.h @@ -57,22 +57,22 @@ class QuaternionBase : public RotationBase { typedef AngleAxis AngleAxisType; /** \returns the \c x coefficient */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); } + EIGEN_DEVICE_FUNC constexpr CoeffReturnType x() const { return this->derived().coeffs().coeff(0); } /** \returns the \c y coefficient */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); } + EIGEN_DEVICE_FUNC constexpr CoeffReturnType y() const { return this->derived().coeffs().coeff(1); } /** \returns the \c z coefficient */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); } + EIGEN_DEVICE_FUNC constexpr CoeffReturnType z() const { return this->derived().coeffs().coeff(2); } /** \returns the \c w coefficient */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); } + EIGEN_DEVICE_FUNC constexpr CoeffReturnType w() const { return this->derived().coeffs().coeff(3); } /** \returns a reference to the \c x coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); } + EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType x() { return this->derived().coeffs().x(); } /** \returns a reference to the \c y coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); } + EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType y() { return this->derived().coeffs().y(); } /** \returns a reference to the \c z coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); } + EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType z() { return this->derived().coeffs().z(); } /** \returns a reference to the \c w coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); } + EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType w() { return this->derived().coeffs().w(); } /** \returns a read-only vector expression of the imaginary part (x,y,z) */ EIGEN_DEVICE_FUNC inline const VectorBlock vec() const { return coeffs().template head<3>(); } @@ -346,13 +346,11 @@ class Quaternion : public QuaternionBase > { // We define a copy constructor, which means we don't get an implicit move constructor or assignment operator. /** Default move constructor */ - EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) - EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible::value) + EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) noexcept(std::is_nothrow_move_constructible::value) : m_coeffs(std::move(other.coeffs())) {} /** Default move assignment operator */ - EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) - EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable::value) { + EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) noexcept(std::is_nothrow_move_assignable::value) { m_coeffs = std::move(other.coeffs()); return *this; } @@ -793,7 +791,7 @@ EIGEN_DEVICE_FUNC Quaternion::Scalar> Quatern } else { // theta is the angle between the 2 quaternions Scalar theta = acos(absD); - Scalar sinTheta = sin(theta); + Scalar sinTheta = numext::sqrt(Scalar(1) - absD * absD); scale0 = sin((Scalar(1) - t) * theta) / sinTheta; scale1 = sin((t * theta)) / sinTheta; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h index b1a9f21fac..a5d7b60837 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Transform.h @@ -353,10 +353,10 @@ class Transform { inline QTransform toQTransform(void) const; #endif - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return int(Mode) == int(Projective) ? m_matrix.cols() : (m_matrix.cols() - 1); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_matrix.cols(); } /** shortcut for m_matrix(row,col); * \sa MatrixBase::operator(Index,Index) const */ @@ -1059,11 +1059,11 @@ EIGEN_DEVICE_FUNC void Transform::computeRotationSca : Scalar(1); // so x has absolute value 1 VectorType sv(svd.singularValues()); sv.coeffRef(Dim - 1) *= x; - if (scaling) *scaling = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); + if (scaling) (*scaling).noalias() = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); if (rotation) { LinearMatrixType m(svd.matrixU()); m.col(Dim - 1) *= x; - *rotation = m * svd.matrixV().adjoint(); + (*rotation).noalias() = m * svd.matrixV().adjoint(); } } @@ -1182,7 +1182,8 @@ EIGEN_DEVICE_FUNC Transform Transform() = -res.matrix().template topLeftCorner() * translation(); + res.matrix().template topRightCorner().noalias() = + -res.matrix().template topLeftCorner() * translation(); res.makeAffine(); // we do need this, because in the beginning res is uninitialized } return res; @@ -1432,7 +1433,7 @@ struct transform_transform_product_impl ResultType; static EIGEN_DEVICE_FUNC ResultType run(const Lhs& lhs, const Rhs& rhs) { ResultType res; - res.linear() = lhs.linear() * rhs.linear(); + res.linear().noalias() = lhs.linear() * rhs.linear(); res.translation() = lhs.linear() * rhs.translation() + lhs.translation(); res.makeAffine(); return res; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h index 682c4c70da..d942ac8935 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Translation.h @@ -69,18 +69,18 @@ class Translation { EIGEN_DEVICE_FUNC explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} /** \brief Returns the x-translation by value. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar x() const { return m_coeffs.x(); } + EIGEN_DEVICE_FUNC constexpr Scalar x() const { return m_coeffs.x(); } /** \brief Returns the y-translation by value. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar y() const { return m_coeffs.y(); } + EIGEN_DEVICE_FUNC constexpr Scalar y() const { return m_coeffs.y(); } /** \brief Returns the z-translation by value. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar z() const { return m_coeffs.z(); } + EIGEN_DEVICE_FUNC constexpr Scalar z() const { return m_coeffs.z(); } /** \brief Returns the x-translation as a reference. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& x() { return m_coeffs.x(); } + EIGEN_DEVICE_FUNC constexpr Scalar& x() { return m_coeffs.x(); } /** \brief Returns the y-translation as a reference. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& y() { return m_coeffs.y(); } + EIGEN_DEVICE_FUNC constexpr Scalar& y() { return m_coeffs.y(); } /** \brief Returns the z-translation as a reference. **/ - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Scalar& z() { return m_coeffs.z(); } + EIGEN_DEVICE_FUNC constexpr Scalar& z() { return m_coeffs.z(); } EIGEN_DEVICE_FUNC const VectorType& vector() const { return m_coeffs; } EIGEN_DEVICE_FUNC VectorType& vector() { return m_coeffs; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h index f8138b9aeb..8ed63449a3 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Umeyama.h @@ -21,8 +21,6 @@ namespace Eigen { -#ifndef EIGEN_PARSED_BY_DOXYGEN - // These helpers are required since it allows to use mixed types as parameters // for the Umeyama. The problem with mixed parameters is that the return type // cannot trivially be deduced when float and double types are mixed. @@ -50,8 +48,6 @@ struct umeyama_transform_matrix_type { } // namespace internal -#endif - /** * \geometry_module \ingroup Geometry_Module * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h index 024c4a4bec..d49c96156d 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h @@ -183,7 +183,7 @@ class HouseholderSequence : public EigenBase householderSequence(const VectorsTy return HouseholderSequence(v, h); } -/** \ingroup Householder_Module \householder_module +/** \ingroup Householder_Module + * \householder_module * \brief Convenience function for constructing a Householder sequence. * \returns A HouseholderSequence constructed from the specified arguments. * \details This function differs from householderSequence() in that the template argument \p OnTheSide of diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h index 0beef60c35..904d853f90 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h @@ -51,8 +51,8 @@ class DiagonalPreconditioner { compute(mat); } - EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); } - EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); } + constexpr Index rows() const noexcept { return m_invdiag.size(); } + constexpr Index cols() const noexcept { return m_invdiag.size(); } template DiagonalPreconditioner& analyzePattern(const MatType&) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h index e3154b497a..8fdeb849bb 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h @@ -31,8 +31,6 @@ namespace internal { template bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, const Preconditioner& precond, Index& iters, typename Dest::RealScalar& tol_error) { - using std::abs; - using std::sqrt; typedef typename Dest::RealScalar RealScalar; typedef typename Dest::Scalar Scalar; typedef Matrix VectorType; @@ -43,14 +41,15 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, const Precondition VectorType r = rhs - mat * x; VectorType r0 = r; - RealScalar r0_sqnorm = r0.squaredNorm(); - RealScalar rhs_sqnorm = rhs.squaredNorm(); - if (rhs_sqnorm == 0) { + RealScalar r0_norm = r0.stableNorm(); + RealScalar r_norm = r0_norm; + RealScalar rhs_norm = rhs.stableNorm(); + if (rhs_norm == 0) { x.setZero(); return true; } Scalar rho(1); - Scalar alpha(1); + Scalar alpha(0); Scalar w(1); VectorType v = VectorType::Zero(n), p = VectorType::Zero(n); @@ -59,21 +58,22 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, const Precondition VectorType s(n), t(n); - RealScalar tol2 = tol * tol * rhs_sqnorm; - RealScalar eps2 = NumTraits::epsilon() * NumTraits::epsilon(); + RealScalar eps = NumTraits::epsilon(); Index i = 0; Index restarts = 0; - while (r.squaredNorm() > tol2 && i < maxIters) { + while (r_norm > tol && i < maxIters) { Scalar rho_old = rho; - rho = r0.dot(r); - if (abs(rho) < eps2 * r0_sqnorm) { + if (Eigen::numext::abs(rho) / Eigen::numext::maxi(r0_norm, r_norm) < eps * Eigen::numext::mini(r0_norm, r_norm)) { // The new residual vector became too orthogonal to the arbitrarily chosen direction r0 // Let's restart with a new r0: r = rhs - mat * x; r0 = r; - rho = r0_sqnorm = r.squaredNorm(); + rho = r.squaredNorm(); + r0_norm = r.stableNorm(); + alpha = Scalar(0); + w = Scalar(1); if (restarts++ == 0) i = 0; } Scalar beta = (rho / rho_old) * (alpha / w); @@ -82,23 +82,38 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x, const Precondition y = precond.solve(p); v.noalias() = mat * y; - - alpha = rho / r0.dot(v); + Scalar theta = r0.dot(v); + // For small angles ∠(r0, v) < eps, random restart. + RealScalar v_norm = v.stableNorm(); + if (Eigen::numext::abs(theta) / Eigen::numext::maxi(r0_norm, v_norm) < eps * Eigen::numext::mini(r0_norm, v_norm)) { + r = rhs - mat * x; + r0.setRandom(); + r0_norm = r0.stableNorm(); + rho = Scalar(1); + alpha = Scalar(0); + w = Scalar(1); + if (restarts++ == 0) i = 0; + continue; + } + alpha = rho / theta; s = r - alpha * v; z = precond.solve(s); t.noalias() = mat * z; RealScalar tmp = t.squaredNorm(); - if (tmp > RealScalar(0)) + if (tmp > RealScalar(0)) { w = t.dot(s) / tmp; - else + } else { w = Scalar(0); + } x += alpha * y + w * z; r = s - w * t; + r_norm = r.stableNorm(); ++i; } - tol_error = sqrt(r.squaredNorm() / rhs_sqnorm); + + tol_error = r_norm / rhs_norm; iters = i; return true; } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h index a97b9054ce..dd40058ab7 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h @@ -39,9 +39,9 @@ namespace Eigen { * * \b Shifting \b strategy: Let \f$ B = S P A P' S \f$ be the scaled matrix on which the factorization is carried out, * and \f$ \beta \f$ be the minimum value of the diagonal. If \f$ \beta > 0 \f$ then, the factorization is directly - * performed on the matrix B, and \sigma = 0. Otherwise, the factorization is performed on the shifted matrix \f$ B + - * \sigma I \f$ for a shifting factor \f$ \sigma \f$. We start with \f$ \sigma = \sigma_0 - \beta \f$, where \f$ - * \sigma_0 \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ + * performed on the matrix B, and \f$ \sigma = 0 \f$. Otherwise, the factorization is performed on the shifted matrix + * \f$ B + \sigma I \f$ for a shifting factor \f$ \sigma \f$. We start with \f$ \sigma = \sigma_0 - \beta \f$, where + * \f$ \sigma_0 \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ * \sigma_0 = 10^{-3} \f$. If the factorization fails, then the shift in doubled until it succeed or a maximum of ten * attempts. If it still fails, as returned by the info() method, then you can either increase the initial shift, or * better use another preconditioning technique. @@ -84,10 +84,10 @@ class IncompleteCholesky : public SparseSolverBase::setFillfactor(int fillfactor) { this->m_fillfactor = fillfactor; } +/** + * get L-Factor + * \return L-Factor is a matrix containing the lower triangular part of the sparse matrix. All elements of the matrix + * above the main diagonal are zero. + **/ +template +const typename IncompleteLUT::FactorType IncompleteLUT::matrixL() const { + eigen_assert(m_factorizationIsOk && "factorize() should be called first"); + return m_lu.template triangularView(); +} + +/** + * get U-Factor + * \return L-Factor is a matrix containing the upper triangular part of the sparse matrix. All elements of the matrix + * below the main diagonal are zero. + **/ +template +const typename IncompleteLUT::FactorType IncompleteLUT::matrixU() const { + eigen_assert(m_factorizationIsOk && "Factorization must be computed first."); + return m_lu.template triangularView(); +} + template template void IncompleteLUT::analyzePattern(const MatrixType_& amat) { diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h index cf85f2e3af..5caa39653f 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h @@ -218,10 +218,10 @@ class IterativeSolverBase : public SparseSolverBase { } /** \internal */ - EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return matrix().rows(); } + constexpr Index rows() const noexcept { return matrix().rows(); } /** \internal */ - EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return matrix().cols(); } + constexpr Index cols() const noexcept { return matrix().cols(); } /** \returns the tolerance threshold used by the stopping criteria. * \sa setTolerance() diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h index 2b146b373b..271679fee9 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h @@ -50,8 +50,8 @@ class SolveWithGuess : public internal::generic_xpr_base > typedef PermutationMatrix PermutationPType; typedef typename MatrixType::PlainObject PlainObject; + /** \brief Reports whether the LU factorization was successful. + * + * \note This function always returns \c Success. It is provided for compatibility + * with other factorization routines. + * \returns \c Success + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "FullPivLU is not initialized."); + return Success; + } + /** * \brief Default Constructor. * @@ -243,7 +254,10 @@ class FullPivLU : public SolverBase > the LU decomposition. */ inline RealScalar rcond() const { - eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); + eigen_assert(m_isInitialized && "FullPivLU is not initialized."); + if (!isInvertible()) { + return RealScalar(0); + } return internal::rcond_estimate_helper(m_l1_norm, *this); } @@ -388,8 +402,8 @@ class FullPivLU : public SolverBase > MatrixType reconstructedMatrix() const; - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); } - EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); } + EIGEN_DEVICE_FUNC constexpr Index rows() const noexcept { return m_lu.rows(); } + EIGEN_DEVICE_FUNC constexpr Index cols() const noexcept { return m_lu.cols(); } #ifndef EIGEN_PARSED_BY_DOXYGEN template @@ -714,7 +728,7 @@ void FullPivLU::_solve_impl(const RhsType& rhs, // Step 2 m_lu.topLeftCorner(smalldim, smalldim).template triangularView().solveInPlace(c.topRows(smalldim)); - if (rows > cols) c.bottomRows(rows - cols) -= m_lu.bottomRows(rows - cols) * c.topRows(cols); + if (rows > cols) c.bottomRows(rows - cols).noalias() -= m_lu.bottomRows(rows - cols) * c.topRows(cols); // Step 3 m_lu.topLeftCorner(nonzero_pivots, nonzero_pivots) diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h index 57fd67735f..fe8859e9ac 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h @@ -268,7 +268,7 @@ struct Assignment, * \note This matrix must be invertible, otherwise the result is undefined. If you need an * invertibility check, do the following: * \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck(). - * \li for the general case, use class FullPivLU. + * \li for the general case, use class PartialPivLU. * * Example: \include MatrixBase_inverse.cpp * Output: \verbinclude MatrixBase_inverse.out diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h index 1edd6b8d05..7ea14f5761 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h @@ -90,6 +90,17 @@ class PartialPivLU : public SolverBase TranspositionType; typedef typename MatrixType::PlainObject PlainObject; + /** \brief Reports whether the LU factorization was successful. + * + * \note This function always returns \c Success. It is provided for compatibility + * with other factorization routines. + * \returns \c Success + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); + return Success; + } + /** * \brief Default Constructor. * @@ -210,8 +221,8 @@ class PartialPivLU : public SolverBase diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h index f6c5be0718..f1ea2ee5bd 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h @@ -46,8 +46,8 @@ #ifndef EIGEN_COLAMD_H #define EIGEN_COLAMD_H +namespace Eigen { namespace internal { - namespace Colamd { /* Ensure that debugging is turned off: */ @@ -318,7 +318,7 @@ static inline void set_defaults(double knobs[NKnobs]) { * * \param n_row number of rows in A * \param n_col number of columns in A - * \param Alen, size of the array A + * \param Alen size of the array A * \param A row indices of the matrix, of size ALen * \param p column pointers of A, of size n_col+1 * \param knobs parameter settings for colamd @@ -1685,6 +1685,6 @@ static inline IndexType clear_mark /* return the new value for tag_mark */ } } // namespace Colamd - } // namespace internal +} // namespace Eigen #endif diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h index 9a1c5357c3..1a65007768 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h @@ -13,11 +13,9 @@ // IWYU pragma: private #include "./InternalHeaderCheck.h" - -namespace Eigen { - #include "Eigen_Colamd.h" +namespace Eigen { namespace internal { /** \internal diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h index cae9ae4da0..d173444595 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h @@ -82,6 +82,17 @@ class FullPivHouseholderQR : public SolverBase::type ColVectorType; typedef typename MatrixType::PlainObject PlainObject; + /** \brief Reports whether the QR factorization was successful. + * + * \note This function always returns \c Success. It is provided for compatibility + * with other factorization routines. + * \returns \c Success + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized."); + return Success; + } + /** \brief Default Constructor. * * The default constructor is useful in cases in which the user intends to diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h index e297372589..497085dbff 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h @@ -75,6 +75,17 @@ class HouseholderQR : public SolverBase> { typedef HouseholderSequence> HouseholderSequenceType; + /** \brief Reports whether the QR factorization was successful. + * + * \note This function always returns \c Success. It is provided for compatibility + * with other factorization routines. + * \returns \c Success + */ + ComputationInfo info() const { + eigen_assert(m_isInitialized && "HouseHolderQR is not initialized."); + return Success; + } + /** * \brief Default Constructor. * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h index 6b85d1d5a7..6fab905e54 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h @@ -147,6 +147,8 @@ class BDCSVD : public SVDBase > { * One \b cannot request unitaries using both the \a Options template parameter * and the constructor. If possible, prefer using the \a Options template parameter. * + * \param rows number of rows for the input matrix + * \param cols number of columns for the input matrix * \param computationOptions specification for computing Thin/Full unitaries U/V * \sa BDCSVD() * @@ -163,7 +165,8 @@ class BDCSVD : public SVDBase > { * * \param matrix the matrix to decompose */ - BDCSVD(const MatrixType& matrix) : m_algoswap(16), m_numIters(0) { + template + BDCSVD(const MatrixBase& matrix) : m_algoswap(16), m_numIters(0) { compute_impl(matrix, internal::get_computation_options(Options)); } @@ -179,7 +182,9 @@ class BDCSVD : public SVDBase > { * \deprecated Will be removed in the next major Eigen version. Options should * be specified in the \a Options template parameter. */ - EIGEN_DEPRECATED BDCSVD(const MatrixType& matrix, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) { + template + EIGEN_DEPRECATED BDCSVD(const MatrixBase& matrix, unsigned int computationOptions) + : m_algoswap(16), m_numIters(0) { internal::check_svd_options_assertions(computationOptions, matrix.rows(), matrix.cols()); compute_impl(matrix, computationOptions); } @@ -191,7 +196,10 @@ class BDCSVD : public SVDBase > { * * \param matrix the matrix to decompose */ - BDCSVD& compute(const MatrixType& matrix) { return compute_impl(matrix, m_computationOptions); } + template + BDCSVD& compute(const MatrixBase& matrix) { + return compute_impl(matrix, m_computationOptions); + } /** \brief Method performing the decomposition of given matrix, as specified by * the `computationOptions` parameter. @@ -202,7 +210,8 @@ class BDCSVD : public SVDBase > { * \deprecated Will be removed in the next major Eigen version. Options should * be specified in the \a Options template parameter. */ - EIGEN_DEPRECATED BDCSVD& compute(const MatrixType& matrix, unsigned int computationOptions) { + template + EIGEN_DEPRECATED BDCSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { internal::check_svd_options_assertions(computationOptions, matrix.rows(), matrix.cols()); return compute_impl(matrix, computationOptions); } @@ -213,7 +222,8 @@ class BDCSVD : public SVDBase > { } private: - BDCSVD& compute_impl(const MatrixType& matrix, unsigned int computationOptions); + template + BDCSVD& compute_impl(const MatrixBase& matrix, unsigned int computationOptions); void divide(Index firstCol, Index lastCol, Index firstRowW, Index firstColW, Index shift); void computeSVDofM(Index firstCol, Index n, MatrixXr& U, VectorType& singVals, MatrixXr& V); void computeSingVals(const ArrayRef& col0, const ArrayRef& diag, const IndicesRef& perm, VectorType& singVals, @@ -305,8 +315,13 @@ void BDCSVD::allocate(Index rows, Index cols, unsigned int } // end allocate template -BDCSVD& BDCSVD::compute_impl(const MatrixType& matrix, +template +BDCSVD& BDCSVD::compute_impl(const MatrixBase& matrix, unsigned int computationOptions) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived, MatrixType); + EIGEN_STATIC_ASSERT((std::is_same::value), + Input matrix must have the same Scalar type as the BDCSVD object.); + #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE std::cout << "\n\n\n=================================================================================================" "=====================\n\n\n"; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h index e81db2dc2d..1abde17fd9 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h @@ -547,6 +547,8 @@ class JacobiSVD : public SVDBase > { * One \b cannot request unitaries using both the \a Options template parameter * and the constructor. If possible, prefer using the \a Options template parameter. * + * \param rows number of rows for the input matrix + * \param cols number of columns for the input matrix * \param computationOptions specify whether to compute Thin/Full unitaries U/V * \sa JacobiSVD() * @@ -563,7 +565,10 @@ class JacobiSVD : public SVDBase > { * * \param matrix the matrix to decompose */ - explicit JacobiSVD(const MatrixType& matrix) { compute_impl(matrix, internal::get_computation_options(Options)); } + template + explicit JacobiSVD(const MatrixBase& matrix) { + compute_impl(matrix, internal::get_computation_options(Options)); + } /** \brief Constructor performing the decomposition of given matrix using specified options * for computing unitaries. @@ -578,8 +583,10 @@ class JacobiSVD : public SVDBase > { * be specified in the \a Options template parameter. */ // EIGEN_DEPRECATED // TODO(cantonios): re-enable after fixing a few 3p libraries that error on deprecation warnings. - JacobiSVD(const MatrixType& matrix, unsigned int computationOptions) { - internal::check_svd_options_assertions(computationOptions, matrix.rows(), matrix.cols()); + template + JacobiSVD(const MatrixBase& matrix, unsigned int computationOptions) { + internal::check_svd_options_assertions, Options>(computationOptions, matrix.rows(), + matrix.cols()); compute_impl(matrix, computationOptions); } @@ -588,7 +595,10 @@ class JacobiSVD : public SVDBase > { * * \param matrix the matrix to decompose */ - JacobiSVD& compute(const MatrixType& matrix) { return compute_impl(matrix, m_computationOptions); } + template + JacobiSVD& compute(const MatrixBase& matrix) { + return compute_impl(matrix, m_computationOptions); + } /** \brief Method performing the decomposition of given matrix, as specified by * the `computationOptions` parameter. @@ -599,8 +609,10 @@ class JacobiSVD : public SVDBase > { * \deprecated Will be removed in the next major Eigen version. Options should * be specified in the \a Options template parameter. */ - EIGEN_DEPRECATED JacobiSVD& compute(const MatrixType& matrix, unsigned int computationOptions) { - internal::check_svd_options_assertions(m_computationOptions, matrix.rows(), matrix.cols()); + template + EIGEN_DEPRECATED JacobiSVD& compute(const MatrixBase& matrix, unsigned int computationOptions) { + internal::check_svd_options_assertions, Options>(m_computationOptions, matrix.rows(), + matrix.cols()); return compute_impl(matrix, computationOptions); } @@ -624,7 +636,8 @@ class JacobiSVD : public SVDBase > { } private: - JacobiSVD& compute_impl(const MatrixType& matrix, unsigned int computationOptions); + template + JacobiSVD& compute_impl(const MatrixBase& matrix, unsigned int computationOptions); protected: using Base::m_computationOptions; @@ -662,8 +675,13 @@ class JacobiSVD : public SVDBase > { }; template -JacobiSVD& JacobiSVD::compute_impl(const MatrixType& matrix, +template +JacobiSVD& JacobiSVD::compute_impl(const MatrixBase& matrix, unsigned int computationOptions) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived, MatrixType); + EIGEN_STATIC_ASSERT((std::is_same::value), + Input matrix must have the same Scalar type as the BDCSVD object.); + using std::abs; allocate(matrix.rows(), matrix.cols(), computationOptions); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h index d1ad63de26..dcb4dba205 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h @@ -379,7 +379,7 @@ void SVDBase::_solve_impl(const RhsType& rhs, DstType& dst) const { Index l_rank = rank(); tmp.noalias() = m_matrixU.leftCols(l_rank).adjoint() * rhs; tmp = m_singularValues.head(l_rank).asDiagonal().inverse() * tmp; - dst = m_matrixV.leftCols(l_rank) * tmp; + dst.noalias() = m_matrixV.leftCols(l_rank) * tmp; } template diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h index d78b30bc89..6df6318c94 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h @@ -172,7 +172,7 @@ void upperbidiagonalization_blocked_helper( // 1 - update the k-th column of A SubColumnType v_k = A.col(k).tail(remainingRows); v_k -= V_k1 * Y.row(k).head(k).adjoint(); - if (k) v_k -= X_k1 * A.col(k).head(k); + if (k) v_k.noalias() -= X_k1 * A.col(k).head(k); // 2 - construct left Householder transform in-place v_k.makeHouseholderInPlace(tau_v, diagonal[k]); @@ -203,7 +203,7 @@ void upperbidiagonalization_blocked_helper( SubRowType u_k(A.row(k).tail(remainingCols)); u_k = u_k.conjugate(); { - u_k -= Y_k * A.row(k).head(k + 1).adjoint(); + u_k.noalias() -= Y_k * A.row(k).head(k + 1).adjoint(); if (k) u_k -= U_k1.adjoint() * X.row(k).head(k).adjoint(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h index f3ce975cfe..3ccbb037e6 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -134,7 +134,7 @@ class SimplicialCholeskyBase : public SparseSolverBase { << "\n"; s << " tree: " << ((total += m_parent.size() * sizeof(int)) >> 20) << "Mb" << "\n"; - s << " nonzeros: " << ((total += m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb" + s << " nonzeros: " << ((total += m_workSpace.size() * sizeof(int)) >> 20) << "Mb" << "\n"; s << " perm: " << ((total += m_P.size() * sizeof(int)) >> 20) << "Mb" << "\n"; @@ -240,7 +240,7 @@ class SimplicialCholeskyBase : public SparseSolverBase { CholMatrixType m_matrix; VectorType m_diag; // the diagonal coefficients (LDLT mode) VectorI m_parent; // elimination tree - VectorI m_nonZerosPerCol; + VectorI m_workSpace; PermutationMatrix m_P; // the permutation PermutationMatrix m_Pinv; // the inverse permutation @@ -830,7 +830,7 @@ void SimplicialCholeskyBase::ordering(const MatrixType& a, ConstCholMat const Index size = a.rows(); pmat = ≈ // Note that ordering methods compute the inverse permutation - if (!internal::is_same >::value) { + if (!internal::is_same >::value) { { CholMatrixType C; internal::permute_symm_to_fullsymm(a, C, NULL); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h index 0b13c56b5d..26cd38eb36 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h @@ -25,40 +25,265 @@ the Mozilla Public License v. 2.0, as stated at the top of this file. namespace Eigen { -template -void SimplicialCholeskyBase::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT) { - const StorageIndex size = StorageIndex(ap.rows()); - m_matrix.resize(size, size); - m_parent.resize(size); - m_nonZerosPerCol.resize(size); +namespace internal { - ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0); +template +struct simpl_chol_helper { + using CholMatrixType = SparseMatrix; + using InnerIterator = typename CholMatrixType::InnerIterator; + using VectorI = Matrix; + static constexpr StorageIndex kEmpty = -1; - for (StorageIndex k = 0; k < size; ++k) { - /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */ - m_parent[k] = -1; /* parent of k is not yet known */ - tags[k] = k; /* mark node k as visited */ - m_nonZerosPerCol[k] = 0; /* count of nonzeros in column k of L */ - for (typename CholMatrixType::InnerIterator it(ap, k); it; ++it) { - StorageIndex i = it.index(); - if (i < k) { - /* follow path from i to root of etree, stop at flagged node */ - for (; tags[i] != k; i = m_parent[i]) { - /* find parent of i if not yet determined */ - if (m_parent[i] == -1) m_parent[i] = k; - m_nonZerosPerCol[i]++; /* L (k,i) is nonzero */ - tags[i] = k; /* mark i as visited */ + // Implementation of a stack or last-in first-out structure with some debugging machinery. + struct Stack { + StorageIndex* m_data; + Index m_size; +#ifndef EIGEN_NO_DEBUG + const Index m_maxSize; + Stack(StorageIndex* data, StorageIndex size, StorageIndex maxSize) + : m_data(data), m_size(size), m_maxSize(maxSize) { + eigen_assert(size >= 0); + eigen_assert(maxSize >= size); + } +#else + Stack(StorageIndex* data, StorageIndex size, StorageIndex /*maxSize*/) : m_data(data), m_size(size) {} +#endif + bool empty() const { return m_size == 0; } + Index size() const { return m_size; } + StorageIndex back() const { + eigen_assert(m_size > 0); + return m_data[m_size - 1]; + } + void push(const StorageIndex& value) { +#ifndef EIGEN_NO_DEBUG + eigen_assert(m_size < m_maxSize); +#endif + m_data[m_size] = value; + m_size++; + } + void pop() { + eigen_assert(m_size > 0); + m_size--; + } + }; + + // Implementation of a disjoint-set or union-find structure with path compression. + struct DisjointSet { + StorageIndex* m_set; + DisjointSet(StorageIndex* set, StorageIndex size) : m_set(set) { std::iota(set, set + size, 0); } + // Find the set representative or root of `u`. + StorageIndex find(StorageIndex u) const { + eigen_assert(u != kEmpty); + while (m_set[u] != u) { + // manually unroll the loop by a factor of 2 to improve performance + u = m_set[m_set[u]]; + } + return u; + } + // Perform full path compression such that each node from `u` to `v` points to `v`. + void compress(StorageIndex u, StorageIndex v) { + eigen_assert(u != kEmpty); + eigen_assert(v != kEmpty); + while (m_set[u] != v) { + StorageIndex next = m_set[u]; + m_set[u] = v; + u = next; + } + }; + }; + + // Computes the higher adjacency pattern by transposing the input lower adjacency matrix. + // Only the index arrays are calculated, as the values are not needed for the symbolic factorization. + // The outer index array provides the size requirements of the inner index array. + + // Computes the outer index array of the higher adjacency matrix. + static void calc_hadj_outer(const StorageIndex size, const CholMatrixType& ap, StorageIndex* outerIndex) { + for (StorageIndex j = 1; j < size; j++) { + for (InnerIterator it(ap, j); it; ++it) { + StorageIndex i = it.index(); + if (i < j) outerIndex[i + 1]++; + } + } + std::partial_sum(outerIndex, outerIndex + size + 1, outerIndex); + } + + // inner index array + static void calc_hadj_inner(const StorageIndex size, const CholMatrixType& ap, const StorageIndex* outerIndex, + StorageIndex* innerIndex, StorageIndex* tmp) { + std::fill_n(tmp, size, 0); + + for (StorageIndex j = 1; j < size; j++) { + for (InnerIterator it(ap, j); it; ++it) { + StorageIndex i = it.index(); + if (i < j) { + StorageIndex b = outerIndex[i] + tmp[i]; + innerIndex[b] = j; + tmp[i]++; } } } } - /* construct Lp index array from m_nonZerosPerCol column counts */ - StorageIndex* Lp = m_matrix.outerIndexPtr(); - Lp[0] = 0; - for (StorageIndex k = 0; k < size; ++k) Lp[k + 1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1); + // Adapted from: + // Joseph W. Liu. (1986). + // A compact row storage scheme for Cholesky factors using elimination trees. + // ACM Trans. Math. Softw. 12, 2 (June 1986), 127-148. https://doi.org/10.1145/6497.6499 - m_matrix.resizeNonZeros(Lp[size]); + // Computes the elimination forest of the lower adjacency matrix, a compact representation of the sparse L factor. + // The L factor may contain multiple elimination trees if a column contains only its diagonal element. + // Each elimination tree is an n-ary tree in which each node points to its parent. + static void calc_etree(const StorageIndex size, const CholMatrixType& ap, StorageIndex* parent, StorageIndex* tmp) { + std::fill_n(parent, size, kEmpty); + + DisjointSet ancestor(tmp, size); + + for (StorageIndex j = 1; j < size; j++) { + for (InnerIterator it(ap, j); it; ++it) { + StorageIndex i = it.index(); + if (i < j) { + StorageIndex r = ancestor.find(i); + if (r != j) parent[r] = j; + ancestor.compress(i, j); + } + } + } + } + + // Computes the child pointers of the parent tree to facilitate a depth-first search traversal. + static void calc_lineage(const StorageIndex size, const StorageIndex* parent, StorageIndex* firstChild, + StorageIndex* firstSibling) { + std::fill_n(firstChild, size, kEmpty); + std::fill_n(firstSibling, size, kEmpty); + + for (StorageIndex j = 0; j < size; j++) { + StorageIndex p = parent[j]; + if (p == kEmpty) continue; + StorageIndex c = firstChild[p]; + if (c == kEmpty) + firstChild[p] = j; + else { + while (firstSibling[c] != kEmpty) c = firstSibling[c]; + firstSibling[c] = j; + } + } + } + + // Computes a post-ordered traversal of the elimination tree. + static void calc_post(const StorageIndex size, const StorageIndex* parent, StorageIndex* firstChild, + const StorageIndex* firstSibling, StorageIndex* post, StorageIndex* dfs) { + Stack post_stack(post, 0, size); + for (StorageIndex j = 0; j < size; j++) { + if (parent[j] != kEmpty) continue; + // Begin at a root + Stack dfs_stack(dfs, 0, size); + dfs_stack.push(j); + while (!dfs_stack.empty()) { + StorageIndex i = dfs_stack.back(); + StorageIndex c = firstChild[i]; + if (c == kEmpty) { + post_stack.push(i); + dfs_stack.pop(); + } else { + dfs_stack.push(c); + // Remove the path from `i` to `c` for future traversals. + firstChild[i] = firstSibling[c]; + } + } + } + eigen_assert(post_stack.size() == size); + eigen_assert(std::all_of(firstChild, firstChild + size, [](StorageIndex a) { return a == kEmpty; })); + } + + // Adapted from: + // Gilbert, J. R., Ng, E., & Peyton, B. W. (1994). + // An efficient algorithm to compute row and column counts for sparse Cholesky factorization. + // SIAM Journal on Matrix Analysis and Applications, 15(4), 1075-1091. + + // Computes the non-zero pattern of the L factor. + static void calc_colcount(const StorageIndex size, const StorageIndex* hadjOuter, const StorageIndex* hadjInner, + const StorageIndex* parent, StorageIndex* prevLeaf, StorageIndex* tmp, + const StorageIndex* post, StorageIndex* nonZerosPerCol, bool doLDLT) { + // initialize nonZerosPerCol with 1 for leaves, 0 for non-leaves + std::fill_n(nonZerosPerCol, size, 1); + for (StorageIndex j = 0; j < size; j++) { + StorageIndex p = parent[j]; + // p is not a leaf + if (p != kEmpty) nonZerosPerCol[p] = 0; + } + + DisjointSet parentSet(tmp, size); + // prevLeaf is already initialized + eigen_assert(std::all_of(prevLeaf, prevLeaf + size, [](StorageIndex a) { return a == kEmpty; })); + + for (StorageIndex j_ = 0; j_ < size; j_++) { + StorageIndex j = post[j_]; + nonZerosPerCol[j] += hadjOuter[j + 1] - hadjOuter[j]; + for (StorageIndex k = hadjOuter[j]; k < hadjOuter[j + 1]; k++) { + StorageIndex i = hadjInner[k]; + eigen_assert(i > j); + StorageIndex prev = prevLeaf[i]; + if (prev != kEmpty) { + StorageIndex q = parentSet.find(prev); + parentSet.compress(prev, q); + nonZerosPerCol[q]--; + } + prevLeaf[i] = j; + } + StorageIndex p = parent[j]; + if (p != kEmpty) parentSet.compress(j, p); + } + + for (StorageIndex j = 0; j < size; j++) { + StorageIndex p = parent[j]; + if (p != kEmpty) nonZerosPerCol[p] += nonZerosPerCol[j] - 1; + if (doLDLT) nonZerosPerCol[j]--; + } + } + + // Finalizes the non zero pattern of the L factor and allocates the memory for the factorization. + static void init_matrix(const StorageIndex size, const StorageIndex* nonZerosPerCol, CholMatrixType& L) { + eigen_assert(L.outerIndexPtr()[0] == 0); + std::partial_sum(nonZerosPerCol, nonZerosPerCol + size, L.outerIndexPtr() + 1); + L.resizeNonZeros(L.outerIndexPtr()[size]); + } + + // Driver routine for the symbolic sparse Cholesky factorization. + static void run(const StorageIndex size, const CholMatrixType& ap, CholMatrixType& L, VectorI& parent, + VectorI& workSpace, bool doLDLT) { + parent.resize(size); + workSpace.resize(4 * size); + L.resize(size, size); + + StorageIndex* tmp1 = workSpace.data(); + StorageIndex* tmp2 = workSpace.data() + size; + StorageIndex* tmp3 = workSpace.data() + 2 * size; + StorageIndex* tmp4 = workSpace.data() + 3 * size; + + // Borrow L's outer index array for the higher adjacency pattern. + StorageIndex* hadj_outer = L.outerIndexPtr(); + calc_hadj_outer(size, ap, hadj_outer); + // Request additional temporary storage for the inner indices of the higher adjacency pattern. + ei_declare_aligned_stack_constructed_variable(StorageIndex, hadj_inner, hadj_outer[size], nullptr); + calc_hadj_inner(size, ap, hadj_outer, hadj_inner, tmp1); + + calc_etree(size, ap, parent.data(), tmp1); + calc_lineage(size, parent.data(), tmp1, tmp2); + calc_post(size, parent.data(), tmp1, tmp2, tmp3, tmp4); + calc_colcount(size, hadj_outer, hadj_inner, parent.data(), tmp1, tmp2, tmp3, tmp4, doLDLT); + init_matrix(size, tmp4, L); + } +}; + +} // namespace internal + +template +void SimplicialCholeskyBase::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT) { + using Helper = internal::simpl_chol_helper; + + eigen_assert(ap.innerSize() == ap.outerSize()); + const StorageIndex size = internal::convert_index(ap.outerSize()); + + Helper::run(size, ap, m_matrix, m_parent, m_workSpace, doLDLT); m_isInitialized = true; m_info = Success; @@ -70,20 +295,21 @@ template template void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& ap) { using std::sqrt; + const StorageIndex size = StorageIndex(ap.rows()); eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); eigen_assert(ap.rows() == ap.cols()); - eigen_assert(m_parent.size() == ap.rows()); - eigen_assert(m_nonZerosPerCol.size() == ap.rows()); + eigen_assert(m_parent.size() == size); + eigen_assert(m_workSpace.size() >= 3 * size); - const StorageIndex size = StorageIndex(ap.rows()); const StorageIndex* Lp = m_matrix.outerIndexPtr(); StorageIndex* Li = m_matrix.innerIndexPtr(); Scalar* Lx = m_matrix.valuePtr(); ei_declare_aligned_stack_constructed_variable(Scalar, y, size, 0); - ei_declare_aligned_stack_constructed_variable(StorageIndex, pattern, size, 0); - ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0); + StorageIndex* nonZerosPerCol = m_workSpace.data(); + StorageIndex* pattern = m_workSpace.data() + size; + StorageIndex* tags = m_workSpace.data() + 2 * size; bool ok = true; m_diag.resize(DoLDLT ? size : 0); @@ -93,7 +319,7 @@ void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& y[k] = Scalar(0); // Y(0:k) is now all zero StorageIndex top = size; // stack for pattern is empty tags[k] = k; // mark node k as visited - m_nonZerosPerCol[k] = 0; // count of nonzeros in column k of L + nonZerosPerCol[k] = 0; // count of nonzeros in column k of L for (typename CholMatrixType::InnerIterator it(ap, k); it; ++it) { StorageIndex i = it.index(); if (i <= k) { @@ -124,13 +350,13 @@ void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& else yi = l_ki = yi / Lx[Lp[i]]; - Index p2 = Lp[i] + m_nonZerosPerCol[i]; + Index p2 = Lp[i] + nonZerosPerCol[i]; Index p; for (p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p) y[Li[p]] -= getSymm(Lx[p]) * yi; d -= getDiag(l_ki * getSymm(yi)); Li[p] = k; /* store L(k,i) in column form of L */ Lx[p] = l_ki; - ++m_nonZerosPerCol[i]; /* increment count of nonzeros in col i */ + ++nonZerosPerCol[i]; /* increment count of nonzeros in col i */ } if (DoLDLT) { m_diag[k] = d; @@ -139,7 +365,7 @@ void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& break; } } else { - Index p = Lp[k] + m_nonZerosPerCol[k]++; + Index p = Lp[k] + nonZerosPerCol[k]++; Li[p] = k; /* store L(k,k) = sqrt (d) in column k */ if (NonHermitian ? d == RealScalar(0) : numext::real(d) <= RealScalar(0)) { ok = false; /* failure, matrix is not positive definite */ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h index caa8ffada3..7ddb6fab90 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h @@ -302,9 +302,10 @@ class SparseMatrix : public SparseCompressedBase= 0 && j >= 0 && j < m_outerSize && "Invalid parameters"); const Index newRows = IsRowMajor ? m_outerSize + num : rows(); @@ -621,10 +622,12 @@ class SparseMatrix : public SparseCompressedBase(m_outerSize); - if (m_outerIndex[m_outerSize] == 0) - std::fill_n(m_innerNonZeros, m_outerSize, StorageIndex(0)); - else + if (m_outerIndex[m_outerSize] == 0) { + using std::fill_n; + fill_n(m_innerNonZeros, m_outerSize, StorageIndex(0)); + } else { for (Index j = 0; j < m_outerSize; j++) m_innerNonZeros[j] = m_outerIndex[j + 1] - m_outerIndex[j]; + } } /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */ @@ -695,9 +698,10 @@ class SparseMatrix : public SparseCompressedBase 0) { StorageIndex lastIdx = m_outerSize == 0 ? StorageIndex(0) : m_outerIndex[m_outerSize]; - std::fill_n(m_outerIndex + m_outerSize, outerChange + 1, lastIdx); + using std::fill_n; + fill_n(m_outerIndex + m_outerSize, outerChange + 1, lastIdx); - if (!isCompressed()) std::fill_n(m_innerNonZeros + m_outerSize, outerChange, StorageIndex(0)); + if (!isCompressed()) fill_n(m_innerNonZeros + m_outerSize, outerChange, StorageIndex(0)); } } m_outerSize = newOuterSize; @@ -741,7 +745,8 @@ class SparseMatrix : public SparseCompressedBase(m_innerNonZeros, m_outerSize); m_innerNonZeros = 0; - std::fill_n(m_outerIndex, m_outerSize + 1, StorageIndex(0)); + using std::fill_n; + fill_n(m_outerIndex, m_outerSize + 1, StorageIndex(0)); } /** \internal @@ -843,7 +848,8 @@ class SparseMatrix : public SparseCompressedBase inline SparseMatrix& operator=(const EigenBase& other) { return Base::operator=(other.derived()); @@ -877,7 +882,6 @@ class SparseMatrix : public SparseCompressedBase inline SparseMatrix& operator=(const Product& other); -#endif // EIGEN_PARSED_BY_DOXYGEN template EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase& other); diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h index b58bb3842b..7ac16f8345 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h @@ -118,7 +118,6 @@ class SparseMatrixBase : public EigenBase { // FIXME storage order do not match evaluator storage order typedef SparseMatrix PlainObject; -#ifndef EIGEN_PARSED_BY_DOXYGEN /** This is the "real scalar" type; if the \a Scalar type is already real numbers * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If * \a Scalar is \a std::complex then RealScalar is \a T. @@ -127,6 +126,7 @@ class SparseMatrixBase : public EigenBase { */ typedef typename NumTraits::Real RealScalar; +#ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal the return type of coeff() */ typedef std::conditional_t CoeffReturnType; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h index e5fb771579..71a9ff4831 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h @@ -30,15 +30,15 @@ #ifndef SPARSELU_COLUMN_DFS_H #define SPARSELU_COLUMN_DFS_H -template -class SparseLUImpl; // IWYU pragma: private #include "./InternalHeaderCheck.h" namespace Eigen { - namespace internal { +template +class SparseLUImpl; + template struct column_dfs_traits : no_assignment_operator { typedef typename ScalarVector::Scalar Scalar; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h index acb0c5ffca..4dc7aa9f8c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h @@ -73,7 +73,7 @@ struct traits > { * detailed in the following paper: * * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing - * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011. + * Sparse QR Factorization", ACM Trans. on Math. Soft. 38(1), 2011. * * Even though it is qualified as "rank-revealing", this strategy might fail for some * rank deficient problems. When this class is used to solve linear or least-square problems @@ -365,7 +365,6 @@ void SparseQR::factorize(const MatrixType& mat) { IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q ScalarVector tval(m); // The dense vector used to compute the current column - RealScalar pivotThreshold = m_threshold; m_R.setZero(); m_Q.setZero(); @@ -401,11 +400,14 @@ void SparseQR::factorize(const MatrixType& mat) { * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 */ + RealScalar pivotThreshold; if (m_useDefaultThreshold) { RealScalar max2Norm = 0.0; for (int j = 0; j < n; j++) max2Norm = numext::maxi(max2Norm, m_pmat.col(j).norm()); if (max2Norm == RealScalar(0)) max2Norm = RealScalar(1); pivotThreshold = 20 * (m + n) * max2Norm * NumTraits::epsilon(); + } else { + pivotThreshold = m_threshold; } // Initialize the numerical permutation diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/ThreadPool/CoreThreadPoolDevice.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/ThreadPool/CoreThreadPoolDevice.h deleted file mode 100644 index acf1d628b6..0000000000 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/ThreadPool/CoreThreadPoolDevice.h +++ /dev/null @@ -1,327 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2023 Charlie Schlosser -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_CORE_THREAD_POOL_DEVICE_H -#define EIGEN_CORE_THREAD_POOL_DEVICE_H - -namespace Eigen { - -// CoreThreadPoolDevice provides an easy-to-understand Device for parallelizing Eigen Core expressions with -// Threadpool. Expressions are recursively split evenly until the evaluation cost is less than the threshold for -// delegating the task to a thread. -/* - a - / \ - / \ - / \ - / \ - / \ - / \ - / \ - a e - / \ / \ - / \ / \ - / \ / \ - a c e g - / \ / \ / \ / \ - / \ / \ / \ / \ - a b c d e f g h -*/ -// Each task descends the binary tree to the left, delegates the right task to a new thread, and continues to the -// left. This ensures that work is evenly distributed to the thread pool as quickly as possible and minimizes the number -// of tasks created during the evaluation. Consider an expression that is divided into 8 chunks. The -// primary task 'a' creates tasks 'e' 'c' and 'b', and executes its portion of the expression at the bottom of the -// tree. Likewise, task 'e' creates tasks 'g' and 'f', and executes its portion of the expression. - -struct CoreThreadPoolDevice { - using Task = std::function; - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE CoreThreadPoolDevice(ThreadPool& pool, float threadCostThreshold = 3e-5f) - : m_pool(pool) { - eigen_assert(threadCostThreshold >= 0.0f && "threadCostThreshold must be non-negative"); - m_costFactor = threadCostThreshold; - } - - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE int calculateLevels(Index size, float cost) const { - eigen_assert(cost >= 0.0f && "cost must be non-negative"); - Index numOps = size / PacketSize; - int actualThreads = numOps < m_pool.NumThreads() ? static_cast(numOps) : m_pool.NumThreads(); - float totalCost = static_cast(numOps) * cost; - float idealThreads = totalCost * m_costFactor; - if (idealThreads < static_cast(actualThreads)) { - idealThreads = numext::maxi(idealThreads, 1.0f); - actualThreads = numext::mini(actualThreads, static_cast(idealThreads)); - } - int maxLevel = internal::log2_ceil(actualThreads); - return maxLevel; - } - -// MSVC does not like inlining parallelForImpl -#if EIGEN_COMP_MSVC && !EIGEN_COMP_CLANG -#define EIGEN_PARALLEL_FOR_INLINE -#else -#define EIGEN_PARALLEL_FOR_INLINE EIGEN_STRONG_INLINE -#endif - - template - EIGEN_DEVICE_FUNC EIGEN_PARALLEL_FOR_INLINE void parallelForImpl(Index begin, Index end, UnaryFunctor& f, - Barrier& barrier, int level) { - while (level > 0) { - level--; - Index size = end - begin; - eigen_assert(size % PacketSize == 0 && "this function assumes size is a multiple of PacketSize"); - Index mid = begin + numext::round_down(size >> 1, PacketSize); - Task right = [this, mid, end, &f, &barrier, level]() { - parallelForImpl(mid, end, f, barrier, level); - }; - m_pool.Schedule(std::move(right)); - end = mid; - } - for (Index i = begin; i < end; i += PacketSize) f(i); - barrier.Notify(); - } - - template - EIGEN_DEVICE_FUNC EIGEN_PARALLEL_FOR_INLINE void parallelForImpl(Index outerBegin, Index outerEnd, Index innerBegin, - Index innerEnd, BinaryFunctor& f, Barrier& barrier, - int level) { - while (level > 0) { - level--; - Index outerSize = outerEnd - outerBegin; - if (outerSize > 1) { - Index outerMid = outerBegin + (outerSize >> 1); - Task right = [this, &f, &barrier, outerMid, outerEnd, innerBegin, innerEnd, level]() { - parallelForImpl(outerMid, outerEnd, innerBegin, innerEnd, f, barrier, level); - }; - m_pool.Schedule(std::move(right)); - outerEnd = outerMid; - } else { - Index innerSize = innerEnd - innerBegin; - eigen_assert(innerSize % PacketSize == 0 && "this function assumes innerSize is a multiple of PacketSize"); - Index innerMid = innerBegin + numext::round_down(innerSize >> 1, PacketSize); - Task right = [this, &f, &barrier, outerBegin, outerEnd, innerMid, innerEnd, level]() { - parallelForImpl(outerBegin, outerEnd, innerMid, innerEnd, f, barrier, level); - }; - m_pool.Schedule(std::move(right)); - innerEnd = innerMid; - } - } - for (Index outer = outerBegin; outer < outerEnd; outer++) - for (Index inner = innerBegin; inner < innerEnd; inner += PacketSize) f(outer, inner); - barrier.Notify(); - } - -#undef EIGEN_PARALLEL_FOR_INLINE - - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void parallelFor(Index begin, Index end, UnaryFunctor& f, float cost) { - Index size = end - begin; - int maxLevel = calculateLevels(size, cost); - Barrier barrier(1 << maxLevel); - parallelForImpl(begin, end, f, barrier, maxLevel); - barrier.Wait(); - } - - template - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void parallelFor(Index outerBegin, Index outerEnd, Index innerBegin, - Index innerEnd, BinaryFunctor& f, float cost) { - Index outerSize = outerEnd - outerBegin; - Index innerSize = innerEnd - innerBegin; - Index size = outerSize * innerSize; - int maxLevel = calculateLevels(size, cost); - Barrier barrier(1 << maxLevel); - parallelForImpl(outerBegin, outerEnd, innerBegin, innerEnd, f, barrier, maxLevel); - barrier.Wait(); - } - - ThreadPool& m_pool; - // costFactor is the cost of delegating a task to a thread - // the inverse is used to avoid a floating point division - float m_costFactor; -}; - -// specialization of coefficient-wise assignment loops for CoreThreadPoolDevice - -namespace internal { - -template -struct cost_helper { - using SrcEvaluatorType = typename Kernel::SrcEvaluatorType; - using DstEvaluatorType = typename Kernel::DstEvaluatorType; - using SrcXprType = typename SrcEvaluatorType::XprType; - using DstXprType = typename DstEvaluatorType::XprType; - static constexpr Index Cost = functor_cost::Cost + functor_cost::Cost; -}; - -template -struct dense_assignment_loop_with_device { - static constexpr Index XprEvaluationCost = cost_helper::Cost; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer, Index inner) { - this->assignCoeffByOuterInner(outer, inner); - } - }; - - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index innerSize = kernel.innerSize(); - const Index outerSize = kernel.outerSize(); - constexpr float cost = static_cast(XprEvaluationCost); - AssignmentFunctor functor(kernel); - device.template parallelFor(0, outerSize, 0, innerSize, functor, cost); - } -}; - -template -struct dense_assignment_loop_with_device { - using DstXprType = typename Kernel::DstEvaluatorType::XprType; - static constexpr Index XprEvaluationCost = cost_helper::Cost, InnerSize = DstXprType::InnerSizeAtCompileTime; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer) { - copy_using_evaluator_DefaultTraversal_InnerUnrolling::run(*this, outer); - } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index outerSize = kernel.outerSize(); - AssignmentFunctor functor(kernel); - constexpr float cost = static_cast(XprEvaluationCost) * static_cast(InnerSize); - device.template parallelFor(0, outerSize, functor, cost); - } -}; - -template -struct dense_assignment_loop_with_device { - using PacketType = typename Kernel::PacketType; - static constexpr Index XprEvaluationCost = cost_helper::Cost, PacketSize = unpacket_traits::size, - SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, - DstAlignment = Kernel::AssignmentTraits::DstAlignment; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer, Index inner) { - this->template assignPacketByOuterInner(outer, inner); - } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index innerSize = kernel.innerSize(); - const Index outerSize = kernel.outerSize(); - const float cost = static_cast(XprEvaluationCost) * static_cast(innerSize); - AssignmentFunctor functor(kernel); - device.template parallelFor(0, outerSize, 0, innerSize, functor, cost); - } -}; - -template -struct dense_assignment_loop_with_device { - using PacketType = typename Kernel::PacketType; - using DstXprType = typename Kernel::DstEvaluatorType::XprType; - static constexpr Index XprEvaluationCost = cost_helper::Cost, PacketSize = unpacket_traits::size, - SrcAlignment = Kernel::AssignmentTraits::SrcAlignment, - DstAlignment = Kernel::AssignmentTraits::DstAlignment, - InnerSize = DstXprType::InnerSizeAtCompileTime; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer) { - copy_using_evaluator_innervec_InnerUnrolling::run(*this, outer); - } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index outerSize = kernel.outerSize(); - constexpr float cost = static_cast(XprEvaluationCost) * static_cast(InnerSize); - AssignmentFunctor functor(kernel); - device.template parallelFor(0, outerSize, functor, cost); - } -}; - -template -struct dense_assignment_loop_with_device { - using Scalar = typename Kernel::Scalar; - using PacketType = typename Kernel::PacketType; - static constexpr Index XprEvaluationCost = cost_helper::Cost, PacketSize = unpacket_traits::size; - struct PacketAssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE PacketAssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer, Index inner) { - this->template assignPacketByOuterInner(outer, inner); - } - }; - struct ScalarAssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ScalarAssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index outer) { - const Index innerSize = this->innerSize(); - const Index packetAccessSize = numext::round_down(innerSize, PacketSize); - for (Index inner = packetAccessSize; inner < innerSize; inner++) this->assignCoeffByOuterInner(outer, inner); - } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index outerSize = kernel.outerSize(); - const Index innerSize = kernel.innerSize(); - const Index packetAccessSize = numext::round_down(innerSize, PacketSize); - constexpr float packetCost = static_cast(XprEvaluationCost); - const float scalarCost = static_cast(XprEvaluationCost) * static_cast(innerSize - packetAccessSize); - PacketAssignmentFunctor packetFunctor(kernel); - ScalarAssignmentFunctor scalarFunctor(kernel); - device.template parallelFor(0, outerSize, 0, packetAccessSize, packetFunctor, - packetCost); - device.template parallelFor(0, outerSize, scalarFunctor, scalarCost); - }; -}; - -template -struct dense_assignment_loop_with_device { - static constexpr Index XprEvaluationCost = cost_helper::Cost; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index index) { this->assignCoeff(index); } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index size = kernel.size(); - constexpr float cost = static_cast(XprEvaluationCost); - AssignmentFunctor functor(kernel); - device.template parallelFor(0, size, functor, cost); - } -}; - -template -struct dense_assignment_loop_with_device { - using Scalar = typename Kernel::Scalar; - using PacketType = typename Kernel::PacketType; - static constexpr Index XprEvaluationCost = cost_helper::Cost, - RequestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment, - PacketSize = unpacket_traits::size, - DstIsAligned = Kernel::AssignmentTraits::DstAlignment >= RequestedAlignment, - DstAlignment = packet_traits::AlignedOnScalar ? RequestedAlignment - : Kernel::AssignmentTraits::DstAlignment, - SrcAlignment = Kernel::AssignmentTraits::JointAlignment; - struct AssignmentFunctor : public Kernel { - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE AssignmentFunctor(Kernel& kernel) : Kernel(kernel) {} - EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void operator()(Index index) { - this->template assignPacket(index); - } - }; - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Kernel& kernel, CoreThreadPoolDevice& device) { - const Index size = kernel.size(); - const Index alignedStart = - DstIsAligned ? 0 : internal::first_aligned(kernel.dstDataPtr(), size); - const Index alignedEnd = alignedStart + numext::round_down(size - alignedStart, PacketSize); - - unaligned_dense_assignment_loop::run(kernel, 0, alignedStart); - - constexpr float cost = static_cast(XprEvaluationCost); - AssignmentFunctor functor(kernel); - device.template parallelFor(alignedStart, alignedEnd, functor, cost); - - unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size); - } -}; - -} // namespace internal - -} // namespace Eigen - -#endif // EIGEN_CORE_THREAD_POOL_DEVICE_H diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc index 10c7a3e214..c8c2434d7c 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.inc @@ -96,9 +96,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const } /** \returns an expression of the coefficient-wise absdiff of \c *this and \a other - * - * Example: \include Cwise_absolute_difference.cpp - * Output: \verbinclude Cwise_absolute_difference.out * * \sa absolute_difference() */ diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc index 93f7eabe5d..753aeb4fb7 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.inc @@ -523,15 +523,11 @@ using UnaryPowReturnType = std::enable_if_t::Real>::value, CwiseUnaryOp, const Derived>>; -#ifndef EIGEN_PARSED_BY_DOXYGEN -template -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryPowReturnType pow( - const ScalarExponent& exponent) const { - return UnaryPowReturnType(derived(), internal::scalar_unary_pow_op(exponent)); -#else /** \returns an expression of the coefficients of \c *this raised to the constant power \a exponent * - * \tparam T is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression. + * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type + * of the given expression. + * \param exponent the scalar exponent value. * * This function computes the coefficient-wise power. The function MatrixBase::pow() in the * unsupported module MatrixFunctions computes the matrix power. @@ -543,6 +539,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryPowReturnType p */ template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const UnaryPowReturnType pow( - const ScalarExponent& exponent) const; -#endif + const ScalarExponent& exponent) const { + return UnaryPowReturnType(derived(), internal::scalar_unary_pow_op(exponent)); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc index 46dc9ddd10..0782aa39af 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.inc @@ -1365,6 +1365,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::conditional_t -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index subVectors() const { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr Index subVectors() const { return (Direction == Vertical) ? cols() : rows(); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc index 95f338a067..f1ba301090 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.inc @@ -43,34 +43,17 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseBinaryOp(derived(), other.derived(), func); } -#ifndef EIGEN_PARSED_BY_DOXYGEN -EIGEN_MAKE_SCALAR_BINARY_OP(operator*, product) -#else /** \returns an expression of \c *this scaled by the scalar factor \a scalar * * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression. */ -template -const CwiseBinaryOp, Derived, Constant > operator*(const T& scalar) const; -/** \returns an expression of \a expr scaled by the scalar factor \a scalar - * - * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression. - */ -template -friend const CwiseBinaryOp, Constant, Derived> operator*( - const T& scalar, const StorageBaseType& expr); -#endif +EIGEN_MAKE_SCALAR_BINARY_OP(operator*, product) -#ifndef EIGEN_PARSED_BY_DOXYGEN -EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator/, quotient) -#else /** \returns an expression of \c *this divided by the scalar value \a scalar * * \tparam T is the scalar type of \a scalar. It must be compatible with the scalar type of the given expression. */ -template -const CwiseBinaryOp, Derived, Constant > operator/(const T& scalar) const; -#endif +EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(operator/, quotient) /** \returns an expression of the coefficient-wise boolean \b and operator of \c *this and \a other * diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc index 325b0fbe0d..ffaf5aab2a 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.inc @@ -60,9 +60,6 @@ EIGEN_DEVICE_FUNC inline const CwiseSqrtReturnType cwiseSqrt() const { return Cw /// \returns an expression of the coefficient-wise cube root of *this. /// -/// Example: \include MatrixBase_cwiseCbrt.cpp -/// Output: \verbinclude MatrixBase_cwiseCbrt.out -/// EIGEN_DOC_UNARY_ADDONS(cwiseCbrt, cube - root) /// /// \sa cwiseSqrt(), cwiseSquare(), cwisePow() diff --git a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index ff955e1d7c..f4e0428ef1 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -23,8 +23,10 @@ namespace internal { * * This struct is used by CwiseUnaryOp to scale a matrix by \f$ 2^{-s} \f$. */ -template +template ::IsComplex> struct MatrixExponentialScalingOp { + using RealScalar = typename NumTraits::Real; + /** \brief Constructor. * * \param[in] squarings The integer \f$ s \f$ in this document. @@ -35,20 +37,30 @@ struct MatrixExponentialScalingOp { * * \param[in,out] x The scalar to be scaled, becoming \f$ 2^{-s} x \f$. */ - inline const RealScalar operator()(const RealScalar& x) const { + inline const Scalar operator()(const Scalar& x) const { using std::ldexp; - return ldexp(x, -m_squarings); + return Scalar(ldexp(Eigen::numext::real(x), -m_squarings), ldexp(Eigen::numext::imag(x), -m_squarings)); } - typedef std::complex ComplexScalar; + private: + int m_squarings; +}; + +template +struct MatrixExponentialScalingOp { + /** \brief Constructor. + * + * \param[in] squarings The integer \f$ s \f$ in this document. + */ + MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) {} /** \brief Scale a matrix coefficient. * * \param[in,out] x The scalar to be scaled, becoming \f$ 2^{-s} x \f$. */ - inline const ComplexScalar operator()(const ComplexScalar& x) const { + inline const Scalar operator()(const Scalar& x) const { using std::ldexp; - return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings)); + return ldexp(x, -m_squarings); } private: @@ -220,6 +232,7 @@ struct matrix_exp_computeUV { template struct matrix_exp_computeUV { + using Scalar = typename traits::Scalar; template static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) { using std::frexp; @@ -234,7 +247,7 @@ struct matrix_exp_computeUV { const float maxnorm = 3.925724783138660f; frexp(l1norm / maxnorm, &squarings); if (squarings < 0) squarings = 0; - MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); + MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); matrix_exp_pade7(A, U, V); } } @@ -242,12 +255,12 @@ struct matrix_exp_computeUV { template struct matrix_exp_computeUV { - typedef typename NumTraits::Scalar>::Real RealScalar; + using Scalar = typename traits::Scalar; template static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) { using std::frexp; using std::pow; - const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff(); + const double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff(); squarings = 0; if (l1norm < 1.495585217958292e-002) { matrix_exp_pade3(arg, U, V); @@ -258,10 +271,10 @@ struct matrix_exp_computeUV { } else if (l1norm < 2.097847961257068e+000) { matrix_exp_pade9(arg, U, V); } else { - const RealScalar maxnorm = 5.371920351148152; + const double maxnorm = 5.371920351148152; frexp(l1norm / maxnorm, &squarings); if (squarings < 0) squarings = 0; - MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); + MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); matrix_exp_pade13(A, U, V); } } @@ -276,6 +289,8 @@ struct matrix_exp_computeUV { #else + using Scalar = typename traits::Scalar; + using std::frexp; using std::pow; const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff(); @@ -295,7 +310,7 @@ struct matrix_exp_computeUV { const long double maxnorm = 4.0246098906697353063L; frexp(l1norm / maxnorm, &squarings); if (squarings < 0) squarings = 0; - MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); + MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); matrix_exp_pade13(A, U, V); } @@ -315,7 +330,7 @@ struct matrix_exp_computeUV { const long double maxnorm = 3.2579440895405400856599663723517L; frexp(l1norm / maxnorm, &squarings); if (squarings < 0) squarings = 0; - MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); + MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); matrix_exp_pade17(A, U, V); } @@ -335,7 +350,7 @@ struct matrix_exp_computeUV { const long double maxnorm = 2.884233277829519311757165057717815L; frexp(l1norm / maxnorm, &squarings); if (squarings < 0) squarings = 0; - MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); + MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp(squarings)); matrix_exp_pade17(A, U, V); } @@ -382,9 +397,7 @@ template void matrix_exp_compute(const ArgType& arg, ResultType& result, false_type) // default { typedef typename ArgType::PlainObject MatrixType; - typedef typename traits::Scalar Scalar; - typedef typename NumTraits::Real RealScalar; - typedef typename std::complex ComplexScalar; + typedef make_complex_t::Scalar> ComplexScalar; result = arg.matrixFunction(internal::stem_function_exp); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h index 68336a5258..0c18ad66ac 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h @@ -382,7 +382,7 @@ struct matrix_function_compute { static const int Rows = Traits::RowsAtCompileTime, Cols = Traits::ColsAtCompileTime; static const int MaxRows = Traits::MaxRowsAtCompileTime, MaxCols = Traits::MaxColsAtCompileTime; - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Matrix ComplexMatrix; ComplexMatrix CA = A.template cast(); @@ -476,7 +476,7 @@ class MatrixFunctionReturnValue : public ReturnByValue::type NestedEvalType; typedef internal::remove_all_t NestedEvalTypeClean; typedef internal::traits Traits; - typedef std::complex::Real> ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Matrix DynMatrixType; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h index 4228166d1c..398971ebbd 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h @@ -330,7 +330,7 @@ class MatrixLogarithmReturnValue : public ReturnByValue::type DerivedEvalType; typedef internal::remove_all_t DerivedEvalTypeClean; typedef internal::traits Traits; - typedef std::complex::Real> ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Matrix DynMatrixType; typedef internal::MatrixLogarithmAtomic AtomicType; diff --git a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h index bff619a9c3..a420ee7095 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h @@ -91,7 +91,7 @@ class MatrixPowerAtomic : internal::noncopyable { enum { RowsAtCompileTime = MatrixType::RowsAtCompileTime, MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime }; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Block ResultType; const MatrixType& m_A; @@ -380,7 +380,7 @@ class MatrixPower : internal::noncopyable { Index cols() const { return m_A.cols(); } private: - typedef std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; typedef Matrix ComplexMatrix; @@ -628,7 +628,7 @@ template class MatrixComplexPowerReturnValue : public ReturnByValue > { public: typedef typename Derived::PlainObject PlainObject; - typedef typename std::complex ComplexScalar; + typedef internal::make_complex_t ComplexScalar; /** * \brief Constructor. @@ -685,7 +685,7 @@ const MatrixPowerReturnValue MatrixBase::pow(const RealScalar& } template -const MatrixComplexPowerReturnValue MatrixBase::pow(const std::complex& p) const { +const MatrixComplexPowerReturnValue MatrixBase::pow(const internal::make_complex_t& p) const { return MatrixComplexPowerReturnValue(derived(), p); } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide index 2cf272a115..054f5eb80e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide @@ -10,13 +10,14 @@ modifiableFileExclude { \.patch$ \.png$ \.svg$ - jormungandr/cpp/Docstrings\.hpp$ + jormungandr/cpp/docstrings\.hpp$ jormungandr/py\.typed$ } includeOtherLibs { ^Eigen/ ^catch2/ + ^gch/ ^nanobind/ ^sleipnir/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide index efa36cee1f..03938557c2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide @@ -6,12 +6,9 @@ cppSrcFileInclude { \.cpp$ } -licenseUpdateExclude { - include/sleipnir/util/small_vector\.hpp$ -} - includeOtherLibs { ^Eigen/ ^fmt/ + ^gch/ ^wpi/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp new file mode 100644 index 0000000000..d3cff4c424 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp @@ -0,0 +1,12 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace gch { + +template +using small_vector = wpi::SmallVector; + +} // namespace gch diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp deleted file mode 100644 index ef9a15bf69..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp +++ /dev/null @@ -1,1144 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include "sleipnir/autodiff/ExpressionType.hpp" -#include "sleipnir/util/IntrusiveSharedPtr.hpp" -#include "sleipnir/util/Pool.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir::detail { - -// The global pool allocator uses a thread-local static pool resource, which -// isn't guaranteed to be initialized properly across DLL boundaries on Windows -#ifdef _WIN32 -inline constexpr bool kUsePoolAllocator = false; -#else -inline constexpr bool kUsePoolAllocator = true; -#endif - -struct SLEIPNIR_DLLEXPORT Expression; - -inline void IntrusiveSharedPtrIncRefCount(Expression* expr); -inline void IntrusiveSharedPtrDecRefCount(Expression* expr); - -/** - * Typedef for intrusive shared pointer to Expression. - */ -using ExpressionPtr = IntrusiveSharedPtr; - -/** - * Creates an intrusive shared pointer to an expression from the global pool - * allocator. - * - * @param args Constructor arguments for Expression. - */ -template -static ExpressionPtr MakeExpressionPtr(Args&&... args) { - if constexpr (kUsePoolAllocator) { - return AllocateIntrusiveShared( - GlobalPoolAllocator(), std::forward(args)...); - } else { - return MakeIntrusiveShared(std::forward(args)...); - } -} - -/** - * An autodiff expression node. - */ -struct SLEIPNIR_DLLEXPORT Expression { - /** - * Binary function taking two doubles and returning a double. - */ - using BinaryFuncDouble = double (*)(double, double); - - /** - * Trinary function taking three doubles and returning a double. - */ - using TrinaryFuncDouble = double (*)(double, double, double); - - /** - * Trinary function taking three expressions and returning an expression. - */ - using TrinaryFuncExpr = ExpressionPtr (*)(const ExpressionPtr&, - const ExpressionPtr&, - const ExpressionPtr&); - - /// The value of the expression node. - double value = 0.0; - - /// The adjoint of the expression node used during autodiff. - double adjoint = 0.0; - - /// Tracks the number of instances of this expression yet to be encountered in - /// an expression tree. - uint32_t duplications = 0; - - /// This expression's row in wrt for autodiff gradient, Jacobian, or Hessian. - /// This is -1 if the expression isn't in wrt. - int32_t row = -1; - - /// The adjoint of the expression node used during gradient expression tree - /// generation. - ExpressionPtr adjointExpr; - - /// Expression argument type. - ExpressionType type = ExpressionType::kConstant; - - /// Reference count for intrusive shared pointer. - uint32_t refCount = 0; - - /// Either nullary operator with no arguments, unary operator with one - /// argument, or binary operator with two arguments. This operator is - /// used to update the node's value. - BinaryFuncDouble valueFunc = nullptr; - - /// Functions returning double adjoints of the children expressions. - /// - /// Parameters: - ///

- std::array gradientValueFuncs{nullptr, nullptr}; - - /// Functions returning Variable adjoints of the children expressions. - /// - /// Parameters: - ///
    - ///
  • lhs: Left argument to binary operator.
    • - ///
  • rhs: Right argument to binary operator.
    • - ///
  • parentAdjoint: Adjoint of parent expression.
    • - ///
- std::array gradientFuncs{nullptr, nullptr}; - - /// Expression arguments. - std::array args{nullptr, nullptr}; - - /** - * Constructs a constant expression with a value of zero. - */ - constexpr Expression() = default; - - /** - * Constructs a nullary expression (an operator with no arguments). - * - * @param value The expression value. - * @param type The expression type. It should be either constant (the default) - * or linear. - */ - explicit constexpr Expression(double value, - ExpressionType type = ExpressionType::kConstant) - : value{value}, type{type} {} - - /** - * Constructs an unary expression (an operator with one argument). - * - * @param type The expression's type. - * @param valueFunc Unary operator that produces this expression's value. - * @param lhsGradientValueFunc Gradient with respect to the operand. - * @param lhsGradientFunc Gradient with respect to the operand. - * @param lhs Unary operator's operand. - */ - constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc, - TrinaryFuncDouble lhsGradientValueFunc, - TrinaryFuncExpr lhsGradientFunc, ExpressionPtr lhs) - : value{valueFunc(lhs->value, 0.0)}, - type{type}, - valueFunc{valueFunc}, - gradientValueFuncs{lhsGradientValueFunc, nullptr}, - gradientFuncs{lhsGradientFunc, nullptr}, - args{lhs, nullptr} {} - - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param type The expression's type. - * @param valueFunc Unary operator that produces this expression's value. - * @param lhsGradientValueFunc Gradient with respect to the left operand. - * @param rhsGradientValueFunc Gradient with respect to the right operand. - * @param lhsGradientFunc Gradient with respect to the left operand. - * @param rhsGradientFunc Gradient with respect to the right operand. - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ - constexpr Expression(ExpressionType type, BinaryFuncDouble valueFunc, - TrinaryFuncDouble lhsGradientValueFunc, - TrinaryFuncDouble rhsGradientValueFunc, - TrinaryFuncExpr lhsGradientFunc, - TrinaryFuncExpr rhsGradientFunc, ExpressionPtr lhs, - ExpressionPtr rhs) - : value{valueFunc(lhs->value, rhs->value)}, - type{type}, - valueFunc{valueFunc}, - gradientValueFuncs{lhsGradientValueFunc, rhsGradientValueFunc}, - gradientFuncs{lhsGradientFunc, rhsGradientFunc}, - args{lhs, rhs} {} - - /** - * Returns true if the expression is the given constant. - * - * @param constant The constant. - */ - constexpr bool IsConstant(double constant) const { - return type == ExpressionType::kConstant && value == constant; - } - - /** - * Expression-Expression multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } else if (rhs->IsConstant(0.0)) { - // Return zero - return rhs; - } else if (lhs->IsConstant(1.0)) { - return rhs; - } else if (rhs->IsConstant(1.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value * rhs->value); - } - - // Evaluate expression type - ExpressionType type; - if (lhs->type == kConstant) { - type = rhs->type; - } else if (rhs->type == kConstant) { - type = lhs->type; - } else if (lhs->type == kLinear && rhs->type == kLinear) { - type = kQuadratic; - } else { - type = kNonlinear; - } - - return MakeExpressionPtr( - type, [](double lhs, double rhs) { return lhs * rhs; }, - [](double, double rhs, double parentAdjoint) { - return parentAdjoint * rhs; - }, - [](double lhs, double, double parentAdjoint) { - return parentAdjoint * lhs; - }, - [](const ExpressionPtr&, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { return parentAdjoint * rhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint * lhs; }, - lhs, rhs); - } - - /** - * Expression-Expression division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } else if (rhs->IsConstant(1.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value / rhs->value); - } - - // Evaluate expression type - ExpressionType type; - if (rhs->type == kConstant) { - type = lhs->type; - } else { - type = kNonlinear; - } - - return MakeExpressionPtr( - type, [](double lhs, double rhs) { return lhs / rhs; }, - [](double, double rhs, double parentAdjoint) { - return parentAdjoint / rhs; - }, - [](double lhs, double rhs, double parentAdjoint) { - return parentAdjoint * -lhs / (rhs * rhs); - }, - [](const ExpressionPtr&, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / rhs; }, - [](const ExpressionPtr& lhs, const ExpressionPtr& rhs, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -lhs / (rhs * rhs); - }, - lhs, rhs); - } - - /** - * Expression-Expression addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs == nullptr || lhs->IsConstant(0.0)) { - return rhs; - } else if (rhs == nullptr || rhs->IsConstant(0.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value + rhs->value); - } - - return MakeExpressionPtr( - std::max(lhs->type, rhs->type), - [](double lhs, double rhs) { return lhs + rhs; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - lhs, rhs); - } - - /** - * Expression-Expression subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, - const ExpressionPtr& rhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - if (rhs->IsConstant(0.0)) { - // Return zero - return rhs; - } else { - return -rhs; - } - } else if (rhs->IsConstant(0.0)) { - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant && rhs->type == kConstant) { - return MakeExpressionPtr(lhs->value - rhs->value); - } - - return MakeExpressionPtr( - std::max(lhs->type, rhs->type), - [](double lhs, double rhs) { return lhs - rhs; }, - [](double, double, double parentAdjoint) { return parentAdjoint; }, - [](double, double, double parentAdjoint) { return -parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, - lhs, rhs); - } - - /** - * Unary minus operator. - * - * @param lhs Operand of unary minus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { - using enum ExpressionType; - - // Prune expression - if (lhs->IsConstant(0.0)) { - // Return zero - return lhs; - } - - // Evaluate constant - if (lhs->type == kConstant) { - return MakeExpressionPtr(-lhs->value); - } - - return MakeExpressionPtr( - lhs->type, [](double lhs, double) { return -lhs; }, - [](double, double, double parentAdjoint) { return -parentAdjoint; }, - [](const ExpressionPtr&, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return -parentAdjoint; }, - lhs); - } - - /** - * Unary plus operator. - * - * @param lhs Operand of unary plus. - */ - friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs) { - return lhs; - } -}; - -SLEIPNIR_DLLEXPORT inline ExpressionPtr exp(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sin(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x); -SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); - -/** - * Refcount increment for intrusive shared pointer. - * - * @param expr The shared pointer's managed object. - */ -inline void IntrusiveSharedPtrIncRefCount(Expression* expr) { - ++expr->refCount; -} - -/** - * Refcount decrement for intrusive shared pointer. - * - * @param expr The shared pointer's managed object. - */ -inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { - // If a deeply nested tree is being deallocated all at once, calling the - // Expression destructor when expr's refcount reaches zero can cause a stack - // overflow. Instead, we iterate over its children to decrement their - // refcounts and deallocate them. - wpi::SmallVector stack; - stack.emplace_back(expr); - - while (!stack.empty()) { - auto elem = stack.back(); - stack.pop_back(); - - // Decrement the current node's refcount. If it reaches zero, deallocate the - // node and enqueue its children so their refcounts are decremented too. - if (--elem->refCount == 0) { - if (elem->adjointExpr != nullptr) { - stack.emplace_back(elem->adjointExpr.Get()); - } - for (auto&& arg : elem->args) { - if (arg != nullptr) { - stack.emplace_back(arg.Get()); - } - } - - // Not calling the destructor here is safe because it only decrements - // refcounts, which was already done above. - if constexpr (kUsePoolAllocator) { - auto alloc = GlobalPoolAllocator(); - std::allocator_traits::deallocate(alloc, elem, - sizeof(Expression)); - } - } - } -} - -/** - * std::abs() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::abs(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::abs(x); }, - [](double x, double, double parentAdjoint) { - if (x < 0.0) { - return -parentAdjoint; - } else if (x > 0.0) { - return parentAdjoint; - } else { - return 0.0; - } - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - if (x->value < 0.0) { - return -parentAdjoint; - } else if (x->value > 0.0) { - return parentAdjoint; - } else { - // Return zero - return MakeExpressionPtr(); - } - }, - x); -} - -/** - * std::acos() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::acos(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::acos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint / std::sqrt(1.0 - x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return -parentAdjoint / - sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x); - }, - x); -} - -/** - * std::asin() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::asin(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::asin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / std::sqrt(1.0 - x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - sleipnir::detail::sqrt(MakeExpressionPtr(1.0) - x * x); - }, - x); -} - -/** - * std::atan() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::atan(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::atan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (1.0 + x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / (MakeExpressionPtr(1.0) + x * x); - }, - x); -} - -/** - * std::atan2() for Expressions. - * - * @param y The y argument. - * @param x The x argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT - const ExpressionPtr& y, const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (y->IsConstant(0.0)) { - // Return zero - return y; - } else if (x->IsConstant(0.0)) { - return MakeExpressionPtr(std::numbers::pi / 2.0); - } - - // Evaluate constant - if (y->type == kConstant && x->type == kConstant) { - return MakeExpressionPtr(std::atan2(y->value, x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double y, double x) { return std::atan2(y, x); }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, - [](double y, double x, double parentAdjoint) { - return parentAdjoint * -y / (y * y + x * x); - }, - [](const ExpressionPtr& y, const ExpressionPtr& x, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * x / (y * y + x * x); - }, - [](const ExpressionPtr& y, const ExpressionPtr& x, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -y / (y * y + x * x); - }, - y, x); -} - -/** - * std::cos() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::cos(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::cos(x); }, - [](double x, double, double parentAdjoint) { - return -parentAdjoint * std::sin(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * -sleipnir::detail::sin(x); - }, - x); -} - -/** - * std::cosh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::cosh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::cosh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::sinh(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::sinh(x); - }, - x); -} - -/** - * std::erf() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::erf(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::erf(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * - std::exp(-x * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * - MakeExpressionPtr(2.0 * std::numbers::inv_sqrtpi) * - sleipnir::detail::exp(-x * x); - }, - x); -} - -/** - * std::exp() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::exp(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::exp(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::exp(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::exp(x); - }, - x); -} - -/** - * std::hypot() for Expressions. - * - * @param x The x argument. - * @param y The y argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT - const ExpressionPtr& x, const ExpressionPtr& y) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - return y; - } else if (y->IsConstant(0.0)) { - return x; - } - - // Evaluate constant - if (x->type == kConstant && y->type == kConstant) { - return MakeExpressionPtr(std::hypot(x->value, y->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double y) { return std::hypot(x, y); }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * x / std::hypot(x, y); - }, - [](double x, double y, double parentAdjoint) { - return parentAdjoint * y / std::hypot(x, y); - }, - [](const ExpressionPtr& x, const ExpressionPtr& y, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * x / sleipnir::detail::hypot(x, y); - }, - [](const ExpressionPtr& x, const ExpressionPtr& y, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * y / sleipnir::detail::hypot(x, y); - }, - x, y); -} - -/** - * std::log() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::log(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::log(x); }, - [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, - x); -} - -/** - * std::log10() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::log10(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::log10(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::numbers::ln10 * x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / (MakeExpressionPtr(std::numbers::ln10) * x); - }, - x); -} - -/** - * std::pow() for Expressions. - * - * @param base The base. - * @param power The power. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT - const ExpressionPtr& base, const ExpressionPtr& power) { - using enum ExpressionType; - - // Prune expression - if (base->IsConstant(0.0)) { - // Return zero - return base; - } else if (base->IsConstant(1.0)) { - return base; - } - if (power->IsConstant(0.0)) { - return MakeExpressionPtr(1.0); - } else if (power->IsConstant(1.0)) { - return base; - } - - // Evaluate constant - if (base->type == kConstant && power->type == kConstant) { - return MakeExpressionPtr(std::pow(base->value, power->value)); - } - - return MakeExpressionPtr( - base->type == kLinear && power->IsConstant(2.0) ? kQuadratic : kNonlinear, - [](double base, double power) { return std::pow(base, power); }, - [](double base, double power, double parentAdjoint) { - return parentAdjoint * std::pow(base, power - 1) * power; - }, - [](double base, double power, double parentAdjoint) { - // Since x * std::log(x) -> 0 as x -> 0 - if (base == 0.0) { - return 0.0; - } else { - return parentAdjoint * std::pow(base, power - 1) * base * - std::log(base); - } - }, - [](const ExpressionPtr& base, const ExpressionPtr& power, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * - sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) * - power; - }, - [](const ExpressionPtr& base, const ExpressionPtr& power, - const ExpressionPtr& parentAdjoint) { - // Since x * std::log(x) -> 0 as x -> 0 - if (base->value == 0.0) { - // Return zero - return base; - } else { - return parentAdjoint * - sleipnir::detail::pow(base, power - MakeExpressionPtr(1.0)) * - base * sleipnir::detail::log(base); - } - }, - base, power); -} - -/** - * sign() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { - using enum ExpressionType; - - // Evaluate constant - if (x->type == kConstant) { - if (x->value < 0.0) { - return MakeExpressionPtr(-1.0); - } else if (x->value == 0.0) { - // Return zero - return x; - } else { - return MakeExpressionPtr(1.0); - } - } - - return MakeExpressionPtr( - kNonlinear, - [](double x, double) { - if (x < 0.0) { - return -1.0; - } else if (x == 0.0) { - return 0.0; - } else { - return 1.0; - } - }, - [](double, double, double) { return 0.0; }, - [](const ExpressionPtr&, const ExpressionPtr&, const ExpressionPtr&) { - // Return zero - return MakeExpressionPtr(); - }, - x); -} - -/** - * std::sin() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::sin(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sin(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cos(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::cos(x); - }, - x); -} - -/** - * std::sinh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::sinh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sinh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint * std::cosh(x); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint * sleipnir::detail::cosh(x); - }, - x); -} - -/** - * std::sqrt() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Evaluate constant - if (x->type == kConstant) { - if (x->value == 0.0) { - // Return zero - return x; - } else if (x->value == 1.0) { - return x; - } else { - return MakeExpressionPtr(std::sqrt(x->value)); - } - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::sqrt(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (2.0 * std::sqrt(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (MakeExpressionPtr(2.0) * sleipnir::detail::sqrt(x)); - }, - x); -} - -/** - * std::tan() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT - const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::tan(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::tan(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cos(x) * std::cos(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (sleipnir::detail::cos(x) * sleipnir::detail::cos(x)); - }, - x); -} - -/** - * std::tanh() for Expressions. - * - * @param x The argument. - */ -SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { - using enum ExpressionType; - - // Prune expression - if (x->IsConstant(0.0)) { - // Return zero - return x; - } - - // Evaluate constant - if (x->type == kConstant) { - return MakeExpressionPtr(std::tanh(x->value)); - } - - return MakeExpressionPtr( - kNonlinear, [](double x, double) { return std::tanh(x); }, - [](double x, double, double parentAdjoint) { - return parentAdjoint / (std::cosh(x) * std::cosh(x)); - }, - [](const ExpressionPtr& x, const ExpressionPtr&, - const ExpressionPtr& parentAdjoint) { - return parentAdjoint / - (sleipnir::detail::cosh(x) * sleipnir::detail::cosh(x)); - }, - x); -} - -} // namespace sleipnir::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp deleted file mode 100644 index 714bcbb829..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Expression.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir::detail { - -/** - * This class is an adaptor type that performs value updates of an expression's - * computational graph in a way that skips duplicates. - */ -class SLEIPNIR_DLLEXPORT ExpressionGraph { - public: - /** - * Generates the deduplicated computational graph for the given expression. - * - * @param root The root node of the expression. - */ - explicit ExpressionGraph(ExpressionPtr& root) { - // If the root type is a constant, Update() is a no-op, so there's no work - // to do - if (root == nullptr || root->type == ExpressionType::kConstant) { - return; - } - - // Breadth-first search (BFS) is used as opposed to a depth-first search - // (DFS) to avoid counting duplicate nodes multiple times. A list of nodes - // ordered from parent to child with no duplicates is generated. - // - // https://en.wikipedia.org/wiki/Breadth-first_search - - // BFS list sorted from parent to child. - wpi::SmallVector stack; - - stack.emplace_back(root.Get()); - - // Initialize the number of instances of each node in the tree - // (Expression::duplications) - while (!stack.empty()) { - auto currentNode = stack.back(); - stack.pop_back(); - - for (auto&& arg : currentNode->args) { - // Only continue if the node is not a constant and hasn't already been - // explored. - if (arg != nullptr && arg->type != ExpressionType::kConstant) { - // If this is the first instance of the node encountered (it hasn't - // been explored yet), add it to stack so it's recursed upon - if (arg->duplications == 0) { - stack.push_back(arg.Get()); - } - ++arg->duplications; - } - } - } - - stack.emplace_back(root.Get()); - - while (!stack.empty()) { - auto currentNode = stack.back(); - stack.pop_back(); - - // BFS lists sorted from parent to child. - m_rowList.emplace_back(currentNode->row); - m_adjointList.emplace_back(currentNode); - if (currentNode->valueFunc != nullptr) { - // Constants are skipped because they have no valueFunc and don't need - // to be updated - m_valueList.emplace_back(currentNode); - } - - for (auto&& arg : currentNode->args) { - // Only add node if it's not a constant and doesn't already exist in the - // tape. - if (arg != nullptr && arg->type != ExpressionType::kConstant) { - // Once the number of node visitations equals the number of - // duplications (the counter hits zero), add it to the stack. Note - // that this means the node is only enqueued once. - --arg->duplications; - if (arg->duplications == 0) { - stack.push_back(arg.Get()); - } - } - } - } - } - - /** - * Update the values of all nodes in this computational tree based on the - * values of their dependent nodes. - */ - void Update() { - // Traverse the BFS list backward from child to parent and update the value - // of each node. - for (auto it = m_valueList.rbegin(); it != m_valueList.rend(); ++it) { - auto& node = *it; - - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr) { - if (rhs != nullptr) { - node->value = node->valueFunc(lhs->value, rhs->value); - } else { - node->value = node->valueFunc(lhs->value, 0.0); - } - } - } - } - - /** - * Returns the variable's gradient tree. - * - * @param wrt Variables with respect to which to compute the gradient. - */ - wpi::SmallVector GenerateGradientTree( - std::span wrt) const { - // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation - // for background on reverse accumulation automatic differentiation. - - for (size_t row = 0; row < wrt.size(); ++row) { - wrt[row]->row = row; - } - - wpi::SmallVector grad; - grad.reserve(wrt.size()); - for (size_t row = 0; row < wrt.size(); ++row) { - grad.emplace_back(MakeExpressionPtr()); - } - - // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1. - if (m_adjointList.size() > 0) { - m_adjointList[0]->adjointExpr = MakeExpressionPtr(1.0); - for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); - ++it) { - auto& node = *it; - node->adjointExpr = MakeExpressionPtr(); - } - } - - // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y - // multiplied by dy/dx. If there are multiple "paths" from the root node to - // variable; the variable's adjoint is the sum of each path's adjoint - // contribution. - for (auto node : m_adjointList) { - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr && !lhs->IsConstant(0.0)) { - lhs->adjointExpr = lhs->adjointExpr + - node->gradientFuncs[0](lhs, rhs, node->adjointExpr); - } - if (rhs != nullptr && !rhs->IsConstant(0.0)) { - rhs->adjointExpr = rhs->adjointExpr + - node->gradientFuncs[1](lhs, rhs, node->adjointExpr); - } - - // If variable is a leaf node, assign its adjoint to the gradient. - if (node->row != -1) { - grad[node->row] = node->adjointExpr; - } - } - - // Unlink adjoints to avoid circular references between them and their - // parent expressions. This ensures all expressions are returned to the free - // list. - for (auto node : m_adjointList) { - for (auto& arg : node->args) { - if (arg != nullptr) { - arg->adjointExpr = nullptr; - } - } - } - - for (size_t row = 0; row < wrt.size(); ++row) { - wrt[row]->row = -1; - } - - return grad; - } - - /** - * Updates the adjoints in the expression graph, effectively computing the - * gradient. - * - * @param func A function that takes two arguments: an int for the gradient - * row, and a double for the adjoint (gradient value). - */ - void ComputeAdjoints(function_ref func) { - // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1. - m_adjointList[0]->adjoint = 1.0; - for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) { - auto& node = *it; - node->adjoint = 0.0; - } - - // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y - // multiplied by dy/dx. If there are multiple "paths" from the root node to - // variable; the variable's adjoint is the sum of each path's adjoint - // contribution. - for (size_t col = 0; col < m_adjointList.size(); ++col) { - auto& node = m_adjointList[col]; - auto& lhs = node->args[0]; - auto& rhs = node->args[1]; - - if (lhs != nullptr) { - if (rhs != nullptr) { - lhs->adjoint += node->gradientValueFuncs[0](lhs->value, rhs->value, - node->adjoint); - rhs->adjoint += node->gradientValueFuncs[1](lhs->value, rhs->value, - node->adjoint); - } else { - lhs->adjoint += - node->gradientValueFuncs[0](lhs->value, 0.0, node->adjoint); - } - } - - // If variable is a leaf node, assign its adjoint to the gradient. - int row = m_rowList[col]; - if (row != -1) { - func(row, node->adjoint); - } - } - } - - private: - // List that maps nodes to their respective row. - wpi::SmallVector m_rowList; - - // List for updating adjoints - wpi::SmallVector m_adjointList; - - // List for updating values - wpi::SmallVector m_valueList; -}; - -} // namespace sleipnir::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp deleted file mode 100644 index 37825e3b2e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -namespace sleipnir { - -/** - * Expression type. - * - * Used for autodiff caching. - */ -enum class ExpressionType : uint8_t { - /// There is no expression. - kNone, - /// The expression is a constant. - kConstant, - /// The expression is composed of linear and lower-order operators. - kLinear, - /// The expression is composed of quadratic and lower-order operators. - kQuadratic, - /// The expression is composed of nonlinear and lower-order operators. - kNonlinear -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp deleted file mode 100644 index 2e60d89e95..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include -#include - -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class calculates the Hessian of a variable with respect to a vector of - * variables. - * - * The gradient tree is cached so subsequent Hessian calculations are faster, - * and the Hessian is only recomputed if the variable expression is nonlinear. - */ -class SLEIPNIR_DLLEXPORT Hessian { - public: - /** - * Constructs a Hessian object. - * - * @param variable Variable of which to compute the Hessian. - * @param wrt Vector of variables with respect to which to compute the - * Hessian. - */ - Hessian(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{ - [&] { - wpi::SmallVector wrtVec; - wrtVec.reserve(wrt.size()); - for (auto& elem : wrt) { - wrtVec.emplace_back(elem.expr); - } - - auto grad = - detail::ExpressionGraph{variable.expr}.GenerateGradientTree( - wrtVec); - - VariableMatrix ret{wrt.Rows()}; - for (int row = 0; row < ret.Rows(); ++row) { - ret(row) = Variable{std::move(grad[row])}; - } - return ret; - }(), - wrt} {} - - /** - * Returns the Hessian as a VariableMatrix. - * - * This is useful when constructing optimization problems with derivatives in - * them. - */ - VariableMatrix Get() const { return m_jacobian.Get(); } - - /** - * Evaluates the Hessian at wrt's value. - */ - const Eigen::SparseMatrix& Value() { return m_jacobian.Value(); } - - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_jacobian.GetProfiler(); } - - private: - Jacobian m_jacobian; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp deleted file mode 100644 index 0c660156c8..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include - -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class calculates the Jacobian of a vector of variables with respect to a - * vector of variables. - * - * The Jacobian is only recomputed if the variable expression is quadratic or - * higher order. - */ -class SLEIPNIR_DLLEXPORT Jacobian { - public: - /** - * Constructs a Jacobian object. - * - * @param variables Vector of variables of which to compute the Jacobian. - * @param wrt Vector of variables with respect to which to compute the - * Jacobian. - */ - Jacobian(const VariableMatrix& variables, const VariableMatrix& wrt) noexcept - : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { - m_profiler.StartSetup(); - - for (int row = 0; row < m_wrt.Rows(); ++row) { - m_wrt(row).expr->row = row; - } - - for (Variable variable : m_variables) { - m_graphs.emplace_back(variable.expr); - } - - // Reserve triplet space for 99% sparsity - m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01); - - for (int row = 0; row < m_variables.Rows(); ++row) { - if (m_variables(row).Type() == ExpressionType::kLinear) { - // If the row is linear, compute its gradient once here and cache its - // triplets. Constant rows are ignored because their gradients have no - // nonzero triplets. - m_graphs[row].ComputeAdjoints([&](int col, double adjoint) { - m_cachedTriplets.emplace_back(row, col, adjoint); - }); - } else if (m_variables(row).Type() > ExpressionType::kLinear) { - // If the row is quadratic or nonlinear, add it to the list of nonlinear - // rows to be recomputed in Value(). - m_nonlinearRows.emplace_back(row); - } - } - - for (int row = 0; row < m_wrt.Rows(); ++row) { - m_wrt(row).expr->row = -1; - } - - if (m_nonlinearRows.empty()) { - m_J.setFromTriplets(m_cachedTriplets.begin(), m_cachedTriplets.end()); - } - - m_profiler.StopSetup(); - } - - /** - * Returns the Jacobian as a VariableMatrix. - * - * This is useful when constructing optimization problems with derivatives in - * them. - */ - VariableMatrix Get() const { - VariableMatrix result{m_variables.Rows(), m_wrt.Rows()}; - - wpi::SmallVector wrtVec; - wrtVec.reserve(m_wrt.size()); - for (auto& elem : m_wrt) { - wrtVec.emplace_back(elem.expr); - } - - for (int row = 0; row < m_variables.Rows(); ++row) { - auto grad = m_graphs[row].GenerateGradientTree(wrtVec); - for (int col = 0; col < m_wrt.Rows(); ++col) { - result(row, col) = Variable{std::move(grad[col])}; - } - } - - return result; - } - - /** - * Evaluates the Jacobian at wrt's value. - */ - const Eigen::SparseMatrix& Value() { - if (m_nonlinearRows.empty()) { - return m_J; - } - - m_profiler.StartSolve(); - - for (auto& graph : m_graphs) { - graph.Update(); - } - - // Copy the cached triplets so triplets added for the nonlinear rows are - // thrown away at the end of the function - auto triplets = m_cachedTriplets; - - // Compute each nonlinear row of the Jacobian - for (int row : m_nonlinearRows) { - m_graphs[row].ComputeAdjoints([&](int col, double adjoint) { - triplets.emplace_back(row, col, adjoint); - }); - } - - m_J.setFromTriplets(triplets.begin(), triplets.end()); - - m_profiler.StopSolve(); - - return m_J; - } - - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_profiler; } - - private: - VariableMatrix m_variables; - VariableMatrix m_wrt; - - wpi::SmallVector m_graphs; - - Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; - - // Cached triplets for gradients of linear rows - wpi::SmallVector> m_cachedTriplets; - - // List of row indices for nonlinear rows whose graients will be computed in - // Value() - wpi::SmallVector m_nonlinearRows; - - Profiler m_profiler; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp deleted file mode 100644 index 5c22d145b3..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Records the number of profiler measurements (start/stop pairs) and the - * average duration between each start and stop call. - */ -class SLEIPNIR_DLLEXPORT Profiler { - public: - /** - * Tell the profiler to start measuring setup time. - */ - void StartSetup() { m_setupStartTime = std::chrono::system_clock::now(); } - - /** - * Tell the profiler to stop measuring setup time. - */ - void StopSetup() { - m_setupDuration = std::chrono::system_clock::now() - m_setupStartTime; - } - - /** - * Tell the profiler to start measuring solve time. - */ - void StartSolve() { m_solveStartTime = std::chrono::system_clock::now(); } - - /** - * Tell the profiler to stop measuring solve time, increment the number of - * averages, and incorporate the latest measurement into the average. - */ - void StopSolve() { - auto now = std::chrono::system_clock::now(); - ++m_solveMeasurements; - m_averageSolveDuration = - (m_solveMeasurements - 1.0) / m_solveMeasurements * - m_averageSolveDuration + - 1.0 / m_solveMeasurements * (now - m_solveStartTime); - } - - /** - * The setup duration in milliseconds as a double. - */ - double SetupDuration() const { - using std::chrono::duration_cast; - using std::chrono::nanoseconds; - return duration_cast(m_setupDuration).count() / 1e6; - } - - /** - * The number of solve measurements taken. - */ - int SolveMeasurements() const { return m_solveMeasurements; } - - /** - * The average solve duration in milliseconds as a double. - */ - double AverageSolveDuration() const { - using std::chrono::duration_cast; - using std::chrono::nanoseconds; - return duration_cast(m_averageSolveDuration).count() / 1e6; - } - - private: - std::chrono::system_clock::time_point m_setupStartTime; - std::chrono::duration m_setupDuration{0.0}; - - int m_solveMeasurements = 0; - std::chrono::duration m_averageSolveDuration{0.0}; - std::chrono::system_clock::time_point m_solveStartTime; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp deleted file mode 100644 index 72bb235917..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp +++ /dev/null @@ -1,765 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -#include - -#include "sleipnir/autodiff/Slice.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/FunctionRef.hpp" - -namespace sleipnir { - -/** - * A submatrix of autodiff variables with reference semantics. - * - * @tparam Mat The type of the matrix whose storage this class points to. - */ -template -class VariableBlock { - public: - VariableBlock(const VariableBlock& values) = default; - - /** - * Assigns a VariableBlock to the block. - * - * @param values VariableBlock of values. - */ - VariableBlock& operator=(const VariableBlock& values) { - if (this == &values) { - return *this; - } - - if (m_mat == nullptr) { - m_mat = values.m_mat; - m_rowSlice = values.m_rowSlice; - m_rowSliceLength = values.m_rowSliceLength; - m_colSlice = values.m_colSlice; - m_colSliceLength = values.m_colSliceLength; - } else { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - } - - return *this; - } - - VariableBlock(VariableBlock&&) = default; - - /** - * Assigns a VariableBlock to the block. - * - * @param values VariableBlock of values. - */ - VariableBlock& operator=(VariableBlock&& values) { - if (this == &values) { - return *this; - } - - if (m_mat == nullptr) { - m_mat = values.m_mat; - m_rowSlice = values.m_rowSlice; - m_rowSliceLength = values.m_rowSliceLength; - m_colSlice = values.m_colSlice; - m_colSliceLength = values.m_colSliceLength; - } else { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - } - - return *this; - } - - /** - * Constructs a Variable block pointing to all of the given matrix. - * - * @param mat The matrix to which to point. - */ - VariableBlock(Mat& mat) // NOLINT - : m_mat{&mat}, - m_rowSlice{0, mat.Rows(), 1}, - m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, - m_colSlice{0, mat.Cols(), 1}, - m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} - - /** - * Constructs a Variable block pointing to a subset of the given matrix. - * - * @param mat The matrix to which to point. - * @param rowOffset The block's row offset. - * @param colOffset The block's column offset. - * @param blockRows The number of rows in the block. - * @param blockCols The number of columns in the block. - */ - VariableBlock(Mat& mat, int rowOffset, int colOffset, int blockRows, - int blockCols) - : m_mat{&mat}, - m_rowSlice{rowOffset, rowOffset + blockRows, 1}, - m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, - m_colSlice{colOffset, colOffset + blockCols, 1}, - m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} - - /** - * Constructs a Variable block pointing to a subset of the given matrix. - * - * Note that the slices are taken as is rather than adjusted. - * - * @param mat The matrix to which to point. - * @param rowSlice The block's row slice. - * @param rowSliceLength The block's row length. - * @param colSlice The block's column slice. - * @param colSliceLength The block's column length. - */ - VariableBlock(Mat& mat, Slice rowSlice, int rowSliceLength, Slice colSlice, - int colSliceLength) - : m_mat{&mat}, - m_rowSlice{std::move(rowSlice)}, - m_rowSliceLength{rowSliceLength}, - m_colSlice{std::move(colSlice)}, - m_colSliceLength{colSliceLength} {} - - /** - * Assigns a double to the block. - * - * This only works for blocks with one row and one column. - */ - VariableBlock& operator=(double value) { - Assert(Rows() == 1 && Cols() == 1); - - (*this)(0, 0) = value; - - return *this; - } - - /** - * Assigns a double to the block. - * - * This only works for blocks with one row and one column. - * - * @param value Value to assign. - */ - void SetValue(double value) { - Assert(Rows() == 1 && Cols() == 1); - - (*this)(0, 0).SetValue(value); - } - - /** - * Assigns an Eigen matrix to the block. - * - * @param values Eigen matrix of values to assign. - */ - template - VariableBlock& operator=(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - - return *this; - } - - /** - * Sets block's internal values. - * - * @param values Eigen matrix of values. - */ - template - requires std::same_as - void SetValue(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col).SetValue(values(row, col)); - } - } - } - - /** - * Assigns a VariableMatrix to the block. - * - * @param values VariableMatrix of values. - */ - VariableBlock& operator=(const Mat& values) { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - return *this; - } - - /** - * Assigns a VariableMatrix to the block. - * - * @param values VariableMatrix of values. - */ - VariableBlock& operator=(Mat&& values) { - Assert(Rows() == values.Rows()); - Assert(Cols() == values.Cols()); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) = std::move(values(row, col)); - } - } - return *this; - } - - /** - * Returns a scalar subblock at the given row and column. - * - * @param row The scalar subblock's row. - * @param col The scalar subblock's column. - */ - Variable& operator()(int row, int col) - requires(!std::is_const_v) - { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step); - } - - /** - * Returns a scalar subblock at the given row and column. - * - * @param row The scalar subblock's row. - * @param col The scalar subblock's column. - */ - const Variable& operator()(int row, int col) const { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step); - } - - /** - * Returns a scalar subblock at the given row. - * - * @param row The scalar subblock's row. - */ - Variable& operator()(int row) - requires(!std::is_const_v) - { - Assert(row >= 0 && row < Rows() * Cols()); - return (*this)(row / Cols(), row % Cols()); - } - - /** - * Returns a scalar subblock at the given row. - * - * @param row The scalar subblock's row. - */ - const Variable& operator()(int row) const { - Assert(row >= 0 && row < Rows() * Cols()); - return (*this)(row / Cols(), row % Cols()); - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - VariableBlock Block(int rowOffset, int colOffset, int blockRows, - int blockCols) { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return (*this)({rowOffset, rowOffset + blockRows, 1}, - {colOffset, colOffset + blockCols, 1}); - } - - /** - * Returns a block slice of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - const VariableBlock Block(int rowOffset, int colOffset, - int blockRows, int blockCols) const { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return (*this)({rowOffset, rowOffset + blockRows, 1}, - {colOffset, colOffset + blockCols, 1}); - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - VariableBlock operator()(Slice rowSlice, Slice colSlice) { - int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); - int colSliceLength = colSlice.Adjust(m_colSliceLength); - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - const VariableBlock operator()(Slice rowSlice, - Slice colSlice) const { - int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); - int colSliceLength = colSlice.Adjust(m_colSliceLength); - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, int colSliceLength) { - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - const VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, - int colSliceLength) const { - return VariableBlock{ - *m_mat, - {m_rowSlice.start + rowSlice.start * m_rowSlice.step, - m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, - rowSliceLength, - {m_colSlice.start + colSlice.start * m_colSlice.step, - m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, - colSliceLength}; - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - VariableBlock Segment(int offset, int length) { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - const VariableBlock Segment(int offset, int length) const { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) const { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) const { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Compound matrix multiplication-assignment operator. - * - * @param rhs Variable to multiply. - */ - VariableBlock& operator*=(const VariableBlock& rhs) { - Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols()); - - for (int i = 0; i < Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < Cols(); ++k) { - sum += (*this)(i, k) * rhs(k, j); - } - (*this)(i, j) = sum; - } - } - - return *this; - } - - /** - * Compound matrix multiplication-assignment operator (only enabled when lhs - * is a scalar). - * - * @param rhs Variable to multiply. - */ - VariableBlock& operator*=(double rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) *= rhs; - } - } - - return *this; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableBlock& operator/=(const VariableBlock& rhs) { - Assert(rhs.Rows() == 1 && rhs.Cols() == 1); - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs(0, 0); - } - } - - return *this; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableBlock& operator/=(double rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs; - } - } - - return *this; - } - - /** - * Compound addition-assignment operator. - * - * @param rhs Variable to add. - */ - VariableBlock& operator+=(const VariableBlock& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) += rhs(row, col); - } - } - - return *this; - } - - /** - * Compound subtraction-assignment operator. - * - * @param rhs Variable to subtract. - */ - VariableBlock& operator-=(const VariableBlock& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) -= rhs(row, col); - } - } - - return *this; - } - - /** - * Returns the transpose of the variable matrix. - */ - std::remove_cv_t T() const { - std::remove_cv_t result{Cols(), Rows()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(col, row) = (*this)(row, col); - } - } - - return result; - } - - /** - * Returns number of rows in the matrix. - */ - int Rows() const { return m_rowSliceLength; } - - /** - * Returns number of columns in the matrix. - */ - int Cols() const { return m_colSliceLength; } - - /** - * Returns an element of the variable matrix. - * - * @param row The row of the element to return. - * @param col The column of the element to return. - */ - double Value(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, - m_colSlice.start + col * m_colSlice.step) - .Value(); - } - - /** - * Returns a row of the variable column vector. - * - * @param index The index of the element to return. - */ - double Value(int index) { - Assert(index >= 0 && index < Rows() * Cols()); - return Value(index / Cols(), index % Cols()); - } - - /** - * Returns the contents of the variable matrix. - */ - Eigen::MatrixXd Value() { - Eigen::MatrixXd result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = Value(row, col); - } - } - - return result; - } - - /** - * Transforms the matrix coefficient-wise with an unary operator. - * - * @param unaryOp The unary operator to use for the transform operation. - */ - std::remove_cv_t CwiseTransform( - function_ref unaryOp) const { - std::remove_cv_t result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = unaryOp((*this)(row, col)); - } - } - - return result; - } - - class iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using reference = Variable&; - - iterator(VariableBlock* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - iterator operator++(int) { - iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const iterator&) const = default; - reference operator*() { return (*m_mat)(m_row, m_col); } - - private: - VariableBlock* m_mat; - int m_row; - int m_col; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using const_reference = const Variable&; - - const_iterator(const VariableBlock* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - const_iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - const_iterator operator++(int) { - const_iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const const_iterator&) const = default; - const_reference operator*() const { return (*m_mat)(m_row, m_col); } - - private: - const VariableBlock* m_mat; - int m_row; - int m_col; - }; - - /** - * Returns begin iterator. - */ - iterator begin() { return iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - iterator end() { return iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator begin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator end() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator cbegin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator cend() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns number of elements in matrix. - */ - size_t size() const { return Rows() * Cols(); } - - private: - Mat* m_mat = nullptr; - - Slice m_rowSlice; - int m_rowSliceLength = 0; - - Slice m_colSlice; - int m_colSliceLength = 0; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp deleted file mode 100644 index 57b09913d1..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp +++ /dev/null @@ -1,1096 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Slice.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableBlock.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * A matrix of autodiff variables. - */ -class SLEIPNIR_DLLEXPORT VariableMatrix { - public: - /** - * Constructs an empty VariableMatrix. - */ - VariableMatrix() = default; - - /** - * Constructs a VariableMatrix column vector with the given rows. - * - * @param rows The number of matrix rows. - */ - explicit VariableMatrix(int rows) : m_rows{rows}, m_cols{1} { - for (int row = 0; row < rows; ++row) { - m_storage.emplace_back(); - } - } - - /** - * Constructs a VariableMatrix with the given dimensions. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} { - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - m_storage.emplace_back(); - } - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * @param list The nested list of Variables. - */ - VariableMatrix( // NOLINT - std::initializer_list> list) { - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of doubles. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ - VariableMatrix(const std::vector>& list) { // NOLINT - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ - VariableMatrix(const std::vector>& list) { // NOLINT - // Get row and column counts for destination matrix - m_rows = list.size(); - m_cols = 0; - if (list.size() > 0) { - m_cols = list.begin()->size(); - } - - // Assert the first and latest column counts are the same - for ([[maybe_unused]] - const auto& row : list) { - Assert(list.begin()->size() == row.size()); - } - - m_storage.reserve(Rows() * Cols()); - for (const auto& row : list) { - std::copy(row.begin(), row.end(), std::back_inserter(m_storage)); - } - } - - /** - * Constructs a VariableMatrix from an Eigen matrix. - * - * @param values Eigen matrix of values. - */ - template - VariableMatrix(const Eigen::MatrixBase& values) // NOLINT - : m_rows{static_cast(values.rows())}, - m_cols{static_cast(values.cols())} { - m_storage.reserve(values.rows() * values.cols()); - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a VariableMatrix from an Eigen diagonal matrix. - * - * @param values Diagonal matrix of values. - */ - template - VariableMatrix(const Eigen::DiagonalBase& values) // NOLINT - : m_rows{static_cast(values.rows())}, - m_cols{static_cast(values.cols())} { - m_storage.reserve(values.rows() * values.cols()); - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - if (row == col) { - m_storage.emplace_back(values.diagonal()(row)); - } else { - m_storage.emplace_back(0.0); - } - } - } - } - - /** - * Assigns an Eigen matrix to a VariableMatrix. - * - * @param values Eigen matrix of values. - */ - template - VariableMatrix& operator=(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col) = values(row, col); - } - } - - return *this; - } - - /** - * Sets the VariableMatrix's internal values. - * - * @param values Eigen matrix of values. - */ - template - requires std::same_as - void SetValue(const Eigen::MatrixBase& values) { - Assert(Rows() == values.rows()); - Assert(Cols() == values.cols()); - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col).SetValue(values(row, col)); - } - } - } - - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ - VariableMatrix(const Variable& variable) // NOLINT - : m_rows{1}, m_cols{1} { - m_storage.emplace_back(variable); - } - - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ - VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { // NOLINT - m_storage.emplace_back(std::move(variable)); - } - - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ - VariableMatrix(const VariableBlock& values) // NOLINT - : m_rows{values.Rows()}, m_cols{values.Cols()} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ - VariableMatrix(const VariableBlock& values) // NOLINT - : m_rows{values.Rows()}, m_cols{values.Cols()} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values(row, col)); - } - } - } - - /** - * Constructs a column vector wrapper around a Variable array. - * - * @param values Variable array to wrap. - */ - explicit VariableMatrix(std::span values) - : m_rows{static_cast(values.size())}, m_cols{1} { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values[row * Cols() + col]); - } - } - } - - /** - * Constructs a matrix wrapper around a Variable array. - * - * @param values Variable array to wrap. - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - VariableMatrix(std::span values, int rows, int cols) - : m_rows{rows}, m_cols{cols} { - Assert(static_cast(values.size()) == Rows() * Cols()); - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - m_storage.emplace_back(values[row * Cols() + col]); - } - } - } - - /** - * Returns a block pointing to the given row and column. - * - * @param row The block row. - * @param col The block column. - */ - Variable& operator()(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col]; - } - - /** - * Returns a block pointing to the given row and column. - * - * @param row The block row. - * @param col The block column. - */ - const Variable& operator()(int row, int col) const { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col]; - } - - /** - * Returns a block pointing to the given row. - * - * @param row The block row. - */ - Variable& operator()(int row) { - Assert(row >= 0 && row < Rows() * Cols()); - return m_storage[row]; - } - - /** - * Returns a block pointing to the given row. - * - * @param row The block row. - */ - const Variable& operator()(int row) const { - Assert(row >= 0 && row < Rows() * Cols()); - return m_storage[row]; - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - VariableBlock Block(int rowOffset, int colOffset, - int blockRows, int blockCols) { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; - } - - /** - * Returns a block of the variable matrix. - * - * @param rowOffset The row offset of the block selection. - * @param colOffset The column offset of the block selection. - * @param blockRows The number of rows in the block selection. - * @param blockCols The number of columns in the block selection. - */ - const VariableBlock Block(int rowOffset, int colOffset, - int blockRows, - int blockCols) const { - Assert(rowOffset >= 0 && rowOffset <= Rows()); - Assert(colOffset >= 0 && colOffset <= Cols()); - Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); - Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - VariableBlock operator()(Slice rowSlice, Slice colSlice) { - int rowSliceLength = rowSlice.Adjust(Rows()); - int colSliceLength = colSlice.Adjust(Cols()); - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * @param rowSlice The row slice. - * @param colSlice The column slice. - */ - const VariableBlock operator()(Slice rowSlice, - Slice colSlice) const { - int rowSliceLength = rowSlice.Adjust(Rows()); - int colSliceLength = colSlice.Adjust(Cols()); - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - * - */ - VariableBlock operator()(Slice rowSlice, int rowSliceLength, - Slice colSlice, int colSliceLength) { - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a slice of the variable matrix. - * - * The given slices aren't adjusted. This overload is for Python bindings - * only. - * - * @param rowSlice The row slice. - * @param rowSliceLength The row slice length. - * @param colSlice The column slice. - * @param colSliceLength The column slice length. - */ - const VariableBlock operator()( - Slice rowSlice, int rowSliceLength, Slice colSlice, - int colSliceLength) const { - return VariableBlock{*this, std::move(rowSlice), rowSliceLength, - std::move(colSlice), colSliceLength}; - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - VariableBlock Segment(int offset, int length) { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a segment of the variable vector. - * - * @param offset The offset of the segment. - * @param length The length of the segment. - */ - const VariableBlock Segment(int offset, - int length) const { - Assert(offset >= 0 && offset < Rows() * Cols()); - Assert(length >= 0 && length <= Rows() * Cols() - offset); - return Block(offset, 0, length, 1); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - VariableBlock Row(int row) { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a row slice of the variable matrix. - * - * @param row The row to slice. - */ - const VariableBlock Row(int row) const { - Assert(row >= 0 && row < Rows()); - return Block(row, 0, 1, Cols()); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - VariableBlock Col(int col) { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Returns a column slice of the variable matrix. - * - * @param col The column to slice. - */ - const VariableBlock Col(int col) const { - Assert(col >= 0 && col < Cols()); - return Block(0, col, Rows(), 1); - } - - /** - * Matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(const VariableMatrix& lhs, const VariableMatrix& rhs) { - Assert(lhs.Cols() == rhs.Rows()); - - VariableMatrix result{lhs.Rows(), rhs.Cols()}; - - for (int i = 0; i < lhs.Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < lhs.Cols(); ++k) { - sum += lhs(i, k) * rhs(k, j); - } - result(i, j) = sum; - } - } - - return result; - } - - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const VariableMatrix& lhs, - const Variable& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) * rhs; - } - } - - return result; - } - - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const VariableMatrix& lhs, - double rhs) { - return lhs * Variable{rhs}; - } - - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(const Variable& lhs, const VariableMatrix& rhs) { - VariableMatrix result{rhs.Rows(), rhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = rhs(row, col) * lhs; - } - } - - return result; - } - - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator*(double lhs, const VariableMatrix& rhs) { - return Variable{lhs} * rhs; - } - - /** - * Compound matrix multiplication-assignment operator. - * - * @param rhs Variable to multiply. - */ - VariableMatrix& operator*=(const VariableMatrix& rhs) { - Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols()); - - for (int i = 0; i < Rows(); ++i) { - for (int j = 0; j < rhs.Cols(); ++j) { - Variable sum; - for (int k = 0; k < Cols(); ++k) { - sum += (*this)(i, k) * rhs(k, j); - } - (*this)(i, j) = sum; - } - } - - return *this; - } - - /** - * Binary division operator (only enabled when rhs is a scalar). - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix operator/(const VariableMatrix& lhs, - const Variable& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) / rhs; - } - } - - return result; - } - - /** - * Compound matrix division-assignment operator (only enabled when rhs - * is a scalar). - * - * @param rhs Variable to divide. - */ - VariableMatrix& operator/=(const Variable& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) /= rhs; - } - } - - return *this; - } - - /** - * Binary addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator+(const VariableMatrix& lhs, const VariableMatrix& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) + rhs(row, col); - } - } - - return result; - } - - /** - * Compound addition-assignment operator. - * - * @param rhs Variable to add. - */ - VariableMatrix& operator+=(const VariableMatrix& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) += rhs(row, col); - } - } - - return *this; - } - - /** - * Binary subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator-(const VariableMatrix& lhs, const VariableMatrix& rhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = lhs(row, col) - rhs(row, col); - } - } - - return result; - } - - /** - * Compound subtraction-assignment operator. - * - * @param rhs Variable to subtract. - */ - VariableMatrix& operator-=(const VariableMatrix& rhs) { - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - (*this)(row, col) -= rhs(row, col); - } - } - - return *this; - } - - /** - * Unary minus operator. - * - * @param lhs Operand for unary minus. - */ - friend SLEIPNIR_DLLEXPORT VariableMatrix - operator-(const VariableMatrix& lhs) { - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < result.Rows(); ++row) { - for (int col = 0; col < result.Cols(); ++col) { - result(row, col) = -lhs(row, col); - } - } - - return result; - } - - /** - * Implicit conversion operator from 1x1 VariableMatrix to Variable. - */ - operator Variable() const { // NOLINT - Assert(Rows() == 1 && Cols() == 1); - return (*this)(0, 0); - } - - /** - * Returns the transpose of the variable matrix. - */ - VariableMatrix T() const { - VariableMatrix result{Cols(), Rows()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(col, row) = (*this)(row, col); - } - } - - return result; - } - - /** - * Returns number of rows in the matrix. - */ - int Rows() const { return m_rows; } - - /** - * Returns number of columns in the matrix. - */ - int Cols() const { return m_cols; } - - /** - * Returns an element of the variable matrix. - * - * @param row The row of the element to return. - * @param col The column of the element to return. - */ - double Value(int row, int col) { - Assert(row >= 0 && row < Rows()); - Assert(col >= 0 && col < Cols()); - return m_storage[row * Cols() + col].Value(); - } - - /** - * Returns a row of the variable column vector. - * - * @param index The index of the element to return. - */ - double Value(int index) { - Assert(index >= 0 && index < Rows() * Cols()); - return m_storage[index].Value(); - } - - /** - * Returns the contents of the variable matrix. - */ - Eigen::MatrixXd Value() { - Eigen::MatrixXd result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = Value(row, col); - } - } - - return result; - } - - /** - * Transforms the matrix coefficient-wise with an unary operator. - * - * @param unaryOp The unary operator to use for the transform operation. - */ - VariableMatrix CwiseTransform( - function_ref unaryOp) const { - VariableMatrix result{Rows(), Cols()}; - - for (int row = 0; row < Rows(); ++row) { - for (int col = 0; col < Cols(); ++col) { - result(row, col) = unaryOp((*this)(row, col)); - } - } - - return result; - } - - class iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using reference = Variable&; - - iterator(VariableMatrix* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - iterator operator++(int) { - iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const iterator&) const = default; - reference operator*() { return (*m_mat)(m_row, m_col); } - - private: - VariableMatrix* m_mat; - int m_row; - int m_col; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = Variable; - using difference_type = std::ptrdiff_t; - using pointer = Variable*; - using const_reference = const Variable&; - - const_iterator(const VariableMatrix* mat, int row, int col) - : m_mat{mat}, m_row{row}, m_col{col} {} - - const_iterator& operator++() { - ++m_col; - if (m_col == m_mat->Cols()) { - m_col = 0; - ++m_row; - } - return *this; - } - const_iterator operator++(int) { - const_iterator retval = *this; - ++(*this); - return retval; - } - bool operator==(const const_iterator&) const = default; - const_reference operator*() const { return (*m_mat)(m_row, m_col); } - - private: - const VariableMatrix* m_mat; - int m_row; - int m_col; - }; - - /** - * Returns begin iterator. - */ - iterator begin() { return iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - iterator end() { return iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator begin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator end() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns begin iterator. - */ - const_iterator cbegin() const { return const_iterator(this, 0, 0); } - - /** - * Returns end iterator. - */ - const_iterator cend() const { return const_iterator(this, Rows(), 0); } - - /** - * Returns number of elements in matrix. - */ - size_t size() const { return m_rows * m_cols; } - - /** - * Returns a variable matrix filled with zeroes. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - static VariableMatrix Zero(int rows, int cols) { - VariableMatrix result{rows, cols}; - - for (auto& elem : result) { - elem = 0.0; - } - - return result; - } - - /** - * Returns a variable matrix filled with ones. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ - static VariableMatrix Ones(int rows, int cols) { - VariableMatrix result{rows, cols}; - - for (auto& elem : result) { - elem = 1.0; - } - - return result; - } - - private: - wpi::SmallVector m_storage; - int m_rows = 0; - int m_cols = 0; -}; - -/** - * Applies a coefficient-wise reduce operation to two matrices. - * - * @param lhs The left-hand side of the binary operator. - * @param rhs The right-hand side of the binary operator. - * @param binaryOp The binary operator to use for the reduce operation. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix CwiseReduce( - const VariableMatrix& lhs, const VariableMatrix& rhs, - function_ref binaryOp) { - Assert(lhs.Rows() == rhs.Rows()); - Assert(lhs.Rows() == rhs.Rows()); - - VariableMatrix result{lhs.Rows(), lhs.Cols()}; - - for (int row = 0; row < lhs.Rows(); ++row) { - for (int col = 0; col < lhs.Cols(); ++col) { - result(row, col) = binaryOp(lhs(row, col), rhs(row, col)); - } - } - - return result; -} - -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * @param list The nested list of blocks. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix Block( - std::initializer_list> list) { - // Get row and column counts for destination matrix - int rows = 0; - int cols = -1; - for (const auto& row : list) { - if (row.size() > 0) { - rows += row.begin()->Rows(); - } - - // Get number of columns in this row - int latestCols = 0; - for (const auto& elem : row) { - // Assert the first and latest row have the same height - Assert(row.begin()->Rows() == elem.Rows()); - - latestCols += elem.Cols(); - } - - // If this is the first row, record the column count. Otherwise, assert the - // first and latest column counts are the same. - if (cols == -1) { - cols = latestCols; - } else { - Assert(cols == latestCols); - } - } - - VariableMatrix result{rows, cols}; - - int rowOffset = 0; - for (const auto& row : list) { - int colOffset = 0; - for (const auto& elem : row) { - result.Block(rowOffset, colOffset, elem.Rows(), elem.Cols()) = elem; - colOffset += elem.Cols(); - } - rowOffset += row.begin()->Rows(); - } - - return result; -} - -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * This overload is for Python bindings only. - * - * @param list The nested list of blocks. - */ -SLEIPNIR_DLLEXPORT inline VariableMatrix Block( - const std::vector>& list) { - // Get row and column counts for destination matrix - int rows = 0; - int cols = -1; - for (const auto& row : list) { - if (row.size() > 0) { - rows += row.begin()->Rows(); - } - - // Get number of columns in this row - int latestCols = 0; - for (const auto& elem : row) { - // Assert the first and latest row have the same height - Assert(row.begin()->Rows() == elem.Rows()); - - latestCols += elem.Cols(); - } - - // If this is the first row, record the column count. Otherwise, assert the - // first and latest column counts are the same. - if (cols == -1) { - cols = latestCols; - } else { - Assert(cols == latestCols); - } - } - - VariableMatrix result{rows, cols}; - - int rowOffset = 0; - for (const auto& row : list) { - int colOffset = 0; - for (const auto& elem : row) { - result.Block(rowOffset, colOffset, elem.Rows(), elem.Cols()) = elem; - colOffset += elem.Cols(); - } - rowOffset += row.begin()->Rows(); - } - - return result; -} - -/** - * Solves the VariableMatrix equation AX = B for X. - * - * @param A The left-hand side. - * @param B The right-hand side. - * @return The solution X. - */ -SLEIPNIR_DLLEXPORT VariableMatrix Solve(const VariableMatrix& A, - const VariableMatrix& B); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp new file mode 100644 index 0000000000..4576e19c96 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp @@ -0,0 +1,178 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/autodiff/expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" + +namespace slp::detail { + +/** + * This class is an adaptor type that performs value updates of an expression's + * adjoint graph. + */ +class AdjointExpressionGraph { + public: + /** + * Generates the adjoint graph for the given expression. + * + * @param root The root node of the expression. + */ + explicit AdjointExpressionGraph(const Variable& root) + : m_top_list{topological_sort(root.expr)} { + for (const auto& node : m_top_list) { + m_col_list.emplace_back(node->col); + } + } + + /** + * Update the values of all nodes in this adjoint graph based on the values of + * their dependent nodes. + */ + void update_values() { detail::update_values(m_top_list); } + + /** + * Returns the variable's gradient tree. + * + * This function lazily allocates variables, so elements of the returned + * VariableMatrix will be empty if the corresponding element of wrt had no + * adjoint. Ensure Variable::expr isn't nullptr before calling member + * functions. + * + * @param wrt Variables with respect to which to compute the gradient. + * @return The variable's gradient tree. + */ + VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const { + // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation + // for background on reverse accumulation automatic differentiation. + + if (m_top_list.empty()) { + return VariableMatrix{VariableMatrix::empty, wrt.rows(), 1}; + } + + // Set root node's adjoint to 1 since df/df is 1 + m_top_list[0]->adjoint_expr = make_expression_ptr(1.0); + + // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y + // multiplied by dy/dx. If there are multiple "paths" from the root node to + // variable; the variable's adjoint is the sum of each path's adjoint + // contribution. + for (auto& node : m_top_list) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr); + if (rhs != nullptr) { + rhs->adjoint_expr += node->grad_expr_r(lhs, rhs, node->adjoint_expr); + } + } + } + + // Move gradient tree to return value + VariableMatrix grad{VariableMatrix::empty, wrt.rows(), 1}; + for (int row = 0; row < grad.rows(); ++row) { + grad[row] = Variable{std::move(wrt[row].expr->adjoint_expr)}; + } + + // Unlink adjoints to avoid circular references between them and their + // parent expressions. This ensures all expressions are returned to the free + // list. + for (auto& node : m_top_list) { + node->adjoint_expr = nullptr; + } + + return grad; + } + + /** + * Updates the adjoints in the expression graph (computes the gradient) then + * appends the adjoints of wrt to the sparse matrix triplets. + * + * @param triplets The sparse matrix triplets. + * @param row The row of wrt. + * @param wrt Vector of variables with respect to which to compute the + * Jacobian. + */ + void append_adjoint_triplets( + gch::small_vector>& triplets, int row, + const VariableMatrix& wrt) const { + // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation + // for background on reverse accumulation automatic differentiation. + + // If wrt has fewer nodes than graph, zero wrt's adjoints + if (static_cast(wrt.rows()) < m_top_list.size()) { + for (const auto& elem : wrt) { + elem.expr->adjoint = 0.0; + } + } + + if (m_top_list.empty()) { + return; + } + + // Set root node's adjoint to 1 since df/df is 1 + m_top_list[0]->adjoint = 1.0; + + // Zero the rest of the adjoints + for (auto& node : m_top_list | std::views::drop(1)) { + node->adjoint = 0.0; + } + + // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y + // multiplied by dy/dx. If there are multiple "paths" from the root node to + // variable; the variable's adjoint is the sum of each path's adjoint + // contribution. + for (const auto& node : m_top_list) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + if (rhs != nullptr) { + lhs->adjoint += node->grad_l(lhs->val, rhs->val, node->adjoint); + rhs->adjoint += node->grad_r(lhs->val, rhs->val, node->adjoint); + } else { + lhs->adjoint += node->grad_l(lhs->val, 0.0, node->adjoint); + } + } + } + + // If wrt has fewer nodes than graph, iterate over wrt + if (static_cast(wrt.rows()) < m_top_list.size()) { + for (int col = 0; col < wrt.rows(); ++col) { + const auto& node = wrt[col].expr; + + // Append adjoints of wrt to sparse matrix triplets + if (node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); + } + } + } else { + for (size_t i = 0; i < m_top_list.size(); ++i) { + const auto& col = m_col_list[i]; + const auto& node = m_top_list[i]; + + // Append adjoints of wrt to sparse matrix triplets + if (col != -1 && node->adjoint != 0.0) { + triplets.emplace_back(row, col, node->adjoint); + } + } + } + } + + private: + // Topological sort of graph from parent to child + gch::small_vector m_top_list; + + // List that maps nodes to their respective column + gch::small_vector m_col_list; +}; + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp new file mode 100644 index 0000000000..1c5f84d22a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp @@ -0,0 +1,1758 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/util/intrusive_shared_ptr.hpp" +#include "sleipnir/util/pool.hpp" + +namespace slp::detail { + +// The global pool allocator uses a thread-local static pool resource, which +// isn't guaranteed to be initialized properly across DLL boundaries on Windows +#ifdef _WIN32 +inline constexpr bool USE_POOL_ALLOCATOR = false; +#else +inline constexpr bool USE_POOL_ALLOCATOR = true; +#endif + +struct Expression; + +inline constexpr void inc_ref_count(Expression* expr); +inline void dec_ref_count(Expression* expr); + +/** + * Typedef for intrusive shared pointer to Expression. + */ +using ExpressionPtr = IntrusiveSharedPtr; + +/** + * Creates an intrusive shared pointer to an expression from the global pool + * allocator. + * + * @tparam T The derived expression type. + * @param args Constructor arguments for Expression. + */ +template +static ExpressionPtr make_expression_ptr(Args&&... args) { + if constexpr (USE_POOL_ALLOCATOR) { + return allocate_intrusive_shared(global_pool_allocator(), + std::forward(args)...); + } else { + return make_intrusive_shared(std::forward(args)...); + } +} + +template +struct BinaryMinusExpression; + +template +struct BinaryPlusExpression; + +struct ConstExpression; + +template +struct DivExpression; + +template +struct MultExpression; + +template +struct UnaryMinusExpression; + +/** + * An autodiff expression node. + */ +struct Expression { + /// The value of the expression node. + double val = 0.0; + + /// The adjoint of the expression node used during autodiff. + double adjoint = 0.0; + + /// Counts incoming edges for this node. + uint32_t incoming_edges = 0; + + /// This expression's column in a Jacobian, or -1 otherwise. + int32_t col = -1; + + /// The adjoint of the expression node used during gradient expression tree + /// generation. + ExpressionPtr adjoint_expr; + + /// Reference count for intrusive shared pointer. + uint32_t ref_count = 0; + + /// Expression arguments. + std::array args{nullptr, nullptr}; + + /** + * Constructs a constant expression with a value of zero. + */ + constexpr Expression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr Expression(double value) : val{value} {} + + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr Expression(ExpressionPtr lhs) + : args{std::move(lhs), nullptr} {} + + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr Expression(ExpressionPtr lhs, ExpressionPtr rhs) + : args{std::move(lhs), std::move(rhs)} {} + + virtual ~Expression() = default; + + /** + * Returns true if the expression is the given constant. + * + * @param constant The constant. + * + * @return True if the expression is the given constant. + */ + constexpr bool is_constant(double constant) const { + return type() == ExpressionType::CONSTANT && val == constant; + } + + /** + * Expression-Expression multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator*(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } else if (rhs->is_constant(0.0)) { + // Return zero + return rhs; + } else if (lhs->is_constant(1.0)) { + return rhs; + } else if (rhs->is_constant(1.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val * rhs->val); + } + + // Evaluate expression type + if (lhs->type() == CONSTANT) { + if (rhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (rhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else if (rhs->type() == CONSTANT) { + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else if (lhs->type() == LINEAR && rhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression division operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator/(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } else if (rhs->is_constant(1.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val / rhs->val); + } + + // Evaluate expression type + if (rhs->type() == CONSTANT) { + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator+(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs == nullptr || lhs->is_constant(0.0)) { + return rhs; + } else if (rhs == nullptr || rhs->is_constant(0.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val + rhs->val); + } + + auto type = std::max(lhs->type(), rhs->type()); + if (type == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (type == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Expression-Expression compound addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator+=(ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + return lhs = lhs + rhs; + } + + /** + * Expression-Expression subtraction operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend ExpressionPtr operator-(const ExpressionPtr& lhs, + const ExpressionPtr& rhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + if (rhs->is_constant(0.0)) { + // Return zero + return rhs; + } else { + return -rhs; + } + } else if (rhs->is_constant(0.0)) { + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT && rhs->type() == CONSTANT) { + return make_expression_ptr(lhs->val - rhs->val); + } + + auto type = std::max(lhs->type(), rhs->type()); + if (type == LINEAR) { + return make_expression_ptr>(lhs, rhs); + } else if (type == QUADRATIC) { + return make_expression_ptr>(lhs, rhs); + } else { + return make_expression_ptr>(lhs, rhs); + } + } + + /** + * Unary minus operator. + * + * @param lhs Operand of unary minus. + */ + friend ExpressionPtr operator-(const ExpressionPtr& lhs) { + using enum ExpressionType; + + // Prune expression + if (lhs->is_constant(0.0)) { + // Return zero + return lhs; + } + + // Evaluate constant + if (lhs->type() == CONSTANT) { + return make_expression_ptr(-lhs->val); + } + + if (lhs->type() == LINEAR) { + return make_expression_ptr>(lhs); + } else if (lhs->type() == QUADRATIC) { + return make_expression_ptr>(lhs); + } else { + return make_expression_ptr>(lhs); + } + } + + /** + * Unary plus operator. + * + * @param lhs Operand of unary plus. + */ + friend ExpressionPtr operator+(const ExpressionPtr& lhs) { return lhs; } + + /** + * Either nullary operator with no arguments, unary operator with one + * argument, or binary operator with two arguments. This operator is used to + * update the node's value. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @return The node's value. + */ + virtual double value([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs) const = 0; + + /** + * Returns the type of this expression (constant, linear, quadratic, or + * nonlinear). + * + * @return The type of this expression. + */ + virtual ExpressionType type() const = 0; + + /** + * Returns double adjoint of the left child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The double adjoint of the left child expression. + */ + virtual double grad_l([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs, + [[maybe_unused]] double parent_adjoint) const { + return 0.0; + } + + /** + * Returns double adjoint of the right child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The double adjoint of the right child expression. + */ + virtual double grad_r([[maybe_unused]] double lhs, + [[maybe_unused]] double rhs, + [[maybe_unused]] double parent_adjoint) const { + return 0.0; + } + + /** + * Returns Expression adjoint of the left child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The Expression adjoint of the left child expression. + */ + virtual ExpressionPtr grad_expr_l( + [[maybe_unused]] const ExpressionPtr& lhs, + [[maybe_unused]] const ExpressionPtr& rhs, + [[maybe_unused]] const ExpressionPtr& parent_adjoint) const { + return make_expression_ptr(); + } + + /** + * Returns Expression adjoint of the right child expression. + * + * @param lhs Left argument to binary operator. + * @param rhs Right argument to binary operator. + * @param parent_adjoint Adjoint of parent expression. + * @return The Expression adjoint of the right child expression. + */ + virtual ExpressionPtr grad_expr_r( + [[maybe_unused]] const ExpressionPtr& lhs, + [[maybe_unused]] const ExpressionPtr& rhs, + [[maybe_unused]] const ExpressionPtr& parent_adjoint) const { + return make_expression_ptr(); + } +}; + +/** + * Derived expression type for binary minus operator. + * + * @tparam T Expression type. + */ +template +struct BinaryMinusExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr BinaryMinusExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs - rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + double grad_r(double, double, double parent_adjoint) const override { + return -parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint; + } +}; + +/** + * Derived expression type for binary plus operator. + * + * @tparam T Expression type. + */ +template +struct BinaryPlusExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr BinaryPlusExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs + rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + double grad_r(double, double, double parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint; + } +}; + +/** + * Derived expression type for constant. + */ +struct ConstExpression final : Expression { + /** + * Constructs a constant expression with a value of zero. + */ + constexpr ConstExpression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr ConstExpression(double value) : Expression{value} {} + + double value(double, double) const override { return val; } + + ExpressionType type() const override { return ExpressionType::CONSTANT; } +}; + +/** + * Derived expression type for decision variable. + */ +struct DecisionVariableExpression final : Expression { + /** + * Constructs a decision variable expression with a value of zero. + */ + constexpr DecisionVariableExpression() = default; + + /** + * Constructs a nullary expression (an operator with no arguments). + * + * @param value The expression value. + */ + explicit constexpr DecisionVariableExpression(double value) + : Expression{value} {} + + double value(double, double) const override { return val; } + + ExpressionType type() const override { return ExpressionType::LINEAR; } +}; + +/** + * Derived expression type for binary division operator. + * + * @tparam T Expression type. + */ +template +struct DivExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr DivExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs / rhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double rhs, double parent_adjoint) const override { + return parent_adjoint / rhs; + }; + + double grad_r(double lhs, double rhs, double parent_adjoint) const override { + return parent_adjoint * -lhs / (rhs * rhs); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / rhs; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -lhs / (rhs * rhs); + } +}; + +/** + * Derived expression type for binary multiplication operator. + * + * @tparam T Expression type. + */ +template +struct MultExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr MultExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double lhs, double rhs) const override { return lhs * rhs; } + + ExpressionType type() const override { return T; } + + double grad_l([[maybe_unused]] double lhs, double rhs, + double parent_adjoint) const override { + return parent_adjoint * rhs; + } + + double grad_r(double lhs, [[maybe_unused]] double rhs, + double parent_adjoint) const override { + return parent_adjoint * lhs; + } + + ExpressionPtr grad_expr_l( + [[maybe_unused]] const ExpressionPtr& lhs, const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * rhs; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& lhs, [[maybe_unused]] const ExpressionPtr& rhs, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * lhs; + } +}; + +/** + * Derived expression type for unary minus operator. + * + * @tparam T Expression type. + */ +template +struct UnaryMinusExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr UnaryMinusExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double lhs, double) const override { return -lhs; } + + ExpressionType type() const override { return T; } + + double grad_l(double, double, double parent_adjoint) const override { + return -parent_adjoint; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint; + } +}; + +inline ExpressionPtr exp(const ExpressionPtr& x); +inline ExpressionPtr sin(const ExpressionPtr& x); +inline ExpressionPtr sinh(const ExpressionPtr& x); +inline ExpressionPtr sqrt(const ExpressionPtr& x); + +/** + * Refcount increment for intrusive shared pointer. + * + * @param expr The shared pointer's managed object. + */ +inline constexpr void inc_ref_count(Expression* expr) { + ++expr->ref_count; +} + +/** + * Refcount decrement for intrusive shared pointer. + * + * @param expr The shared pointer's managed object. + */ +inline void dec_ref_count(Expression* expr) { + // If a deeply nested tree is being deallocated all at once, calling the + // Expression destructor when expr's refcount reaches zero can cause a stack + // overflow. Instead, we iterate over its children to decrement their + // refcounts and deallocate them. + gch::small_vector stack; + stack.emplace_back(expr); + + while (!stack.empty()) { + auto elem = stack.back(); + stack.pop_back(); + + // Decrement the current node's refcount. If it reaches zero, deallocate the + // node and enqueue its children so their refcounts are decremented too. + if (--elem->ref_count == 0) { + if (elem->adjoint_expr != nullptr) { + stack.emplace_back(elem->adjoint_expr.get()); + } + for (auto& arg : elem->args) { + if (arg != nullptr) { + stack.emplace_back(arg.get()); + } + } + + // Not calling the destructor here is safe because it only decrements + // refcounts, which was already done above. + if constexpr (USE_POOL_ALLOCATOR) { + auto alloc = global_pool_allocator(); + std::allocator_traits::deallocate(alloc, elem, + sizeof(Expression)); + } + } + } +} + +/** + * Derived expression type for std::abs(). + */ +struct AbsExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AbsExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::abs(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + if (x < 0.0) { + return -parent_adjoint; + } else if (x > 0.0) { + return parent_adjoint; + } else { + return 0.0; + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + if (x->val < 0.0) { + return -parent_adjoint; + } else if (x->val > 0.0) { + return parent_adjoint; + } else { + // Return zero + return make_expression_ptr(); + } + } +}; + +/** + * std::abs() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr abs(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::abs(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::acos(). + */ +struct AcosExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AcosExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::acos(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return -parent_adjoint / std::sqrt(1.0 - x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return -parent_adjoint / + slp::detail::sqrt(make_expression_ptr(1.0) - x * x); + } +}; + +/** + * std::acos() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr acos(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(std::numbers::pi / 2.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::acos(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::asin(). + */ +struct AsinExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AsinExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::asin(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / std::sqrt(1.0 - x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + slp::detail::sqrt(make_expression_ptr(1.0) - x * x); + } +}; + +/** + * std::asin() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr asin(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::asin(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::atan(). + */ +struct AtanExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr AtanExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::atan(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (1.0 + x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (make_expression_ptr(1.0) + x * x); + } +}; + +/** + * std::atan() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr atan(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::atan(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::atan2(). + */ +struct Atan2Expression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double y, double x) const override { return std::atan2(y, x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double y, double x, double parent_adjoint) const override { + return parent_adjoint * x / (y * y + x * x); + } + + double grad_r(double y, double x, double parent_adjoint) const override { + return parent_adjoint * -y / (y * y + x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& y, const ExpressionPtr& x, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * x / (y * y + x * x); + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& y, const ExpressionPtr& x, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -y / (y * y + x * x); + } +}; + +/** + * std::atan2() for Expressions. + * + * @param y The y argument. + * @param x The x argument. + */ +inline ExpressionPtr atan2(const ExpressionPtr& y, const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (y->is_constant(0.0)) { + // Return zero + return y; + } else if (x->is_constant(0.0)) { + return make_expression_ptr(std::numbers::pi / 2.0); + } + + // Evaluate constant + if (y->type() == CONSTANT && x->type() == CONSTANT) { + return make_expression_ptr(std::atan2(y->val, x->val)); + } + + return make_expression_ptr(y, x); +} + +/** + * Derived expression type for std::cos(). + */ +struct CosExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr CosExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::cos(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return -parent_adjoint * std::sin(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * -slp::detail::sin(x); + } +}; + +/** + * std::cos() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr cos(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::cos(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::cosh(). + */ +struct CoshExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr CoshExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::cosh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::sinh(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::sinh(x); + } +}; + +/** + * std::cosh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr cosh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::cosh(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::erf(). + */ +struct ErfExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr ErfExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::erf(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * + make_expression_ptr(2.0 * + std::numbers::inv_sqrtpi) * + slp::detail::exp(-x * x); + } +}; + +/** + * std::erf() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr erf(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::erf(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::exp(). + */ +struct ExpExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr ExpExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::exp(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::exp(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::exp(x); + } +}; + +/** + * std::exp() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr exp(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return make_expression_ptr(1.0); + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::exp(x->val)); + } + + return make_expression_ptr(x); +} + +inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y); + +/** + * Derived expression type for std::hypot(). + */ +struct HypotExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double x, double y) const override { return std::hypot(x, y); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double y, double parent_adjoint) const override { + return parent_adjoint * x / std::hypot(x, y); + } + + double grad_r(double x, double y, double parent_adjoint) const override { + return parent_adjoint * y / std::hypot(x, y); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr& y, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * x / slp::detail::hypot(x, y); + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& x, const ExpressionPtr& y, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * y / slp::detail::hypot(x, y); + } +}; + +/** + * std::hypot() for Expressions. + * + * @param x The x argument. + * @param y The y argument. + */ +inline ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + return y; + } else if (y->is_constant(0.0)) { + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT && y->type() == CONSTANT) { + return make_expression_ptr(std::hypot(x->val, y->val)); + } + + return make_expression_ptr(x, y); +} + +/** + * Derived expression type for std::log(). + */ +struct LogExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr LogExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::log(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / x; + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / x; + } +}; + +/** + * std::log() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr log(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::log(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::log10(). + */ +struct Log10Expression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr Log10Expression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::log10(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::numbers::ln10 * x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + (make_expression_ptr(std::numbers::ln10) * x); + } +}; + +/** + * std::log10() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr log10(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::log10(x->val)); + } + + return make_expression_ptr(x); +} + +inline ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power); + +/** + * Derived expression type for std::pow(). + * + * @tparam Expression type. + */ +template +struct PowExpression final : Expression { + /** + * Constructs a binary expression (an operator with two arguments). + * + * @param lhs Binary operator's left operand. + * @param rhs Binary operator's right operand. + */ + constexpr PowExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + double value(double base, double power) const override { + return std::pow(base, power); + } + + ExpressionType type() const override { return T; } + + double grad_l(double base, double power, + double parent_adjoint) const override { + return parent_adjoint * std::pow(base, power - 1) * power; + } + + double grad_r(double base, double power, + double parent_adjoint) const override { + // Since x * std::log(x) -> 0 as x -> 0 + if (base == 0.0) { + return 0.0; + } else { + return parent_adjoint * std::pow(base, power - 1) * base * std::log(base); + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& base, const ExpressionPtr& power, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * + slp::detail::pow(base, + power - make_expression_ptr(1.0)) * + power; + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& base, const ExpressionPtr& power, + const ExpressionPtr& parent_adjoint) const override { + // Since x * std::log(x) -> 0 as x -> 0 + if (base->val == 0.0) { + // Return zero + return base; + } else { + return parent_adjoint * + slp::detail::pow( + base, power - make_expression_ptr(1.0)) * + base * slp::detail::log(base); + } + } +}; + +/** + * std::pow() for Expressions. + * + * @param base The base. + * @param power The power. + */ +inline ExpressionPtr pow(const ExpressionPtr& base, + const ExpressionPtr& power) { + using enum ExpressionType; + + // Prune expression + if (base->is_constant(0.0)) { + // Return zero + return base; + } else if (base->is_constant(1.0)) { + // Return one + return base; + } + if (power->is_constant(0.0)) { + return make_expression_ptr(1.0); + } else if (power->is_constant(1.0)) { + return base; + } + + // Evaluate constant + if (base->type() == CONSTANT && power->type() == CONSTANT) { + return make_expression_ptr( + std::pow(base->val, power->val)); + } + + if (power->is_constant(2.0)) { + if (base->type() == LINEAR) { + return make_expression_ptr>(base, base); + } else { + return make_expression_ptr>(base, base); + } + } + + return make_expression_ptr>(base, power); +} + +/** + * Derived expression type for sign(). + */ +struct SignExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SignExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { + if (x < 0.0) { + return -1.0; + } else if (x == 0.0) { + return 0.0; + } else { + return 1.0; + } + } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double, double, double) const override { return 0.0; } + + ExpressionPtr grad_expr_l(const ExpressionPtr&, const ExpressionPtr&, + const ExpressionPtr&) const override { + // Return zero + return make_expression_ptr(); + } +}; + +/** + * sign() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sign(const ExpressionPtr& x) { + using enum ExpressionType; + + // Evaluate constant + if (x->type() == CONSTANT) { + if (x->val < 0.0) { + return make_expression_ptr(-1.0); + } else if (x->val == 0.0) { + // Return zero + return x; + } else { + return make_expression_ptr(1.0); + } + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sin(). + */ +struct SinExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SinExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sin(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::cos(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::cos(x); + } +}; + +/** + * std::sin() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sin(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::sin(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sinh(). + */ +struct SinhExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SinhExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sinh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint * std::cosh(x); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint * slp::detail::cosh(x); + } +}; + +/** + * std::sinh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sinh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::sinh(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::sqrt(). + */ +struct SqrtExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr SqrtExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::sqrt(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (2.0 * std::sqrt(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / + (make_expression_ptr(2.0) * slp::detail::sqrt(x)); + } +}; + +/** + * std::sqrt() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr sqrt(const ExpressionPtr& x) { + using enum ExpressionType; + + // Evaluate constant + if (x->type() == CONSTANT) { + if (x->val == 0.0) { + // Return zero + return x; + } else if (x->val == 1.0) { + return x; + } else { + return make_expression_ptr(std::sqrt(x->val)); + } + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::tan(). + */ +struct TanExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr TanExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::tan(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::cos(x) * std::cos(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (slp::detail::cos(x) * slp::detail::cos(x)); + } +}; + +/** + * std::tan() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr tan(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::tan(x->val)); + } + + return make_expression_ptr(x); +} + +/** + * Derived expression type for std::tanh(). + */ +struct TanhExpression final : Expression { + /** + * Constructs an unary expression (an operator with one argument). + * + * @param lhs Unary operator's operand. + */ + explicit constexpr TanhExpression(ExpressionPtr lhs) + : Expression{std::move(lhs)} {} + + double value(double x, double) const override { return std::tanh(x); } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + double grad_l(double x, double, double parent_adjoint) const override { + return parent_adjoint / (std::cosh(x) * std::cosh(x)); + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& x, const ExpressionPtr&, + const ExpressionPtr& parent_adjoint) const override { + return parent_adjoint / (slp::detail::cosh(x) * slp::detail::cosh(x)); + } +}; + +/** + * std::tanh() for Expressions. + * + * @param x The argument. + */ +inline ExpressionPtr tanh(const ExpressionPtr& x) { + using enum ExpressionType; + + // Prune expression + if (x->is_constant(0.0)) { + // Return zero + return x; + } + + // Evaluate constant + if (x->type() == CONSTANT) { + return make_expression_ptr(std::tanh(x->val)); + } + + return make_expression_ptr(x); +} + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp new file mode 100644 index 0000000000..a1d9c33539 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -0,0 +1,94 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/autodiff/expression.hpp" + +namespace slp::detail { + +/** + * Generate a topological sort of an expression graph from parent to child. + * + * https://en.wikipedia.org/wiki/Topological_sorting + * + * @param root The root node of the expression. + */ +inline gch::small_vector topological_sort( + const ExpressionPtr& root) { + gch::small_vector list; + + // If the root type is a constant, Update() is a no-op, so there's no work + // to do + if (root == nullptr || root->type() == ExpressionType::CONSTANT) { + return list; + } + + // Stack of nodes to explore + gch::small_vector stack; + + // Enumerate incoming edges for each node via depth-first search + stack.emplace_back(root.get()); + while (!stack.empty()) { + auto node = stack.back(); + stack.pop_back(); + + for (auto& arg : node->args) { + // If the node hasn't been explored yet, add it to the stack + if (arg != nullptr && ++arg->incoming_edges == 1) { + stack.push_back(arg.get()); + } + } + } + + // Generate topological sort of graph from parent to child. + // + // A node is only added to the stack after all its incoming edges have been + // traversed. Expression::incoming_edges is a decrementing counter for + // tracking this. + // + // https://en.wikipedia.org/wiki/Topological_sorting + stack.emplace_back(root.get()); + while (!stack.empty()) { + auto node = stack.back(); + stack.pop_back(); + + list.emplace_back(node); + + for (auto& arg : node->args) { + // If we traversed all this node's incoming edges, add it to the stack + if (arg != nullptr && --arg->incoming_edges == 0) { + stack.push_back(arg.get()); + } + } + } + + return list; +} + +/** + * Update the values of all nodes in this graph based on the values of + * their dependent nodes. + * + * @param list Topological sort of graph from parent to child. + */ +inline void update_values(const gch::small_vector& list) { + // Traverse graph from child to parent and update values + for (auto& node : list | std::views::reverse) { + auto& lhs = node->args[0]; + auto& rhs = node->args[1]; + + if (lhs != nullptr) { + if (rhs != nullptr) { + node->val = node->value(lhs->val, rhs->val); + } else { + node->val = node->value(lhs->val, 0.0); + } + } + } +} + +} // namespace slp::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp new file mode 100644 index 0000000000..9e323d87f4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp @@ -0,0 +1,56 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Expression type. + * + * Used for autodiff caching. + */ +enum class ExpressionType : uint8_t { + /// There is no expression. + NONE, + /// The expression is a constant. + CONSTANT, + /// The expression is composed of linear and lower-order operators. + LINEAR, + /// The expression is composed of quadratic and lower-order operators. + QUADRATIC, + /// The expression is composed of nonlinear and lower-order operators. + NONLINEAR +}; + +/** + * Returns user-readable message corresponding to the expression type. + * + * @param type Expression type. + */ +SLEIPNIR_DLLEXPORT constexpr std::string_view to_message( + const ExpressionType& type) { + using enum ExpressionType; + + switch (type) { + case NONE: + return "none"; + case CONSTANT: + return "constant"; + case LINEAR: + return "linear"; + case QUADRATIC: + return "quadratic"; + case NONLINEAR: + return "nonlinear"; + default: + return "unknown"; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp similarity index 61% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp index cf6a4171e7..728259d3f5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp @@ -6,13 +6,13 @@ #include -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/autodiff/Profiler.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/jacobian.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * This class calculates the gradient of a a variable with respect to a vector @@ -30,7 +30,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param wrt Variable with respect to which to compute the gradient. */ Gradient(Variable variable, Variable wrt) noexcept - : Gradient{std::move(variable), VariableMatrix{wrt}} {} + : m_jacobian{std::move(variable), std::move(wrt)} {} /** * Constructs a Gradient object. @@ -39,35 +39,34 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param wrt Vector of variables with respect to which to compute the * gradient. */ - Gradient(Variable variable, const VariableMatrix& wrt) noexcept - : m_jacobian{variable, wrt} {} + Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept + : m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {} /** * Returns the gradient as a VariableMatrix. * * This is useful when constructing optimization problems with derivatives in * them. + * + * @return The gradient as a VariableMatrix. */ - VariableMatrix Get() const { return m_jacobian.Get().T(); } + VariableMatrix get() const { return m_jacobian.get().T(); } /** * Evaluates the gradient at wrt's value. + * + * @return The gradient at wrt's value. */ - const Eigen::SparseVector& Value() { - m_g = m_jacobian.Value(); + const Eigen::SparseVector& value() { + m_g = m_jacobian.value(); return m_g; } - /** - * Returns the profiler. - */ - Profiler& GetProfiler() { return m_jacobian.GetProfiler(); } - private: Eigen::SparseVector m_g; Jacobian m_jacobian; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp new file mode 100644 index 0000000000..8b048ab3ba --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -0,0 +1,169 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +#include "sleipnir/autodiff/adjoint_expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class calculates the Hessian of a variable with respect to a vector of + * variables. + * + * The gradient tree is cached so subsequent Hessian calculations are faster, + * and the Hessian is only recomputed if the variable expression is nonlinear. + * + * @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper). + */ +template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) +class SLEIPNIR_DLLEXPORT Hessian { + public: + /** + * Constructs a Hessian object. + * + * @param variable Variable of which to compute the Hessian. + * @param wrt Variable with respect to which to compute the Hessian. + */ + Hessian(Variable variable, Variable wrt) noexcept + : Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {} + + /** + * Constructs a Hessian object. + * + * @param variable Variable of which to compute the Hessian. + * @param wrt Vector of variables with respect to which to compute the + * Hessian. + */ + Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept + : m_variables{detail::AdjointExpressionGraph{variable} + .generate_gradient_tree(wrt)}, + m_wrt{wrt} { + // Initialize column each expression's adjoint occupies in the Jacobian + for (size_t col = 0; col < m_wrt.size(); ++col) { + m_wrt[col].expr->col = col; + } + + for (auto& variable : m_variables) { + m_graphs.emplace_back(variable); + } + + // Reset col to -1 + for (auto& node : m_wrt) { + node.expr->col = -1; + } + + for (int row = 0; row < m_variables.rows(); ++row) { + if (m_variables[row].expr == nullptr) { + continue; + } + + if (m_variables[row].type() == ExpressionType::LINEAR) { + // If the row is linear, compute its gradient once here and cache its + // triplets. Constant rows are ignored because their gradients have no + // nonzero triplets. + m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt); + } else if (m_variables[row].type() > ExpressionType::LINEAR) { + // If the row is quadratic or nonlinear, add it to the list of nonlinear + // rows to be recomputed in Value(). + m_nonlinear_rows.emplace_back(row); + } + } + + if (m_nonlinear_rows.empty()) { + m_H.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); + } + } + } + + /** + * Returns the Hessian as a VariableMatrix. + * + * This is useful when constructing optimization problems with derivatives in + * them. + * + * @return The Hessian as a VariableMatrix. + */ + VariableMatrix get() const { + VariableMatrix result{VariableMatrix::empty, m_variables.rows(), + m_wrt.rows()}; + + for (int row = 0; row < m_variables.rows(); ++row) { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { + result(row, col) = std::move(grad[col]); + } else { + result(row, col) = Variable{0.0}; + } + } + } + + return result; + } + + /** + * Evaluates the Hessian at wrt's value. + * + * @return The Hessian at wrt's value. + */ + const Eigen::SparseMatrix& value() { + if (m_nonlinear_rows.empty()) { + return m_H; + } + + for (auto& graph : m_graphs) { + graph.update_values(); + } + + // Copy the cached triplets so triplets added for the nonlinear rows are + // thrown away at the end of the function + auto triplets = m_cached_triplets; + + // Compute each nonlinear row of the Hessian + for (int row : m_nonlinear_rows) { + m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); + } + + if (!triplets.empty()) { + m_H.setFromTriplets(triplets.begin(), triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); + } + } else { + // setFromTriplets() is a no-op on empty triplets, so explicitly zero out + // the storage + m_H.setZero(); + } + + return m_H; + } + + private: + VariableMatrix m_variables; + VariableMatrix m_wrt; + + gch::small_vector m_graphs; + + Eigen::SparseMatrix m_H{m_variables.rows(), m_wrt.rows()}; + + // Cached triplets for gradients of linear rows + gch::small_vector> m_cached_triplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() + gch::small_vector m_nonlinear_rows; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp new file mode 100644 index 0000000000..7e7e1340d0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -0,0 +1,158 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +#include "sleipnir/autodiff/adjoint_expression_graph.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class calculates the Jacobian of a vector of variables with respect to a + * vector of variables. + * + * The Jacobian is only recomputed if the variable expression is quadratic or + * higher order. + */ +class SLEIPNIR_DLLEXPORT Jacobian { + public: + /** + * Constructs a Jacobian object. + * + * @param variable Variable of which to compute the Jacobian. + * @param wrt Variable with respect to which to compute the Jacobian. + */ + Jacobian(Variable variable, Variable wrt) noexcept + : Jacobian{VariableMatrix{std::move(variable)}, + VariableMatrix{std::move(wrt)}} {} + + /** + * Constructs a Jacobian object. + * + * @param variables Vector of variables of which to compute the Jacobian. + * @param wrt Vector of variables with respect to which to compute the + * Jacobian. + */ + Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept + : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { + // Initialize column each expression's adjoint occupies in the Jacobian + for (size_t col = 0; col < m_wrt.size(); ++col) { + m_wrt[col].expr->col = col; + } + + for (auto& variable : m_variables) { + m_graphs.emplace_back(variable); + } + + // Reset col to -1 + for (auto& node : m_wrt) { + node.expr->col = -1; + } + + for (int row = 0; row < m_variables.rows(); ++row) { + if (m_variables[row].expr == nullptr) { + continue; + } + + if (m_variables[row].type() == ExpressionType::LINEAR) { + // If the row is linear, compute its gradient once here and cache its + // triplets. Constant rows are ignored because their gradients have no + // nonzero triplets. + m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt); + } else if (m_variables[row].type() > ExpressionType::LINEAR) { + // If the row is quadratic or nonlinear, add it to the list of nonlinear + // rows to be recomputed in Value(). + m_nonlinear_rows.emplace_back(row); + } + } + + if (m_nonlinear_rows.empty()) { + m_J.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end()); + } + } + + /** + * Returns the Jacobian as a VariableMatrix. + * + * This is useful when constructing optimization problems with derivatives in + * them. + * + * @return The Jacobian as a VariableMatrix. + */ + VariableMatrix get() const { + VariableMatrix result{VariableMatrix::empty, m_variables.rows(), + m_wrt.rows()}; + + for (int row = 0; row < m_variables.rows(); ++row) { + auto grad = m_graphs[row].generate_gradient_tree(m_wrt); + for (int col = 0; col < m_wrt.rows(); ++col) { + if (grad[col].expr != nullptr) { + result(row, col) = std::move(grad[col]); + } else { + result(row, col) = Variable{0.0}; + } + } + } + + return result; + } + + /** + * Evaluates the Jacobian at wrt's value. + * + * @return The Jacobian at wrt's value. + */ + const Eigen::SparseMatrix& value() { + if (m_nonlinear_rows.empty()) { + return m_J; + } + + for (auto& graph : m_graphs) { + graph.update_values(); + } + + // Copy the cached triplets so triplets added for the nonlinear rows are + // thrown away at the end of the function + auto triplets = m_cached_triplets; + + // Compute each nonlinear row of the Jacobian + for (int row : m_nonlinear_rows) { + m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); + } + + if (!triplets.empty()) { + m_J.setFromTriplets(triplets.begin(), triplets.end()); + } else { + // setFromTriplets() is a no-op on empty triplets, so explicitly zero out + // the storage + m_J.setZero(); + } + + return m_J; + } + + private: + VariableMatrix m_variables; + VariableMatrix m_wrt; + + gch::small_vector m_graphs; + + Eigen::SparseMatrix m_J{m_variables.rows(), m_wrt.rows()}; + + // Cached triplets for gradients of linear rows + gch::small_vector> m_cached_triplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() + gch::small_vector m_nonlinear_rows; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp similarity index 79% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp index 2f2b18993b..abb11f22f6 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp @@ -6,18 +6,28 @@ #include #include -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { namespace slicing { +/** + * Type tag used to designate an omitted argument of the slice. + */ struct none_t {}; + +/** + * Designates an omitted argument of the slice. + */ static inline constexpr none_t _; } // namespace slicing +/** + * Represents a sequence of elements in an iterable object. + */ class SLEIPNIR_DLLEXPORT Slice { public: /// Start index (inclusive). @@ -36,14 +46,20 @@ class SLEIPNIR_DLLEXPORT Slice { /** * Constructs a slice. - * - * @param stop Slice stop index (exclusive). */ - template - requires std::same_as || - std::convertible_to - constexpr Slice(Stop stop) // NOLINT - : Slice(slicing::_, std::move(stop), 1) {} + constexpr Slice(slicing::none_t) // NOLINT + : Slice(0, std::numeric_limits::max(), 1) {} + + /** + * Constructs a slice. + * + * @param start Slice start index (inclusive). + */ + constexpr Slice(int start) { // NOLINT + this->start = start; + this->stop = (start == -1) ? std::numeric_limits::max() : start + 1; + this->step = 1; + } /** * Constructs a slice. @@ -77,7 +93,7 @@ class SLEIPNIR_DLLEXPORT Slice { if constexpr (std::same_as) { this->step = 1; } else { - Assert(step != 0); + slp_assert(step != 0); this->step = step; } @@ -115,9 +131,9 @@ class SLEIPNIR_DLLEXPORT Slice { * @param length The sequence length. * @return The slice length. */ - constexpr int Adjust(int length) { - Assert(step != 0); - Assert(step >= -std::numeric_limits::max()); + constexpr int adjust(int length) { + slp_assert(step != 0); + slp_assert(step >= -std::numeric_limits::max()); if (start < 0) { start += length; @@ -155,4 +171,4 @@ class SLEIPNIR_DLLEXPORT Slice { } }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp similarity index 67% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index f25c6d1533..03b929c778 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -5,23 +5,32 @@ #include #include #include +#include #include #include #include #include -#include +#include -#include "sleipnir/autodiff/Expression.hpp" -#include "sleipnir/autodiff/ExpressionGraph.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/expression.hpp" +#include "sleipnir/autodiff/expression_graph.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +#include "sleipnir/util/print.hpp" +#endif + +namespace slp { // Forward declarations for friend declarations in Variable +namespace detail { +class AdjointExpressionGraph; +} // namespace detail +template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) class SLEIPNIR_DLLEXPORT Hessian; class SLEIPNIR_DLLEXPORT Jacobian; @@ -36,11 +45,25 @@ class SLEIPNIR_DLLEXPORT Variable { Variable() = default; /** - * Constructs a Variable from a double. + * Constructs an empty Variable. + */ + explicit Variable(std::nullptr_t) : expr{nullptr} {} + + /** + * Constructs a Variable from a floating point type. * * @param value The value of the Variable. */ - Variable(double value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT + Variable(std::floating_point auto value) // NOLINT + : expr{detail::make_expression_ptr(value)} {} + + /** + * Constructs a Variable from an integral type. + * + * @param value The value of the Variable. + */ + Variable(std::integral auto value) // NOLINT + : expr{detail::make_expression_ptr(value)} {} /** * Constructs a Variable pointing to the specified expression. @@ -60,9 +83,10 @@ class SLEIPNIR_DLLEXPORT Variable { * Assignment operator for double. * * @param value The value of the Variable. + * @return This variable. */ Variable& operator=(double value) { - expr = detail::MakeExpressionPtr(value); + expr = detail::make_expression_ptr(value); return *this; } @@ -72,19 +96,22 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param value The value of the Variable. */ - void SetValue(double value) { - if (expr->IsConstant(0.0)) { - expr = detail::MakeExpressionPtr(value); + void set_value(double value) { + if (expr->is_constant(0.0)) { + expr = detail::make_expression_ptr(value); } else { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // We only need to check the first argument since unary and binary // operators both use it - if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) { - sleipnir::println( + if (expr->args[0] != nullptr) { + auto location = std::source_location::current(); + slp::println( stderr, - "WARNING: {}:{}: Modified the value of a dependent variable", - __FILE__, __LINE__); + "WARNING: {}:{}: {}: Modified the value of a dependent variable", + location.file_name(), location.line(), location.function_name()); } - expr->value = value; +#endif + expr->val = value; } } @@ -93,6 +120,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of multiplication. */ friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs, const Variable& rhs) { @@ -103,6 +131,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound multiplication operator. * * @param rhs Operator right-hand side. + * @return Result of multiplication. */ Variable& operator*=(const Variable& rhs) { *this = *this * rhs; @@ -114,6 +143,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of division. */ friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs, const Variable& rhs) { @@ -124,6 +154,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound division operator. * * @param rhs Operator right-hand side. + * @return Result of division. */ Variable& operator/=(const Variable& rhs) { *this = *this / rhs; @@ -135,6 +166,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of addition. */ friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs, const Variable& rhs) { @@ -145,6 +177,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound addition operator. * * @param rhs Operator right-hand side. + * @return Result of addition. */ Variable& operator+=(const Variable& rhs) { *this = *this + rhs; @@ -156,6 +189,7 @@ class SLEIPNIR_DLLEXPORT Variable { * * @param lhs Operator left-hand side. * @param rhs Operator right-hand side. + * @return Result of subtraction. */ friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs, const Variable& rhs) { @@ -166,6 +200,7 @@ class SLEIPNIR_DLLEXPORT Variable { * Variable-Variable compound subtraction operator. * * @param rhs Operator right-hand side. + * @return Result of subtraction. */ Variable& operator-=(const Variable& rhs) { *this = *this - rhs; @@ -192,25 +227,38 @@ class SLEIPNIR_DLLEXPORT Variable { /** * Returns the value of this variable. + * + * @return The value of this variable. */ - double Value() { - // Updates the value of this variable based on the values of its dependent - // variables - detail::ExpressionGraph{expr}.Update(); + double value() { + if (!m_graph_initialized) { + m_graph = detail::topological_sort(expr); + m_graph_initialized = true; + } + detail::update_values(m_graph); - return expr->value; + return expr->val; } /** * Returns the type of this expression (constant, linear, quadratic, or * nonlinear). + * + * @return The type of this expression. */ - ExpressionType Type() const { return expr->type; } + ExpressionType type() const { return expr->type(); } private: - /// The expression node. + /// The expression node detail::ExpressionPtr expr = - detail::MakeExpressionPtr(0.0, ExpressionType::kLinear); + detail::make_expression_ptr(); + + /// Used to update the value of this variable based on the values of its + /// dependent variables + gch::small_vector m_graph; + + /// Used for lazy initialization of m_graph + bool m_graph_initialized = false; friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x); @@ -237,6 +285,9 @@ class SLEIPNIR_DLLEXPORT Variable { friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y, const Variable& z); + friend class detail::AdjointExpressionGraph; + template + requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) friend class SLEIPNIR_DLLEXPORT Hessian; friend class SLEIPNIR_DLLEXPORT Jacobian; }; @@ -425,8 +476,7 @@ SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) { */ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y, const Variable& z) { - return Variable{sleipnir::sqrt(sleipnir::pow(x, 2) + sleipnir::pow(y, 2) + - sleipnir::pow(z, 2))}; + return Variable{slp::sqrt(slp::pow(x, 2) + slp::pow(y, 2) + slp::pow(z, 2))}; } /** @@ -441,85 +491,38 @@ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y, * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { - wpi::SmallVector constraints; + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) +gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { + gch::small_vector constraints; - if constexpr (ScalarLike> && - ScalarLike>) { + if constexpr (ScalarLike && ScalarLike) { constraints.emplace_back(lhs - rhs); - } else if constexpr (ScalarLike> && - MatrixLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = rhs.rows(); - cols = rhs.cols(); - } else { - rows = rhs.Rows(); - cols = rhs.Cols(); - } + } else if constexpr (ScalarLike && MatrixLike) { + constraints.reserve(rhs.rows() * rhs.cols()); - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { + for (int row = 0; row < rhs.rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs - rhs(row, col)); } } - } else if constexpr (MatrixLike> && - ScalarLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = lhs.rows(); - cols = lhs.cols(); - } else { - rows = lhs.Rows(); - cols = lhs.Cols(); - } + } else if constexpr (MatrixLike && ScalarLike) { + constraints.reserve(lhs.rows() * lhs.cols()); - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs(row, col) - rhs); } } - } else if constexpr (MatrixLike> && - MatrixLike>) { - int lhsRows; - int lhsCols; - if constexpr (EigenMatrixLike>) { - lhsRows = lhs.rows(); - lhsCols = lhs.cols(); - } else { - lhsRows = lhs.Rows(); - lhsCols = lhs.Cols(); - } + } else if constexpr (MatrixLike && MatrixLike) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + constraints.reserve(lhs.rows() * lhs.cols()); - [[maybe_unused]] - int rhsRows; - [[maybe_unused]] - int rhsCols; - if constexpr (EigenMatrixLike>) { - rhsRows = rhs.rows(); - rhsCols = rhs.cols(); - } else { - rhsRows = rhs.Rows(); - rhsCols = rhs.Cols(); - } - - Assert(lhsRows == rhsRows && lhsCols == rhsCols); - constraints.reserve(lhsRows * lhsCols); - - for (int row = 0; row < lhsRows; ++row) { - for (int col = 0; col < lhsCols; ++col) { + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero constraints.emplace_back(lhs(row, col) - rhs(row, col)); } @@ -534,16 +537,16 @@ wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { */ struct SLEIPNIR_DLLEXPORT EqualityConstraints { /// A vector of scalar equality constraints. - wpi::SmallVector constraints; + gch::small_vector constraints; /** * Concatenates multiple equality constraints. * - * @param equalityConstraints The list of EqualityConstraints to concatenate. + * @param equality_constraints The list of EqualityConstraints to concatenate. */ - EqualityConstraints( // NOLINT - std::initializer_list equalityConstraints) { - for (const auto& elem : equalityConstraints) { + EqualityConstraints( + std::initializer_list equality_constraints) { + for (const auto& elem : equality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -554,11 +557,11 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * * This overload is for Python bindings only. * - * @param equalityConstraints The list of EqualityConstraints to concatenate. + * @param equality_constraints The list of EqualityConstraints to concatenate. */ explicit EqualityConstraints( - const std::vector& equalityConstraints) { - for (const auto& elem : equalityConstraints) { + const std::vector& equality_constraints) { + for (const auto& elem : equality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -574,20 +577,19 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) EqualityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} + : constraints{make_constraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() == 0.0; }); + return std::ranges::all_of(constraints, [](auto& constraint) { + return constraint.value() == 0.0; + }); } }; @@ -596,17 +598,17 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { */ struct SLEIPNIR_DLLEXPORT InequalityConstraints { /// A vector of scalar inequality constraints. - wpi::SmallVector constraints; + gch::small_vector constraints; /** * Concatenates multiple inequality constraints. * - * @param inequalityConstraints The list of InequalityConstraints to + * @param inequality_constraints The list of InequalityConstraints to * concatenate. */ InequalityConstraints( // NOLINT - std::initializer_list inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { + std::initializer_list inequality_constraints) { + for (const auto& elem : inequality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -617,12 +619,12 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * * This overload is for Python bindings only. * - * @param inequalityConstraints The list of InequalityConstraints to + * @param inequality_constraints The list of InequalityConstraints to * concatenate. */ explicit InequalityConstraints( - const std::vector& inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { + const std::vector& inequality_constraints) { + for (const auto& elem : inequality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), elem.constraints.end()); } @@ -638,20 +640,19 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} + : constraints{make_constraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() >= 0.0; }); + return std::ranges::all_of(constraints, [](auto& constraint) { + return constraint.value() >= 0.0; + }); } }; @@ -662,10 +663,9 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints{lhs, rhs}; } @@ -678,10 +678,9 @@ EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -694,10 +693,9 @@ InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -710,10 +708,9 @@ InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { return lhs >= rhs; } @@ -726,15 +723,14 @@ InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) + requires(ScalarLike || MatrixLike) && + (ScalarLike || MatrixLike) && + (SleipnirType || SleipnirType) InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints{lhs, rhs}; } -} // namespace sleipnir +} // namespace slp namespace Eigen { @@ -742,20 +738,28 @@ namespace Eigen { * NumTraits specialization that allows instantiating Eigen types with Variable. */ template <> -struct NumTraits : NumTraits { - using Real = sleipnir::Variable; - using NonInteger = sleipnir::Variable; - using Nested = sleipnir::Variable; +struct NumTraits : NumTraits { + /// Real type. + using Real = slp::Variable; + /// Non-integer type. + using NonInteger = slp::Variable; + /// Nested type. + using Nested = slp::Variable; - enum { - IsComplex = 0, - IsInteger = 0, - IsSigned = 1, - RequireInitialization = 1, - ReadCost = 1, - AddCost = 3, - MulCost = 3 - }; + /// Is complex. + static constexpr int IsComplex = 0; + /// Is integer. + static constexpr int IsInteger = 0; + /// Is signed. + static constexpr int IsSigned = 1; + /// Require initialization. + static constexpr int RequireInitialization = 1; + /// Read cost. + static constexpr int ReadCost = 1; + /// Add cost. + static constexpr int AddCost = 3; + /// Multiply cost. + static constexpr int MulCost = 3; }; } // namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp new file mode 100644 index 0000000000..632d44beb5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp @@ -0,0 +1,872 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include + +#include "sleipnir/autodiff/slice.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/function_ref.hpp" + +namespace slp { + +/** + * A submatrix of autodiff variables with reference semantics. + * + * @tparam Mat The type of the matrix whose storage this class points to. + */ +template +class VariableBlock { + public: + /** + * Copy constructor. + */ + VariableBlock(const VariableBlock&) = default; + + /** + * Assigns a VariableBlock to the block. + * + * @param values VariableBlock of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(const VariableBlock& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_row_slice = values.m_row_slice; + m_row_slice_length = values.m_row_slice_length; + m_col_slice = values.m_col_slice; + m_col_slice_length = values.m_col_slice_length; + } else { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + } + + return *this; + } + + /** + * Move constructor. + */ + VariableBlock(VariableBlock&&) = default; + + /** + * Assigns a VariableBlock to the block. + * + * @param values VariableBlock of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(VariableBlock&& values) { + if (this == &values) { + return *this; + } + + if (m_mat == nullptr) { + m_mat = values.m_mat; + m_row_slice = values.m_row_slice; + m_row_slice_length = values.m_row_slice_length; + m_col_slice = values.m_col_slice; + m_col_slice_length = values.m_col_slice_length; + } else { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + } + + return *this; + } + + /** + * Constructs a Variable block pointing to all of the given matrix. + * + * @param mat The matrix to which to point. + */ + VariableBlock(Mat& mat) // NOLINT + : m_mat{&mat}, + m_row_slice{0, mat.rows(), 1}, + m_row_slice_length{m_row_slice.adjust(mat.rows())}, + m_col_slice{0, mat.cols(), 1}, + m_col_slice_length{m_col_slice.adjust(mat.cols())} {} + + /** + * Constructs a Variable block pointing to a subset of the given matrix. + * + * @param mat The matrix to which to point. + * @param row_offset The block's row offset. + * @param col_offset The block's column offset. + * @param block_rows The number of rows in the block. + * @param block_cols The number of columns in the block. + */ + VariableBlock(Mat& mat, int row_offset, int col_offset, int block_rows, + int block_cols) + : m_mat{&mat}, + m_row_slice{row_offset, row_offset + block_rows, 1}, + m_row_slice_length{m_row_slice.adjust(mat.rows())}, + m_col_slice{col_offset, col_offset + block_cols, 1}, + m_col_slice_length{m_col_slice.adjust(mat.cols())} {} + + /** + * Constructs a Variable block pointing to a subset of the given matrix. + * + * Note that the slices are taken as is rather than adjusted. + * + * @param mat The matrix to which to point. + * @param row_slice The block's row slice. + * @param row_slice_length The block's row length. + * @param col_slice The block's column slice. + * @param col_slice_length The block's column length. + */ + VariableBlock(Mat& mat, Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) + : m_mat{&mat}, + m_row_slice{std::move(row_slice)}, + m_row_slice_length{row_slice_length}, + m_col_slice{std::move(col_slice)}, + m_col_slice_length{col_slice_length} {} + + /** + * Assigns a double to the block. + * + * This only works for blocks with one row and one column. + * + * @param value Value to assign. + * @return This VariableBlock. + */ + VariableBlock& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0) = value; + + return *this; + } + + /** + * Assigns a double to the block. + * + * This only works for blocks with one row and one column. + * + * @param value Value to assign. + */ + void set_value(double value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0).set_value(value); + } + + /** + * Assigns an Eigen matrix to the block. + * + * @param values Eigen matrix of values to assign. + * @return This VariableBlock. + */ + template + VariableBlock& operator=(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + + return *this; + } + + /** + * Sets block's internal values. + * + * @param values Eigen matrix of values. + */ + template + requires std::same_as + void set_value(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col).set_value(values(row, col)); + } + } + } + + /** + * Assigns a VariableMatrix to the block. + * + * @param values VariableMatrix of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(const Mat& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + return *this; + } + + /** + * Assigns a VariableMatrix to the block. + * + * @param values VariableMatrix of values. + * @return This VariableBlock. + */ + VariableBlock& operator=(Mat&& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) = std::move(values(row, col)); + } + } + return *this; + } + + /** + * Returns a scalar subblock at the given row and column. + * + * @param row The scalar subblock's row. + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ + Variable& operator()(int row, int col) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step); + } + + /** + * Returns a scalar subblock at the given row and column. + * + * @param row The scalar subblock's row. + * @param col The scalar subblock's column. + * @return A scalar subblock at the given row and column. + */ + const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step); + } + + /** + * Returns a scalar subblock at the given row. + * + * @param row The scalar subblock's row. + * @return A scalar subblock at the given row. + */ + Variable& operator[](int row) + requires(!std::is_const_v) + { + slp_assert(row >= 0 && row < rows() * cols()); + return (*this)(row / cols(), row % cols()); + } + + /** + * Returns a scalar subblock at the given row. + * + * @param row The scalar subblock's row. + * @return A scalar subblock at the given row. + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); + return (*this)(row / cols(), row % cols()); + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + VariableBlock block(int row_offset, int col_offset, int block_rows, + int block_cols) { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return (*this)({row_offset, row_offset + block_rows, 1}, + {col_offset, col_offset + block_cols, 1}); + } + + /** + * Returns a block slice of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block slice of the variable matrix. + */ + const VariableBlock block(int row_offset, int col_offset, + int block_rows, int block_cols) const { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return (*this)({row_offset, row_offset + block_rows, 1}, + {col_offset, col_offset + block_cols, 1}); + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(m_row_slice_length); + int col_slice_length = col_slice.adjust(m_col_slice_length); + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, int row_slice_length, + Slice col_slice, int col_slice_length) { + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) const { + return VariableBlock{ + *m_mat, + {m_row_slice.start + row_slice.start * m_row_slice.step, + m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step}, + row_slice_length, + {m_col_slice.start + col_slice.start * m_col_slice.step, + m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step}, + col_slice_length}; + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + VariableBlock segment(int offset, int length) { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + const VariableBlock segment(int offset, int length) const { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) const { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) const { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableBlock& operator*=(const MatrixLike auto& rhs) { + slp_assert(cols() == rhs.rows() && cols() == rhs.cols()); + + for (int i = 0; i < rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { + sum += (*this)(i, k) * rhs(k, j); + } + (*this)(i, j) = sum; + } + } + + return *this; + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableBlock& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) *= rhs; + } + } + + return *this; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableBlock& operator/=(const MatrixLike auto& rhs) { + slp_assert(rhs.rows() == 1 && rhs.cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs(0, 0); + } + } + + return *this; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableBlock& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs; + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableBlock& operator+=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs(row, col); + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableBlock& operator+=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs; + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableBlock& operator-=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs(row, col); + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableBlock& operator-=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs; + } + } + + return *this; + } + + /** + * Implicit conversion operator from 1x1 VariableBlock to Variable. + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); + return (*this)(0, 0); + } + + /** + * Returns the transpose of the variable matrix. + * + * @return The transpose of the variable matrix. + */ + std::remove_cv_t T() const { + std::remove_cv_t result{Mat::empty, cols(), rows()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(col, row) = (*this)(row, col); + } + } + + return result; + } + + /** + * Returns the number of rows in the matrix. + * + * @return The number of rows in the matrix. + */ + int rows() const { return m_row_slice_length; } + + /** + * Returns the number of columns in the matrix. + * + * @return The number of columns in the matrix. + */ + int cols() const { return m_col_slice_length; } + + /** + * Returns an element of the variable matrix. + * + * @param row The row of the element to return. + * @param col The column of the element to return. + * @return An element of the variable matrix. + */ + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return (*m_mat)(m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step) + .value(); + } + + /** + * Returns a row of the variable column vector. + * + * @param index The index of the element to return. + * @return A row of the variable column vector. + */ + double value(int index) { + slp_assert(index >= 0 && index < rows() * cols()); + return value(index / cols(), index % cols()); + } + + /** + * Returns the contents of the variable matrix. + * + * @return The contents of the variable matrix. + */ + Eigen::MatrixXd value() { + Eigen::MatrixXd result{rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = value(row, col); + } + } + + return result; + } + + /** + * Transforms the matrix coefficient-wise with an unary operator. + * + * @param unary_op The unary operator to use for the transform operation. + * @return Result of the unary operator. + */ + std::remove_cv_t cwise_transform( + function_ref unary_op) const { + std::remove_cv_t result{Mat::empty, rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = unary_op((*this)(row, col)); + } + } + + return result; + } + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + + class iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using reference = Variable&; + + constexpr iterator() noexcept = default; + + constexpr iterator(VariableBlock* mat, int index) noexcept + : m_mat{mat}, m_index{index} {} + + constexpr iterator& operator++() noexcept { + ++m_index; + return *this; + } + + constexpr iterator operator++(int) noexcept { + iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const iterator&) const noexcept = default; + + constexpr reference operator*() const noexcept { return (*m_mat)[m_index]; } + + private: + VariableBlock* m_mat = nullptr; + int m_index = 0; + }; + + class const_iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using const_reference = const Variable&; + + constexpr const_iterator() noexcept = default; + + constexpr const_iterator(const VariableBlock* mat, int index) noexcept + : m_mat{mat}, m_index{index} {} + + constexpr const_iterator& operator++() noexcept { + ++m_index; + return *this; + } + + constexpr const_iterator operator++(int) noexcept { + const_iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const const_iterator&) const noexcept = default; + + constexpr const_reference operator*() const noexcept { + return (*m_mat)[m_index]; + } + + private: + const VariableBlock* m_mat = nullptr; + int m_index = 0; + }; + +#endif // DOXYGEN_SHOULD_SKIP_THIS + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + iterator begin() { return iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + iterator end() { return iterator(this, rows() * cols()); } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator begin() const { return const_iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator end() const { return const_iterator(this, rows() * cols()); } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator cbegin() const { return const_iterator(this, 0); } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator cend() const { return const_iterator(this, rows() * cols()); } + + /** + * Returns number of elements in matrix. + * + * @return Number of elements in matrix. + */ + size_t size() const { return rows() * cols(); } + + private: + Mat* m_mat = nullptr; + + Slice m_row_slice; + int m_row_slice_length = 0; + + Slice m_col_slice; + int m_col_slice_length = 0; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp new file mode 100644 index 0000000000..4dc2cea00c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -0,0 +1,1283 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/autodiff/slice.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_block.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/function_ref.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * A matrix of autodiff variables. + */ +class SLEIPNIR_DLLEXPORT VariableMatrix { + public: + /** + * Type tag used to designate an uninitialized VariableMatrix. + */ + struct empty_t {}; + + /** + * Designates an uninitialized VariableMatrix. + */ + static constexpr empty_t empty{}; + + /** + * Constructs an empty VariableMatrix. + */ + VariableMatrix() = default; + + /** + * Constructs a VariableMatrix column vector with the given rows. + * + * @param rows The number of matrix rows. + */ + explicit VariableMatrix(int rows) : m_rows{rows}, m_cols{1} { + m_storage.reserve(rows); + for (int row = 0; row < rows; ++row) { + m_storage.emplace_back(); + } + } + + /** + * Constructs a zero-initialized VariableMatrix with the given dimensions. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} { + m_storage.reserve(rows * cols); + for (int index = 0; index < rows * cols; ++index) { + m_storage.emplace_back(); + } + } + + /** + * Constructs an empty VariableMatrix with the given dimensions. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(empty_t, int rows, int cols) : m_rows{rows}, m_cols{cols} { + m_storage.reserve(rows * cols); + for (int index = 0; index < rows * cols; ++index) { + m_storage.emplace_back(nullptr); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of Variables. + * + * @param list The nested list of Variables. + */ + VariableMatrix( // NOLINT + std::initializer_list> list) { + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of doubles. + * + * This overload is for Python bindings only. + * + * @param list The nested list of Variables. + */ + VariableMatrix(const std::vector>& list) { // NOLINT + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a scalar VariableMatrix from a nested list of Variables. + * + * This overload is for Python bindings only. + * + * @param list The nested list of Variables. + */ + VariableMatrix(const std::vector>& list) { // NOLINT + // Get row and column counts for destination matrix + m_rows = list.size(); + m_cols = 0; + if (list.size() > 0) { + m_cols = list.begin()->size(); + } + + // Assert the first and latest column counts are the same + for ([[maybe_unused]] + const auto& row : list) { + slp_assert(list.begin()->size() == row.size()); + } + + m_storage.reserve(rows() * cols()); + for (const auto& row : list) { + std::ranges::copy(row, std::back_inserter(m_storage)); + } + } + + /** + * Constructs a VariableMatrix from an Eigen matrix. + * + * @param values Eigen matrix of values. + */ + template + VariableMatrix(const Eigen::MatrixBase& values) // NOLINT + : m_rows{static_cast(values.rows())}, + m_cols{static_cast(values.cols())} { + m_storage.reserve(values.rows() * values.cols()); + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a VariableMatrix from an Eigen diagonal matrix. + * + * @param values Diagonal matrix of values. + */ + template + VariableMatrix(const Eigen::DiagonalBase& values) // NOLINT + : m_rows{static_cast(values.rows())}, + m_cols{static_cast(values.cols())} { + m_storage.reserve(values.rows() * values.cols()); + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + if (row == col) { + m_storage.emplace_back(values.diagonal()[row]); + } else { + m_storage.emplace_back(0.0); + } + } + } + } + + /** + * Assigns an Eigen matrix to a VariableMatrix. + * + * @param values Eigen matrix of values. + * @return This VariableMatrix. + */ + template + VariableMatrix& operator=(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + (*this)(row, col) = values(row, col); + } + } + + return *this; + } + + /** + * Assigns a double to the matrix. + * + * This only works for matrices with one row and one column. + * + * @param value Value to assign. + * @return This VariableMatrix. + */ + VariableMatrix& operator=(ScalarLike auto value) { + slp_assert(rows() == 1 && cols() == 1); + + (*this)(0, 0) = value; + + return *this; + } + + /** + * Sets the VariableMatrix's internal values. + * + * @param values Eigen matrix of values. + */ + template + requires std::same_as + void set_value(const Eigen::MatrixBase& values) { + slp_assert(rows() == values.rows() && cols() == values.cols()); + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { + (*this)(row, col).set_value(values(row, col)); + } + } + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + * + * @param variable Variable. + */ + VariableMatrix(const Variable& variable) // NOLINT + : m_rows{1}, m_cols{1} { + m_storage.emplace_back(variable); + } + + /** + * Constructs a scalar VariableMatrix from a Variable. + * + * @param variable Variable. + */ + VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { // NOLINT + m_storage.emplace_back(std::move(variable)); + } + + /** + * Constructs a VariableMatrix from a VariableBlock. + * + * @param values VariableBlock of values. + */ + VariableMatrix(const VariableBlock& values) // NOLINT + : m_rows{values.rows()}, m_cols{values.cols()} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a VariableMatrix from a VariableBlock. + * + * @param values VariableBlock of values. + */ + VariableMatrix(const VariableBlock& values) // NOLINT + : m_rows{values.rows()}, m_cols{values.cols()} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values(row, col)); + } + } + } + + /** + * Constructs a column vector wrapper around a Variable array. + * + * @param values Variable array to wrap. + */ + explicit VariableMatrix(std::span values) + : m_rows{static_cast(values.size())}, m_cols{1} { + m_storage.reserve(rows() * cols()); + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + m_storage.emplace_back(values[row * cols() + col]); + } + } + } + + /** + * Constructs a matrix wrapper around a Variable array. + * + * @param values Variable array to wrap. + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + */ + VariableMatrix(std::span values, int rows, int cols) + : m_rows{rows}, m_cols{cols} { + slp_assert(static_cast(values.size()) == rows * cols); + m_storage.reserve(rows * cols); + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + m_storage.emplace_back(values[row * cols + col]); + } + } + } + + /** + * Returns a block pointing to the given row and column. + * + * @param row The block row. + * @param col The block column. + * @return A block pointing to the given row and column. + */ + Variable& operator()(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; + } + + /** + * Returns a block pointing to the given row and column. + * + * @param row The block row. + * @param col The block column. + * @return A block pointing to the given row and column. + */ + const Variable& operator()(int row, int col) const { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col]; + } + + /** + * Returns a block pointing to the given row. + * + * @param row The block row. + * @return A block pointing to the given row. + */ + Variable& operator[](int row) { + slp_assert(row >= 0 && row < rows() * cols()); + return m_storage[row]; + } + + /** + * Returns a block pointing to the given row. + * + * @param row The block row. + * @return A block pointing to the given row. + */ + const Variable& operator[](int row) const { + slp_assert(row >= 0 && row < rows() * cols()); + return m_storage[row]; + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + VariableBlock block(int row_offset, int col_offset, + int block_rows, int block_cols) { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return VariableBlock{*this, row_offset, col_offset, block_rows, block_cols}; + } + + /** + * Returns a block of the variable matrix. + * + * @param row_offset The row offset of the block selection. + * @param col_offset The column offset of the block selection. + * @param block_rows The number of rows in the block selection. + * @param block_cols The number of columns in the block selection. + * @return A block of the variable matrix. + */ + const VariableBlock block(int row_offset, + int col_offset, + int block_rows, + int block_cols) const { + slp_assert(row_offset >= 0 && row_offset <= rows()); + slp_assert(col_offset >= 0 && col_offset <= cols()); + slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset); + slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset); + return VariableBlock{*this, row_offset, col_offset, block_rows, block_cols}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + VariableBlock operator()(Slice row_slice, Slice col_slice) { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param row_slice The row slice. + * @param col_slice The column slice. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()(Slice row_slice, + Slice col_slice) const { + int row_slice_length = row_slice.adjust(rows()); + int col_slice_length = col_slice.adjust(cols()); + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + * + */ + VariableBlock operator()(Slice row_slice, + int row_slice_length, + Slice col_slice, + int col_slice_length) { + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param row_slice The row slice. + * @param row_slice_length The row slice length. + * @param col_slice The column slice. + * @param col_slice_length The column slice length. + * @return A slice of the variable matrix. + */ + const VariableBlock operator()( + Slice row_slice, int row_slice_length, Slice col_slice, + int col_slice_length) const { + return VariableBlock{*this, std::move(row_slice), row_slice_length, + std::move(col_slice), col_slice_length}; + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + VariableBlock segment(int offset, int length) { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + * @return A segment of the variable vector. + */ + const VariableBlock segment(int offset, + int length) const { + slp_assert(offset >= 0 && offset < rows() * cols()); + slp_assert(length >= 0 && length <= rows() * cols() - offset); + return block(offset, 0, length, 1); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + VariableBlock row(int row) { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a row slice of the variable matrix. + * + * @param row The row to slice. + * @return A row slice of the variable matrix. + */ + const VariableBlock row(int row) const { + slp_assert(row >= 0 && row < rows()); + return block(row, 0, 1, cols()); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + VariableBlock col(int col) { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Returns a column slice of the variable matrix. + * + * @param col The column to slice. + * @return A column slice of the variable matrix. + */ + const VariableBlock col(int col) const { + slp_assert(col >= 0 && col < cols()); + return block(0, col, rows(), 1); + } + + /** + * Matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.cols() == rhs.rows()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols()); + +#if __GNUC__ >= 12 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < lhs.cols(); ++k) { + sum += lhs(i, k) * rhs(k, j); + } + result(i, j) = sum; + } + } +#if __GNUC__ >= 12 +#pragma GCC diagnostic pop +#endif + + return result; + } + + /** + * Matrix-scalar multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const SleipnirMatrixLike auto& lhs, const ScalarLike auto& rhs) { + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) * rhs; + } + } + + return result; + } + + /** + * Matrix-scalar multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix operator*(const MatrixLike auto& lhs, + const Variable& rhs) { + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) * rhs; + } + } + + return result; + } + + /** + * Scalar-matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const ScalarLike auto& lhs, const SleipnirMatrixLike auto& rhs) { + VariableMatrix result{VariableMatrix::empty, rhs.rows(), rhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = rhs(row, col) * lhs; + } + } + + return result; + } + + /** + * Scalar-matrix multiplication operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator*(const Variable& lhs, const MatrixLike auto& rhs) { + VariableMatrix result(VariableMatrix::empty, rhs.rows(), rhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = rhs(row, col) * lhs; + } + } + + return result; + } + + /** + * Compound matrix multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableMatrix& operator*=(const MatrixLike auto& rhs) { + slp_assert(cols() == rhs.rows() && cols() == rhs.cols()); + + for (int i = 0; i < rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum; + for (int k = 0; k < cols(); ++k) { + sum += (*this)(i, k) * rhs(k, j); + } + (*this)(i, j) = sum; + } + } + + return *this; + } + + /** + * Compound matrix-scalar multiplication-assignment operator. + * + * @param rhs Variable to multiply. + * @return Result of multiplication. + */ + VariableMatrix& operator*=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < rhs.cols(); ++col) { + (*this)(row, col) *= rhs; + } + } + + return *this; + } + + /** + * Binary division operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of division. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator/(const MatrixLike auto& lhs, const ScalarLike auto& rhs) { + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) / rhs; + } + } + + return result; + } + + /** + * Compound matrix division-assignment operator. + * + * @param rhs Variable to divide. + * @return Result of division. + */ + VariableMatrix& operator/=(const ScalarLike auto& rhs) { + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) /= rhs; + } + } + + return *this; + } + + /** + * Binary addition operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of addition. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator+(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) + rhs(row, col); + } + } + + return result; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableMatrix& operator+=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs(row, col); + } + } + + return *this; + } + + /** + * Compound addition-assignment operator. + * + * @param rhs Variable to add. + * @return Result of addition. + */ + VariableMatrix& operator+=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) += rhs; + } + } + + return *this; + } + + /** + * Binary subtraction operator. + * + * @param lhs Operator left-hand side. + * @param rhs Operator right-hand side. + * @return Result of subtraction. + */ + template + requires SleipnirMatrixLike || SleipnirMatrixLike + friend SLEIPNIR_DLLEXPORT VariableMatrix operator-(const LHS& lhs, + const RHS& rhs) { + slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + + VariableMatrix result(VariableMatrix::empty, lhs.rows(), lhs.cols()); + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = lhs(row, col) - rhs(row, col); + } + } + + return result; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableMatrix& operator-=(const MatrixLike auto& rhs) { + slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs(row, col); + } + } + + return *this; + } + + /** + * Compound subtraction-assignment operator. + * + * @param rhs Variable to subtract. + * @return Result of subtraction. + */ + VariableMatrix& operator-=(const ScalarLike auto& rhs) { + slp_assert(rows() == 1 && cols() == 1); + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + (*this)(row, col) -= rhs; + } + } + + return *this; + } + + /** + * Unary minus operator. + * + * @param lhs Operand for unary minus. + */ + friend SLEIPNIR_DLLEXPORT VariableMatrix + operator-(const SleipnirMatrixLike auto& lhs) { + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + result(row, col) = -lhs(row, col); + } + } + + return result; + } + + /** + * Implicit conversion operator from 1x1 VariableMatrix to Variable. + */ + operator Variable() const { // NOLINT + slp_assert(rows() == 1 && cols() == 1); + return (*this)(0, 0); + } + + /** + * Returns the transpose of the variable matrix. + * + * @return The transpose of the variable matrix. + */ + VariableMatrix T() const { + VariableMatrix result{VariableMatrix::empty, cols(), rows()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(col, row) = (*this)(row, col); + } + } + + return result; + } + + /** + * Returns the number of rows in the matrix. + * + * @return The number of rows in the matrix. + */ + int rows() const { return m_rows; } + + /** + * Returns the number of columns in the matrix. + * + * @return The number of columns in the matrix. + */ + int cols() const { return m_cols; } + + /** + * Returns an element of the variable matrix. + * + * @param row The row of the element to return. + * @param col The column of the element to return. + * @return An element of the variable matrix. + */ + double value(int row, int col) { + slp_assert(row >= 0 && row < rows()); + slp_assert(col >= 0 && col < cols()); + return m_storage[row * cols() + col].value(); + } + + /** + * Returns a row of the variable column vector. + * + * @param index The index of the element to return. + * @return A row of the variable column vector. + */ + double value(int index) { + slp_assert(index >= 0 && index < rows() * cols()); + return m_storage[index].value(); + } + + /** + * Returns the contents of the variable matrix. + * + * @return The contents of the variable matrix. + */ + Eigen::MatrixXd value() { + Eigen::MatrixXd result{rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = value(row, col); + } + } + + return result; + } + + /** + * Transforms the matrix coefficient-wise with an unary operator. + * + * @param unary_op The unary operator to use for the transform operation. + * @return Result of the unary operator. + */ + VariableMatrix cwise_transform( + function_ref unary_op) const { + VariableMatrix result{VariableMatrix::empty, rows(), cols()}; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(row, col) = unary_op((*this)(row, col)); + } + } + + return result; + } + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + + class iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using reference = Variable&; + + constexpr iterator() noexcept = default; + + explicit constexpr iterator( + gch::small_vector::iterator it) noexcept + : m_it{it} {} + + constexpr iterator& operator++() noexcept { + ++m_it; + return *this; + } + + constexpr iterator operator++(int) noexcept { + iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const iterator&) const noexcept = default; + + constexpr reference operator*() const noexcept { return *m_it; } + + private: + gch::small_vector::iterator m_it; + }; + + class const_iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = Variable; + using difference_type = std::ptrdiff_t; + using pointer = Variable*; + using const_reference = const Variable&; + + constexpr const_iterator() noexcept = default; + + explicit constexpr const_iterator( + gch::small_vector::const_iterator it) noexcept + : m_it{it} {} + + constexpr const_iterator& operator++() noexcept { + ++m_it; + return *this; + } + + constexpr const_iterator operator++(int) noexcept { + const_iterator retval = *this; + ++(*this); + return retval; + } + + constexpr bool operator==(const const_iterator&) const noexcept = default; + + constexpr const_reference operator*() const noexcept { return *m_it; } + + private: + gch::small_vector::const_iterator m_it; + }; + +#endif // DOXYGEN_SHOULD_SKIP_THIS + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + iterator begin() { return iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + iterator end() { return iterator{m_storage.end()}; } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator begin() const { return const_iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator end() const { return const_iterator{m_storage.end()}; } + + /** + * Returns begin iterator. + * + * @return Begin iterator. + */ + const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } + + /** + * Returns end iterator. + * + * @return End iterator. + */ + const_iterator cend() const { return const_iterator{m_storage.end()}; } + + /** + * Returns number of elements in matrix. + * + * @return Number of elements in matrix. + */ + size_t size() const { return m_storage.size(); } + + /** + * Returns a variable matrix filled with zeroes. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + * @return A variable matrix filled with zeroes. + */ + static VariableMatrix zero(int rows, int cols) { + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + for (auto& elem : result) { + elem = 0.0; + } + + return result; + } + + /** + * Returns a variable matrix filled with ones. + * + * @param rows The number of matrix rows. + * @param cols The number of matrix columns. + * @return A variable matrix filled with ones. + */ + static VariableMatrix ones(int rows, int cols) { + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + for (auto& elem : result) { + elem = 1.0; + } + + return result; + } + + private: + gch::small_vector m_storage; + int m_rows = 0; + int m_cols = 0; +}; + +/** + * Applies a coefficient-wise reduce operation to two matrices. + * + * @param lhs The left-hand side of the binary operator. + * @param rhs The right-hand side of the binary operator. + * @param binary_op The binary operator to use for the reduce operation. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( + const VariableMatrix& lhs, const VariableMatrix& rhs, + function_ref binary_op) { + slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows()); + + VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; + + for (int row = 0; row < lhs.rows(); ++row) { + for (int col = 0; col < lhs.cols(); ++col) { + result(row, col) = binary_op(lhs(row, col), rhs(row, col)); + } + } + + return result; +} + +/** + * Assemble a VariableMatrix from a nested list of blocks. + * + * Each row's blocks must have the same height, and the assembled block rows + * must have the same width. For example, for the block matrix [[A, B], [C]] to + * be constructible, the number of rows in A and B must match, and the number of + * columns in [A, B] and [C] must match. + * + * @param list The nested list of blocks. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix block( + std::initializer_list> list) { + // Get row and column counts for destination matrix + int rows = 0; + int cols = -1; + for (const auto& row : list) { + if (row.size() > 0) { + rows += row.begin()->rows(); + } + + // Get number of columns in this row + int latest_cols = 0; + for (const auto& elem : row) { + // Assert the first and latest row have the same height + slp_assert(row.begin()->rows() == elem.rows()); + + latest_cols += elem.cols(); + } + + // If this is the first row, record the column count. Otherwise, assert the + // first and latest column counts are the same. + if (cols == -1) { + cols = latest_cols; + } else { + slp_assert(cols == latest_cols); + } + } + + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + int row_offset = 0; + for (const auto& row : list) { + int col_offset = 0; + for (const auto& elem : row) { + result.block(row_offset, col_offset, elem.rows(), elem.cols()) = elem; + col_offset += elem.cols(); + } + row_offset += row.begin()->rows(); + } + + return result; +} + +/** + * Assemble a VariableMatrix from a nested list of blocks. + * + * Each row's blocks must have the same height, and the assembled block rows + * must have the same width. For example, for the block matrix [[A, B], [C]] to + * be constructible, the number of rows in A and B must match, and the number of + * columns in [A, B] and [C] must match. + * + * This overload is for Python bindings only. + * + * @param list The nested list of blocks. + */ +SLEIPNIR_DLLEXPORT inline VariableMatrix block( + const std::vector>& list) { + // Get row and column counts for destination matrix + int rows = 0; + int cols = -1; + for (const auto& row : list) { + if (row.size() > 0) { + rows += row.begin()->rows(); + } + + // Get number of columns in this row + int latest_cols = 0; + for (const auto& elem : row) { + // Assert the first and latest row have the same height + slp_assert(row.begin()->rows() == elem.rows()); + + latest_cols += elem.cols(); + } + + // If this is the first row, record the column count. Otherwise, assert the + // first and latest column counts are the same. + if (cols == -1) { + cols = latest_cols; + } else { + slp_assert(cols == latest_cols); + } + } + + VariableMatrix result{VariableMatrix::empty, rows, cols}; + + int row_offset = 0; + for (const auto& row : list) { + int col_offset = 0; + for (const auto& elem : row) { + result.block(row_offset, col_offset, elem.rows(), elem.cols()) = elem; + col_offset += elem.cols(); + } + row_offset += row.begin()->rows(); + } + + return result; +} + +/** + * Solves the VariableMatrix equation AX = B for X. + * + * @param A The left-hand side. + * @param B The right-hand side. + * @return The solution X. + */ +SLEIPNIR_DLLEXPORT VariableMatrix solve(const VariableMatrix& A, + const VariableMatrix& B); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp similarity index 50% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp index 663e81d9ba..d917442666 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp @@ -7,14 +7,14 @@ #include #include -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/OptimizationProblem.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/problem.hpp" +#include "sleipnir/util/assert.hpp" +#include "sleipnir/util/concepts.hpp" +#include "sleipnir/util/function_ref.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt. @@ -26,7 +26,7 @@ namespace sleipnir { * @param dt The time over which to integrate. */ template -State RK4(F&& f, State x, Input u, Time t0, Time dt) { +State rk4(F&& f, State x, Input u, Time t0, Time dt) { auto halfdt = dt * 0.5; State k1 = f(t0, x, u, dt); State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt); @@ -42,13 +42,13 @@ State RK4(F&& f, State x, Input u, Time t0, Time dt) { enum class TranscriptionMethod : uint8_t { /// Each state is a decision variable constrained to the integrated dynamics /// of the previous state. - kDirectTranscription, + DIRECT_TRANSCRIPTION, /// The trajectory is modeled as a series of cubic polynomials where the /// centerpoint slope is constrained. - kDirectCollocation, + DIRECT_COLLOCATION, /// States depend explicitly as a function of all previous states and all /// previous inputs. - kSingleShooting + SINGLE_SHOOTING }; /** @@ -56,9 +56,9 @@ enum class TranscriptionMethod : uint8_t { */ enum class DynamicsType : uint8_t { /// The dynamics are a function in the form dx/dt = f(t, x, u). - kExplicitODE, + EXPLICIT_ODE, /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ). - kDiscrete + DISCRETE }; /** @@ -66,12 +66,12 @@ enum class DynamicsType : uint8_t { */ enum class TimestepMethod : uint8_t { /// The timestep is a fixed constant. - kFixed, + FIXED, /// The timesteps are allowed to vary as independent decision variables. - kVariable, + VARIABLE, /// The timesteps are equal length but allowed to vary as a single decision /// variable. - kVariableSingle + VARIABLE_SINGLE }; /** @@ -100,179 +100,177 @@ enum class TimestepMethod : uint8_t { * https://underactuated.mit.edu/trajopt.html goes into more detail on each * transcription method. */ -class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { +class SLEIPNIR_DLLEXPORT OCP : public Problem { public: /** * Build an optimization problem using a system evolution function (explicit * ODE or discrete state transition function). * - * @param numStates The number of system states. - * @param numInputs The number of system inputs. + * @param num_states The number of system states. + * @param num_inputs The number of system inputs. * @param dt The timestep for fixed-step integration. - * @param numSteps The number of control points. + * @param num_steps The number of control points. * @param dynamics Function representing an explicit or implicit ODE, or a * discrete state transition function. * - Explicit: dx/dt = f(x, u, *) * - Implicit: f([x dx/dt]', u, *) = 0 * - State transition: xₖ₊₁ = f(xₖ, uₖ) - * @param dynamicsType The type of system evolution function. - * @param timestepMethod The timestep method. + * @param dynamics_type The type of system evolution function. + * @param timestep_method The timestep method. * @param method The transcription method. */ - OCPSolver( - int numStates, int numInputs, std::chrono::duration dt, - int numSteps, + OCP(int num_states, int num_inputs, std::chrono::duration dt, + int num_steps, function_ref dynamics, - DynamicsType dynamicsType = DynamicsType::kExplicitODE, - TimestepMethod timestepMethod = TimestepMethod::kFixed, - TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) - : OCPSolver{numStates, - numInputs, - dt, - numSteps, - [=]([[maybe_unused]] const VariableMatrix& t, - const VariableMatrix& x, const VariableMatrix& u, - [[maybe_unused]] - const VariableMatrix& dt) -> VariableMatrix { - return dynamics(x, u); - }, - dynamicsType, - timestepMethod, - method} {} + DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, + TimestepMethod timestep_method = TimestepMethod::FIXED, + TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + : OCP{num_states, + num_inputs, + dt, + num_steps, + [=]([[maybe_unused]] const VariableMatrix& t, + const VariableMatrix& x, const VariableMatrix& u, + [[maybe_unused]] + const VariableMatrix& dt) -> VariableMatrix { + return dynamics(x, u); + }, + dynamics_type, + timestep_method, + method} {} /** * Build an optimization problem using a system evolution function (explicit * ODE or discrete state transition function). * - * @param numStates The number of system states. - * @param numInputs The number of system inputs. + * @param num_states The number of system states. + * @param num_inputs The number of system inputs. * @param dt The timestep for fixed-step integration. - * @param numSteps The number of control points. + * @param num_steps The number of control points. * @param dynamics Function representing an explicit or implicit ODE, or a * discrete state transition function. * - Explicit: dx/dt = f(t, x, u, *) * - Implicit: f(t, [x dx/dt]', u, *) = 0 * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) - * @param dynamicsType The type of system evolution function. - * @param timestepMethod The timestep method. + * @param dynamics_type The type of system evolution function. + * @param timestep_method The timestep method. * @param method The transcription method. */ - OCPSolver( - int numStates, int numInputs, std::chrono::duration dt, - int numSteps, + OCP(int num_states, int num_inputs, std::chrono::duration dt, + int num_steps, function_ref dynamics, - DynamicsType dynamicsType = DynamicsType::kExplicitODE, - TimestepMethod timestepMethod = TimestepMethod::kFixed, - TranscriptionMethod method = TranscriptionMethod::kDirectTranscription) - : m_numStates{numStates}, - m_numInputs{numInputs}, + DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, + TimestepMethod timestep_method = TimestepMethod::FIXED, + TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + : m_num_states{num_states}, + m_num_inputs{num_inputs}, m_dt{dt}, - m_numSteps{numSteps}, - m_transcriptionMethod{method}, - m_dynamicsType{dynamicsType}, - m_dynamicsFunction{std::move(dynamics)}, - m_timestepMethod{timestepMethod} { - // u is numSteps + 1 so that the final constraintFunction evaluation works - m_U = DecisionVariable(m_numInputs, m_numSteps + 1); + m_num_steps{num_steps}, + m_transcription_method{method}, + m_dynamics_type{dynamics_type}, + m_dynamics_function{std::move(dynamics)}, + m_timestep_method{timestep_method} { + // u is num_steps + 1 so that the final constraint function evaluation works + m_U = decision_variable(m_num_inputs, m_num_steps + 1); - if (m_timestepMethod == TimestepMethod::kFixed) { - m_DT = VariableMatrix{1, m_numSteps + 1}; - for (int i = 0; i < numSteps + 1; ++i) { + if (m_timestep_method == TimestepMethod::FIXED) { + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { m_DT(0, i) = m_dt.count(); } - } else if (m_timestepMethod == TimestepMethod::kVariableSingle) { - Variable DT = DecisionVariable(); - DT.SetValue(m_dt.count()); + } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable dt = decision_variable(); + dt.set_value(m_dt.count()); // Set the member variable matrix to track the decision variable - m_DT = VariableMatrix{1, m_numSteps + 1}; - for (int i = 0; i < numSteps + 1; ++i) { - m_DT(0, i) = DT; + m_DT = VariableMatrix{1, m_num_steps + 1}; + for (int i = 0; i < num_steps + 1; ++i) { + m_DT(0, i) = dt; } - } else if (m_timestepMethod == TimestepMethod::kVariable) { - m_DT = DecisionVariable(1, m_numSteps + 1); - for (int i = 0; i < numSteps + 1; ++i) { - m_DT(0, i).SetValue(m_dt.count()); + } else if (m_timestep_method == TimestepMethod::VARIABLE) { + m_DT = decision_variable(1, m_num_steps + 1); + for (int i = 0; i < num_steps + 1; ++i) { + m_DT(0, i).set_value(m_dt.count()); } } - if (m_transcriptionMethod == TranscriptionMethod::kDirectTranscription) { - m_X = DecisionVariable(m_numStates, m_numSteps + 1); - ConstrainDirectTranscription(); - } else if (m_transcriptionMethod == - TranscriptionMethod::kDirectCollocation) { - m_X = DecisionVariable(m_numStates, m_numSteps + 1); - ConstrainDirectCollocation(); - } else if (m_transcriptionMethod == TranscriptionMethod::kSingleShooting) { + if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { + m_X = decision_variable(m_num_states, m_num_steps + 1); + constrain_direct_transcription(); + } else if (m_transcription_method == + TranscriptionMethod::DIRECT_COLLOCATION) { + m_X = decision_variable(m_num_states, m_num_steps + 1); + constrain_direct_collocation(); + } else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) { // In single-shooting the states aren't decision variables, but instead // depend on the input and previous states - m_X = VariableMatrix{m_numStates, m_numSteps + 1}; - ConstrainSingleShooting(); + m_X = VariableMatrix{m_num_states, m_num_steps + 1}; + constrain_single_shooting(); } } /** * Utility function to constrain the initial state. * - * @param initialState the initial state to constrain to. + * @param initial_state the initial state to constrain to. */ template requires ScalarLike || MatrixLike - void ConstrainInitialState(const T& initialState) { - SubjectTo(InitialState() == initialState); + void constrain_initial_state(const T& initial_state) { + subject_to(this->initial_state() == initial_state); } /** * Utility function to constrain the final state. * - * @param finalState the final state to constrain to. + * @param final_state the final state to constrain to. */ template requires ScalarLike || MatrixLike - void ConstrainFinalState(const T& finalState) { - SubjectTo(FinalState() == finalState); + void constrain_final_state(const T& final_state) { + subject_to(this->final_state() == final_state); } /** * Set the constraint evaluation function. This function is called - * `numSteps+1` times, with the corresponding state and input + * `num_steps+1` times, with the corresponding state and input * VariableMatrices. * * @param callback The callback f(x, u) where x is the state and u is the * input vector. */ - void ForEachStep( + void for_each_step( const function_ref callback) { - for (int i = 0; i < m_numSteps + 1; ++i) { - auto x = X().Col(i); - auto u = U().Col(i); + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); callback(x, u); } } /** * Set the constraint evaluation function. This function is called - * `numSteps+1` times, with the corresponding state and input + * `num_steps+1` times, with the corresponding state and input * VariableMatrices. * * @param callback The callback f(t, x, u, dt) where t is time, x is the state * vector, u is the input vector, and dt is the timestep duration. */ - void ForEachStep( + void for_each_step( const function_ref callback) { Variable time = 0.0; - for (int i = 0; i < m_numSteps + 1; ++i) { - auto x = X().Col(i); - auto u = U().Col(i); - auto dt = DT()(0, i); + for (int i = 0; i < m_num_steps + 1; ++i) { + auto x = X().col(i); + auto u = U().col(i); + auto dt = this->dt()(0, i); callback(time, x, u, dt); time += dt; @@ -282,115 +280,115 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { /** * Convenience function to set a lower bound on the input. * - * @param lowerBound The lower bound that inputs must always be above. Must be - * shaped (numInputs)x1. + * @param lower_bound The lower bound that inputs must always be above. Must + * be shaped (num_inputs)x1. */ template requires ScalarLike || MatrixLike - void SetLowerInputBound(const T& lowerBound) { - for (int i = 0; i < m_numSteps + 1; ++i) { - SubjectTo(U().Col(i) >= lowerBound); + void set_lower_input_bound(const T& lower_bound) { + for (int i = 0; i < m_num_steps + 1; ++i) { + subject_to(U().col(i) >= lower_bound); } } /** * Convenience function to set an upper bound on the input. * - * @param upperBound The upper bound that inputs must always be below. Must be - * shaped (numInputs)x1. + * @param upper_bound The upper bound that inputs must always be below. Must + * be shaped (num_inputs)x1. */ template requires ScalarLike || MatrixLike - void SetUpperInputBound(const T& upperBound) { - for (int i = 0; i < m_numSteps + 1; ++i) { - SubjectTo(U().Col(i) <= upperBound); + void set_upper_input_bound(const T& upper_bound) { + for (int i = 0; i < m_num_steps + 1; ++i) { + subject_to(U().col(i) <= upper_bound); } } /** * Convenience function to set a lower bound on the timestep. * - * @param minTimestep The minimum timestep. + * @param min_timestep The minimum timestep. */ - void SetMinTimestep(std::chrono::duration minTimestep) { - SubjectTo(DT() >= minTimestep.count()); + void set_min_timestep(std::chrono::duration min_timestep) { + subject_to(dt() >= min_timestep.count()); } /** * Convenience function to set an upper bound on the timestep. * - * @param maxTimestep The maximum timestep. + * @param max_timestep The maximum timestep. */ - void SetMaxTimestep(std::chrono::duration maxTimestep) { - SubjectTo(DT() <= maxTimestep.count()); + void set_max_timestep(std::chrono::duration max_timestep) { + subject_to(dt() <= max_timestep.count()); } /** * Get the state variables. After the problem is solved, this will contain the * optimized trajectory. * - * Shaped (numStates)x(numSteps+1). + * Shaped (num_states)x(num_steps+1). * - * @returns The state variable matrix. + * @return The state variable matrix. */ - VariableMatrix& X() { return m_X; }; + VariableMatrix& X() { return m_X; } /** * Get the input variables. After the problem is solved, this will contain the * inputs corresponding to the optimized trajectory. * - * Shaped (numInputs)x(numSteps+1), although the last input step is unused in - * the trajectory. + * Shaped (num_inputs)x(num_steps+1), although the last input step is unused + * in the trajectory. * - * @returns The input variable matrix. + * @return The input variable matrix. */ - VariableMatrix& U() { return m_U; }; + VariableMatrix& U() { return m_U; } /** * Get the timestep variables. After the problem is solved, this will contain * the timesteps corresponding to the optimized trajectory. * - * Shaped 1x(numSteps+1), although the last timestep is unused in + * Shaped 1x(num_steps+1), although the last timestep is unused in * the trajectory. * - * @returns The timestep variable matrix. + * @return The timestep variable matrix. */ - VariableMatrix& DT() { return m_DT; }; + VariableMatrix& dt() { return m_DT; } /** * Convenience function to get the initial state in the trajectory. * - * @returns The initial state of the trajectory. + * @return The initial state of the trajectory. */ - VariableMatrix InitialState() { return m_X.Col(0); } + VariableMatrix initial_state() { return m_X.col(0); } /** * Convenience function to get the final state in the trajectory. * - * @returns The final state of the trajectory. + * @return The final state of the trajectory. */ - VariableMatrix FinalState() { return m_X.Col(m_numSteps); } + VariableMatrix final_state() { return m_X.col(m_num_steps); } private: - void ConstrainDirectCollocation() { - Assert(m_dynamicsType == DynamicsType::kExplicitODE); + void constrain_direct_collocation() { + slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE); Variable time = 0.0; // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ - for (int i = 0; i < m_numSteps; ++i) { - Variable h = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + Variable h = dt()(0, i); - auto& f = m_dynamicsFunction; + auto& f = m_dynamics_function; auto t_begin = time; auto t_end = t_begin + h; - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); - auto u_begin = U().Col(i); - auto u_end = U().Col(i + 1); + auto u_begin = U().col(i); + auto u_end = U().col(i + 1); auto xdot_begin = f(t_begin, x_begin, u_begin, h); auto xdot_end = f(t_end, x_end, u_end, h); @@ -401,71 +399,71 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end); auto u_c = 0.5 * (u_begin + u_end); - SubjectTo(xdot_c == f(t_c, x_c, u_c, h)); + subject_to(xdot_c == f(t_c, x_c, u_c, h)); time += h; } } - void ConstrainDirectTranscription() { + void constrain_direct_transcription() { Variable time = 0.0; - for (int i = 0; i < m_numSteps; ++i) { - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); - auto u = U().Col(i); - Variable dt = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); + Variable dt = this->dt()(0, i); - if (m_dynamicsType == DynamicsType::kExplicitODE) { - SubjectTo(x_end == RK4( - m_dynamicsFunction, x_begin, u, time, dt)); - } else if (m_dynamicsType == DynamicsType::kDiscrete) { - SubjectTo(x_end == m_dynamicsFunction(time, x_begin, u, dt)); + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + subject_to(x_end == rk4( + m_dynamics_function, x_begin, u, time, dt)); + } else if (m_dynamics_type == DynamicsType::DISCRETE) { + subject_to(x_end == m_dynamics_function(time, x_begin, u, dt)); } time += dt; } } - void ConstrainSingleShooting() { + void constrain_single_shooting() { Variable time = 0.0; - for (int i = 0; i < m_numSteps; ++i) { - auto x_begin = X().Col(i); - auto x_end = X().Col(i + 1); - auto u = U().Col(i); - Variable dt = DT()(0, i); + for (int i = 0; i < m_num_steps; ++i) { + auto x_begin = X().col(i); + auto x_end = X().col(i + 1); + auto u = U().col(i); + Variable dt = this->dt()(0, i); - if (m_dynamicsType == DynamicsType::kExplicitODE) { - x_end = RK4(m_dynamicsFunction, x_begin, u, + if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { + x_end = rk4(m_dynamics_function, x_begin, u, time, dt); - } else if (m_dynamicsType == DynamicsType::kDiscrete) { - x_end = m_dynamicsFunction(time, x_begin, u, dt); + } else if (m_dynamics_type == DynamicsType::DISCRETE) { + x_end = m_dynamics_function(time, x_begin, u, dt); } time += dt; } } - int m_numStates; - int m_numInputs; + int m_num_states; + int m_num_inputs; std::chrono::duration m_dt; - int m_numSteps; - TranscriptionMethod m_transcriptionMethod; + int m_num_steps; + TranscriptionMethod m_transcription_method; - DynamicsType m_dynamicsType; + DynamicsType m_dynamics_type; function_ref - m_dynamicsFunction; + m_dynamics_function; - TimestepMethod m_timestepMethod; + TimestepMethod m_timestep_method; VariableMatrix m_X; VariableMatrix m_U; VariableMatrix m_DT; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp deleted file mode 100644 index 66883fed98..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/optimization/solver/InteriorPoint.hpp" -#include "sleipnir/optimization/solver/SQP.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * This class allows the user to pose a constrained nonlinear optimization - * problem in natural mathematical notation and solve it. - * - * This class supports problems of the form: -@verbatim - minₓ f(x) -subject to cₑ(x) = 0 - cᵢ(x) ≥ 0 -@endverbatim - * - * where f(x) is the scalar cost function, x is the vector of decision variables - * (variables the solver can tweak to minimize the cost function), cᵢ(x) are the - * inequality constraints, and cₑ(x) are the equality constraints. Constraints - * are equations or inequalities of the decision variables that constrain what - * values the solver is allowed to use when searching for an optimal solution. - * - * The nice thing about this class is users don't have to put their system in - * the form shown above manually; they can write it in natural mathematical form - * and it'll be converted for them. - */ -class SLEIPNIR_DLLEXPORT OptimizationProblem { - public: - /** - * Construct the optimization problem. - */ - OptimizationProblem() noexcept = default; - - /** - * Create a decision variable in the optimization problem. - */ - [[nodiscard]] - Variable DecisionVariable() { - m_decisionVariables.emplace_back(); - return m_decisionVariables.back(); - } - - /** - * Create a matrix of decision variables in the optimization problem. - * - * @param rows Number of matrix rows. - * @param cols Number of matrix columns. - */ - [[nodiscard]] - VariableMatrix DecisionVariable(int rows, int cols = 1) { - m_decisionVariables.reserve(m_decisionVariables.size() + rows * cols); - - VariableMatrix vars{rows, cols}; - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - m_decisionVariables.emplace_back(); - vars(row, col) = m_decisionVariables.back(); - } - } - - return vars; - } - - /** - * Create a symmetric matrix of decision variables in the optimization - * problem. - * - * Variable instances are reused across the diagonal, which helps reduce - * problem dimensionality. - * - * @param rows Number of matrix rows. - */ - [[nodiscard]] - VariableMatrix SymmetricDecisionVariable(int rows) { - // We only need to store the lower triangle of an n x n symmetric matrix; - // the other elements are duplicates. The lower triangle has (n² + n)/2 - // elements. - // - // n - // Σ k = (n² + n)/2 - // k=1 - m_decisionVariables.reserve(m_decisionVariables.size() + - (rows * rows + rows) / 2); - - VariableMatrix vars{rows, rows}; - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col <= row; ++col) { - m_decisionVariables.emplace_back(); - vars(row, col) = m_decisionVariables.back(); - vars(col, row) = m_decisionVariables.back(); - } - } - - return vars; - } - - /** - * Tells the solver to minimize the output of the given cost function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param cost The cost function to minimize. - */ - void Minimize(const Variable& cost) { - m_f = cost; - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to minimize the output of the given cost function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param cost The cost function to minimize. - */ - void Minimize(Variable&& cost) { - m_f = std::move(cost); - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to maximize the output of the given objective function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param objective The objective function to maximize. - */ - void Maximize(const Variable& objective) { - // Maximizing a cost function is the same as minimizing its negative - m_f = -objective; - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to maximize the output of the given objective function. - * - * Note that this is optional. If only constraints are specified, the solver - * will find the closest solution to the initial conditions that's in the - * feasible set. - * - * @param objective The objective function to maximize. - */ - void Maximize(Variable&& objective) { - // Maximizing a cost function is the same as minimizing its negative - m_f = -std::move(objective); - status.costFunctionType = m_f.value().Type(); - } - - /** - * Tells the solver to solve the problem while satisfying the given equality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(const EqualityConstraints& constraint) { - // Get the highest order equality constraint expression type - for (const auto& c : constraint.constraints) { - status.equalityConstraintType = - std::max(status.equalityConstraintType, c.Type()); - } - - m_equalityConstraints.reserve(m_equalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_equalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given equality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(EqualityConstraints&& constraint) { - // Get the highest order equality constraint expression type - for (const auto& c : constraint.constraints) { - status.equalityConstraintType = - std::max(status.equalityConstraintType, c.Type()); - } - - m_equalityConstraints.reserve(m_equalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_equalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given inequality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(const InequalityConstraints& constraint) { - // Get the highest order inequality constraint expression type - for (const auto& c : constraint.constraints) { - status.inequalityConstraintType = - std::max(status.inequalityConstraintType, c.Type()); - } - - m_inequalityConstraints.reserve(m_inequalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_inequalityConstraints)); - } - - /** - * Tells the solver to solve the problem while satisfying the given inequality - * constraint. - * - * @param constraint The constraint to satisfy. - */ - void SubjectTo(InequalityConstraints&& constraint) { - // Get the highest order inequality constraint expression type - for (const auto& c : constraint.constraints) { - status.inequalityConstraintType = - std::max(status.inequalityConstraintType, c.Type()); - } - - m_inequalityConstraints.reserve(m_inequalityConstraints.size() + - constraint.constraints.size()); - std::copy(constraint.constraints.begin(), constraint.constraints.end(), - std::back_inserter(m_inequalityConstraints)); - } - - /** - * Solve the optimization problem. The solution will be stored in the original - * variables used to construct the problem. - * - * @param config Configuration options for the solver. - */ - SolverStatus Solve(const SolverConfig& config = SolverConfig{}) { - // Create the initial value column vector - Eigen::VectorXd x{m_decisionVariables.size()}; - for (size_t i = 0; i < m_decisionVariables.size(); ++i) { - x(i) = m_decisionVariables[i].Value(); - } - - status.exitCondition = SolverExitCondition::kSuccess; - - // If there's no cost function, make it zero and continue - if (!m_f.has_value()) { - m_f = Variable(); - } - - if (config.diagnostics) { - constexpr std::array kExprTypeToName{"empty", "constant", "linear", - "quadratic", "nonlinear"}; - - // Print cost function and constraint expression types - sleipnir::println( - "The cost function is {}.", - kExprTypeToName[static_cast(status.costFunctionType)]); - sleipnir::println( - "The equality constraints are {}.", - kExprTypeToName[static_cast(status.equalityConstraintType)]); - sleipnir::println( - "The inequality constraints are {}.", - kExprTypeToName[static_cast(status.inequalityConstraintType)]); - sleipnir::println(""); - - // Print problem dimensionality - sleipnir::println("Number of decision variables: {}", - m_decisionVariables.size()); - sleipnir::println("Number of equality constraints: {}", - m_equalityConstraints.size()); - sleipnir::println("Number of inequality constraints: {}\n", - m_inequalityConstraints.size()); - } - - // If the problem is empty or constant, there's nothing to do - if (status.costFunctionType <= ExpressionType::kConstant && - status.equalityConstraintType <= ExpressionType::kConstant && - status.inequalityConstraintType <= ExpressionType::kConstant) { - return status; - } - - // Solve the optimization problem - if (m_inequalityConstraints.empty()) { - SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback, - config, x, &status); - } else { - Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); - InteriorPoint(m_decisionVariables, m_equalityConstraints, - m_inequalityConstraints, m_f.value(), m_callback, config, - false, x, s, &status); - } - - if (config.diagnostics) { - sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition)); - } - - // Assign the solution to the original Variable instances - VariableMatrix{m_decisionVariables}.SetValue(x); - - return status; - } - - /** - * Sets a callback to be called at each solver iteration. - * - * The callback for this overload should return void. - * - * @param callback The callback. - */ - template - requires requires(F callback, const SolverIterationInfo& info) { - { callback(info) } -> std::same_as; - } - void Callback(F&& callback) { - m_callback = [=, callback = std::forward(callback)]( - const SolverIterationInfo& info) { - callback(info); - return false; - }; - } - - /** - * Sets a callback to be called at each solver iteration. - * - * The callback for this overload should return bool. - * - * @param callback The callback. Returning true from the callback causes the - * solver to exit early with the solution it has so far. - */ - template - requires requires(F callback, const SolverIterationInfo& info) { - { callback(info) } -> std::same_as; - } - void Callback(F&& callback) { - m_callback = std::forward(callback); - } - - private: - // The list of decision variables, which are the root of the problem's - // expression tree - wpi::SmallVector m_decisionVariables; - - // The cost function: f(x) - std::optional m_f; - - // The list of equality constraints: cₑ(x) = 0 - wpi::SmallVector m_equalityConstraints; - - // The list of inequality constraints: cᵢ(x) ≥ 0 - wpi::SmallVector m_inequalityConstraints; - - // The user callback - std::function m_callback = - [](const SolverIterationInfo&) { return false; }; - - // The solver status - SolverStatus status; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp deleted file mode 100644 index f7323f7388..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Solver configuration. - */ -struct SLEIPNIR_DLLEXPORT SolverConfig { - /// The solver will stop once the error is below this tolerance. - double tolerance = 1e-8; - - /// The maximum number of solver iterations before returning a solution. - int maxIterations = 5000; - - /// The solver will stop once the error is below this tolerance for - /// `acceptableIterations` iterations. This is useful in cases where the - /// solver might not be able to achieve the desired level of accuracy due to - /// floating-point round-off. - double acceptableTolerance = 1e-6; - - /// The solver will stop once the error is below `acceptableTolerance` for - /// this many iterations. - int maxAcceptableIterations = 15; - - /// The maximum elapsed wall clock time before returning a solution. - std::chrono::duration timeout{ - std::numeric_limits::infinity()}; - - /// Enables the feasible interior-point method. When the inequality - /// constraints are all feasible, step sizes are reduced when necessary to - /// prevent them becoming infeasible again. This is useful when parts of the - /// problem are ill-conditioned in infeasible regions (e.g., square root of a - /// negative value). This can slow or prevent progress toward a solution - /// though, so only enable it if necessary. - bool feasibleIPM = false; - - /// Enables diagnostic prints. - bool diagnostics = false; - - /// Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files named H.spy, - /// A_e.spy, and A_i.spy respectively during solve. - /// - /// Use tools/spy.py to plot them. - bool spy = false; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp deleted file mode 100644 index 7d1445297e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Solver exit condition. - */ -enum class SolverExitCondition : int8_t { - /// Solved the problem to the desired tolerance. - kSuccess = 0, - /// Solved the problem to an acceptable tolerance, but not the desired one. - kSolvedToAcceptableTolerance = 1, - /// The solver returned its solution so far after the user requested a stop. - kCallbackRequestedStop = 2, - /// The solver determined the problem to be overconstrained and gave up. - kTooFewDOFs = -1, - /// The solver determined the problem to be locally infeasible and gave up. - kLocallyInfeasible = -2, - /// The solver failed to reach the desired tolerance, and feasibility - /// restoration failed to converge. - kFeasibilityRestorationFailed = -3, - /// The solver encountered nonfinite initial cost or constraints and gave up. - kNonfiniteInitialCostOrConstraints = -4, - /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up. - kDivergingIterates = -5, - /// The solver returned its solution so far after exceeding the maximum number - /// of iterations. - kMaxIterationsExceeded = -6, - /// The solver returned its solution so far after exceeding the maximum - /// elapsed wall clock time. - kTimeout = -7 -}; - -/** - * Returns user-readable message corresponding to the exit condition. - * - * @param exitCondition Solver exit condition. - */ -SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage( - const SolverExitCondition& exitCondition) { - using enum SolverExitCondition; - - switch (exitCondition) { - case kSuccess: - return "solved to desired tolerance"; - case kSolvedToAcceptableTolerance: - return "solved to acceptable tolerance"; - case kCallbackRequestedStop: - return "callback requested stop"; - case kTooFewDOFs: - return "problem has too few degrees of freedom"; - case kLocallyInfeasible: - return "problem is locally infeasible"; - case kFeasibilityRestorationFailed: - return "solver failed to reach the desired tolerance, and feasibility " - "restoration failed to converge"; - case kNonfiniteInitialCostOrConstraints: - return "solver encountered nonfinite initial cost or constraints and " - "gave up"; - case kDivergingIterates: - return "solver encountered diverging primal iterates xₖ and/or sₖ and " - "gave up"; - case kMaxIterationsExceeded: - return "solution returned after maximum iterations exceeded"; - case kTimeout: - return "solution returned after maximum wall clock time exceeded"; - default: - return "unknown"; - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp deleted file mode 100644 index 122941cda7..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include "sleipnir/autodiff/ExpressionType.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Return value of OptimizationProblem::Solve() containing the cost function and - * constraint types and solver's exit condition. - */ -struct SLEIPNIR_DLLEXPORT SolverStatus { - /// The cost function type detected by the solver. - ExpressionType costFunctionType = ExpressionType::kNone; - - /// The equality constraint type detected by the solver. - ExpressionType equalityConstraintType = ExpressionType::kNone; - - /// The inequality constraint type detected by the solver. - ExpressionType inequalityConstraintType = ExpressionType::kNone; - - /// The solver's exit condition. - SolverExitCondition exitCondition = SolverExitCondition::kSuccess; - - /// The solution's cost. - double cost = 0.0; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp similarity index 54% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp index 09b1b2f3bf..ebe177ef21 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp @@ -6,12 +6,12 @@ #include #include -#include +#include -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/util/function_ref.hpp" -namespace sleipnir { +namespace slp { /** * The result of a multistart solve. @@ -21,7 +21,11 @@ namespace sleipnir { */ template struct MultistartResult { - SolverStatus status; + /// The solver exit status. + ExitStatus status; + /// The solution's cost. + double cost; + /// The decision variables. DecisionVariables variables; }; @@ -37,39 +41,37 @@ struct MultistartResult { * guess. * @param solve A user-provided function that takes a decision variable initial * guess and returns a MultistartResult. - * @param initialGuesses A list of decision variable initial guesses to try. + * @param initial_guesses A list of decision variable initial guesses to try. */ template MultistartResult Multistart( function_ref( - const DecisionVariables& initialGuess)> + const DecisionVariables& initial_guess)> solve, - std::span initialGuesses) { - wpi::SmallVector>> futures; - futures.reserve(initialGuesses.size()); + std::span initial_guesses) { + gch::small_vector>> futures; + futures.reserve(initial_guesses.size()); - for (const auto& initialGuess : initialGuesses) { - futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); + for (const auto& initial_guess : initial_guesses) { + futures.emplace_back(std::async(std::launch::async, solve, initial_guess)); } - wpi::SmallVector> results; + gch::small_vector> results; results.reserve(futures.size()); for (auto& future : futures) { results.emplace_back(future.get()); } - return *std::min_element( - results.cbegin(), results.cend(), [](const auto& a, const auto& b) { - // Prioritize successful solve - if (a.status.exitCondition == SolverExitCondition::kSuccess && - b.status.exitCondition != SolverExitCondition::kSuccess) { - return true; - } + return *std::ranges::min_element(results, [](const auto& a, const auto& b) { + // Prioritize successful solve + if (a.status == ExitStatus::SUCCESS && b.status != ExitStatus::SUCCESS) { + return true; + } - // Otherwise prioritize solution with lower cost - return a.status.cost < b.status.cost; - }); + // Otherwise prioritize solution with lower cost + return a.cost < b.cost; + }); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp new file mode 100644 index 0000000000..b484ec08d6 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -0,0 +1,342 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * This class allows the user to pose a constrained nonlinear optimization + * problem in natural mathematical notation and solve it. + * + * This class supports problems of the form: +@verbatim + minₓ f(x) +subject to cₑ(x) = 0 + cᵢ(x) ≥ 0 +@endverbatim + * + * where f(x) is the scalar cost function, x is the vector of decision variables + * (variables the solver can tweak to minimize the cost function), cᵢ(x) are the + * inequality constraints, and cₑ(x) are the equality constraints. Constraints + * are equations or inequalities of the decision variables that constrain what + * values the solver is allowed to use when searching for an optimal solution. + * + * The nice thing about this class is users don't have to put their system in + * the form shown above manually; they can write it in natural mathematical form + * and it'll be converted for them. + */ +class SLEIPNIR_DLLEXPORT Problem { + public: + /** + * Construct the optimization problem. + */ + Problem() noexcept = default; + + /** + * Create a decision variable in the optimization problem. + * + * @return A decision variable in the optimization problem. + */ + [[nodiscard]] + Variable decision_variable() { + m_decision_variables.emplace_back(); + return m_decision_variables.back(); + } + + /** + * Create a matrix of decision variables in the optimization problem. + * + * @param rows Number of matrix rows. + * @param cols Number of matrix columns. + * @return A matrix of decision variables in the optimization problem. + */ + [[nodiscard]] + VariableMatrix decision_variable(int rows, int cols = 1) { + m_decision_variables.reserve(m_decision_variables.size() + rows * cols); + + VariableMatrix vars{rows, cols}; + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + m_decision_variables.emplace_back(); + vars(row, col) = m_decision_variables.back(); + } + } + + return vars; + } + + /** + * Create a symmetric matrix of decision variables in the optimization + * problem. + * + * Variable instances are reused across the diagonal, which helps reduce + * problem dimensionality. + * + * @param rows Number of matrix rows. + * @return A symmetric matrix of decision varaibles in the optimization + * problem. + */ + [[nodiscard]] + VariableMatrix symmetric_decision_variable(int rows) { + // We only need to store the lower triangle of an n x n symmetric matrix; + // the other elements are duplicates. The lower triangle has (n² + n)/2 + // elements. + // + // n + // Σ k = (n² + n)/2 + // k=1 + m_decision_variables.reserve(m_decision_variables.size() + + (rows * rows + rows) / 2); + + VariableMatrix vars{rows, rows}; + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col <= row; ++col) { + m_decision_variables.emplace_back(); + vars(row, col) = m_decision_variables.back(); + vars(col, row) = m_decision_variables.back(); + } + } + + return vars; + } + + /** + * Tells the solver to minimize the output of the given cost function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param cost The cost function to minimize. + */ + void minimize(const Variable& cost) { m_f = cost; } + + /** + * Tells the solver to minimize the output of the given cost function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param cost The cost function to minimize. + */ + void minimize(Variable&& cost) { m_f = std::move(cost); } + + /** + * Tells the solver to maximize the output of the given objective function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param objective The objective function to maximize. + */ + void maximize(const Variable& objective) { + // Maximizing a cost function is the same as minimizing its negative + m_f = -objective; + } + + /** + * Tells the solver to maximize the output of the given objective function. + * + * Note that this is optional. If only constraints are specified, the solver + * will find the closest solution to the initial conditions that's in the + * feasible set. + * + * @param objective The objective function to maximize. + */ + void maximize(Variable&& objective) { + // Maximizing a cost function is the same as minimizing its negative + m_f = -std::move(objective); + } + + /** + * Tells the solver to solve the problem while satisfying the given equality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(const EqualityConstraints& constraint) { + m_equality_constraints.reserve(m_equality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_equality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given equality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(EqualityConstraints&& constraint) { + m_equality_constraints.reserve(m_equality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_equality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given inequality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(const InequalityConstraints& constraint) { + m_inequality_constraints.reserve(m_inequality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_inequality_constraints)); + } + + /** + * Tells the solver to solve the problem while satisfying the given inequality + * constraint. + * + * @param constraint The constraint to satisfy. + */ + void subject_to(InequalityConstraints&& constraint) { + m_inequality_constraints.reserve(m_inequality_constraints.size() + + constraint.constraints.size()); + std::ranges::copy(constraint.constraints, + std::back_inserter(m_inequality_constraints)); + } + + /** + * Returns the cost function's type. + * + * @return The cost function's type. + */ + ExpressionType cost_function_type() const { + if (m_f) { + return m_f.value().type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Returns the type of the highest order equality constraint. + * + * @return The type of the highest order equality constraint. + */ + ExpressionType equality_constraint_type() const { + if (!m_equality_constraints.empty()) { + return std::ranges::max(m_equality_constraints, {}, &Variable::type) + .type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Returns the type of the highest order inequality constraint. + * + * @return The type of the highest order inequality constraint. + */ + ExpressionType inequality_constraint_type() const { + if (!m_inequality_constraints.empty()) { + return std::ranges::max(m_inequality_constraints, {}, &Variable::type) + .type(); + } else { + return ExpressionType::NONE; + } + } + + /** + * Solve the optimization problem. The solution will be stored in the original + * variables used to construct the problem. + * + * @param options Solver options. + * @param spy Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files + * named H.spy, A_e.spy, and A_i.spy respectively during solve. Use + * tools/spy.py to plot them. + * @return The solver status. + */ + ExitStatus solve(const Options& options = Options{}, + [[maybe_unused]] bool spy = false); + + /** + * Adds a callback to be called at the beginning of each solver iteration. + * + * The callback for this overload should return void. + * + * @param callback The callback. + */ + template + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_callback(F&& callback) { + m_iteration_callbacks.emplace_back( + [=, callback = std::forward(callback)](const IterationInfo& info) { + callback(info); + return false; + }); + } + + /** + * Adds a callback to be called at the beginning of each solver iteration. + * + * The callback for this overload should return bool. + * + * @param callback The callback. Returning true from the callback causes the + * solver to exit early with the solution it has so far. + */ + template + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_callback(F&& callback) { + m_iteration_callbacks.emplace_back(std::forward(callback)); + } + + /** + * Clears the registered callbacks. + */ + void clear_callbacks() { m_iteration_callbacks.clear(); } + + private: + // The list of decision variables, which are the root of the problem's + // expression tree + gch::small_vector m_decision_variables; + + // The cost function: f(x) + std::optional m_f; + + // The list of equality constraints: cₑ(x) = 0 + gch::small_vector m_equality_constraints; + + // The list of inequality constraints: cᵢ(x) ≥ 0 + gch::small_vector m_inequality_constraints; + + // The iteration callbacks + gch::small_vector> + m_iteration_callbacks; + + void print_exit_conditions([[maybe_unused]] const Options& options); + void print_problem_analysis(); +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp deleted file mode 100644 index 51d8f97305..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** -Finds the optimal solution to a nonlinear program using the interior-point -method. - -A nonlinear program has the form: - -@verbatim - min_x f(x) -subject to cₑ(x) = 0 - cᵢ(x) ≥ 0 -@endverbatim - -where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) -are the inequality constraints. - -@param[in] decisionVariables The list of decision variables. -@param[in] equalityConstraints The list of equality constraints. -@param[in] inequalityConstraints The list of inequality constraints. -@param[in] f The cost function. -@param[in] callback The user callback. -@param[in] config Configuration options for the solver. -@param[in] feasibilityRestoration Whether to use feasibility restoration instead - of the normal algorithm. -@param[in,out] x The initial guess and output location for the decision - variables. -@param[in,out] s The initial guess and output location for the inequality - constraint slack variables. -@param[out] status The solver status. -*/ -SLEIPNIR_DLLEXPORT void InteriorPoint( - std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x, - Eigen::VectorXd& s, SolverStatus* status); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp deleted file mode 100644 index ed10ffedff..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** -Finds the optimal solution to a nonlinear program using Sequential Quadratic -Programming (SQP). - -A nonlinear program has the form: - -@verbatim - min_x f(x) -subject to cₑ(x) = 0 -@endverbatim - -where f(x) is the cost function and cₑ(x) are the equality constraints. - -@param[in] decisionVariables The list of decision variables. -@param[in] equalityConstraints The list of equality constraints. -@param[in] f The cost function. -@param[in] callback The user callback. -@param[in] config Configuration options for the solver. -@param[in,out] x The initial guess and output location for the decision - variables. -@param[out] status The solver status. -*/ -SLEIPNIR_DLLEXPORT void SQP( - std::span decisionVariables, - std::span equalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status); - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp new file mode 100644 index 0000000000..f3acdcaf2d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp @@ -0,0 +1,82 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Solver exit status. Negative values indicate failure. + */ +enum class ExitStatus : int8_t { + /// Solved the problem to the desired tolerance. + SUCCESS = 0, + /// The solver returned its solution so far after the user requested a stop. + CALLBACK_REQUESTED_STOP = 1, + /// The solver determined the problem to be overconstrained and gave up. + TOO_FEW_DOFS = -1, + /// The solver determined the problem to be locally infeasible and gave up. + LOCALLY_INFEASIBLE = -2, + /// The problem setup frontend determined the problem to have an empty + /// feasible region. + GLOBALLY_INFEASIBLE = -3, + /// The linear system factorization failed. + FACTORIZATION_FAILED = -4, + /// The backtracking line search failed, and the problem isn't locally + /// infeasible. + LINE_SEARCH_FAILED = -5, + /// The solver encountered nonfinite initial cost or constraints and gave up. + NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6, + /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up. + DIVERGING_ITERATES = -7, + /// The solver returned its solution so far after exceeding the maximum number + /// of iterations. + MAX_ITERATIONS_EXCEEDED = -8, + /// The solver returned its solution so far after exceeding the maximum + /// elapsed wall clock time. + TIMEOUT = -9, +}; + +/** + * Returns user-readable message corresponding to the solver exit status. + * + * @param exit_status Solver exit status. + */ +SLEIPNIR_DLLEXPORT constexpr std::string_view to_message( + const ExitStatus& exit_status) { + using enum ExitStatus; + + switch (exit_status) { + case SUCCESS: + return "success"; + case CALLBACK_REQUESTED_STOP: + return "callback requested stop"; + case TOO_FEW_DOFS: + return "too few degrees of freedom"; + case LOCALLY_INFEASIBLE: + return "locally infeasible"; + case GLOBALLY_INFEASIBLE: + return "globally infeasible"; + case FACTORIZATION_FAILED: + return "factorization failed"; + case LINE_SEARCH_FAILED: + return "line search failed"; + case NONFINITE_INITIAL_COST_OR_CONSTRAINTS: + return "nonfinite initial cost or constraints"; + case DIVERGING_ITERATES: + return "diverging iterates"; + case MAX_ITERATIONS_EXCEEDED: + return "max iterations exceeded"; + case TIMEOUT: + return "timeout"; + default: + return "unknown"; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp new file mode 100644 index 0000000000..2777f9e833 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp @@ -0,0 +1,232 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the interior-point method solver. + */ +struct SLEIPNIR_DLLEXPORT InteriorPointMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter. + /// + /// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
znum_inequality_constraints1
∇ₓₓ²L(x, y, z)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x, + const Eigen::VectorXd& y, + const Eigen::VectorXd& z)> + H; + + /// Equality constraint value cₑ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cₑ(x)num_equality_constraints1
+ std::function c_e; + + /// Equality constraint Jacobian ∂cₑ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcₑ₁(xₖ)] + /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aₑ(x)num_equality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_e; + + /// Inequality constraint value cᵢ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cᵢ(x)num_inequality_constraints1
+ std::function c_i; + + /// Inequality constraint Jacobian ∂cᵢ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcᵢ₁(xₖ)] + /// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcᵢₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aᵢ(x)num_inequality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_i; +}; + +/** +Finds the optimal solution to a nonlinear program using the interior-point +method. + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 + cᵢ(x) ≥ 0 +@endverbatim + +where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) +are the inequality constraints. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + const Eigen::ArrayX& bound_constraint_mask, +#endif + Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp similarity index 72% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp index bb915b856e..027f4e5a0c 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp @@ -5,21 +5,18 @@ #include #include -namespace sleipnir { +namespace slp { /** - * Solver iteration information exposed to a user callback. + * Solver iteration information exposed to an iteration callback. */ -struct SolverIterationInfo { +struct IterationInfo { /// The solver iteration. int iteration; /// The decision variables. const Eigen::VectorXd& x; - /// The inequality constraint slack variables. - const Eigen::VectorXd& s; - /// The gradient of the cost function. const Eigen::SparseVector& g; @@ -33,4 +30,4 @@ struct SolverIterationInfo { const Eigen::SparseMatrix& A_i; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp new file mode 100644 index 0000000000..210ed87475 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp @@ -0,0 +1,113 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the Newton's method solver. + */ +struct SLEIPNIR_DLLEXPORT NewtonMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x) getter. + /// + /// L(xₖ) = f(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇ₓₓ²L(x)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> H; +}; + +/** +Finds the optimal solution to a nonlinear program using Newton's method. + +A nonlinear program has the form: + +@verbatim + min_x f(x) +@endverbatim + +where f(x) is the cost function. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +newton(const NewtonMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp new file mode 100644 index 0000000000..8b9cb93400 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp @@ -0,0 +1,94 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Solver options. + */ +struct SLEIPNIR_DLLEXPORT Options { + /// The solver will stop once the error is below this tolerance. + double tolerance = 1e-8; + + /// The maximum number of solver iterations before returning a solution. + int max_iterations = 5000; + + /// The maximum elapsed wall clock time before returning a solution. + std::chrono::duration timeout{ + std::numeric_limits::infinity()}; + + /// Enables the feasible interior-point method. When the inequality + /// constraints are all feasible, step sizes are reduced when necessary to + /// prevent them becoming infeasible again. This is useful when parts of the + /// problem are ill-conditioned in infeasible regions (e.g., square root of a + /// negative value). This can slow or prevent progress toward a solution + /// though, so only enable it if necessary. + bool feasible_ipm = false; + + /// Enables diagnostic prints. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
HeadingDescription
iterIteration number
typeIteration type (normal, accepted second-order correction, rejected + /// second-order correction)
time (ms)Duration of iteration in milliseconds
errorError estimate
costCost function value at current iterate
infeas.Constraint infeasibility at current iterate
complement.Complementary slackness at current iterate (sᵀz)
μBarrier parameter
regIteration matrix regularization
primal αPrimal step size
dual αDual step size
↩Number of line search backtracks
+ bool diagnostics = false; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp new file mode 100644 index 0000000000..c1dfb00bf1 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp @@ -0,0 +1,171 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Matrix callbacks for the Sequential Quadratic Programming (SQP) solver. + */ +struct SLEIPNIR_DLLEXPORT SQPMatrixCallbacks { + /// Cost function value f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
f(x)11
+ std::function f; + + /// Cost function gradient ∇f(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
∇f(x)num_decision_variables1
+ std::function(const Eigen::VectorXd& x)> g; + + /// Lagrangian Hessian ∇ₓₓ²L(x, y) getter. + /// + /// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
∇ₓₓ²L(x, y)num_decision_variablesnum_decision_variables
+ std::function(const Eigen::VectorXd& x, + const Eigen::VectorXd& y)> + H; + + /// Equality constraint value cₑ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
cₑ(x)num_equality_constraints1
+ std::function c_e; + + /// Equality constraint Jacobian ∂cₑ/∂x getter. + /// + /// @verbatim + /// [∇ᵀcₑ₁(xₖ)] + /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(xₖ)] + /// @endverbatim + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
Aₑ(x)num_equality_constraintsnum_decision_variables
+ std::function(const Eigen::VectorXd& x)> A_e; +}; + +/** +Finds the optimal solution to a nonlinear program using Sequential Quadratic +Programming (SQP). + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 +@endverbatim + +where f(x) is the cost function and cₑ(x) are the equality constraints. + +@param[in] matrix_callbacks Matrix callbacks. +@param[in] iteration_callbacks The list of callbacks to call at the beginning of + each iteration. +@param[in] options Solver options. +@param[in,out] x The initial guess and output location for the decision + variables. +@return The exit status. +*/ +SLEIPNIR_DLLEXPORT ExitStatus +sqp(const SQPMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x); + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp deleted file mode 100644 index ba381ef8f4..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#ifdef JORMUNGANDR -#include -#include -/** - * Throw an exception in Python. - */ -#define Assert(condition) \ - do { \ - if (!(condition)) { \ - throw std::invalid_argument( \ - std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ - __func__, #condition)); \ - } \ - } while (0); -#else -#include -/** - * Abort in C++. - */ -#define Assert(condition) assert(condition) -#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp deleted file mode 100644 index 723284117e..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include - -namespace sleipnir { - -template -concept ScalarLike = requires(T t) { - t + 1.0; - t = 1.0; -}; - -template -concept SleipnirMatrixLike = requires(T t, int rows, int cols) { - t.Rows(); - t.Cols(); - t(rows, cols); -}; - -template -concept EigenMatrixLike = std::derived_from>; - -template -concept MatrixLike = SleipnirMatrixLike || EigenMatrixLike; - -template -concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); }; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp deleted file mode 100644 index 7f526a2d99..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -#include -#include - -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Write the sparsity pattern of a sparse matrix to a file. - * - * Each character represents an element with '.' representing zero, '+' - * representing positive, and '-' representing negative. Here's an example for a - * 3x3 identity matrix. - * - * "+.." - * ".+." - * "..+" - * - * @param[out] file A file stream. - * @param[in] mat The sparse matrix. - */ -SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file, - const Eigen::SparseMatrix& mat) { - const int cells_width = mat.cols() + 1; - const int cells_height = mat.rows(); - - wpi::SmallVector cells; - - // Allocate space for matrix of characters plus trailing newlines - cells.reserve(cells_width * cells_height); - - // Initialize cell array - for (int row = 0; row < mat.rows(); ++row) { - for (int col = 0; col < mat.cols(); ++col) { - cells.emplace_back('.'); - } - cells.emplace_back('\n'); - } - - // Fill in non-sparse entries - for (int k = 0; k < mat.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it{mat, k}; it; ++it) { - if (it.value() < 0.0) { - cells[it.row() * cells_width + it.col()] = '-'; - } else if (it.value() > 0.0) { - cells[it.row() * cells_width + it.col()] = '+'; - } - } - } - - // Write cell array to file - for (const auto& c : cells) { - file << c; - } -} - -/** - * Write the sparsity pattern of a sparse matrix to a file. - * - * Each character represents an element with "." representing zero, "+" - * representing positive, and "-" representing negative. Here's an example for a - * 3x3 identity matrix. - * - * "+.." - * ".+." - * "..+" - * - * @param[in] filename The filename. - * @param[in] mat The sparse matrix. - */ -SLEIPNIR_DLLEXPORT inline void Spy(std::string_view filename, - const Eigen::SparseMatrix& mat) { - std::ofstream file{std::string{filename}}; - if (!file.is_open()) { - return; - } - - Spy(file, mat); -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp new file mode 100644 index 0000000000..75d8ffca32 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -0,0 +1,27 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#ifdef JORMUNGANDR +#include +#include +#include +/** + * Throw an exception in Python. + */ +#define slp_assert(condition) \ + do { \ + if (!(condition)) { \ + auto location = std::source_location::current(); \ + throw std::invalid_argument(std::format( \ + "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ + location.line(), location.function_name(), #condition)); \ + } \ + } while (0); +#else +#include +/** + * Abort in C++. + */ +#define slp_assert(condition) assert(condition) +#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp new file mode 100644 index 0000000000..38fac92dcd --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp @@ -0,0 +1,42 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include + +namespace slp { + +template +concept ScalarLike = requires(std::decay_t t) { + t + 1.0; + t = 1.0; +}; + +template +concept SleipnirScalarLike = requires(std::decay_t t) { + t + 1.0; + t = 1.0; + { t.value() } -> std::same_as; +}; + +template +concept EigenMatrixLike = + std::derived_from, Eigen::MatrixBase>>; + +template +concept SleipnirMatrixLike = requires(std::decay_t t, int rows, int cols) { + t.rows(); + t.cols(); + { t.value() } -> std::same_as; +} && !EigenMatrixLike; + +template +concept SleipnirType = SleipnirScalarLike || SleipnirMatrixLike; + +template +concept MatrixLike = SleipnirMatrixLike || EigenMatrixLike; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp similarity index 86% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp index 14a46903dc..547dfc6ed8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp @@ -7,15 +7,15 @@ #include #include -namespace sleipnir { +namespace slp { + +template +class function_ref; /** * An implementation of std::function_ref, a lightweight non-owning reference to * a callable. */ -template -class function_ref; - template class function_ref { public: @@ -23,12 +23,17 @@ class function_ref { /** * Creates a `function_ref` which refers to the same callable as `rhs`. + * + * @param rhs Other `function_ref`. */ constexpr function_ref(const function_ref& rhs) noexcept = default; /** * Constructs a `function_ref` referring to `f`. + * + * @tparam F Callable type. + * @param f Callable to which to refer. */ template requires(!std::is_same_v, function_ref> && @@ -45,12 +50,18 @@ class function_ref { /** * Makes `*this` refer to the same callable as `rhs`. + * + * @param rhs Other `function_ref`. + * @return `*this` */ constexpr function_ref& operator=( const function_ref& rhs) noexcept = default; /** * Makes `*this` refer to `f`. + * + * @param f Callable to which to refer. + * @return `*this` */ template requires std::is_invocable_r_v @@ -67,6 +78,8 @@ class function_ref { /** * Swaps the referred callables of `*this` and `rhs`. + * + * @param rhs Other `function_ref`. */ constexpr void swap(function_ref& rhs) noexcept { std::swap(obj_, rhs.obj_); @@ -75,6 +88,9 @@ class function_ref { /** * Call the stored callable with the given arguments. + * + * @param args The arguments. + * @return The return value of the callable. */ R operator()(Args... args) const { return callback_(obj_, std::forward(args)...); @@ -97,4 +113,4 @@ constexpr void swap(function_ref& lhs, template function_ref(R (*)(Args...)) -> function_ref; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp similarity index 60% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp index f1290e5837..b3f74e7bde 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp @@ -2,11 +2,12 @@ #pragma once +#include #include #include #include -namespace sleipnir { +namespace slp { /** * A custom intrusive shared pointer implementation without thread @@ -16,17 +17,20 @@ namespace sleipnir { * * 1. A zero-initialized public counter variable that serves as the shared * pointer's reference count. - * 2. A free function `void IntrusiveSharedPtrIncRefCount(T*)` that increments - * the reference count. - * 3. A free function `void IntrusiveSharedPtrDecRefCount(T*)` that decrements - * the reference count and deallocates the pointed to object if the reference - * count reaches zero. + * 2. A free function `void inc_ref_count(T*)` that increments the reference + * count. + * 3. A free function `void dec_ref_count(T*)` that decrements the reference + * count and deallocates the pointed to object if the reference count reaches + * zero. * * @tparam T The type of the object to be reference counted. */ template class IntrusiveSharedPtr { public: + template + friend class IntrusiveSharedPtr; + /** * Constructs an empty intrusive shared pointer. */ @@ -40,31 +44,53 @@ class IntrusiveSharedPtr { /** * Constructs an intrusive shared pointer from the given pointer and takes * ownership. + * + * @param ptr The pointer of which to take ownership. */ explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} { if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); } } constexpr ~IntrusiveSharedPtr() { if (m_ptr != nullptr) { - IntrusiveSharedPtrDecRefCount(m_ptr); + dec_ref_count(m_ptr); } } /** * Copy constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. */ constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr& rhs) noexcept : m_ptr{rhs.m_ptr} { if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); + } + } + + /** + * Copy constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr( // NOLINT + const IntrusiveSharedPtr& rhs) noexcept + : m_ptr{rhs.m_ptr} { + if (m_ptr != nullptr) { + inc_ref_count(m_ptr); } } /** * Makes a copy of the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ constexpr IntrusiveSharedPtr& operator=( // NOLINT const IntrusiveSharedPtr& rhs) noexcept { @@ -73,13 +99,40 @@ class IntrusiveSharedPtr { } if (m_ptr != nullptr) { - IntrusiveSharedPtrDecRefCount(m_ptr); + dec_ref_count(m_ptr); } m_ptr = rhs.m_ptr; if (m_ptr != nullptr) { - IntrusiveSharedPtrIncRefCount(m_ptr); + inc_ref_count(m_ptr); + } + + return *this; + } + + /** + * Makes a copy of the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr& operator=( // NOLINT + const IntrusiveSharedPtr& rhs) noexcept { + if (m_ptr == rhs.m_ptr) { + return *this; + } + + if (m_ptr != nullptr) { + dec_ref_count(m_ptr); + } + + m_ptr = rhs.m_ptr; + + if (m_ptr != nullptr) { + inc_ref_count(m_ptr); } return *this; @@ -87,12 +140,28 @@ class IntrusiveSharedPtr { /** * Move constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. */ constexpr IntrusiveSharedPtr(IntrusiveSharedPtr&& rhs) noexcept : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {} + /** + * Move constructs from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + */ + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr( // NOLINT + IntrusiveSharedPtr&& rhs) noexcept + : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {} + /** * Move assigns from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ constexpr IntrusiveSharedPtr& operator=( IntrusiveSharedPtr&& rhs) noexcept { @@ -106,28 +175,58 @@ class IntrusiveSharedPtr { } /** - * Returns the internal pointer. + * Move assigns from the given intrusive shared pointer. + * + * @param rhs The other intrusive shared pointer. + * @return This intrusive shared pointer. */ - constexpr T* Get() const noexcept { return m_ptr; } + template + requires(!std::same_as && std::convertible_to) + constexpr IntrusiveSharedPtr& operator=( + IntrusiveSharedPtr&& rhs) noexcept { + if (m_ptr == rhs.m_ptr) { + return *this; + } + + std::swap(m_ptr, rhs.m_ptr); + + return *this; + } + + /** + * Returns the internal pointer. + * + * @return The internal pointer. + */ + constexpr T* get() const noexcept { return m_ptr; } /** * Returns the object pointed to by the internal pointer. + * + * @return The object pointed to by the internal pointer. */ constexpr T& operator*() const noexcept { return *m_ptr; } /** * Returns the internal pointer. + * + * @return The internal pointer. */ constexpr T* operator->() const noexcept { return m_ptr; } /** * Returns true if the internal pointer isn't nullptr. + * + * @return True if the internal pointer isn't nullptr. */ explicit constexpr operator bool() const noexcept { return m_ptr != nullptr; } /** * Returns true if the given intrusive shared pointers point to the same * object. + * + * @param lhs The left-hand side. + * @param rhs The right-hand side. */ friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, const IntrusiveSharedPtr& rhs) noexcept { @@ -137,6 +236,9 @@ class IntrusiveSharedPtr { /** * Returns true if the given intrusive shared pointers point to different * objects. + * + * @param lhs The left-hand side. + * @param rhs The right-hand side. */ friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, const IntrusiveSharedPtr& rhs) noexcept { @@ -145,6 +247,8 @@ class IntrusiveSharedPtr { /** * Returns true if the left-hand intrusive shared pointer points to nullptr. + * + * @param lhs The left-hand side. */ friend constexpr bool operator==(const IntrusiveSharedPtr& lhs, std::nullptr_t) noexcept { @@ -153,6 +257,8 @@ class IntrusiveSharedPtr { /** * Returns true if the right-hand intrusive shared pointer points to nullptr. + * + * @param rhs The right-hand side. */ friend constexpr bool operator==(std::nullptr_t, const IntrusiveSharedPtr& rhs) noexcept { @@ -162,6 +268,8 @@ class IntrusiveSharedPtr { /** * Returns true if the left-hand intrusive shared pointer doesn't point to * nullptr. + * + * @param lhs The left-hand side. */ friend constexpr bool operator!=(const IntrusiveSharedPtr& lhs, std::nullptr_t) noexcept { @@ -171,6 +279,8 @@ class IntrusiveSharedPtr { /** * Returns true if the right-hand intrusive shared pointer doesn't point to * nullptr. + * + * @param rhs The right-hand side. */ friend constexpr bool operator!=(std::nullptr_t, const IntrusiveSharedPtr& rhs) noexcept { @@ -190,7 +300,7 @@ class IntrusiveSharedPtr { * @param args Constructor arguments for T. */ template -IntrusiveSharedPtr MakeIntrusiveShared(Args&&... args) { +IntrusiveSharedPtr make_intrusive_shared(Args&&... args) { return IntrusiveSharedPtr{new T(std::forward(args)...)}; } @@ -206,11 +316,11 @@ IntrusiveSharedPtr MakeIntrusiveShared(Args&&... args) { * @param args Constructor arguments for T. */ template -IntrusiveSharedPtr AllocateIntrusiveShared(Alloc alloc, Args&&... args) { +IntrusiveSharedPtr allocate_intrusive_shared(Alloc alloc, Args&&... args) { auto ptr = std::allocator_traits::allocate(alloc, sizeof(T)); std::allocator_traits::construct(alloc, ptr, std::forward(args)...); return IntrusiveSharedPtr{ptr}; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp similarity index 58% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp index 1951bd1ee8..1386056b40 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp @@ -5,11 +5,11 @@ #include #include -#include +#include -#include "sleipnir/util/SymbolExports.hpp" +#include "sleipnir/util/symbol_exports.hpp" -namespace sleipnir { +namespace slp { /** * This class implements a pool memory resource. @@ -23,14 +23,33 @@ class SLEIPNIR_DLLEXPORT PoolResource { /** * Constructs a default PoolResource. * - * @param blocksPerChunk Number of blocks per chunk of memory. + * @param blocks_per_chunk Number of blocks per chunk of memory. */ - explicit PoolResource(size_t blocksPerChunk) - : blocksPerChunk{blocksPerChunk} {} + explicit PoolResource(size_t blocks_per_chunk) + : blocks_per_chunk{blocks_per_chunk} {} + /** + * Copy constructor. + */ PoolResource(const PoolResource&) = delete; + + /** + * Copy assignment operator. + * + * @return This pool resource. + */ PoolResource& operator=(const PoolResource&) = delete; + + /** + * Move constructor. + */ PoolResource(PoolResource&&) = default; + + /** + * Move assignment operator. + * + * @return This pool resource. + */ PoolResource& operator=(PoolResource&&) = default; /** @@ -38,16 +57,17 @@ class SLEIPNIR_DLLEXPORT PoolResource { * * @param bytes Number of bytes in the block. * @param alignment Alignment of the block (unused). + * @return A block of memory from the pool. */ [[nodiscard]] void* allocate(size_t bytes, [[maybe_unused]] size_t alignment = alignof(std::max_align_t)) { - if (m_freeList.empty()) { - AddChunk(bytes); + if (m_free_list.empty()) { + add_chunk(bytes); } - auto ptr = m_freeList.back(); - m_freeList.pop_back(); + auto ptr = m_free_list.back(); + m_free_list.pop_back(); return ptr; } @@ -61,11 +81,14 @@ class SLEIPNIR_DLLEXPORT PoolResource { void deallocate( void* p, [[maybe_unused]] size_t bytes, [[maybe_unused]] size_t alignment = alignof(std::max_align_t)) { - m_freeList.emplace_back(p); + m_free_list.emplace_back(p); } /** * Returns true if this pool resource has the same backing storage as another. + * + * @param other The other pool resource. + * @return True if this pool resource has the same backing storage as another. */ bool is_equal(const PoolResource& other) const noexcept { return this == &other; @@ -73,26 +96,28 @@ class SLEIPNIR_DLLEXPORT PoolResource { /** * Returns the number of blocks from this pool resource that are in use. + * + * @return The number of blocks from this pool resource that are in use. */ size_t blocks_in_use() const noexcept { - return m_buffer.size() * blocksPerChunk - m_freeList.size(); + return m_buffer.size() * blocks_per_chunk - m_free_list.size(); } private: - wpi::SmallVector> m_buffer; - wpi::SmallVector m_freeList; - size_t blocksPerChunk; + gch::small_vector> m_buffer; + gch::small_vector m_free_list; + size_t blocks_per_chunk; /** * Adds a memory chunk to the pool, partitions it into blocks with the given * number of bytes, and appends pointers to them to the free list. * - * @param bytesPerBlock Number of bytes in the block. + * @param bytes_per_block Number of bytes in the block. */ - void AddChunk(size_t bytesPerBlock) { - m_buffer.emplace_back(new std::byte[bytesPerBlock * blocksPerChunk]); - for (int i = blocksPerChunk - 1; i >= 0; --i) { - m_freeList.emplace_back(m_buffer.back().get() + bytesPerBlock * i); + void add_chunk(size_t bytes_per_block) { + m_buffer.emplace_back(new std::byte[bytes_per_block * blocks_per_chunk]); + for (int i = blocks_per_chunk - 1; i >= 0; --i) { + m_free_list.emplace_back(m_buffer.back().get() + bytes_per_block * i); } } }; @@ -115,19 +140,29 @@ class PoolAllocator { * * @param r The pool resource. */ - explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {} + explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {} - constexpr PoolAllocator(const PoolAllocator& other) = default; + /** + * Copy constructor. + */ + constexpr PoolAllocator(const PoolAllocator&) = default; + + /** + * Copy assignment operator. + * + * @return This pool allocator. + */ constexpr PoolAllocator& operator=(const PoolAllocator&) = default; /** * Returns a block of memory from the pool. * * @param n Number of bytes in the block. + * @return A block of memory from the pool. */ [[nodiscard]] constexpr T* allocate(size_t n) { - return static_cast(m_memoryResource->allocate(n)); + return static_cast(m_memory_resource->allocate(n)); } /** @@ -137,17 +172,17 @@ class PoolAllocator { * @param n Number of bytes in the block. */ constexpr void deallocate(T* p, size_t n) { - m_memoryResource->deallocate(p, n); + m_memory_resource->deallocate(p, n); } private: - PoolResource* m_memoryResource; + PoolResource* m_memory_resource; }; /** * Returns a global pool memory resource. */ -SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource(); +SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource(); /** * Returns an allocator for a global pool memory resource. @@ -155,8 +190,8 @@ SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource(); * @tparam T The type of object in the pool. */ template -PoolAllocator GlobalPoolAllocator() { - return PoolAllocator{&GlobalPoolResource()}; +PoolAllocator global_pool_allocator() { + return PoolAllocator{&global_pool_resource()}; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp similarity index 81% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp index c01fd4ac67..055d5c9fa2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp @@ -2,6 +2,7 @@ #pragma once +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #include #include #include @@ -12,7 +13,11 @@ #include #endif -namespace sleipnir { +#endif + +namespace slp { + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS /** * Wrapper around fmt::print() that squelches write failure exceptions. @@ -58,4 +63,14 @@ inline void println(std::FILE* f, fmt::format_string fmt, T&&... args) { } } -} // namespace sleipnir +#else + +template +inline void print([[maybe_unused]] Args&&... args) {} + +template +inline void println([[maybe_unused]] Args&&... args) {} + +#endif + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp new file mode 100644 index 0000000000..8cd7d4353a --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp @@ -0,0 +1,127 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + +#include + +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/util/symbol_exports.hpp" + +namespace slp { + +/** + * Writes the sparsity pattern of a sparse matrix to a file. + * + * Each file represents the sparsity pattern of one matrix over time. spy.py + * can display it as an animation. + * + * The file starts with the following header: + *
    + *
  1. Plot title (length as a little-endian int32, then characters)
    2. + *
  2. Row label (length as a little-endian int32, then characters)
    3. + *
  3. Column label (length as a little-endian int32, then characters)
    4. + *
+ * + * Then, each sparsity pattern starts with: + *
    + *
  1. Number of coordinates as a little-endian int32
    2. + *
+ * + * followed by that many coordinates in the following format: + *
    + *
  1. Row index as a little-endian int32
    2. + *
  2. Column index as a little-endian int32
    3. + *
  3. Sign as a character ('+' for positive, '-' for negative, or '0' for + * zero)
    4. + *
+ * + * @param[out] file A file stream. + * @param[in] mat The sparse matrix. + */ +class SLEIPNIR_DLLEXPORT Spy { + public: + /** + * Constructs a Spy instance. + * + * @param filename The filename. + * @param title Plot title. + * @param row_label Row label. + * @param col_label Column label. + * @param rows The sparse matrix's number of rows. + * @param cols The sparse matrix's number of columns. + */ + Spy(std::string_view filename, std::string_view title, + std::string_view row_label, std::string_view col_label, int rows, + int cols) + : m_file{std::string{filename}, std::ios::binary} { + // Write title + write32le(title.size()); + m_file.write(title.data(), title.size()); + + // Write row label + write32le(row_label.size()); + m_file.write(row_label.data(), row_label.size()); + + // Write column label + write32le(col_label.size()); + m_file.write(col_label.data(), col_label.size()); + + // Write row and column counts + write32le(rows); + write32le(cols); + } + + /** + * Adds a matrix to the file. + * + * @param mat The matrix. + */ + void add(const Eigen::SparseMatrix& mat) { + // Write number of coordinates + write32le(mat.nonZeros()); + + // Write coordinates + for (int k = 0; k < mat.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it{mat, k}; it; ++it) { + write32le(it.row()); + write32le(it.col()); + if (it.value() > 0.0) { + m_file << '+'; + } else if (it.value() < 0.0) { + m_file << '-'; + } else { + m_file << '0'; + } + } + } + } + + private: + std::ofstream m_file; + + /** + * Writes a 32-bit signed integer to the file as little-endian. + * + * @param num A 32-bit signed integer. + */ + void write32le(int32_t num) { + if constexpr (std::endian::native != std::endian::little) { + num = wpi::byteswap(num); + } + m_file.write(reinterpret_cast(&num), sizeof(num)); + } +}; + +} // namespace slp + +#endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp similarity index 100% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide index ad739cea6d..1b6652d3d5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -8,5 +8,5 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ - ^wpi/ + ^gch/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp deleted file mode 100644 index 589ff8822a..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/autodiff/VariableMatrix.hpp" - -#include - -namespace sleipnir { - -VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) { - // m x n * n x p = m x p - Assert(A.Rows() == B.Rows()); - - if (A.Rows() == 1 && A.Cols() == 1) { - // Compute optimal inverse instead of using Eigen's general solver - return B(0, 0) / A(0, 0); - } else if (A.Rows() == 2 && A.Cols() == 2) { - // Compute optimal inverse instead of using Eigen's general solver - // - // [a b]⁻¹ ___1___ [ d −b] - // [c d] = ad − bc [−c a] - - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(1, 0); - const auto& d = A(1, 1); - - sleipnir::VariableMatrix Ainv{{d, -b}, {-c, a}}; - auto detA = a * d - b * c; - Ainv /= detA; - - return Ainv * B; - } else if (A.Rows() == 3 && A.Cols() == 3) { - // Compute optimal inverse instead of using Eigen's general solver - // - // [a b c]⁻¹ - // [d e f] - // [g h i] - // 1 [ei − fh ch − bi bf − ce] - // = --------------------------------- [fg − di ai − cg cd − af] - // aei − afh − bdi + bfg + cdh − ceg [dh − eg bg − ah ae − bd] - - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(0, 2); - const auto& d = A(1, 0); - const auto& e = A(1, 1); - const auto& f = A(1, 2); - const auto& g = A(2, 0); - const auto& h = A(2, 1); - const auto& i = A(2, 2); - - sleipnir::VariableMatrix Ainv{ - {e * i - f * h, c * h - b * i, b * f - c * e}, - {f * g - d * i, a * i - c * g, c * d - a * f}, - {d * h - e * g, b * g - a * h, a * e - b * d}}; - auto detA = - a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g; - Ainv /= detA; - - return Ainv * B; - } else { - using MatrixXv = Eigen::Matrix; - - MatrixXv eigenA{A.Rows(), A.Cols()}; - for (int row = 0; row < A.Rows(); ++row) { - for (int col = 0; col < A.Cols(); ++col) { - eigenA(row, col) = A(row, col); - } - } - - MatrixXv eigenB{B.Rows(), B.Cols()}; - for (int row = 0; row < B.Rows(); ++row) { - for (int col = 0; col < B.Cols(); ++col) { - eigenB(row, col) = B(row, col); - } - } - - MatrixXv eigenX = eigenA.householderQr().solve(eigenB); - - VariableMatrix X{A.Cols(), B.Cols()}; - for (int row = 0; row < X.Rows(); ++row) { - for (int col = 0; col < X.Cols(); ++col) { - X(row, col) = eigenX(row, col); - } - } - - return X; - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp new file mode 100644 index 0000000000..71f8153d34 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp @@ -0,0 +1,259 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/autodiff/variable_matrix.hpp" + +#include + +namespace slp { + +VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { + // m x n * n x p = m x p + slp_assert(A.rows() == B.rows()); + + if (A.rows() == 1 && A.cols() == 1) { + // Compute optimal inverse instead of using Eigen's general solver + return B(0, 0) / A(0, 0); + } else if (A.rows() == 2 && A.cols() == 2) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b]⁻¹ ___1___ [ d −b] + // [c d] = ad − bc [−c a] + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(1, 0); + const auto& d = A(1, 1); + + slp::VariableMatrix adj_A{{d, -b}, {-c, a}}; + auto det_A = a * d - b * c; + return adj_A / det_A * B; + } else if (A.rows() == 3 && A.cols() == 3) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b c]⁻¹ + // [d e f] + // [g h i] + // 1 [ei − fh ch − bi bf − ce] + // = ------------------------------------ [fg − di ai − cg cd − af] + // a(ei − fh) + b(fg − di) + c(dh − eg) [dh − eg bg − ah ae − bd] + // + // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(0, 2); + const auto& d = A(1, 0); + const auto& e = A(1, 1); + const auto& f = A(1, 2); + const auto& g = A(2, 0); + const auto& h = A(2, 1); + const auto& i = A(2, 2); + + auto ae = a * e; + auto af = a * f; + auto ah = a * h; + auto ai = a * i; + auto bd = b * d; + auto bf = b * f; + auto bg = b * g; + auto bi = b * i; + auto cd = c * d; + auto ce = c * e; + auto cg = c * g; + auto ch = c * h; + auto dh = d * h; + auto di = d * i; + auto eg = e * g; + auto ei = e * i; + auto fg = f * g; + auto fh = f * h; + + auto adj_A00 = ei - fh; + auto adj_A10 = fg - di; + auto adj_A20 = dh - eg; + + slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, + {adj_A10, ai - cg, cd - af}, + {adj_A20, bg - ah, ae - bd}}; + auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20; + return adj_A / det_A * B; + } else if (A.rows() == 4 && A.cols() == 4) { + // Compute optimal inverse instead of using Eigen's general solver + // + // [a b c d]⁻¹ + // [e f g h] + // [i j k l] + // [m n o p] + // + // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D + + const auto& a = A(0, 0); + const auto& b = A(0, 1); + const auto& c = A(0, 2); + const auto& d = A(0, 3); + const auto& e = A(1, 0); + const auto& f = A(1, 1); + const auto& g = A(1, 2); + const auto& h = A(1, 3); + const auto& i = A(2, 0); + const auto& j = A(2, 1); + const auto& k = A(2, 2); + const auto& l = A(2, 3); + const auto& m = A(3, 0); + const auto& n = A(3, 1); + const auto& o = A(3, 2); + const auto& p = A(3, 3); + + auto afk = a * f * k; + auto afl = a * f * l; + auto afo = a * f * o; + auto afp = a * f * p; + auto agj = a * g * j; + auto agl = a * g * l; + auto agn = a * g * n; + auto agp = a * g * p; + auto ahj = a * h * j; + auto ahk = a * h * k; + auto ahn = a * h * n; + auto aho = a * h * o; + auto ajo = a * j * o; + auto ajp = a * j * p; + auto akn = a * k * n; + auto akp = a * k * p; + auto aln = a * l * n; + auto alo = a * l * o; + auto bek = b * e * k; + auto bel = b * e * l; + auto beo = b * e * o; + auto bep = b * e * p; + auto bgi = b * g * i; + auto bgl = b * g * l; + auto bgm = b * g * m; + auto bgp = b * g * p; + auto bhi = b * h * i; + auto bhk = b * h * k; + auto bhm = b * h * m; + auto bho = b * h * o; + auto bio = b * i * o; + auto bip = b * i * p; + auto bjp = b * j * p; + auto bkm = b * k * m; + auto bkp = b * k * p; + auto blm = b * l * m; + auto blo = b * l * o; + auto cej = c * e * j; + auto cel = c * e * l; + auto cen = c * e * n; + auto cep = c * e * p; + auto cfi = c * f * i; + auto cfl = c * f * l; + auto cfm = c * f * m; + auto cfp = c * f * p; + auto chi = c * h * i; + auto chj = c * h * j; + auto chm = c * h * m; + auto chn = c * h * n; + auto cin = c * i * n; + auto cip = c * i * p; + auto cjm = c * j * m; + auto cjp = c * j * p; + auto clm = c * l * m; + auto cln = c * l * n; + auto dej = d * e * j; + auto dek = d * e * k; + auto den = d * e * n; + auto deo = d * e * o; + auto dfi = d * f * i; + auto dfk = d * f * k; + auto dfm = d * f * m; + auto dfo = d * f * o; + auto dgi = d * g * i; + auto dgj = d * g * j; + auto dgm = d * g * m; + auto dgn = d * g * n; + auto din = d * i * n; + auto dio = d * i * o; + auto djm = d * j * m; + auto djo = d * j * o; + auto dkm = d * k * m; + auto dkn = d * k * n; + auto ejo = e * j * o; + auto ejp = e * j * p; + auto ekn = e * k * n; + auto ekp = e * k * p; + auto eln = e * l * n; + auto elo = e * l * o; + auto fio = f * i * o; + auto fip = f * i * p; + auto fkm = f * k * m; + auto fkp = f * k * p; + auto flm = f * l * m; + auto flo = f * l * o; + auto gin = g * i * n; + auto gip = g * i * p; + auto gjm = g * j * m; + auto gjp = g * j * p; + auto glm = g * l * m; + auto gln = g * l * n; + auto hin = h * i * n; + auto hio = h * i * o; + auto hjm = h * j * m; + auto hjo = h * j * o; + auto hkm = h * k * m; + auto hkn = h * k * n; + + auto adj_A00 = fkp - flo - gjp + gln + hjo - hkn; + auto adj_A01 = -bkp + blo + cjp - cln - djo + dkn; + auto adj_A02 = bgp - bho - cfp + chn + dfo - dgn; + auto adj_A03 = -bgl + bhk + cfl - chj - dfk + dgj; + auto adj_A10 = -ekp + elo + gip - glm - hio + hkm; + auto adj_A11 = akp - alo - cip + clm + dio - dkm; + auto adj_A12 = -agp + aho + cep - chm - deo + dgm; + auto adj_A13 = agl - ahk - cel + chi + dek - dgi; + auto adj_A20 = ejp - eln - fip + flm + hin - hjm; + auto adj_A21 = -ajp + aln + bip - blm - din + djm; + auto adj_A22 = afp - ahn - bep + bhm + den - dfm; + auto adj_A23 = -afl + ahj + bel - bhi - dej + dfi; + auto adj_A30 = -ejo + ekn + fio - fkm - gin + gjm; + // NOLINTNEXTLINE + auto adj_A31 = ajo - akn - bio + bkm + cin - cjm; + auto adj_A32 = -afo + agn + beo - bgm - cen + cfm; + auto adj_A33 = afk - agj - bek + bgi + cej - cfi; + + slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, + {adj_A10, adj_A11, adj_A12, adj_A13}, + {adj_A20, adj_A21, adj_A22, adj_A23}, + {adj_A30, adj_A31, adj_A32, adj_A33}}; + auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30; + return adj_A / det_A * B; + } else { + using MatrixXv = Eigen::Matrix; + + MatrixXv eigen_A{A.rows(), A.cols()}; + for (int row = 0; row < A.rows(); ++row) { + for (int col = 0; col < A.cols(); ++col) { + eigen_A(row, col) = A(row, col); + } + } + + MatrixXv eigen_B{B.rows(), B.cols()}; + for (int row = 0; row < B.rows(); ++row) { + for (int col = 0; col < B.cols(); ++col) { + eigen_B(row, col) = B(row, col); + } + } + + MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B); + + VariableMatrix X{A.cols(), B.cols()}; + for (int row = 0; row < X.rows(); ++row) { + for (int col = 0; col < X.cols(); ++col) { + X(row, col) = eigen_X(row, col); + } + } + + return X; + } +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp deleted file mode 100644 index 8ef2ebe0ce..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include "sleipnir/util/Concepts.hpp" - -namespace sleipnir { - -/** - * Represents the inertia of a matrix (the number of positive, negative, and - * zero eigenvalues). - */ -class Inertia { - public: - size_t positive = 0; - size_t negative = 0; - size_t zero = 0; - - constexpr Inertia() = default; - - /** - * Constructs the Inertia type with the given number of positive, negative, - * and zero eigenvalues. - * - * @param positive The number of positive eigenvalues. - * @param negative The number of negative eigenvalues. - * @param zero The number of zero eigenvalues. - */ - constexpr Inertia(size_t positive, size_t negative, size_t zero) - : positive{positive}, negative{negative}, zero{zero} {} - - /** - * Constructs the Inertia type with the inertia of the given LDLT - * decomposition. - * - * @tparam Solver Eigen sparse linear system solver. - * @param solver The LDLT decomposition of which to compute the inertia. - */ - template - explicit Inertia(const Solver& solver) { - const auto& D = solver.vectorD(); - for (int row = 0; row < D.rows(); ++row) { - if (D(row) > 0.0) { - ++positive; - } else if (D(row) < 0.0) { - ++negative; - } else { - ++zero; - } - } - } - - bool operator==(const Inertia&) const = default; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp deleted file mode 100644 index d2488b7998..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include - -#include -#include -#include - -#include "optimization/Inertia.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions - -namespace sleipnir { - -/** - * Solves systems of linear equations using a regularized LDLT factorization. - */ -class RegularizedLDLT { - public: - using Solver = Eigen::SimplicialLDLT, - Eigen::Lower, Eigen::AMDOrdering>; - - /** - * Constructs a RegularizedLDLT instance. - */ - RegularizedLDLT() = default; - - /** - * Reports whether previous computation was successful. - */ - Eigen::ComputationInfo Info() { return m_info; } - - /** - * Computes the regularized LDLT factorization of a matrix. - * - * @param lhs Left-hand side of the system. - * @param numEqualityConstraints The number of equality constraints in the - * system. - * @param μ The barrier parameter for the current interior-point iteration. - */ - void Compute(const Eigen::SparseMatrix& lhs, - size_t numEqualityConstraints, double μ) { - // The regularization procedure is based on algorithm B.1 of [1] - m_numDecisionVariables = lhs.rows() - numEqualityConstraints; - m_numEqualityConstraints = numEqualityConstraints; - - const Inertia idealInertia{m_numDecisionVariables, m_numEqualityConstraints, - 0}; - Inertia inertia; - - double δ = 0.0; - double γ = 0.0; - - AnalyzePattern(lhs); - m_solver.factorize(lhs); - - if (m_solver.info() == Eigen::Success) { - inertia = Inertia{m_solver}; - - // If the inertia is ideal, don't regularize the system - if (inertia == idealInertia) { - m_info = Eigen::Success; - return; - } - } - - // If the decomposition succeeded and the inertia has some zero eigenvalues, - // or the decomposition failed, regularize the equality constraints - if ((m_solver.info() == Eigen::Success && inertia.zero > 0) || - m_solver.info() != Eigen::Success) { - γ = 1e-8 * std::pow(μ, 0.25); - } - - // Also regularize the Hessian. If the Hessian wasn't regularized in a - // previous run of Compute(), start at a small value of δ. Otherwise, - // attempt a δ half as big as the previous run so δ can trend downwards over - // time. - if (m_δOld == 0.0) { - δ = 1e-4; - } else { - δ = m_δOld / 2.0; - } - - while (true) { - // Regularize lhs by adding a multiple of the identity matrix - // - // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] - // [ Aₑ −γI ] - Eigen::SparseMatrix lhsReg = lhs + Regularization(δ, γ); - AnalyzePattern(lhsReg); - m_solver.factorize(lhsReg); - inertia = Inertia{m_solver}; - - // If the inertia is ideal, store that value of δ and return. - // Otherwise, increase δ by an order of magnitude and try again. - if (inertia == idealInertia) { - m_δOld = δ; - m_info = Eigen::Success; - return; - } else { - δ *= 10.0; - - // If the Hessian perturbation is too high, report failure. This can - // happen due to a rank-deficient equality constraint Jacobian with - // linearly dependent constraints. - if (δ > 1e20) { - m_info = Eigen::NumericalIssue; - return; - } - } - } - } - - /** - * Solve the system of equations using a regularized LDLT factorization. - * - * @param rhs Right-hand side of the system. - */ - template - auto Solve(const Eigen::MatrixBase& rhs) { - return m_solver.solve(rhs); - } - - private: - Solver m_solver; - - Eigen::ComputationInfo m_info = Eigen::Success; - - /// The number of decision variables in the system. - size_t m_numDecisionVariables = 0; - - /// The number of equality constraints in the system. - size_t m_numEqualityConstraints = 0; - - /// The value of δ from the previous run of Compute(). - double m_δOld = 0.0; - - // Number of non-zeros in LHS. - int m_nonZeros = -1; - - /** - * Reanalize LHS matrix's sparsity pattern if it changed. - * - * @param lhs Matrix to analyze. - */ - void AnalyzePattern(const Eigen::SparseMatrix& lhs) { - int nonZeros = lhs.nonZeros(); - if (m_nonZeros != nonZeros) { - m_solver.analyzePattern(lhs); - m_nonZeros = nonZeros; - } - } - - /** - * Returns regularization matrix. - * - * @param δ The Hessian regularization factor. - * @param γ The equality constraint Jacobian regularization factor. - */ - Eigen::SparseMatrix Regularization(double δ, double γ) { - Eigen::VectorXd vec{m_numDecisionVariables + m_numEqualityConstraints}; - size_t row = 0; - while (row < m_numDecisionVariables) { - vec(row) = δ; - ++row; - } - while (row < m_numDecisionVariables + m_numEqualityConstraints) { - vec(row) = -γ; - ++row; - } - - return Eigen::SparseMatrix{vec.asDiagonal()}; - } -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp new file mode 100644 index 0000000000..9b68a72a8d --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp @@ -0,0 +1,220 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/util/assert.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace slp { + +struct Bounds { + /// Which constraints, if any, are bound constraints. + Eigen::ArrayX bound_constraint_mask; + + /// The tightest bounds on each decision variable. + gch::small_vector> bounds; + + /// Whether or not the constraints are feasible (given previously encountered + /// bounds). + gch::small_vector> + conflicting_bound_indices; +}; + +/** + * A "bound constraint" is any linear constraint in one scalar variable. + * + * Computes which constraints, if any, are bound constraints, the tightest + * bounds on each decision variable, and whether or not they're feasible (given + * previously encountered bounds), + * + * @param decision_variables Decision variables corresponding to each column of + * A_i. + * @param inequality_constraints Variables representing the left-hand side of + * cᵢ(decision_variables) ≥ 0. + * @param A_i The Jacobian of inequality_constraints wrt decision_variables, + * evaluated anywhere, in *row-major* storage; in practice, since we typically + * store Jacobians column-major, the user of this function must perform a + * transpose. + */ +inline Bounds get_bounds( + std::span decision_variables, + std::span inequality_constraints, + const Eigen::SparseMatrix& A_i) { + // TODO: A blocked, out-of-place transpose should be much faster than + // traversing row major on a column major matrix unless we have few linear + // constraints (using a heuristic to choose between this and staying column + // major based on the number of constraints would be an easy performance + // improvement.) + + // NB: Casting to long is unspecified if the size of decision_variable.size() + // is greater than the max long value, but then we wouldn't be able to fill + // A_i correctly anyway. + slp_assert(static_cast(decision_variables.size()) == + A_i.innerSize()); + slp_assert(static_cast(inequality_constraints.size()) == + A_i.outerSize()); + + // Maps each decision variable's index to the indices of its upper and lower + // bounds if they exist, or NO_BOUND if they do not; used only for bookkeeping + // to compute conflicting bounds + constexpr Eigen::Index NO_BOUND = -1; + gch::small_vector> + decision_var_indices_to_constraint_indices{decision_variables.size(), + {NO_BOUND, NO_BOUND}}; + // Lists pairs of indices of bound constraints in the inequality constraint + // list that conflict with each other + gch::small_vector> + conflicting_bound_indices; + + // Maps each decision variable's index to its upper and lower bounds + gch::small_vector> decision_var_indices_to_bounds{ + decision_variables.size(), + {-std::numeric_limits::infinity(), + std::numeric_limits::infinity()}}; + + // Vector corresponding to the inequality constraints where the i-th element + // is 1 if the i-th inequality constraint is a bound and 0 otherwise. + Eigen::ArrayX bound_constraint_mask{inequality_constraints.size()}; + bound_constraint_mask.fill(false); + + for (decltype(inequality_constraints)::size_type constraint_index = 0; + constraint_index < inequality_constraints.size(); ++constraint_index) { + // A constraint is a bound iff it is linear and its gradient has a + // single nonzero value. + if (inequality_constraints[constraint_index].type() != + ExpressionType::LINEAR) { + continue; + } + const Eigen::SparseVector& row_A_i = + A_i.innerVector(constraint_index); + const auto non_zeros = row_A_i.nonZeros(); + slp_assert(non_zeros != 0); + if (non_zeros > 1) { + // Constraint is in more than one variable and therefore not a bound. + continue; + } + + // Claim: The bound given by a bound constraint is the constraint evaluated + // at zero divided by the nonzero element of the constraint's gradient. + // + // Proof: If c(x) is a bound constraint, then by definition c is a linear + // function in one variable, hence there exist a, b ∈ ℝ s.t. c(x) = axᵢ + b + // and a ≠ 0. The gradient of c is then aeᵢ (where eᵢ denotes the i-th basis + // element), and c(0) = b. If c(x) ≥ 0, then since either a < 0 or a > 0, we + // have either x ≤ -b/a or x ≥ -b/a, respectively. ∎ + Eigen::SparseVector::InnerIterator row_iter(row_A_i); + const auto constraint_coefficient = + row_iter + .value(); // The nonzero value of the j-th constraint's gradient. + const auto decision_variable_index = row_iter.index(); + const auto decision_variable_value = + decision_variables[decision_variable_index].value(); + double constraint_constant; + // We need to evaluate this constraint *exactly* at zero. + if (decision_variable_value != 0.0) { + decision_variables[decision_variable_index].set_value(0.0); + constraint_constant = inequality_constraints[constraint_index].value(); + decision_variables[decision_variable_index].set_value( + decision_variable_value); + } else { + constraint_constant = inequality_constraints[constraint_index].value(); + } + + // Shouldn't happen since the constraint is supposed to be linear and not a + // constant. + slp_assert(constraint_coefficient != 0.0); + + // We should always get a finite value when evaluating the constraint at + // x = 0 since the constraint is linear. + slp_assert(std::isfinite(constraint_constant)); + + // This is possible if the user has provided a starting point at which their + // problem is ill-defined. + slp_assert(std::isfinite(constraint_coefficient)); + + // Update bounds; we assume constraints of the form c(x) ≥ 0. + auto& [lower_bound, upper_bound] = + decision_var_indices_to_bounds[decision_variable_index]; + auto& [lower_index, upper_index] = + decision_var_indices_to_constraint_indices[decision_variable_index]; + const auto detected_bound = -constraint_constant / constraint_coefficient; + if (constraint_coefficient < 0.0 && detected_bound < upper_bound) { + upper_bound = detected_bound; + upper_index = constraint_index; + } else if (constraint_coefficient > 0.0 && detected_bound > lower_bound) { + lower_bound = detected_bound; + lower_index = constraint_index; + } + + // Update conflicting bounds + if (lower_bound > upper_bound) { + conflicting_bound_indices.emplace_back(lower_index, upper_index); + } + + // Set the bound constraint mask appropriately. + bound_constraint_mask[constraint_index] = true; + } + return {bound_constraint_mask, decision_var_indices_to_bounds, + conflicting_bound_indices}; +} + +/** + * Projects the decision variables onto the given bounds, while ensuring some + * configurable distance from the boundary if possible. This is designed to + * match the algorithm given in section 3.6 of [2]. + * + * @param x A vector of decision variables. + * @param decision_variable_indices_to_bounds An array of bounds (stored [lower, + * upper]) for each decision variable in x. + * @param κ_1 A constant controlling distance from the lower or upper bound when + * the difference between the upper and lower bound is small. + * @param κ_2 A constant controlling distance from the lower or upper bound when + * the difference between the upper and lower bound is large (including when + * one of the bounds is ±∞). + */ +template + requires(static_cast(Eigen::DenseBase::IsVectorAtCompileTime)) +inline void project_onto_bounds( + Eigen::DenseBase& x, + std::span::Scalar, + typename Eigen::DenseBase::Scalar>> + decision_variable_indices_to_bounds, + const typename Eigen::DenseBase::Scalar κ_1 = 1e-2, + const typename Eigen::DenseBase::Scalar κ_2 = 1e-2) { + slp_assert(κ_1 > 0 && κ_2 > 0 && κ_2 < 0.5); + + Eigen::Index decision_variable_idx = 0; + for (const auto& [lower, upper] : decision_variable_indices_to_bounds) { + typename Eigen::DenseBase::Scalar& x_i = + x[decision_variable_idx++]; + + // We assume that bound infeasibility is handled elsewhere. + slp_assert(lower <= upper); + + // See B.2 in [5] and section 3.6 in [2] + if (std::isfinite(lower) && std::isfinite(upper)) { + auto p_L = + std::min(κ_1 * std::max(1.0, std::abs(lower)), κ_2 * (upper - lower)); + auto p_U = + std::min(κ_1 * std::max(1.0, std::abs(upper)), κ_2 * (upper - lower)); + x_i = std::min(std::max(lower + p_L, x_i), upper - p_U); + } else if (std::isfinite(lower)) { + x_i = std::max(x_i, lower + κ_1 * std::max(1.0, std::abs(lower))); + } else if (std::isfinite(upper)) { + x_i = std::min(x_i, upper - κ_1 * std::max(1.0, std::abs(upper))); + } + } +} +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp new file mode 100644 index 0000000000..8666c3ed68 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp @@ -0,0 +1,53 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +/** + * Represents the inertia of a matrix (the number of positive, negative, and + * zero eigenvalues). + */ +class Inertia { + public: + int positive = 0; + int negative = 0; + int zero = 0; + + constexpr Inertia() = default; + + /** + * Constructs Inertia with the given number of positive, negative, and zero + * eigenvalues. + * + * @param positive The number of positive eigenvalues. + * @param negative The number of negative eigenvalues. + * @param zero The number of zero eigenvalues. + */ + constexpr Inertia(int positive, int negative, int zero) + : positive{positive}, negative{negative}, zero{zero} {} + + /** + * Constructs Inertia from the D matrix of an LDLT decomposition + * (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia). + * + * @param D The D matrix of an LDLT decomposition in vector form. + */ + explicit Inertia(const Eigen::VectorXd& D) { + for (const auto& elem : D) { + if (elem > 0.0) { + ++positive; + } else if (elem < 0.0) { + ++negative; + } else { + ++zero; + } + } + } + + bool operator==(const Inertia&) const = default; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp new file mode 100644 index 0000000000..5532b39624 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp @@ -0,0 +1,391 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/problem.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/bounds.hpp" +#include "sleipnir/autodiff/expression_type.hpp" +#include "sleipnir/autodiff/gradient.hpp" +#include "sleipnir/autodiff/hessian.hpp" +#include "sleipnir/autodiff/jacobian.hpp" +#include "sleipnir/autodiff/variable.hpp" +#include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/interior_point.hpp" +#include "sleipnir/optimization/solver/newton.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/optimization/solver/sqp.hpp" +#include "sleipnir/util/print.hpp" +#include "sleipnir/util/spy.hpp" +#include "util/print_diagnostics.hpp" +#include "util/setup_profiler.hpp" + +namespace slp { + +ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { + // Create the initial value column vector + Eigen::VectorXd x{m_decision_variables.size()}; + for (size_t i = 0; i < m_decision_variables.size(); ++i) { + x[i] = m_decision_variables[i].value(); + } + + if (options.diagnostics) { + print_exit_conditions(options); + print_problem_analysis(); + } + + // Get the highest order constraint expression types + auto f_type = cost_function_type(); + auto c_e_type = equality_constraint_type(); + auto c_i_type = inequality_constraint_type(); + + // If the problem is empty or constant, there's nothing to do + if (f_type <= ExpressionType::CONSTANT && + c_e_type <= ExpressionType::CONSTANT && + c_i_type <= ExpressionType::CONSTANT) { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + if (options.diagnostics) { + slp::println("\nInvoking no-op solver...\n"); + } +#endif + return ExitStatus::SUCCESS; + } + + gch::small_vector ad_setup_profilers; + ad_setup_profilers.emplace_back("setup").start(); + + VariableMatrix x_ad{m_decision_variables}; + + // Set up cost function + Variable f = m_f.value_or(0.0); + + // Set up gradient autodiff + ad_setup_profilers.emplace_back(" ↳ ∇f(x)").start(); + Gradient g{f, x_ad}; + ad_setup_profilers.back().stop(); + + [[maybe_unused]] + int num_decision_variables = m_decision_variables.size(); + [[maybe_unused]] + int num_equality_constraints = m_equality_constraints.size(); + [[maybe_unused]] + int num_inequality_constraints = m_inequality_constraints.size(); + + // Solve the optimization problem + ExitStatus status; + if (m_equality_constraints.empty() && m_inequality_constraints.empty()) { + if (options.diagnostics) { + slp::println("\nInvoking Newton solver...\n"); + } + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{f, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + return false; + }); + } +#endif + + // Invoke Newton solver + status = newton( + NewtonMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return H.value(); + }}, + m_iteration_callbacks, options, x); + } else if (m_inequality_constraints.empty()) { + if (options.diagnostics) { + slp::println("\nInvoking SQP solver\n"); + } + + VariableMatrix c_e_ad{m_equality_constraints}; + + // Set up equality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); + Jacobian A_e{c_e_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up Lagrangian + VariableMatrix y_ad(num_equality_constraints); + Variable L = f - (y_ad.T() * c_e_ad)[0]; + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{L, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + std::unique_ptr A_e_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + A_e_spy = std::make_unique("A_e.spy", "Equality constraint Jacobian", + "Constraints", "Decision variables", + num_equality_constraints, + num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + return false; + }); + } +#endif + + // Invoke SQP solver + status = + sqp(SQPMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x, + const Eigen::VectorXd& y) -> Eigen::SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + return H.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }}, + m_iteration_callbacks, options, x); + } else { + if (options.diagnostics) { + slp::println("\nInvoking IPM solver...\n"); + } + + VariableMatrix c_e_ad{m_equality_constraints}; + VariableMatrix c_i_ad{m_inequality_constraints}; + + // Set up equality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); + Jacobian A_e{c_e_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up inequality constraint Jacobian autodiff + ad_setup_profilers.emplace_back(" ↳ ∂cᵢ/∂x").start(); + Jacobian A_i{c_i_ad, x_ad}; + ad_setup_profilers.back().stop(); + + // Set up Lagrangian + VariableMatrix y_ad(num_equality_constraints); + VariableMatrix z_ad(num_inequality_constraints); + Variable L = f - (y_ad.T() * c_e_ad)[0] - (z_ad.T() * c_i_ad)[0]; + + // Set up Lagrangian Hessian autodiff + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + Hessian H{L, x_ad}; + ad_setup_profilers.back().stop(); + + ad_setup_profilers[0].stop(); + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + // Sparsity pattern files written when spy flag is set + std::unique_ptr H_spy; + std::unique_ptr A_e_spy; + std::unique_ptr A_i_spy; + + if (spy) { + H_spy = std::make_unique( + "H.spy", "Hessian", "Decision variables", "Decision variables", + num_decision_variables, num_decision_variables); + A_e_spy = std::make_unique("A_e.spy", "Equality constraint Jacobian", + "Constraints", "Decision variables", + num_equality_constraints, + num_decision_variables); + A_i_spy = std::make_unique( + "A_i.spy", "Inequality constraint Jacobian", "Constraints", + "Decision variables", num_inequality_constraints, + num_decision_variables); + m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + A_i_spy->add(info.A_i); + return false; + }); + } +#endif + + const auto [bound_constraint_mask, bounds, conflicting_bound_indices] = + get_bounds(m_decision_variables, m_inequality_constraints, A_i.value()); + if (!conflicting_bound_indices.empty()) { + if (options.diagnostics) { + print_bound_constraint_global_infeasibility_error( + conflicting_bound_indices); + } + return ExitStatus::GLOBALLY_INFEASIBLE; + } + +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + project_onto_bounds(x, bounds); +#endif + // Invoke interior-point method solver + status = interior_point( + InteriorPointMatrixCallbacks{ + [&](const Eigen::VectorXd& x) -> double { + x_ad.set_value(x); + return f.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const Eigen::VectorXd& x, const Eigen::VectorXd& y, + const Eigen::VectorXd& z) -> Eigen::SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + z_ad.set_value(z); + return H.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + x_ad.set_value(x); + return c_i_ad.value(); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + x_ad.set_value(x); + return A_i.value(); + }}, + m_iteration_callbacks, options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + bound_constraint_mask, +#endif + x); + } + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + if (spy) { + m_iteration_callbacks.pop_back(); + } +#endif + + if (options.diagnostics) { + print_autodiff_diagnostics(ad_setup_profilers); + slp::println("\nExit: {}", to_message(status)); + } + + // Assign the solution to the original Variable instances + VariableMatrix{m_decision_variables}.set_value(x); + + return status; +} + +void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { + // Print possible exit conditions + slp::println("User-configured exit conditions:"); + slp::println(" ↳ error below {}", options.tolerance); + if (!m_iteration_callbacks.empty()) { + slp::println(" ↳ iteration callback requested stop"); + } + if (std::isfinite(options.max_iterations)) { + slp::println(" ↳ executed {} iterations", options.max_iterations); + } + if (std::isfinite(options.timeout.count())) { + slp::println(" ↳ {} elapsed", options.timeout.count()); + } +} + +void Problem::print_problem_analysis() { + constexpr std::array types{"no", "constant", "linear", "quadratic", + "nonlinear"}; + + // Print problem structure + slp::println("\nProblem structure:"); + slp::println(" ↳ {} cost function", + types[static_cast(cost_function_type())]); + slp::println(" ↳ {} equality constraints", + types[static_cast(equality_constraint_type())]); + slp::println(" ↳ {} inequality constraints", + types[static_cast(inequality_constraint_type())]); + + if (m_decision_variables.size() == 1) { + slp::print("\n1 decision variable\n"); + } else { + slp::print("\n{} decision variables\n", m_decision_variables.size()); + } + + auto print_constraint_types = + [](const gch::small_vector& constraints) { + std::array counts{}; + for (const auto& constraint : constraints) { + ++counts[static_cast(constraint.type())]; + } + for (size_t i = 0; i < counts.size(); ++i) { + constexpr std::array names{"empty", "constant", "linear", "quadratic", + "nonlinear"}; + const auto& count = counts[i]; + const auto& name = names[i]; + if (count > 0) { + slp::println(" ↳ {} {}", count, name); + } + } + }; + + // Print constraint types + if (m_equality_constraints.size() == 1) { + slp::println("1 equality constraint"); + } else { + slp::println("{} equality constraints", m_equality_constraints.size()); + } + print_constraint_types(m_equality_constraints); + if (m_inequality_constraints.size() == 1) { + slp::println("1 inequality constraint"); + } else { + slp::println("{} inequality constraints", m_inequality_constraints.size()); + } + print_constraint_types(m_inequality_constraints); +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp new file mode 100644 index 0000000000..2a017e4f82 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp @@ -0,0 +1,227 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include "optimization/inertia.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace slp { + +/** + * Solves systems of linear equations using a regularized LDLT factorization. + */ +class RegularizedLDLT { + public: + /** + * Constructs a RegularizedLDLT instance. + * + * @param num_decision_variables The number of decision variables in the + * system. + * @param num_equality_constraints The number of equality constraints in the + * system. + */ + RegularizedLDLT(int num_decision_variables, int num_equality_constraints) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints} {} + + /** + * Reports whether previous computation was successful. + * + * @return Whether previous computation was successful. + */ + Eigen::ComputationInfo info() const { return m_info; } + + /** + * Computes the regularized LDLT factorization of a matrix. + * + * @param lhs Left-hand side of the system. + * @return The factorization. + */ + RegularizedLDLT& compute(const Eigen::SparseMatrix& lhs) { + // The regularization procedure is based on algorithm B.1 of [1] + + // Max density is 50% due to the caller only providing the lower triangle. + // We consider less than 25% to be sparse. + m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size(); + + m_info = m_is_sparse ? compute_sparse(lhs).info() + : m_dense_solver.compute(lhs).info(); + + Inertia inertia; + + if (m_info == Eigen::Success) { + inertia = m_is_sparse ? Inertia{m_sparse_solver.vectorD()} + : Inertia{m_dense_solver.vectorD()}; + + // If the inertia is ideal, don't regularize the system + if (inertia == ideal_inertia) { + m_prev_δ = 0.0; + return *this; + } + } + + // Also regularize the Hessian. If the Hessian wasn't regularized in a + // previous run of compute(), start at small values of δ and γ. Otherwise, + // attempt a δ and γ half as big as the previous run so δ and γ can trend + // downwards over time. + double δ = m_prev_δ == 0.0 ? 1e-4 : m_prev_δ / 2.0; + double γ = 1e-10; + + while (true) { + // Regularize lhs by adding a multiple of the identity matrix + // + // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] + // [ Aₑ −γI] + if (m_is_sparse) { + m_info = compute_sparse(lhs + regularization(δ, γ)).info(); + if (m_info == Eigen::Success) { + inertia = Inertia{m_sparse_solver.vectorD()}; + } + } else { + m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info(); + if (m_info == Eigen::Success) { + inertia = Inertia{m_dense_solver.vectorD()}; + } + } + + if (m_info == Eigen::Success) { + if (inertia == ideal_inertia) { + // If the inertia is ideal, store δ and return + m_prev_δ = δ; + return *this; + } else if (inertia.zero > 0) { + // If there's zero eigenvalues, increase δ and γ by an order of + // magnitude and try again + δ *= 10.0; + γ *= 10.0; + } else if (inertia.negative > ideal_inertia.negative) { + // If there's too many negative eigenvalues, increase δ by an order of + // magnitude and try again + δ *= 10.0; + } else if (inertia.positive > ideal_inertia.positive) { + // If there's too many positive eigenvalues, increase γ by an order of + // magnitude and try again + γ *= 10.0; + } + } else { + // If the decomposition failed, increase δ and γ by an order of + // magnitude and try again + δ *= 10.0; + γ *= 10.0; + } + + // If the Hessian perturbation is too high, report failure. This can be + // caused by ill-conditioning. + if (δ > 1e20 || γ > 1e20) { + m_info = Eigen::NumericalIssue; + return *this; + } + } + } + + /** + * Solves the system of equations using a regularized LDLT factorization. + * + * @param rhs Right-hand side of the system. + * @return The solution. + */ + template + Eigen::VectorXd solve(const Eigen::MatrixBase& rhs) { + if (m_is_sparse) { + return m_sparse_solver.solve(rhs); + } else { + return m_dense_solver.solve(rhs); + } + } + + /** + * Solves the system of equations using a regularized LDLT factorization. + * + * @param rhs Right-hand side of the system. + * @return The solution. + */ + template + Eigen::VectorXd solve(const Eigen::SparseMatrixBase& rhs) { + if (m_is_sparse) { + return m_sparse_solver.solve(rhs); + } else { + return m_dense_solver.solve(rhs.toDense()); + } + } + + /** + * Returns the Hessian regularization factor. + * + * @return Hessian regularization factor. + */ + double hessian_regularization() const { return m_prev_δ; } + + private: + using SparseSolver = Eigen::SimplicialLDLT>; + using DenseSolver = Eigen::LDLT; + + SparseSolver m_sparse_solver; + DenseSolver m_dense_solver; + bool m_is_sparse = true; + + Eigen::ComputationInfo m_info = Eigen::Success; + + /// The number of decision variables in the system. + int m_num_decision_variables = 0; + + /// The number of equality constraints in the system. + int m_num_equality_constraints = 0; + + /// The ideal system inertia. + Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints, + 0}; + + /// The value of δ from the previous run of compute(). + double m_prev_δ = 0.0; + + // Number of non-zeros in LHS. + int m_non_zeros = -1; + + /** + * Computes factorization of a sparse matrix. + * + * @param lhs Matrix to factorize. + * @return The factorization. + */ + SparseSolver& compute_sparse(const Eigen::SparseMatrix& lhs) { + // Reanalize lhs's sparsity pattern if it changed + int non_zeros = lhs.nonZeros(); + if (m_non_zeros != non_zeros) { + m_sparse_solver.analyzePattern(lhs); + m_non_zeros = non_zeros; + } + + m_sparse_solver.factorize(lhs); + + return m_sparse_solver; + } + + /** + * Returns regularization matrix. + * + * @param δ The Hessian regularization factor. + * @param γ The equality constraint Jacobian regularization factor. + * @return Regularization matrix. + */ + Eigen::SparseMatrix regularization(double δ, double γ) { + Eigen::VectorXd vec{m_num_decision_variables + m_num_equality_constraints}; + vec.segment(0, m_num_decision_variables).setConstant(δ); + vec.segment(m_num_decision_variables, m_num_equality_constraints) + .setConstant(-γ); + + return Eigen::SparseMatrix{vec.asDiagonal()}; + } +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp deleted file mode 100644 index d3981c59d1..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ /dev/null @@ -1,837 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/optimization/solver/InteriorPoint.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include "optimization/RegularizedLDLT.hpp" -#include "optimization/solver/util/ErrorEstimate.hpp" -#include "optimization/solver/util/FeasibilityRestoration.hpp" -#include "optimization/solver/util/Filter.hpp" -#include "optimization/solver/util/FractionToTheBoundaryRule.hpp" -#include "optimization/solver/util/IsLocallyInfeasible.hpp" -#include "optimization/solver/util/KKTError.hpp" -#include "sleipnir/autodiff/Gradient.hpp" -#include "sleipnir/autodiff/Hessian.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/Spy.hpp" -#include "util/ScopeExit.hpp" -#include "util/ToMilliseconds.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. -// -// See docs/algorithms.md#Interior-point_method for a derivation of the -// interior-point method formulation being used. - -namespace sleipnir { - -void InteriorPoint(std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, bool feasibilityRestoration, - Eigen::VectorXd& x, Eigen::VectorXd& s, - SolverStatus* status) { - const auto solveStartTime = std::chrono::system_clock::now(); - - // Map decision variables and constraints to VariableMatrices for Lagrangian - VariableMatrix xAD{decisionVariables}; - xAD.SetValue(x); - VariableMatrix c_eAD{equalityConstraints}; - VariableMatrix c_iAD{inequalityConstraints}; - - // Create autodiff variables for s, y, and z for Lagrangian - VariableMatrix sAD(inequalityConstraints.size()); - sAD.SetValue(s); - VariableMatrix yAD(equalityConstraints.size()); - for (auto& y : yAD) { - y.SetValue(0.0); - } - VariableMatrix zAD(inequalityConstraints.size()); - for (auto& z : zAD) { - z.SetValue(1.0); - } - - // Lagrangian L - // - // L(xₖ, sₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀ(cᵢ(xₖ) − sₖ) - auto L = f - (yAD.T() * c_eAD)(0) - (zAD.T() * (c_iAD - sAD))(0); - - // Equality constraint Jacobian Aₑ - // - // [∇ᵀcₑ₁(xₖ)] - // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcₑₘ(xₖ)] - Jacobian jacobianCe{c_eAD, xAD}; - Eigen::SparseMatrix A_e = jacobianCe.Value(); - - // Inequality constraint Jacobian Aᵢ - // - // [∇ᵀcᵢ₁(xₖ)] - // Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcᵢₘ(xₖ)] - Jacobian jacobianCi{c_iAD, xAD}; - Eigen::SparseMatrix A_i = jacobianCi.Value(); - - // Gradient of f ∇f - Gradient gradientF{f, xAD}; - Eigen::SparseVector g = gradientF.Value(); - - // Hessian of the Lagrangian H - // - // Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ) - Hessian hessianL{L, xAD}; - Eigen::SparseMatrix H = hessianL.Value(); - - Eigen::VectorXd y = yAD.Value(); - Eigen::VectorXd z = zAD.Value(); - Eigen::VectorXd c_e = c_eAD.Value(); - Eigen::VectorXd c_i = c_iAD.Value(); - - // Check for overconstrained problem - if (equalityConstraints.size() > decisionVariables.size()) { - if (config.diagnostics) { - sleipnir::println("The problem has too few degrees of freedom."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kTooFewDOFs; - return; - } - - // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ) - if (!std::isfinite(f.Value()) || !c_e.allFinite() || !c_i.allFinite()) { - status->exitCondition = - SolverExitCondition::kNonfiniteInitialCostOrConstraints; - return; - } - - // Sparsity pattern files written when spy flag is set in SolverConfig - std::ofstream H_spy; - std::ofstream A_e_spy; - std::ofstream A_i_spy; - if (config.spy) { - A_e_spy.open("A_e.spy"); - A_i_spy.open("A_i.spy"); - H_spy.open("H.spy"); - } - - if (config.diagnostics && !feasibilityRestoration) { - sleipnir::println("Error tolerance: {}\n", config.tolerance); - } - - std::chrono::system_clock::time_point iterationsStartTime; - - int iterations = 0; - - // Prints final diagnostics when the solver exits - scope_exit exit{[&] { - status->cost = f.Value(); - - if (config.diagnostics && !feasibilityRestoration) { - auto solveEndTime = std::chrono::system_clock::now(); - - sleipnir::println("\nSolve time: {:.3f} ms", - ToMilliseconds(solveEndTime - solveStartTime)); - sleipnir::println(" ↳ {:.3f} ms (solver setup)", - ToMilliseconds(iterationsStartTime - solveStartTime)); - if (iterations > 0) { - sleipnir::println( - " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", - ToMilliseconds(solveEndTime - iterationsStartTime), iterations, - ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); - } - sleipnir::println(""); - - sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", - "setup (ms)", "avg solve (ms)", "solves"); - sleipnir::println("{:=^47}", ""); - constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; - sleipnir::println(format, "∇f(x)", - gradientF.GetProfiler().SetupDuration(), - gradientF.GetProfiler().AverageSolveDuration(), - gradientF.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), - hessianL.GetProfiler().AverageSolveDuration(), - hessianL.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cₑ/∂x", - jacobianCe.GetProfiler().SetupDuration(), - jacobianCe.GetProfiler().AverageSolveDuration(), - jacobianCe.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cᵢ/∂x", - jacobianCi.GetProfiler().SetupDuration(), - jacobianCi.GetProfiler().AverageSolveDuration(), - jacobianCi.GetProfiler().SolveMeasurements()); - sleipnir::println(""); - } - }}; - - // Barrier parameter minimum - const double μ_min = config.tolerance / 10.0; - - // Barrier parameter μ - double μ = 0.1; - - // Fraction-to-the-boundary rule scale factor minimum - constexpr double τ_min = 0.99; - - // Fraction-to-the-boundary rule scale factor τ - double τ = τ_min; - - Filter filter{f}; - - // This should be run when the error estimate is below a desired threshold for - // the current barrier parameter - auto UpdateBarrierParameterAndResetFilter = [&] { - // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). - constexpr double κ_μ = 0.2; - - // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, - // 2). - constexpr double θ_μ = 1.5; - - // Update the barrier parameter. - // - // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) - // - // See equation (7) of [2]. - μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); - - // Update the fraction-to-the-boundary rule scaling factor. - // - // τⱼ = max(τₘᵢₙ, 1 − μⱼ) - // - // See equation (8) of [2]. - τ = std::max(τ_min, 1.0 - μ); - - // Reset the filter when the barrier parameter is updated - filter.Reset(); - }; - - // Kept outside the loop so its storage can be reused - wpi::SmallVector> triplets; - - RegularizedLDLT solver; - - // Variables for determining when a step is acceptable - constexpr double α_red_factor = 0.5; - int acceptableIterCounter = 0; - - int fullStepRejectedCounter = 0; - int stepTooSmallCounter = 0; - - // Error estimate - double E_0 = std::numeric_limits::infinity(); - - if (config.diagnostics) { - iterationsStartTime = std::chrono::system_clock::now(); - } - - while (E_0 > config.tolerance && - acceptableIterCounter < config.maxAcceptableIterations) { - std::chrono::system_clock::time_point innerIterStartTime; - if (config.diagnostics) { - innerIterStartTime = std::chrono::system_clock::now(); - } - - // Check for local equality constraint infeasibility - if (IsEqualityLocallyInfeasible(A_e, c_e)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is locally infeasible due to violated equality " - "constraints."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for local inequality constraint infeasibility - if (IsInequalityLocallyInfeasible(A_i, c_i)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is infeasible due to violated inequality " - "constraints."); - sleipnir::println( - "Violated constraints (cᵢ(x) ≥ 0) in order of declaration:"); - for (int row = 0; row < c_i.rows(); ++row) { - if (c_i(row) < 0.0) { - sleipnir::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for diverging iterates - if (x.lpNorm() > 1e20 || !x.allFinite() || - s.lpNorm() > 1e20 || !s.allFinite()) { - status->exitCondition = SolverExitCondition::kDivergingIterates; - return; - } - - // Write out spy file contents if that's enabled - if (config.spy) { - // Gap between sparsity patterns - if (iterations > 0) { - A_e_spy << "\n"; - A_i_spy << "\n"; - H_spy << "\n"; - } - - Spy(H_spy, H); - Spy(A_e_spy, A_e); - Spy(A_i_spy, A_i); - } - - // Call user callback - if (callback({iterations, x, s, g, H, A_e, A_i})) { - status->exitCondition = SolverExitCondition::kCallbackRequestedStop; - return; - } - - // [s₁ 0 ⋯ 0 ] - // S = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 sₘ] - const auto S = s.asDiagonal(); - Eigen::SparseMatrix Sinv; - Sinv = s.cwiseInverse().asDiagonal(); - - // [z₁ 0 ⋯ 0 ] - // Z = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 zₘ] - const auto Z = z.asDiagonal(); - Eigen::SparseMatrix Zinv; - Zinv = z.cwiseInverse().asDiagonal(); - - // Σ = S⁻¹Z - const Eigen::SparseMatrix Σ = Sinv * Z; - - // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] - // [ Aₑ 0 ] - // - // Don't assign upper triangle because solver only uses lower triangle. - const Eigen::SparseMatrix topLeft = - H.triangularView() + - (A_i.transpose() * Σ * A_i).triangularView(); - triplets.clear(); - triplets.reserve(topLeft.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - Eigen::SparseMatrix lhs( - decisionVariables.size() + equalityConstraints.size(), - decisionVariables.size() + equalityConstraints.size()); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); - - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑ ] - Eigen::VectorXd rhs{x.rows() + y.rows()}; - rhs.segment(0, x.rows()) = - -(g - A_e.transpose() * y + - A_i.transpose() * (Sinv * (Z * c_i - μ * e) - z)); - rhs.segment(x.rows(), y.rows()) = -c_e; - - // Solve the Newton-KKT system - // - // [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ Aₑ 0 ][−pₖʸ] [ cₑ ] - solver.Compute(lhs, equalityConstraints.size(), μ); - Eigen::VectorXd step{x.rows() + y.rows()}; - if (solver.Info() == Eigen::Success) { - step = solver.Solve(rhs); - } else { - // The regularization procedure failed due to a rank-deficient equality - // constraint Jacobian with linearly dependent constraints. Set the step - // length to zero and let second-order corrections attempt to restore - // feasibility. - step.setZero(); - } - - // step = [ pₖˣ] - // [−pₖʸ] - Eigen::VectorXd p_x = step.segment(0, x.rows()); - Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - Eigen::VectorXd p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - Eigen::VectorXd p_s = μ * Zinv * e - s - Zinv * S * p_z; - - // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - const double α_max = FractionToTheBoundaryRule(s, p_s, τ); - double α = α_max; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z = FractionToTheBoundaryRule(z, p_z, τ); - - // Loop until a step is accepted. If a step becomes acceptable, the loop - // will exit early. - while (1) { - Eigen::VectorXd trial_x = x + α * p_x; - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce - // step size immediately - if (!std::isfinite(f.Value()) || !trial_c_e.allFinite() || - !trial_c_i.allFinite()) { - // Reduce step size - α *= α_red_factor; - continue; - } - - Eigen::VectorXd trial_s; - if (config.feasibleIPM && c_i.cwiseGreater(0.0).all()) { - // If the inequality constraints are all feasible, prevent them from - // becoming infeasible again. - // - // See equation (19.30) in [1]. - trial_s = trial_c_i; - } else { - trial_s = s + α * p_s; - } - - // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.TryAdd(entry)) { - // Accept step - break; - } - - double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); - double nextConstraintViolation = - trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); - - // Second-order corrections - // - // If first trial point was rejected and constraint violation stayed the - // same or went up, apply second-order corrections - if (nextConstraintViolation >= prevConstraintViolation) { - // Apply second-order corrections. See section 2.4 of [2]. - Eigen::VectorXd p_x_cor = p_x; - Eigen::VectorXd p_y_soc = p_y; - Eigen::VectorXd p_z_soc = p_z; - Eigen::VectorXd p_s_soc = p_s; - - double α_soc = α; - Eigen::VectorXd c_e_soc = c_e; - - bool stepAcceptable = false; - for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; - ++soc_iteration) { - // Rebuild Newton-KKT rhs with updated constraint values. - // - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑˢᵒᶜ ] - // - // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) - c_e_soc = α_soc * c_e_soc + trial_c_e; - rhs.bottomRows(y.rows()) = -c_e_soc; - - // Solve the Newton-KKT system - step = solver.Solve(rhs); - - p_x_cor = step.segment(0, x.rows()); - p_y_soc = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - p_s_soc = μ * Zinv * e - s - Zinv * S * p_z_soc; - - // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ); - trial_x = x + α_soc * p_x_cor; - trial_s = s + α_soc * p_s_soc; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ); - trial_y = y + α_z_soc * p_y_soc; - trial_z = z + α_z_soc * p_z_soc; - - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - trial_c_i = c_iAD.Value(); - - // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.TryAdd(entry)) { - p_x = p_x_cor; - p_y = p_y_soc; - p_z = p_z_soc; - p_s = p_s_soc; - α = α_soc; - α_z = α_z_soc; - stepAcceptable = true; - } - } - - if (stepAcceptable) { - // Accept step - break; - } - } - - // If we got here and α is the full step, the full step was rejected. - // Increment the full-step rejected counter to keep track of how many full - // steps have been rejected in a row. - if (α == α_max) { - ++fullStepRejectedCounter; - } - - // If the full step was rejected enough times in a row, reset the filter - // because it may be impeding progress. - // - // See section 3.2 case I of [2]. - if (fullStepRejectedCounter >= 4 && - filter.maxConstraintViolation > entry.constraintViolation / 10.0) { - filter.maxConstraintViolation *= 0.1; - filter.Reset(); - continue; - } - - // Reduce step size - α *= α_red_factor; - - // Safety factor for the minimal step size - constexpr double α_min_frac = 0.05; - - // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, invoke feasibility restoration. - if (α < α_min_frac * Filter::γConstraint) { - double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ); - - Eigen::VectorXd trial_x = x + α_max * p_x; - Eigen::VectorXd trial_s = s + α_max * p_s; - - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - // Upate autodiff - xAD.SetValue(trial_x); - sAD.SetValue(trial_s); - yAD.SetValue(trial_y); - zAD.SetValue(trial_z); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - double nextKKTError = KKTError(gradientF.Value(), jacobianCe.Value(), - trial_c_e, jacobianCi.Value(), trial_c_i, - trial_s, trial_y, trial_z, μ); - - // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway - if (nextKKTError <= 0.999 * currentKKTError) { - α = α_max; - - // Accept step - break; - } - - // If the step direction was bad and feasibility restoration is - // already running, running it again won't help - if (feasibilityRestoration) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ); - - // Feasibility restoration phase - Eigen::VectorXd fr_x = x; - Eigen::VectorXd fr_s = s; - SolverStatus fr_status; - FeasibilityRestoration( - decisionVariables, equalityConstraints, inequalityConstraints, μ, - [&](const SolverIterationInfo& info) { - Eigen::VectorXd trial_x = - info.x.segment(0, decisionVariables.size()); - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_s = - info.s.segment(0, inequalityConstraints.size()); - sAD.SetValue(trial_s); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - Eigen::VectorXd trial_c_i = c_iAD.Value(); - - // If current iterate is acceptable to normal filter and - // constraint violation has sufficiently reduced, stop - // feasibility restoration - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); - if (filter.IsAcceptable(entry) && - entry.constraintViolation < - 0.9 * initialEntry.constraintViolation) { - return true; - } - - return false; - }, - config, fr_x, fr_s, &fr_status); - - if (fr_status.exitCondition == - SolverExitCondition::kCallbackRequestedStop) { - p_x = fr_x - x; - p_s = fr_s - s; - - // Lagrange mutliplier estimates - // - // [y] = (ÂÂᵀ)⁻¹Â[ ∇f] - // [z] [−μe] - // - // where  = [Aₑ 0] - // [Aᵢ −S] - // - // See equation (19.37) of [1]. - { - xAD.SetValue(fr_x); - sAD.SetValue(c_iAD.Value()); - - A_e = jacobianCe.Value(); - A_i = jacobianCi.Value(); - g = gradientF.Value(); - - //  = [Aₑ 0] - // [Aᵢ −S] - triplets.clear(); - triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows()); - for (int col = 0; col < A_e.cols(); ++col) { - // Append column of Aₑ in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aᵢ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_i, col}; it; - ++it) { - triplets.emplace_back(A_e.rows() + it.row(), it.col(), - it.value()); - } - } - // Append −S in bottom-right quadrant - for (int i = 0; i < s.rows(); ++i) { - triplets.emplace_back(A_e.rows() + i, A_e.cols() + i, -s(i)); - } - Eigen::SparseMatrix Ahat{A_e.rows() + A_i.rows(), - A_e.cols() + s.rows()}; - Ahat.setFromSortedTriplets( - triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - // lhs = ÂÂᵀ - Eigen::SparseMatrix lhs = Ahat * Ahat.transpose(); - - // rhs = Â[ ∇f] - // [−μe] - Eigen::VectorXd rhsTemp{g.rows() + e.rows()}; - rhsTemp.block(0, 0, g.rows(), 1) = g; - rhsTemp.block(g.rows(), 0, s.rows(), 1) = -μ * e; - Eigen::VectorXd rhs = Ahat * rhsTemp; - - Eigen::SimplicialLDLT> yzEstimator{lhs}; - Eigen::VectorXd sol = yzEstimator.solve(rhs); - - p_y = y - sol.block(0, 0, y.rows(), 1); - p_z = z - sol.block(y.rows(), 0, z.rows(), 1); - } - - α = 1.0; - α_z = 1.0; - - // Accept step - break; - } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - x = fr_x; - return; - } else { - status->exitCondition = - SolverExitCondition::kFeasibilityRestorationFailed; - x = fr_x; - return; - } - } - } - - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - fullStepRejectedCounter = 0; - } - - // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when - // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. - // - // See section 3.9 of [2]. - double maxStepScaled = 0.0; - for (int row = 0; row < x.rows(); ++row) { - maxStepScaled = std::max(maxStepScaled, - std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); - } - if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { - α = α_max; - ++stepTooSmallCounter; - } else { - stepTooSmallCounter = 0; - } - - // xₖ₊₁ = xₖ + αₖpₖˣ - // sₖ₊₁ = sₖ + αₖpₖˢ - // yₖ₊₁ = yₖ + αₖᶻpₖʸ - // zₖ₊₁ = zₖ + αₖᶻpₖᶻ - x += α * p_x; - s += α * p_s; - y += α_z * p_y; - z += α_z * p_z; - - // A requirement for the convergence proof is that the "primal-dual barrier - // term Hessian" Σₖ does not deviate arbitrarily much from the "primal - // Hessian" μⱼSₖ⁻². We ensure this by resetting - // - // zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾)) - // - // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. - { - // Barrier parameter scale factor for inequality constraint Lagrange - // multiplier safeguard - constexpr double κ_Σ = 1e10; - - for (int row = 0; row < z.rows(); ++row) { - z(row) = - std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row))); - } - } - - // Update autodiff for Jacobians and Hessian - xAD.SetValue(x); - sAD.SetValue(s); - yAD.SetValue(y); - zAD.SetValue(z); - A_e = jacobianCe.Value(); - A_i = jacobianCi.Value(); - g = gradientF.Value(); - H = hessianL.Value(); - - c_e = c_eAD.Value(); - c_i = c_iAD.Value(); - - // Update the error estimate - E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); - if (E_0 < config.acceptableTolerance) { - ++acceptableIterCounter; - } else { - acceptableIterCounter = 0; - } - - // Update the barrier parameter if necessary - if (E_0 > config.tolerance) { - // Barrier parameter scale factor for tolerance checks - constexpr double κ_ε = 10.0; - - // While the error estimate is below the desired threshold for this - // barrier parameter value, decrease the barrier parameter further - double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - while (μ > μ_min && E_μ <= κ_ε * μ) { - UpdateBarrierParameterAndResetFilter(); - E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - } - } - - const auto innerIterEndTime = std::chrono::system_clock::now(); - - // Diagnostics for current iteration - if (config.diagnostics) { - if (iterations % 20 == 0) { - sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", - "time (ms)", "error", "cost", "infeasibility"); - sleipnir::println("{:=^61}", ""); - } - - sleipnir::println("{:4}{} {:9.3f} {:13e} {:13e} {:13e}", iterations, - feasibilityRestoration ? "r" : " ", - ToMilliseconds(innerIterEndTime - innerIterStartTime), - E_0, f.Value(), - c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()); - } - - ++iterations; - - // Check for max iterations - if (iterations >= config.maxIterations) { - status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; - return; - } - - // Check for max wall clock time - if (innerIterEndTime - solveStartTime > config.timeout) { - status->exitCondition = SolverExitCondition::kTimeout; - return; - } - - // Check for solve to acceptable tolerance - if (E_0 > config.tolerance && - acceptableIterCounter == config.maxAcceptableIterations) { - status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; - return; - } - - // The search direction has been very small twice, so assume the problem has - // been solved as well as possible given finite precision and reduce the - // barrier parameter. - // - // See section 3.9 of [2]. - if (stepTooSmallCounter >= 2 && μ > μ_min) { - UpdateBarrierParameterAndResetFilter(); - continue; - } - } -} // NOLINT(readability/fn_size) - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp deleted file mode 100644 index 662abc2fb6..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp +++ /dev/null @@ -1,559 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/optimization/solver/SQP.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include "optimization/RegularizedLDLT.hpp" -#include "optimization/solver/util/ErrorEstimate.hpp" -#include "optimization/solver/util/FeasibilityRestoration.hpp" -#include "optimization/solver/util/Filter.hpp" -#include "optimization/solver/util/IsLocallyInfeasible.hpp" -#include "optimization/solver/util/KKTError.hpp" -#include "sleipnir/autodiff/Gradient.hpp" -#include "sleipnir/autodiff/Hessian.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/Print.hpp" -#include "sleipnir/util/Spy.hpp" -#include "util/ScopeExit.hpp" -#include "util/ToMilliseconds.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. - -namespace sleipnir { - -void SQP(std::span decisionVariables, - std::span equalityConstraints, Variable& f, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { - const auto solveStartTime = std::chrono::system_clock::now(); - - // Map decision variables and constraints to VariableMatrices for Lagrangian - VariableMatrix xAD{decisionVariables}; - xAD.SetValue(x); - VariableMatrix c_eAD{equalityConstraints}; - - // Create autodiff variables for y for Lagrangian - VariableMatrix yAD(equalityConstraints.size()); - for (auto& y : yAD) { - y.SetValue(0.0); - } - - // Lagrangian L - // - // L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) - auto L = f - (yAD.T() * c_eAD)(0); - - // Equality constraint Jacobian Aₑ - // - // [∇ᵀcₑ₁(xₖ)] - // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - // [ ⋮ ] - // [∇ᵀcₑₘ(xₖ)] - Jacobian jacobianCe{c_eAD, xAD}; - Eigen::SparseMatrix A_e = jacobianCe.Value(); - - // Gradient of f ∇f - Gradient gradientF{f, xAD}; - Eigen::SparseVector g = gradientF.Value(); - - // Hessian of the Lagrangian H - // - // Hₖ = ∇²ₓₓL(xₖ, yₖ) - Hessian hessianL{L, xAD}; - Eigen::SparseMatrix H = hessianL.Value(); - - Eigen::VectorXd y = yAD.Value(); - Eigen::VectorXd c_e = c_eAD.Value(); - - // Check for overconstrained problem - if (equalityConstraints.size() > decisionVariables.size()) { - if (config.diagnostics) { - sleipnir::println("The problem has too few degrees of freedom."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kTooFewDOFs; - return; - } - - // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) - if (!std::isfinite(f.Value()) || !c_e.allFinite()) { - status->exitCondition = - SolverExitCondition::kNonfiniteInitialCostOrConstraints; - return; - } - - // Sparsity pattern files written when spy flag is set in SolverConfig - std::ofstream H_spy; - std::ofstream A_e_spy; - if (config.spy) { - A_e_spy.open("A_e.spy"); - H_spy.open("H.spy"); - } - - if (config.diagnostics) { - sleipnir::println("Error tolerance: {}\n", config.tolerance); - } - - std::chrono::system_clock::time_point iterationsStartTime; - - int iterations = 0; - - // Prints final diagnostics when the solver exits - scope_exit exit{[&] { - status->cost = f.Value(); - - if (config.diagnostics) { - auto solveEndTime = std::chrono::system_clock::now(); - - sleipnir::println("\nSolve time: {:.3f} ms", - ToMilliseconds(solveEndTime - solveStartTime)); - sleipnir::println(" ↳ {:.3f} ms (solver setup)", - ToMilliseconds(iterationsStartTime - solveStartTime)); - if (iterations > 0) { - sleipnir::println( - " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", - ToMilliseconds(solveEndTime - iterationsStartTime), iterations, - ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); - } - sleipnir::println(""); - - sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", - "setup (ms)", "avg solve (ms)", "solves"); - sleipnir::println("{:=^47}", ""); - constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; - sleipnir::println(format, "∇f(x)", - gradientF.GetProfiler().SetupDuration(), - gradientF.GetProfiler().AverageSolveDuration(), - gradientF.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), - hessianL.GetProfiler().AverageSolveDuration(), - hessianL.GetProfiler().SolveMeasurements()); - sleipnir::println(format, "∂cₑ/∂x", - jacobianCe.GetProfiler().SetupDuration(), - jacobianCe.GetProfiler().AverageSolveDuration(), - jacobianCe.GetProfiler().SolveMeasurements()); - sleipnir::println(""); - } - }}; - - Filter filter{f}; - - // Kept outside the loop so its storage can be reused - wpi::SmallVector> triplets; - - RegularizedLDLT solver; - - // Variables for determining when a step is acceptable - constexpr double α_red_factor = 0.5; - int acceptableIterCounter = 0; - - int fullStepRejectedCounter = 0; - - // Error estimate - double E_0 = std::numeric_limits::infinity(); - - if (config.diagnostics) { - iterationsStartTime = std::chrono::system_clock::now(); - } - - while (E_0 > config.tolerance && - acceptableIterCounter < config.maxAcceptableIterations) { - std::chrono::system_clock::time_point innerIterStartTime; - if (config.diagnostics) { - innerIterStartTime = std::chrono::system_clock::now(); - } - - // Check for local equality constraint infeasibility - if (IsEqualityLocallyInfeasible(A_e, c_e)) { - if (config.diagnostics) { - sleipnir::println( - "The problem is locally infeasible due to violated equality " - "constraints."); - sleipnir::println( - "Violated constraints (cₑ(x) = 0) in order of declaration:"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // Check for diverging iterates - if (x.lpNorm() > 1e20 || !x.allFinite()) { - status->exitCondition = SolverExitCondition::kDivergingIterates; - return; - } - - // Write out spy file contents if that's enabled - if (config.spy) { - // Gap between sparsity patterns - if (iterations > 0) { - A_e_spy << "\n"; - H_spy << "\n"; - } - - Spy(H_spy, H); - Spy(A_e_spy, A_e); - } - - // Call user callback - if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e, - Eigen::SparseMatrix{}})) { - status->exitCondition = SolverExitCondition::kCallbackRequestedStop; - return; - } - - // lhs = [H Aₑᵀ] - // [Aₑ 0 ] - // - // Don't assign upper triangle because solver only uses lower triangle. - const Eigen::SparseMatrix topLeft = - H.triangularView(); - triplets.clear(); - triplets.reserve(topLeft.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; - ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - Eigen::SparseMatrix lhs( - decisionVariables.size() + equalityConstraints.size(), - decisionVariables.size() + equalityConstraints.size()); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); - - // rhs = −[∇f − Aₑᵀy] - // [ cₑ ] - Eigen::VectorXd rhs{x.rows() + y.rows()}; - rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y); - rhs.segment(x.rows(), y.rows()) = -c_e; - - // Solve the Newton-KKT system - // - // [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy] - // [Aₑ 0 ][−pₖʸ] [ cₑ ] - solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0); - Eigen::VectorXd step{x.rows() + y.rows()}; - if (solver.Info() == Eigen::Success) { - step = solver.Solve(rhs); - } else { - // The regularization procedure failed due to a rank-deficient equality - // constraint Jacobian with linearly dependent constraints - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return; - } - - // step = [ pₖˣ] - // [−pₖʸ] - Eigen::VectorXd p_x = step.segment(0, x.rows()); - Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); - - constexpr double α_max = 1.0; - double α = α_max; - - // Loop until a step is accepted. If a step becomes acceptable, the loop - // will exit early. - while (1) { - Eigen::VectorXd trial_x = x + α * p_x; - Eigen::VectorXd trial_y = y + α * p_y; - - xAD.SetValue(trial_x); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - - // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size - // immediately - if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) { - // Reduce step size - α *= α_red_factor; - continue; - } - - // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_c_e); - if (filter.TryAdd(entry)) { - // Accept step - break; - } - - double prevConstraintViolation = c_e.lpNorm<1>(); - double nextConstraintViolation = trial_c_e.lpNorm<1>(); - - // Second-order corrections - // - // If first trial point was rejected and constraint violation stayed the - // same or went up, apply second-order corrections - if (nextConstraintViolation >= prevConstraintViolation) { - // Apply second-order corrections. See section 2.4 of [2]. - Eigen::VectorXd p_x_cor = p_x; - Eigen::VectorXd p_y_soc = p_y; - - double α_soc = α; - Eigen::VectorXd c_e_soc = c_e; - - bool stepAcceptable = false; - for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; - ++soc_iteration) { - // Rebuild Newton-KKT rhs with updated constraint values. - // - // rhs = −[∇f − Aₑᵀy] - // [ cₑˢᵒᶜ ] - // - // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) - c_e_soc = α_soc * c_e_soc + trial_c_e; - rhs.bottomRows(y.rows()) = -c_e_soc; - - // Solve the Newton-KKT system - step = solver.Solve(rhs); - - p_x_cor = step.segment(0, x.rows()); - p_y_soc = -step.segment(x.rows(), y.rows()); - - trial_x = x + α_soc * p_x_cor; - trial_y = y + α_soc * p_y_soc; - - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - - // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_c_e); - if (filter.TryAdd(entry)) { - p_x = p_x_cor; - p_y = p_y_soc; - α = α_soc; - stepAcceptable = true; - } - } - - if (stepAcceptable) { - // Accept step - break; - } - } - - // If we got here and α is the full step, the full step was rejected. - // Increment the full-step rejected counter to keep track of how many full - // steps have been rejected in a row. - if (α == α_max) { - ++fullStepRejectedCounter; - } - - // If the full step was rejected enough times in a row, reset the filter - // because it may be impeding progress. - // - // See section 3.2 case I of [2]. - if (fullStepRejectedCounter >= 4 && - filter.maxConstraintViolation > entry.constraintViolation / 10.0) { - filter.maxConstraintViolation *= 0.1; - filter.Reset(); - continue; - } - - // Reduce step size - α *= α_red_factor; - - // Safety factor for the minimal step size - constexpr double α_min_frac = 0.05; - - // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, report infeasible. - if (α < α_min_frac * Filter::γConstraint) { - double currentKKTError = KKTError(g, A_e, c_e, y); - - Eigen::VectorXd trial_x = x + α_max * p_x; - Eigen::VectorXd trial_y = y + α_max * p_y; - - // Upate autodiff - xAD.SetValue(trial_x); - yAD.SetValue(trial_y); - - Eigen::VectorXd trial_c_e = c_eAD.Value(); - - double nextKKTError = - KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y); - - // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway - if (nextKKTError <= 0.999 * currentKKTError) { - α = α_max; - - // Accept step - break; - } - - auto initialEntry = filter.MakeEntry(c_e); - - // Feasibility restoration phase - Eigen::VectorXd fr_x = x; - SolverStatus fr_status; - FeasibilityRestoration( - decisionVariables, equalityConstraints, - [&](const SolverIterationInfo& info) { - trial_x = info.x.segment(0, decisionVariables.size()); - xAD.SetValue(trial_x); - - trial_c_e = c_eAD.Value(); - - // If current iterate is acceptable to normal filter and - // constraint violation has sufficiently reduced, stop - // feasibility restoration - entry = filter.MakeEntry(trial_c_e); - if (filter.IsAcceptable(entry) && - entry.constraintViolation < - 0.9 * initialEntry.constraintViolation) { - return true; - } - - return false; - }, - config, fr_x, &fr_status); - - if (fr_status.exitCondition == - SolverExitCondition::kCallbackRequestedStop) { - p_x = fr_x - x; - - // Lagrange mutliplier estimates - // - // y = (AₑAₑᵀ)⁻¹Aₑ∇f - // - // See equation (19.37) of [1]. - { - xAD.SetValue(fr_x); - - A_e = jacobianCe.Value(); - g = gradientF.Value(); - - // lhs = AₑAₑᵀ - Eigen::SparseMatrix lhs = A_e * A_e.transpose(); - - // rhs = Aₑ∇f - Eigen::VectorXd rhs = A_e * g; - - Eigen::SimplicialLDLT> yEstimator{lhs}; - Eigen::VectorXd sol = yEstimator.solve(rhs); - - p_y = y - sol.block(0, 0, y.rows(), 1); - } - - α = 1.0; - - // Accept step - break; - } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - x = fr_x; - return; - } else { - status->exitCondition = - SolverExitCondition::kFeasibilityRestorationFailed; - x = fr_x; - return; - } - } - } - - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - fullStepRejectedCounter = 0; - } - - // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when - // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. - // - // See section 3.9 of [2]. - double maxStepScaled = 0.0; - for (int row = 0; row < x.rows(); ++row) { - maxStepScaled = std::max(maxStepScaled, - std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); - } - if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { - α = α_max; - } - - // xₖ₊₁ = xₖ + αₖpₖˣ - // yₖ₊₁ = yₖ + αₖpₖʸ - x += α * p_x; - y += α * p_y; - - // Update autodiff for Jacobians and Hessian - xAD.SetValue(x); - yAD.SetValue(y); - A_e = jacobianCe.Value(); - g = gradientF.Value(); - H = hessianL.Value(); - - c_e = c_eAD.Value(); - - // Update the error estimate - E_0 = ErrorEstimate(g, A_e, c_e, y); - if (E_0 < config.acceptableTolerance) { - ++acceptableIterCounter; - } else { - acceptableIterCounter = 0; - } - - const auto innerIterEndTime = std::chrono::system_clock::now(); - - // Diagnostics for current iteration - if (config.diagnostics) { - if (iterations % 20 == 0) { - sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", - "time (ms)", "error", "cost", "infeasibility"); - sleipnir::println("{:=^61}", ""); - } - - sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations, - ToMilliseconds(innerIterEndTime - innerIterStartTime), - E_0, f.Value(), c_e.lpNorm<1>()); - } - - ++iterations; - - // Check for max iterations - if (iterations >= config.maxIterations) { - status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; - return; - } - - // Check for max wall clock time - if (innerIterEndTime - solveStartTime > config.timeout) { - status->exitCondition = SolverExitCondition::kTimeout; - return; - } - - // Check for solve to acceptable tolerance - if (E_0 > config.tolerance && - acceptableIterCounter == config.maxAcceptableIterations) { - status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; - return; - } - } -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp new file mode 100644 index 0000000000..b1421bf961 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp @@ -0,0 +1,664 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/interior_point.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/fraction_to_the_boundary_rule.hpp" +#include "optimization/solver/util/is_locally_infeasible.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. +// +// See docs/algorithms.md#Interior-point_method for a derivation of the +// interior-point method formulation being used. + +namespace { + +/** + * Interior-point method step direction. + */ +struct Step { + /// Primal step. + Eigen::VectorXd p_x; + /// Equality constraint dual step. + Eigen::VectorXd p_y; + /// Inequality constraint slack variable step. + Eigen::VectorXd p_s; + /// Inequality constraint dual step. + Eigen::VectorXd p_z; +}; + +} // namespace + +namespace slp { + +ExitStatus interior_point( + const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + const Eigen::ArrayX& bound_constraint_mask, +#endif + Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix build"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); + solve_profilers.emplace_back(" ↳ cᵢ(x)"); + solve_profilers.emplace_back(" ↳ ∂cᵢ/∂x"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_build_prof = solve_profilers[5]; + auto& linear_system_compute_prof = solve_profilers[6]; + auto& linear_system_solve_prof = solve_profilers[7]; + auto& line_search_prof = solve_profilers[8]; + auto& soc_prof = solve_profilers[9]; + auto& next_iter_prep_prof = solve_profilers[10]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[11]; + auto& g_prof = solve_profilers[12]; + auto& H_prof = solve_profilers[13]; + auto& c_e_prof = solve_profilers[14]; + auto& A_e_prof = solve_profilers[15]; + auto& c_i_prof = solve_profilers[16]; + auto& A_i_prof = solve_profilers[17]; + + InteriorPointMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x, const Eigen::VectorXd& y, + const Eigen::VectorXd& z) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x, y, z); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_e_prof}; + return matrix_callbacks.c_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_e_prof}; + return matrix_callbacks.A_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_i_prof}; + return matrix_callbacks.c_i(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_i_prof}; + return matrix_callbacks.A_i(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + Eigen::VectorXd c_e = matrices.c_e(x); + Eigen::VectorXd c_i = matrices.c_i(x); + + int num_decision_variables = x.rows(); + int num_equality_constraints = c_e.rows(); + int num_inequality_constraints = c_i.rows(); + + // Check for overconstrained problem + if (num_equality_constraints > num_decision_variables) { + if (options.diagnostics) { + print_too_few_dofs_error(c_e); + } + + return ExitStatus::TOO_FEW_DOFS; + } + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix A_e = matrices.A_e(x); + Eigen::SparseMatrix A_i = matrices.A_i(x); + + Eigen::VectorXd s = Eigen::VectorXd::Ones(num_inequality_constraints); +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + // We set sʲ = cᵢʲ(x) for each bound inequality constraint index j + s = bound_constraint_mask.select(c_i, s); +#endif + Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints); + Eigen::VectorXd z = Eigen::VectorXd::Ones(num_inequality_constraints); + + Eigen::SparseMatrix H = matrices.H(x, y, z); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(A_e.rows() == num_equality_constraints); + slp_assert(A_e.cols() == num_decision_variables); + slp_assert(A_i.rows() == num_inequality_constraints); + slp_assert(A_i.cols() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ) + if (!std::isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + // Barrier parameter minimum + const double μ_min = options.tolerance / 10.0; + + // Barrier parameter μ + double μ = 0.1; + + // Fraction-to-the-boundary rule scale factor minimum + constexpr double τ_min = 0.99; + + // Fraction-to-the-boundary rule scale factor τ + double τ = τ_min; + + Filter filter; + + // This should be run when the error estimate is below a desired threshold for + // the current barrier parameter + auto update_barrier_parameter_and_reset_filter = [&] { + // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). + constexpr double κ_μ = 0.2; + + // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, + // 2). + constexpr double θ_μ = 1.5; + + // Update the barrier parameter. + // + // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) + // + // See equation (7) of [2]. + μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); + + // Update the fraction-to-the-boundary rule scaling factor. + // + // τⱼ = max(τₘᵢₙ, 1 − μⱼ) + // + // See equation (8) of [2]. + τ = std::max(τ_min, 1.0 - μ); + + // Reset the filter when the barrier parameter is updated + filter.reset(); + }; + + // Kept outside the loop so its storage can be reused + gch::small_vector> triplets; + + RegularizedLDLT solver{num_decision_variables, num_equality_constraints}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-7; + + int full_step_rejected_counter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for local equality constraint infeasibility + if (is_equality_locally_infeasible(A_e, c_e)) { + if (options.diagnostics) { + print_c_e_local_infeasibility_error(c_e); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for local inequality constraint infeasibility + if (is_inequality_locally_infeasible(A_i, c_i)) { + if (options.diagnostics) { + print_c_i_local_infeasibility_error(c_i); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite() || + s.lpNorm() > 1e10 || !s.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, A_e, A_i})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_build_profiler{linear_system_build_prof}; + + // S = diag(s) + // Z = diag(z) + // Σ = S⁻¹Z + const Eigen::SparseMatrix Σ{s.cwiseInverse().asDiagonal() * + z.asDiagonal()}; + + // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] + // [ Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + const Eigen::SparseMatrix top_left = + H + (A_i.transpose() * Σ * A_i).triangularView(); + triplets.clear(); + triplets.reserve(top_left.nonZeros() + A_e.nonZeros()); + for (int col = 0; col < H.cols(); ++col) { + // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{top_left, col}; it; + ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + num_decision_variables + num_equality_constraints, + num_decision_variables + num_equality_constraints); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = + -g + A_e.transpose() * y + + A_i.transpose() * (-Σ * c_i + μ * s.cwiseInverse() + z); + rhs.segment(x.rows(), y.rows()) = -c_e; + + linear_system_build_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + Step step; + double α_max = 1.0; + double α = 1.0; + double α_z = 1.0; + + // Solve the Newton-KKT system + // + // [H + AᵢᵀΣAᵢ Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ Aₑ 0 ][−pʸ] [ cₑ ] + if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] { + return ExitStatus::FACTORIZATION_FAILED; + } + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + auto compute_step = [&](Step& step) { + // p = [ pˣ] + // [−pʸ] + Eigen::VectorXd p = solver.solve(rhs); + step.p_x = p.segment(0, x.rows()); + step.p_y = -p.segment(x.rows(), y.rows()); + + // pˢ = cᵢ − s + Aᵢpˣ + // pᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpˣ + step.p_s = c_i - s + A_i * step.p_x; + step.p_z = -Σ * c_i + μ * s.cwiseInverse() - Σ * A_i * step.p_x; + }; + compute_step(step); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + α_max = fraction_to_the_boundary_rule(s, step.p_s, τ); + α = α_max; + + // If maximum step size is below minimum, report line search failure + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + α_z = fraction_to_the_boundary_rule(z, step.p_z, τ); + + // Loop until a step is accepted + while (1) { + Eigen::VectorXd trial_x = x + α * step.p_x; + Eigen::VectorXd trial_y = y + α_z * step.p_y; + Eigen::VectorXd trial_z = z + α_z * step.p_z; + + double trial_f = matrices.f(trial_x); + Eigen::VectorXd trial_c_e = matrices.c_e(trial_x); + Eigen::VectorXd trial_c_i = matrices.c_i(trial_x); + + // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce + // step size immediately + if (!std::isfinite(trial_f) || !trial_c_e.allFinite() || + !trial_c_i.allFinite()) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + Eigen::VectorXd trial_s; + if (options.feasible_ipm && c_i.cwiseGreater(0.0).all()) { + // If the inequality constraints are all feasible, prevent them from + // becoming infeasible again. + // + // See equation (19.30) in [1]. + trial_s = trial_c_i; + } else { + trial_s = s + α * step.p_s; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, + α)) { + // Accept step + break; + } + + double prev_constraint_violation = + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); + double next_constraint_violation = + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (α == α_max && + next_constraint_violation >= prev_constraint_violation) { + // Apply second-order corrections. See section 2.4 of [2]. + auto soc_step = step; + + double α_soc = α; + double α_z_soc = α_z; + Eigen::VectorXd c_e_soc = c_e; + + bool step_acceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; + ++soc_iteration) { + ScopedProfiler soc_profiler{soc_prof}; + + scope_exit soc_exit{[&] { + soc_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, + step_acceptable ? IterationType::ACCEPTED_SOC + : IterationType::REJECTED_SOC, + soc_profiler.current_duration(), + error_estimate(g, A_e, trial_c_e, trial_y), trial_f, + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(), + trial_s.dot(trial_z), μ, solver.hessian_regularization(), + α_soc, 1.0, α_reduction_factor, α_z_soc); + } + }}; + + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + compute_step(soc_step); + + // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + α_soc = fraction_to_the_boundary_rule(s, soc_step.p_s, τ); + trial_x = x + α_soc * soc_step.p_x; + trial_s = s + α_soc * soc_step.p_s; + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + α_z_soc = fraction_to_the_boundary_rule(z, soc_step.p_z, τ); + trial_y = y + α_z_soc * soc_step.p_y; + trial_z = z + α_z_soc * soc_step.p_z; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + trial_c_i = matrices.c_i(trial_x); + + // Constraint violation scale factor for second-order corrections + constexpr double κ_soc = 0.99; + + // If constraint violation hasn't been sufficiently reduced, stop + // making second-order corrections + next_constraint_violation = + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); + if (next_constraint_violation > κ_soc * prev_constraint_violation) { + break; + } + + // Check whether filter accepts trial iterate + if (filter.try_add( + FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, α)) { + step = soc_step; + α = α_soc; + α_z = α_z_soc; + step_acceptable = true; + } + } + + if (step_acceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++full_step_rejected_counter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (full_step_rejected_counter >= 4 && + filter.max_constraint_violation > + filter.back().constraint_violation / 10.0) { + filter.max_constraint_violation *= 0.1; + filter.reset(); + continue; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report line search failure. + if (α < α_min) { + double current_kkt_error = kkt_error(g, A_e, c_e, A_i, c_i, s, y, z, μ); + + trial_x = x + α_max * step.p_x; + trial_s = s + α_max * step.p_s; + + trial_y = y + α_z * step.p_y; + trial_z = z + α_z * step.p_z; + + trial_c_e = matrices.c_e(trial_x); + trial_c_i = matrices.c_i(trial_x); + + double next_kkt_error = kkt_error( + matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x), + matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + full_step_rejected_counter = 0; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // sₖ₊₁ = sₖ + αₖpₖˢ + // yₖ₊₁ = yₖ + αₖᶻpₖʸ + // zₖ₊₁ = zₖ + αₖᶻpₖᶻ + x += α * step.p_x; + s += α * step.p_s; + y += α_z * step.p_y; + z += α_z * step.p_z; + + // A requirement for the convergence proof is that the primal-dual barrier + // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal + // barrier term Hessian μSₖ₊₁⁻². + // + // Σₖ₊₁ = μSₖ₊₁⁻² + // Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻² + // Zₖ₊₁ = μSₖ₊₁⁻¹ + // + // We ensure this by resetting + // + // zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁) + // + // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. + for (int row = 0; row < z.rows(); ++row) { + constexpr double κ_Σ = 1e10; + z[row] = std::clamp(z[row], 1.0 / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); + } + + // Update autodiff for Jacobians and Hessian + f = matrices.f(x); + A_e = matrices.A_e(x); + A_i = matrices.A_i(x); + g = matrices.g(x); + H = matrices.H(x, y, z); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + c_e = matrices.c_e(x); + c_i = matrices.c_i(x); + + // Update the error estimate + E_0 = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); + + // Update the barrier parameter if necessary + if (E_0 > options.tolerance) { + // Barrier parameter scale factor for tolerance checks + constexpr double κ_ε = 10.0; + + // While the error estimate is below the desired threshold for this + // barrier parameter value, decrease the barrier parameter further + double E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + while (μ > μ_min && E_μ <= κ_ε * μ) { + update_barrier_parameter_and_reset_filter(); + E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + } + } + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics(iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, + f, c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(), + s.dot(z), μ, solver.hessian_regularization(), + α, α_max, α_reduction_factor, α_z); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp new file mode 100644 index 0000000000..98ed6c941f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp @@ -0,0 +1,259 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/newton.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace slp { + +ExitStatus newton(const NewtonMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_compute_prof = solve_profilers[5]; + auto& linear_system_solve_prof = solve_profilers[6]; + auto& line_search_prof = solve_profilers[7]; + auto& next_iter_prep_prof = solve_profilers[8]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[9]; + auto& g_prof = solve_profilers[10]; + auto& H_prof = solve_profilers[11]; + + NewtonMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + + int num_decision_variables = x.rows(); + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix H = matrices.H(x); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ) + if (!std::isfinite(f)) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + Filter filter; + + RegularizedLDLT solver{num_decision_variables, 0}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-20; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, Eigen::SparseMatrix{}, + Eigen::SparseMatrix{}})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + // Solve the Newton-KKT system + // + // Hpˣ = −∇f + solver.compute(H); + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + Eigen::VectorXd p_x = solver.solve(-g); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + constexpr double α_max = 1.0; + double α = α_max; + + // Loop until a step is accepted. If a step becomes acceptable, the loop + // will exit early. + while (1) { + Eigen::VectorXd trial_x = x + α * p_x; + + double trial_f = matrices.f(trial_x); + + // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately + if (!std::isfinite(trial_f)) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f}, α)) { + // Accept step + break; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report bad line search. + if (α < α_min) { + double current_kkt_error = kkt_error(g); + + Eigen::VectorXd trial_x = x + α_max * p_x; + + double next_kkt_error = kkt_error(matrices.g(trial_x)); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // xₖ₊₁ = xₖ + αₖpₖˣ + x += α * p_x; + + // Update autodiff for Hessian + f = matrices.f(x); + g = matrices.g(x); + H = matrices.H(x); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + // Update the error estimate + E_0 = error_estimate(g); + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, f, 0.0, 0.0, 0.0, + solver.hessian_regularization(), α, α_max, α_reduction_factor, 1.0); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp new file mode 100644 index 0000000000..04447f8892 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp @@ -0,0 +1,480 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/sqp.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "optimization/regularized_ldlt.hpp" +#include "optimization/solver/util/error_estimate.hpp" +#include "optimization/solver/util/filter.hpp" +#include "optimization/solver/util/is_locally_infeasible.hpp" +#include "optimization/solver/util/kkt_error.hpp" +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/util/assert.hpp" +#include "util/print_diagnostics.hpp" +#include "util/scope_exit.hpp" +#include "util/scoped_profiler.hpp" +#include "util/solve_profiler.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace { + +/** + * SQP step direction. + */ +struct Step { + /// Primal step. + Eigen::VectorXd p_x; + /// Dual step. + Eigen::VectorXd p_y; +}; + +} // namespace + +namespace slp { + +ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, + std::span> + iteration_callbacks, + const Options& options, Eigen::VectorXd& x) { + const auto solve_start_time = std::chrono::steady_clock::now(); + + gch::small_vector solve_profilers; + solve_profilers.emplace_back("solver"); + solve_profilers.emplace_back(" ↳ setup"); + solve_profilers.emplace_back(" ↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility ✓"); + solve_profilers.emplace_back(" ↳ iteration callbacks"); + solve_profilers.emplace_back(" ↳ iter matrix build"); + solve_profilers.emplace_back(" ↳ iter matrix compute"); + solve_profilers.emplace_back(" ↳ iter matrix solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); + + auto& solver_prof = solve_profilers[0]; + auto& setup_prof = solve_profilers[1]; + auto& inner_iter_prof = solve_profilers[2]; + auto& feasibility_check_prof = solve_profilers[3]; + auto& iteration_callbacks_prof = solve_profilers[4]; + auto& linear_system_build_prof = solve_profilers[5]; + auto& linear_system_compute_prof = solve_profilers[6]; + auto& linear_system_solve_prof = solve_profilers[7]; + auto& line_search_prof = solve_profilers[8]; + auto& soc_prof = solve_profilers[9]; + auto& next_iter_prep_prof = solve_profilers[10]; + + // Set up profiled matrix callbacks +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + auto& f_prof = solve_profilers[11]; + auto& g_prof = solve_profilers[12]; + auto& H_prof = solve_profilers[13]; + auto& c_e_prof = solve_profilers[14]; + auto& A_e_prof = solve_profilers[15]; + + SQPMatrixCallbacks matrices{ + [&](const Eigen::VectorXd& x) -> double { + ScopedProfiler prof{f_prof}; + return matrix_callbacks.f(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseVector { + ScopedProfiler prof{g_prof}; + return matrix_callbacks.g(x); + }, + [&](const Eigen::VectorXd& x, + const Eigen::VectorXd& y) -> Eigen::SparseMatrix { + ScopedProfiler prof{H_prof}; + return matrix_callbacks.H(x, y); + }, + [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + ScopedProfiler prof{c_e_prof}; + return matrix_callbacks.c_e(x); + }, + [&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix { + ScopedProfiler prof{A_e_prof}; + return matrix_callbacks.A_e(x); + }}; +#else + const auto& matrices = matrix_callbacks; +#endif + + solver_prof.start(); + setup_prof.start(); + + double f = matrices.f(x); + Eigen::VectorXd c_e = matrices.c_e(x); + + int num_decision_variables = x.rows(); + int num_equality_constraints = c_e.rows(); + + // Check for overconstrained problem + if (num_equality_constraints > num_decision_variables) { + if (options.diagnostics) { + print_too_few_dofs_error(c_e); + } + + return ExitStatus::TOO_FEW_DOFS; + } + + Eigen::SparseVector g = matrices.g(x); + Eigen::SparseMatrix A_e = matrices.A_e(x); + + Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints); + + Eigen::SparseMatrix H = matrices.H(x, y); + + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == num_decision_variables); + slp_assert(A_e.rows() == num_equality_constraints); + slp_assert(A_e.cols() == num_decision_variables); + slp_assert(H.rows() == num_decision_variables); + slp_assert(H.cols() == num_decision_variables); + + // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) + if (!std::isfinite(f) || !c_e.allFinite()) { + return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + } + + int iterations = 0; + + Filter filter; + + // Kept outside the loop so its storage can be reused + gch::small_vector> triplets; + + RegularizedLDLT solver{num_decision_variables, num_equality_constraints}; + + // Variables for determining when a step is acceptable + constexpr double α_reduction_factor = 0.5; + constexpr double α_min = 1e-7; + + int full_step_rejected_counter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + setup_prof.stop(); + + // Prints final solver diagnostics when the solver exits + scope_exit exit{[&] { + if (options.diagnostics) { + solver_prof.stop(); + if (iterations > 0) { + print_bottom_iteration_diagnostics(); + } + print_solver_diagnostics(solve_profilers); + } + }}; + + while (E_0 > options.tolerance) { + ScopedProfiler inner_iter_profiler{inner_iter_prof}; + ScopedProfiler feasibility_check_profiler{feasibility_check_prof}; + + // Check for local equality constraint infeasibility + if (is_equality_locally_infeasible(A_e, c_e)) { + if (options.diagnostics) { + print_c_e_local_infeasibility_error(c_e); + } + + return ExitStatus::LOCALLY_INFEASIBLE; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e10 || !x.allFinite()) { + return ExitStatus::DIVERGING_ITERATES; + } + + feasibility_check_profiler.stop(); + ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof}; + + // Call iteration callbacks + for (const auto& callback : iteration_callbacks) { + if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix{}})) { + return ExitStatus::CALLBACK_REQUESTED_STOP; + } + } + + iteration_callbacks_profiler.stop(); + ScopedProfiler linear_system_build_profiler{linear_system_build_prof}; + + // lhs = [H Aₑᵀ] + // [Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + triplets.clear(); + triplets.reserve(H.nonZeros() + A_e.nonZeros()); + for (int col = 0; col < H.cols(); ++col) { + // Append column of H lower triangle in top-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{H, col}; it; ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + num_decision_variables + num_equality_constraints, + num_decision_variables + num_equality_constraints); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = -g + A_e.transpose() * y; + rhs.segment(x.rows(), y.rows()) = -c_e; + + linear_system_build_profiler.stop(); + ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof}; + + Step step; + constexpr double α_max = 1.0; + double α = 1.0; + + // Solve the Newton-KKT system + // + // [H Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy] + // [Aₑ 0 ][−pʸ] [ cₑ ] + if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] { + return ExitStatus::FACTORIZATION_FAILED; + } + + linear_system_compute_profiler.stop(); + ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof}; + + auto compute_step = [&](Step& step) { + // p = [ pˣ] + // [−pʸ] + Eigen::VectorXd p = solver.solve(rhs); + step.p_x = p.segment(0, x.rows()); + step.p_y = -p.segment(x.rows(), y.rows()); + }; + compute_step(step); + + linear_system_solve_profiler.stop(); + ScopedProfiler line_search_profiler{line_search_prof}; + + α = α_max; + + // Loop until a step is accepted + while (1) { + Eigen::VectorXd trial_x = x + α * step.p_x; + Eigen::VectorXd trial_y = y + α * step.p_y; + + double trial_f = matrices.f(trial_x); + Eigen::VectorXd trial_c_e = matrices.c_e(trial_x); + + // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size + // immediately + if (!std::isfinite(trial_f) || !trial_c_e.allFinite()) { + // Reduce step size + α *= α_reduction_factor; + + if (α < α_min) { + return ExitStatus::LINE_SEARCH_FAILED; + } + continue; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + // Accept step + break; + } + + double prev_constraint_violation = c_e.lpNorm<1>(); + double next_constraint_violation = trial_c_e.lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (α == α_max && + next_constraint_violation >= prev_constraint_violation) { + // Apply second-order corrections. See section 2.4 of [2]. + auto soc_step = step; + + double α_soc = α; + Eigen::VectorXd c_e_soc = c_e; + + bool step_acceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; + ++soc_iteration) { + ScopedProfiler soc_profiler{soc_prof}; + + scope_exit soc_exit{[&] { + soc_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics( + iterations, + step_acceptable ? IterationType::ACCEPTED_SOC + : IterationType::REJECTED_SOC, + soc_profiler.current_duration(), + error_estimate(g, A_e, trial_c_e, trial_y), trial_f, + trial_c_e.lpNorm<1>(), 0.0, 0.0, + solver.hessian_regularization(), α_soc, 1.0, + α_reduction_factor, 1.0); + } + }}; + + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + compute_step(soc_step); + + trial_x = x + α_soc * soc_step.p_x; + trial_y = y + α_soc * soc_step.p_y; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + + // Constraint violation scale factor for second-order corrections + constexpr double κ_soc = 0.99; + + // If constraint violation hasn't been sufficiently reduced, stop + // making second-order corrections + next_constraint_violation = trial_c_e.lpNorm<1>(); + if (next_constraint_violation > κ_soc * prev_constraint_violation) { + break; + } + + // Check whether filter accepts trial iterate + if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + step = soc_step; + α = α_soc; + step_acceptable = true; + } + } + + if (step_acceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++full_step_rejected_counter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (full_step_rejected_counter >= 4 && + filter.max_constraint_violation > + filter.back().constraint_violation / 10.0) { + filter.max_constraint_violation *= 0.1; + filter.reset(); + continue; + } + + // Reduce step size + α *= α_reduction_factor; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report line search failure. + if (α < α_min) { + double current_kkt_error = kkt_error(g, A_e, c_e, y); + + trial_x = x + α_max * step.p_x; + trial_y = y + α_max * step.p_y; + + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + + double next_kkt_error = kkt_error( + matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, trial_y); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (next_kkt_error <= 0.999 * current_kkt_error) { + α = α_max; + + // Accept step + break; + } + + return ExitStatus::LINE_SEARCH_FAILED; + } + } + + line_search_profiler.stop(); + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + full_step_rejected_counter = 0; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // yₖ₊₁ = yₖ + αₖpₖʸ + x += α * step.p_x; + y += α * step.p_y; + + // Update autodiff for Jacobians and Hessian + f = matrices.f(x); + A_e = matrices.A_e(x); + g = matrices.g(x); + H = matrices.H(x, y); + + ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; + + c_e = matrices.c_e(x); + + // Update the error estimate + E_0 = error_estimate(g, A_e, c_e, y); + + next_iter_prep_profiler.stop(); + inner_iter_profiler.stop(); + + if (options.diagnostics) { + print_iteration_diagnostics(iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, + f, c_e.lpNorm<1>(), 0.0, 0.0, + solver.hessian_regularization(), α, α_max, + α_reduction_factor, α); + } + + ++iterations; + + // Check for max iterations + if (iterations >= options.max_iterations) { + return ExitStatus::MAX_ITERATIONS_EXCEEDED; + } + + // Check for max wall clock time + if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) { + return ExitStatus::TIMEOUT; + } + } + + return ExitStatus::SUCCESS; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp deleted file mode 100644 index 79b5d99ae2..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ /dev/null @@ -1,400 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/SolverConfig.hpp" -#include "sleipnir/optimization/SolverIterationInfo.hpp" -#include "sleipnir/optimization/SolverStatus.hpp" -#include "sleipnir/optimization/solver/InteriorPoint.hpp" -#include "sleipnir/util/FunctionRef.hpp" - -namespace sleipnir { - -/** - * Finds the iterate that minimizes the constraint violation while not deviating - * too far from the starting point. This is a fallback procedure when the normal - * Sequential Quadratic Programming method fails to converge to a feasible - * point. - * - * @param[in] decisionVariables The list of decision variables. - * @param[in] equalityConstraints The list of equality constraints. - * @param[in] callback The user callback. - * @param[in] config Configuration options for the solver. - * @param[in,out] x The current iterate from the normal solve. - * @param[out] status The solver status. - */ -inline void FeasibilityRestoration( - std::span decisionVariables, - std::span equalityConstraints, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { - // Feasibility restoration - // - // min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) - // x - // pₑ,nₑ - // - // s.t. cₑ(x) - pₑ + nₑ = 0 - // pₑ ≥ 0 - // nₑ ≥ 0 - // - // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original - // iterate before feasibility restoration, and D_R is a scaling matrix defined - // by - // - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - - constexpr double ρ = 1000.0; - double μ = config.tolerance / 10.0; - - wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size()); - - // Assign x - fr_decisionVariables.assign(decisionVariables.begin(), - decisionVariables.end()); - - // Allocate pₑ and nₑ - for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) { - fr_decisionVariables.emplace_back(); - } - - auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); - - VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - // Set initial values for pₑ and nₑ. - // - // - // From equation (33) of [2]: - // ______________________ - // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) - // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) - // 2ρ √ ( 2ρ ) 2ρ - // - // The quadratic formula: - // ________ - // -b + √b² - 4ac - // x = −−−−−−−−−−−−−− (2) - // 2a - // - // Rearrange (1) to fit the quadratic formula better: - // _________________________ - // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) - // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− - // 2ρ - // - // Solve for coefficients: - // - // a = ρ (3) - // b = ρ c(x) - μ (4) - // - // -4ac = μ c(x) 2ρ - // -4(ρ)c = 2ρ μ c(x) - // -4c = 2μ c(x) - // c = -μ c(x)/2 (5) - // - // p = c(x) + n (6) - for (int row = 0; row < p_e.Rows(); ++row) { - double c_e = equalityConstraints[row].Value(); - - constexpr double a = 2 * ρ; - double b = ρ * c_e - μ; - double c = -μ * c_e / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_e + n; - - p_e(row).SetValue(p); - n_e(row).SetValue(n); - } - - // cₑ(x) - pₑ + nₑ = 0 - wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { - auto& constraint = fr_equalityConstraints[row]; - constraint = constraint - p_e(row) + n_e(row); - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 - wpi::SmallVector fr_inequalityConstraints; - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nₑ ≥ 0 - std::copy(n_e.begin(), n_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - Variable J = 0.0; - - // J += ρ Σ (pₑ + nₑ) - for (auto& elem : p_e) { - J += elem; - } - for (auto& elem : n_e) { - J += elem; - } - J *= ρ; - - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - Eigen::VectorXd D_R{x.rows()}; - for (int row = 0; row < D_R.rows(); ++row) { - D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); - } - - // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) - for (int row = 0; row < x.rows(); ++row) { - J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); - } - - Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); - - // Set up initial value for inequality constraint slack variables - Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; - fr_s.setOnes(); - - InteriorPoint(fr_decisionVariables, fr_equalityConstraints, - fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, - status); - - x = fr_x.segment(0, decisionVariables.size()); -} - -/** - * Finds the iterate that minimizes the constraint violation while not deviating - * too far from the starting point. This is a fallback procedure when the normal - * interior-point method fails to converge to a feasible point. - * - * @param[in] decisionVariables The list of decision variables. - * @param[in] equalityConstraints The list of equality constraints. - * @param[in] inequalityConstraints The list of inequality constraints. - * @param[in] μ Barrier parameter. - * @param[in] callback The user callback. - * @param[in] config Configuration options for the solver. - * @param[in,out] x The current iterate from the normal solve. - * @param[in,out] s The current inequality constraint slack variables from the - * normal solve. - * @param[out] status The solver status. - */ -inline void FeasibilityRestoration( - std::span decisionVariables, - std::span equalityConstraints, - std::span inequalityConstraints, double μ, - function_ref callback, - const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s, - SolverStatus* status) { - // Feasibility restoration - // - // min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) - // x - // pₑ,nₑ - // pᵢ,nᵢ - // - // s.t. cₑ(x) - pₑ + nₑ = 0 - // cᵢ(x) - s - pᵢ + nᵢ = 0 - // pₑ ≥ 0 - // nₑ ≥ 0 - // pᵢ ≥ 0 - // nᵢ ≥ 0 - // - // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original - // iterate before feasibility restoration, and D_R is a scaling matrix defined - // by - // - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - - constexpr double ρ = 1000.0; - - wpi::SmallVector fr_decisionVariables; - fr_decisionVariables.reserve(decisionVariables.size() + - 2 * equalityConstraints.size() + - 2 * inequalityConstraints.size()); - - // Assign x - fr_decisionVariables.assign(decisionVariables.begin(), - decisionVariables.end()); - - // Allocate pₑ, nₑ, pᵢ, and nᵢ - for (size_t row = 0; - row < 2 * equalityConstraints.size() + 2 * inequalityConstraints.size(); - ++row) { - fr_decisionVariables.emplace_back(); - } - - auto it = fr_decisionVariables.begin(); - - VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; - it += decisionVariables.size(); - - VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; - it += equalityConstraints.size(); - - VariableMatrix p_i{std::span{it, it + inequalityConstraints.size()}}; - it += inequalityConstraints.size(); - - VariableMatrix n_i{std::span{it, it + inequalityConstraints.size()}}; - - // Set initial values for pₑ, nₑ, pᵢ, and nᵢ. - // - // - // From equation (33) of [2]: - // ______________________ - // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) - // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) - // 2ρ √ ( 2ρ ) 2ρ - // - // The quadratic formula: - // ________ - // -b + √b² - 4ac - // x = −−−−−−−−−−−−−− (2) - // 2a - // - // Rearrange (1) to fit the quadratic formula better: - // _________________________ - // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) - // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− - // 2ρ - // - // Solve for coefficients: - // - // a = ρ (3) - // b = ρ c(x) - μ (4) - // - // -4ac = μ c(x) 2ρ - // -4(ρ)c = 2ρ μ c(x) - // -4c = 2μ c(x) - // c = -μ c(x)/2 (5) - // - // p = c(x) + n (6) - for (int row = 0; row < p_e.Rows(); ++row) { - double c_e = equalityConstraints[row].Value(); - - constexpr double a = 2 * ρ; - double b = ρ * c_e - μ; - double c = -μ * c_e / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_e + n; - - p_e(row).SetValue(p); - n_e(row).SetValue(n); - } - for (int row = 0; row < p_i.Rows(); ++row) { - double c_i = inequalityConstraints[row].Value() - s(row); - - constexpr double a = 2 * ρ; - double b = ρ * c_i - μ; - double c = -μ * c_i / 2.0; - - double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); - double p = c_i + n; - - p_i(row).SetValue(p); - n_i(row).SetValue(n); - } - - // cₑ(x) - pₑ + nₑ = 0 - wpi::SmallVector fr_equalityConstraints; - fr_equalityConstraints.assign(equalityConstraints.begin(), - equalityConstraints.end()); - for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { - auto& constraint = fr_equalityConstraints[row]; - constraint = constraint - p_e(row) + n_e(row); - } - - // cᵢ(x) - s - pᵢ + nᵢ = 0 - wpi::SmallVector fr_inequalityConstraints; - fr_inequalityConstraints.assign(inequalityConstraints.begin(), - inequalityConstraints.end()); - for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { - auto& constraint = fr_inequalityConstraints[row]; - constraint = constraint - s(row) - p_i(row) + n_i(row); - } - - // pₑ ≥ 0 - std::copy(p_e.begin(), p_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // pᵢ ≥ 0 - std::copy(p_i.begin(), p_i.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nₑ ≥ 0 - std::copy(n_e.begin(), n_e.end(), - std::back_inserter(fr_inequalityConstraints)); - - // nᵢ ≥ 0 - std::copy(n_i.begin(), n_i.end(), - std::back_inserter(fr_inequalityConstraints)); - - Variable J = 0.0; - - // J += ρ Σ (pₑ + nₑ + pᵢ + nᵢ) - for (auto& elem : p_e) { - J += elem; - } - for (auto& elem : p_i) { - J += elem; - } - for (auto& elem : n_e) { - J += elem; - } - for (auto& elem : n_i) { - J += elem; - } - J *= ρ; - - // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) - Eigen::VectorXd D_R{x.rows()}; - for (int row = 0; row < D_R.rows(); ++row) { - D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); - } - - // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) - for (int row = 0; row < x.rows(); ++row) { - J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); - } - - Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); - - // Set up initial value for inequality constraint slack variables - Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; - fr_s.segment(0, inequalityConstraints.size()) = s; - fr_s.segment(inequalityConstraints.size(), - fr_s.size() - inequalityConstraints.size()) - .setOnes(); - - InteriorPoint(fr_decisionVariables, fr_equalityConstraints, - fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, - status); - - x = fr_x.segment(0, decisionVariables.size()); - s = fr_s.segment(0, inequalityConstraints.size()); -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp deleted file mode 100644 index 0c14efd7b8..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "sleipnir/autodiff/Variable.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions. - -namespace sleipnir { - -/** - * Filter entry consisting of cost and constraint violation. - */ -struct FilterEntry { - /// The cost function's value - double cost = 0.0; - - /// The constraint violation - double constraintViolation = 0.0; - - constexpr FilterEntry() = default; - - /** - * Constructs a FilterEntry. - * - * @param cost The cost function's value. - * @param constraintViolation The constraint violation. - */ - constexpr FilterEntry(double cost, double constraintViolation) - : cost{cost}, constraintViolation{constraintViolation} {} -}; - -/** - * Step filter. - * - * See the section on filters in chapter 15 of [1]. - */ -class Filter { - public: - static constexpr double γCost = 1e-8; - static constexpr double γConstraint = 1e-5; - - double maxConstraintViolation = 1e4; - - /** - * Construct an empty filter. - * - * @param f The cost function. - */ - explicit Filter(Variable& f) { - m_f = &f; - - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - maxConstraintViolation); - } - - /** - * Reset the filter. - */ - void Reset() { - m_filter.clear(); - - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - maxConstraintViolation); - } - - /** - * Creates a new Sequential Quadratic Programming filter entry. - * - * @param c_e The equality constraint values (nonzero means violation). - */ - FilterEntry MakeEntry(const Eigen::VectorXd& c_e) { - return FilterEntry{m_f->Value(), c_e.lpNorm<1>()}; - } - - /** - * Creates a new interior-point method filter entry. - * - * @param s The inequality constraint slack variables. - * @param c_e The equality constraint values (nonzero means violation). - * @param c_i The inequality constraint values (negative means violation). - * @param μ The barrier parameter. - */ - FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e, - const Eigen::VectorXd& c_i, double μ) { - return FilterEntry{m_f->Value() - μ * s.array().log().sum(), - c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()}; - } - - /** - * Add a new entry to the filter. - * - * @param entry The entry to add to the filter. - */ - void Add(const FilterEntry& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraintViolation <= elem.constraintViolation; - }); - - m_filter.push_back(entry); - } - - /** - * Add a new entry to the filter. - * - * @param entry The entry to add to the filter. - */ - void Add(FilterEntry&& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraintViolation <= elem.constraintViolation; - }); - - m_filter.push_back(entry); - } - - /** - * Returns true if the given iterate is accepted by the filter. - * - * @param entry The entry to attempt adding to the filter. - */ - bool TryAdd(const FilterEntry& entry) { - if (IsAcceptable(entry)) { - Add(entry); - return true; - } else { - return false; - } - } - - /** - * Returns true if the given iterate is accepted by the filter. - * - * @param entry The entry to attempt adding to the filter. - */ - bool TryAdd(FilterEntry&& entry) { - if (IsAcceptable(entry)) { - Add(std::move(entry)); - return true; - } else { - return false; - } - } - - /** - * Returns true if the given entry is acceptable to the filter. - * - * @param entry The entry to check. - */ - bool IsAcceptable(const FilterEntry& entry) { - if (!std::isfinite(entry.cost) || - !std::isfinite(entry.constraintViolation)) { - return false; - } - - // If current filter entry is better than all prior ones in some respect, - // accept it - return std::all_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) { - return entry.cost <= elem.cost - γCost * elem.constraintViolation || - entry.constraintViolation <= - (1.0 - γConstraint) * elem.constraintViolation; - }); - } - - private: - Variable* m_f = nullptr; - wpi::SmallVector m_filter; -}; - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp similarity index 66% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp index f1490028dd..5a85732811 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp @@ -9,7 +9,21 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { + +/** + * Returns the error estimate using the KKT conditions for Newton's method. + * + * @param g Gradient of the cost function ∇f. + */ +inline double error_estimate(const Eigen::VectorXd& g) { + // Update the error estimate using the KKT conditions from equations (19.5a) + // through (19.5d) of [1]. + // + // ∇f = 0 + + return g.lpNorm(); +} /** * Returns the error estimate using the KKT conditions for SQP. @@ -21,12 +35,10 @@ namespace sleipnir { * iterate. * @param y Equality constraint dual variables. */ -inline double ErrorEstimate(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::VectorXd& y) { - int numEqualityConstraints = A_e.rows(); - +inline double error_estimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::VectorXd& y) { // Update the error estimate using the KKT conditions from equations (19.5a) // through (19.5d) of [1]. // @@ -41,7 +53,7 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ constexpr double s_max = 100.0; - double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max; + double s_d = std::max(s_max, y.lpNorm<1>() / y.rows()) / s_max; return std::max({(g - A_e.transpose() * y).lpNorm() / s_d, c_e.lpNorm()}); @@ -65,16 +77,13 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, * @param z Inequality constraint dual variables. * @param μ Barrier parameter. */ -inline double ErrorEstimate(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, - const Eigen::VectorXd& s, const Eigen::VectorXd& y, - const Eigen::VectorXd& z, double μ) { - int numEqualityConstraints = A_e.rows(); - int numInequalityConstraints = A_i.rows(); - +inline double error_estimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, + const Eigen::VectorXd& s, const Eigen::VectorXd& y, + const Eigen::VectorXd& z, double μ) { // Update the error estimate using the KKT conditions from equations (19.5a) // through (19.5d) of [1]. // @@ -94,23 +103,21 @@ inline double ErrorEstimate(const Eigen::VectorXd& g, // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ constexpr double s_max = 100.0; double s_d = - std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / - (numEqualityConstraints + numInequalityConstraints)) / + std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / (y.rows() + z.rows())) / s_max; // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ - double s_c = - std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max; + double s_c = std::max(s_max, z.lpNorm<1>() / z.rows()) / s_max; const auto S = s.asDiagonal(); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ); return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) .lpNorm() / s_d, - (S * z - μ * e).lpNorm() / s_c, + (S * z - μe).lpNorm() / s_c, c_e.lpNorm(), (c_i - s).lpNorm()}); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp new file mode 100644 index 0000000000..093ce3674f --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp @@ -0,0 +1,197 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include + +#include +#include + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace slp { + +/** + * Filter entry consisting of cost and constraint violation. + */ +struct FilterEntry { + /// The cost function's value + double cost = 0.0; + + /// The constraint violation + double constraint_violation = 0.0; + + constexpr FilterEntry() = default; + + /** + * Constructs a FilterEntry. + * + * @param cost The cost function's value. + * @param constraint_violation The constraint violation. + */ + constexpr FilterEntry(double cost, double constraint_violation) + : cost{cost}, constraint_violation{constraint_violation} {} + + /** + * Constructs a Newton's method filter entry. + * + * @param f The cost function value. + */ + explicit FilterEntry(double f) : FilterEntry{f, 0.0} {} + + /** + * Constructs a Sequential Quadratic Programming filter entry. + * + * @param f The cost function value. + * @param c_e The equality constraint values (nonzero means violation). + */ + FilterEntry(double f, const Eigen::VectorXd& c_e) + : FilterEntry{f, c_e.lpNorm<1>()} {} + + /** + * Constructs an interior-point method filter entry. + * + * @param f The cost function value. + * @param s The inequality constraint slack variables. + * @param c_e The equality constraint values (nonzero means violation). + * @param c_i The inequality constraint values (negative means violation). + * @param μ The barrier parameter. + */ + FilterEntry(double f, Eigen::VectorXd& s, const Eigen::VectorXd& c_e, + const Eigen::VectorXd& c_i, double μ) + : FilterEntry{f - μ * s.array().log().sum(), + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {} +}; + +/** + * Step filter. + * + * See the section on filters in chapter 15 of [1]. + */ +class Filter { + public: + double max_constraint_violation = 1e4; + + /** + * Constructs an empty filter. + */ + Filter() { + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + max_constraint_violation); + } + + /** + * Resets the filter. + */ + void reset() { + m_filter.clear(); + + // Initial filter entry rejects constraint violations above max + m_filter.emplace_back(std::numeric_limits::infinity(), + max_constraint_violation); + } + + /** + * Adds a new entry to the filter. + * + * @param entry The entry to add to the filter. + */ + void add(const FilterEntry& entry) { + // Remove dominated entries + erase_if(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost && + entry.constraint_violation <= elem.constraint_violation; + }); + + m_filter.push_back(entry); + } + + /** + * Adds a new entry to the filter. + * + * @param entry The entry to add to the filter. + */ + void add(FilterEntry&& entry) { + // Remove dominated entries + erase_if(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost && + entry.constraint_violation <= elem.constraint_violation; + }); + + m_filter.push_back(entry); + } + + /** + * Returns true if the given iterate is accepted by the filter. + * + * @param entry The entry to attempt adding to the filter. + * @param α The step size (0, 1]. + */ + bool try_add(const FilterEntry& entry, double α) { + if (is_acceptable(entry, α)) { + add(entry); + return true; + } else { + return false; + } + } + + /** + * Returns true if the given iterate is accepted by the filter. + * + * @param entry The entry to attempt adding to the filter. + * @param α The step size (0, 1]. + */ + bool try_add(FilterEntry&& entry, double α) { + if (is_acceptable(entry, α)) { + add(std::move(entry)); + return true; + } else { + return false; + } + } + + /** + * Returns true if the given entry is acceptable to the filter. + * + * @param entry The entry to check. + * @param α The step size (0, 1]. + */ + bool is_acceptable(const FilterEntry& entry, double α) { + if (!std::isfinite(entry.cost) || + !std::isfinite(entry.constraint_violation)) { + return false; + } + + double ϕ = std::pow(α, 1.5); + + // If current filter entry is better than all prior ones in some respect, + // accept it. + // + // See equation (2.13) of [4]. + return std::ranges::all_of(m_filter, [&](const auto& elem) { + return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation || + entry.constraint_violation <= + (1.0 - ϕ * γ_constraint) * elem.constraint_violation; + }); + } + + /** + * Returns the most recently added filter entry. + * + * @return The most recently added filter entry. + */ + const FilterEntry& back() const { return m_filter.back(); } + + private: + static constexpr double γ_cost = 1e-8; + static constexpr double γ_constraint = 1e-5; + + gch::small_vector m_filter; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp similarity index 92% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp index 98a117f49a..595219ca37 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp @@ -6,7 +6,7 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { /** * Applies fraction-to-the-boundary rule to a variable and its iterate, then @@ -17,7 +17,7 @@ namespace sleipnir { * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1]. * @return Fraction of the iterate step size within (0, 1]. */ -inline double FractionToTheBoundaryRule( +inline double fraction_to_the_boundary_rule( const Eigen::Ref& x, const Eigen::Ref& p, double τ) { // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) @@ -42,4 +42,4 @@ inline double FractionToTheBoundaryRule( return α; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp similarity index 87% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp index dc3992ea12..11a798b0d8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp @@ -7,7 +7,7 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { /** * Returns true if the problem's equality constraints are locally infeasible. @@ -17,8 +17,8 @@ namespace sleipnir { * @param c_e The problem's equality constraints cₑ(x) evaluated at the current * iterate. */ -inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e) { +inline bool is_equality_locally_infeasible( + const Eigen::SparseMatrix& A_e, const Eigen::VectorXd& c_e) { // The equality constraints are locally infeasible if // // Aₑᵀcₑ → 0 @@ -37,7 +37,7 @@ inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix& A_e, * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the * current iterate. */ -inline bool IsInequalityLocallyInfeasible( +inline bool is_inequality_locally_infeasible( const Eigen::SparseMatrix& A_i, const Eigen::VectorXd& c_i) { // The inequality constraints are locally infeasible if // @@ -60,4 +60,4 @@ inline bool IsInequalityLocallyInfeasible( return false; } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp similarity index 61% rename from wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp index 3b603159d2..e01c064d2a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp @@ -7,7 +7,21 @@ // See docs/algorithms.md#Works_cited for citation definitions -namespace sleipnir { +namespace slp { + +/** + * Returns the KKT error for Newton's method. + * + * @param g Gradient of the cost function ∇f. + */ +inline double kkt_error(const Eigen::VectorXd& g) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f = 0 + + return g.lpNorm<1>(); +} /** * Returns the KKT error for Sequential Quadratic Programming. @@ -19,9 +33,9 @@ namespace sleipnir { * iterate. * @param y Equality constraint dual variables. */ -inline double KKTError(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { +inline double kkt_error(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { // Compute the KKT error as the 1-norm of the KKT conditions from equations // (19.5a) through (19.5d) of [1]. // @@ -48,13 +62,13 @@ inline double KKTError(const Eigen::VectorXd& g, * @param z Inequality constraint dual variables. * @param μ Barrier parameter. */ -inline double KKTError(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, - double μ) { +inline double kkt_error(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, + const Eigen::VectorXd& y, const Eigen::VectorXd& z, + double μ) { // Compute the KKT error as the 1-norm of the KKT conditions from equations // (19.5a) through (19.5d) of [1]. // @@ -64,10 +78,10 @@ inline double KKTError(const Eigen::VectorXd& g, // cᵢ − s = 0 const auto S = s.asDiagonal(); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ); return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() + - (S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); + (S * z - μe).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); } -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp deleted file mode 100644 index 33b137b961..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#include "sleipnir/util/Pool.hpp" - -namespace sleipnir { - -PoolResource& GlobalPoolResource() { - thread_local PoolResource pool{16384}; - return pool; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp deleted file mode 100644 index 0e00418725..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -namespace sleipnir { - -/** - * Converts std::chrono::duration to a number of milliseconds rounded to three - * decimals. - */ -template > -constexpr double ToMilliseconds( - const std::chrono::duration& duration) { - using std::chrono::duration_cast; - using std::chrono::microseconds; - return duration_cast(duration).count() / 1e3; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp new file mode 100644 index 0000000000..a8b086c811 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp @@ -0,0 +1,12 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/util/pool.hpp" + +namespace slp { + +PoolResource& global_pool_resource() { + thread_local PoolResource pool{16384}; + return pool; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp new file mode 100644 index 0000000000..82e0e082b0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp @@ -0,0 +1,357 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/util/print.hpp" +#include "util/setup_profiler.hpp" +#include "util/solve_profiler.hpp" + +namespace slp { + +/** + * Iteration type. + */ +enum class IterationType : uint8_t { + /// Normal iteration. + NORMAL, + /// Accepted second-order correction iteration. + ACCEPTED_SOC, + /// Rejected second-order correction iteration. + REJECTED_SOC +}; + +/** + * Converts std::chrono::duration to a number of milliseconds rounded to three + * decimals. + */ +template > +constexpr double to_ms(const std::chrono::duration& duration) { + using std::chrono::duration_cast; + using std::chrono::microseconds; + return duration_cast(duration).count() / 1e3; +} + +/** + * Renders value as power of 10. + * + * @param value Value. + */ +inline std::string power_of_10(double value) { + if (value == 0.0) { + return " 0"; + } else { + int exponent = std::log10(value); + + if (exponent == 0) { + return " 1"; + } else if (exponent == 1) { + return "10"; + } else { + // Gather exponent digits + int n = std::abs(exponent); + gch::small_vector digits; + do { + digits.emplace_back(n % 10); + n /= 10; + } while (n > 0); + + std::string output = "10"; + + // Append exponent + if (exponent < 0) { + output += "⁻"; + } + constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴", + "⁵", "⁶", "⁷", "⁸", "⁹"}; + for (const auto& digit : digits | std::views::reverse) { + output += strs[digit]; + } + + return output; + } + } +} + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints error for too few degrees of freedom. + * + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + */ +inline void print_too_few_dofs_error(const Eigen::VectorXd& c_e) { + slp::println("The problem has too few degrees of freedom."); + slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e[row] < 0.0) { + slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]); + } + } +} +#else +#define print_too_few_dofs_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints equality constraint local infeasibility error. + * + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + */ +inline void print_c_e_local_infeasibility_error(const Eigen::VectorXd& c_e) { + slp::println( + "The problem is locally infeasible due to violated equality " + "constraints."); + slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e[row] < 0.0) { + slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]); + } + } +} +#else +#define print_c_e_local_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints inequality constraint local infeasibility error. + * + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + */ +inline void print_c_i_local_infeasibility_error(const Eigen::VectorXd& c_i) { + slp::println( + "The problem is locally infeasible due to violated inequality " + "constraints."); + slp::println("Violated constraints (cᵢ(x) ≥ 0) in order of declaration:"); + for (int row = 0; row < c_i.rows(); ++row) { + if (c_i[row] < 0.0) { + slp::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i[row]); + } + } +} +#else +#define print_c_i_local_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +inline void print_bound_constraint_global_infeasibility_error( + const std::span> + conflicting_lower_upper_bound_indices) { + slp::println( + "The problem is globally infeasible due to conflicting bound " + "constraints:"); + for (const auto& [lower_bound_idx, upper_bound_idx] : + conflicting_lower_upper_bound_indices) { + slp::println( + " Inequality constraint {} gives a lower bound that is greater than " + "the upper bound given by inequality constraint {}", + lower_bound_idx, upper_bound_idx); + } +} +#else +#define print_bound_constraint_global_infeasibility_error(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints diagnostics for the current iteration. + * + * @param iterations Number of iterations. + * @param type The iteration's type. + * @param time The iteration duration. + * @param error The error. + * @param cost The cost. + * @param infeasibility The infeasibility. + * @param complementarity The complementarity. + * @param μ The barrier parameter. + * @param δ The Hessian regularization factor. + * @param primal_α The primal step size. + * @param primal_α_max The max primal step size. + * @param α_reduction_factor Factor by which primal_α is reduced during + * backtracking. + * @param dual_α The dual step size. + */ +template > +void print_iteration_diagnostics(int iterations, IterationType type, + const std::chrono::duration& time, + double error, double cost, + double infeasibility, double complementarity, + double μ, double δ, double primal_α, + double primal_α_max, double α_reduction_factor, + double dual_α) { + if (iterations % 20 == 0) { + if (iterations == 0) { + slp::print("┏"); + } else { + slp::print("┢"); + } + slp::print( + "{:━^4}┯{:━^4}┯{:━^9}┯{:━^12}┯{:━^13}┯{:━^12}┯{:━^12}┯{:━^8}┯{:━^5}┯" + "{:━^8}┯{:━^8}┯{:━^2}", + "", "", "", "", "", "", "", "", "", "", "", ""); + if (iterations == 0) { + slp::println("┓"); + } else { + slp::println("┪"); + } + slp::println( + "┃{:^4}│{:^4}│{:^9}│{:^12}│{:^13}│{:^12}│{:^12}│{:^8}│{:^5}│{:^8}│{:^8}" + "│{:^2}┃", + "iter", "type", "time (ms)", "error", "cost", "infeas.", "complement.", + "μ", "reg", "primal α", "dual α", "↩"); + slp::println( + "┡{:━^4}┷{:━^4}┷{:━^9}┷{:━^12}┷{:━^13}┷{:━^12}┷{:━^12}┷{:━^8}┷{:━^5}┷" + "{:━^8}┷{:━^8}┷{:━^2}┩", + "", "", "", "", "", "", "", "", "", "", "", ""); + } + + // For the number of backtracks, we want x such that: + // + // αᵐᵃˣrˣ = α + // + // where r ∈ (0, 1) is the reduction factor. + // + // rˣ = α/αᵐᵃˣ + // ln(rˣ) = ln(α/αᵐᵃˣ) + // x ln(r) = ln(α/αᵐᵃˣ) + // x = ln(α/αᵐᵃˣ)/ln(r) + int backtracks = static_cast(std::log(primal_α / primal_α_max) / + std::log(α_reduction_factor)); + + constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC"}; + slp::println( + "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " + "{:.2e} {:2d}│", + iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), + error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, + dual_α, backtracks); +} +#else +#define print_iteration_diagnostics(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints bottom of iteration diagnostics table. + */ +inline void print_bottom_iteration_diagnostics() { + slp::println("└{:─^108}┘", ""); +} +#else +#define print_bottom_iteration_diagnostics(...) +#endif + +/** + * Renders histogram of the given normalized value. + * + * @tparam Width Width of the histogram in characters. + * @param value Normalized value from 0 to 1. + */ +template + requires(Width > 0) +std::string histogram(double value) { + value = std::clamp(value, 0.0, 1.0); + + double ipart; + int fpart = static_cast(std::modf(value * Width, &ipart) * 8); + + constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"}; + std::string hist; + + int index = 0; + while (index < ipart) { + hist += strs[8]; + ++index; + } + if (fpart > 0) { + hist += strs[fpart]; + ++index; + } + while (index < Width) { + hist += strs[0]; + ++index; + } + + return hist; +} + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints solver diagnostics. + * + * @param solve_profilers Solve profilers. + */ +inline void print_solver_diagnostics( + const gch::small_vector& solve_profilers) { + auto solve_duration = to_ms(solve_profilers[0].total_duration()); + + slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent", + "total (ms)", "each (ms)", "runs"); + slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + + for (auto& profiler : solve_profilers) { + double norm = solve_duration == 0.0 + ? (&profiler == &solve_profilers[0] ? 1.0 : 0.0) + : to_ms(profiler.total_duration()) / solve_duration; + slp::println("│{:<25} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + profiler.name(), norm * 100.0, histogram<9>(norm), + to_ms(profiler.total_duration()), + to_ms(profiler.average_duration()), profiler.num_solves()); + } + + slp::println("└{:─^70}┘", ""); +} +#else +#define print_solver_diagnostics(...) +#endif + +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS +/** + * Prints autodiff diagnostics. + * + * @param setup_profilers Autodiff setup profilers. + */ +inline void print_autodiff_diagnostics( + const gch::small_vector& setup_profilers) { + auto setup_duration = to_ms(setup_profilers[0].duration()); + + // Print heading + slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "autodiff trace", + "percent", "total (ms)", "each (ms)", "runs"); + slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + + // Print setup profilers + for (auto& profiler : setup_profilers) { + double norm = setup_duration == 0.0 + ? (&profiler == &setup_profilers[0] ? 1.0 : 0.0) + : to_ms(profiler.duration()) / setup_duration; + slp::println("│{:<25} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + profiler.name(), norm * 100.0, histogram<9>(norm), + to_ms(profiler.duration()), to_ms(profiler.duration()), "1"); + } + + slp::println("└{:─^70}┘", ""); +} +#else +#define print_autodiff_diagnostics(...) +#endif + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp similarity index 92% rename from wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp index dedc7ae4d3..937121f1a8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp @@ -4,7 +4,7 @@ #include -namespace sleipnir { +namespace slp { template class scope_exit { @@ -32,4 +32,4 @@ class scope_exit { bool m_active = true; }; -} // namespace sleipnir +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp new file mode 100644 index 0000000000..066a2b3dbe --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp @@ -0,0 +1,78 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include "util/setup_profiler.hpp" +#include "util/solve_profiler.hpp" + +namespace slp { + +/** + * Starts a profiler in the constructor and stops it in the destructor. + */ +template + requires std::same_as || + std::same_as +class ScopedProfiler { + public: + /** + * Starts a profiler. + * + * @param profiler The profiler. + */ + explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} { + m_profiler->start(); + } + + /** + * Stops a profiler. + */ + ~ScopedProfiler() { + if (m_active) { + m_profiler->stop(); + } + } + + /** + * Move constructor. + * + * @param rhs The other ScopedProfiler. + */ + ScopedProfiler(ScopedProfiler&& rhs) noexcept + : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} { + rhs.m_active = false; + } + + ScopedProfiler(const ScopedProfiler&) = delete; + ScopedProfiler& operator=(const ScopedProfiler&) = delete; + + /** + * Stops the profiler. + * + * If this is called, the destructor is a no-op. + */ + void stop() { + if (m_active) { + m_profiler->stop(); + m_active = false; + } + } + + /** + * Returns the most recent solve duration in milliseconds as a double. + * + * @return The most recent solve duration in milliseconds as a double. + */ + const std::chrono::duration& current_duration() const { + return m_profiler->current_duration(); + } + + private: + Profiler* m_profiler; + bool m_active = true; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp new file mode 100644 index 0000000000..031f99d2e5 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp @@ -0,0 +1,68 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +namespace slp { + +/** + * Records the number of profiler measurements (start/stop pairs) and the + * average duration between each start and stop call. + */ +class SetupProfiler { + public: + /** + * Constructs a SetupProfiler. + * + * @param name Name of measurement to show in diagnostics. + */ + explicit SetupProfiler(std::string_view name) : m_name{name} {} + + /** + * Tell the profiler to start measuring setup time. + */ + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_start_time = std::chrono::steady_clock::now(); +#endif + } + + /** + * Tell the profiler to stop measuring setup time. + */ + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_stop_time = std::chrono::steady_clock::now(); + m_setup_duration = m_setup_stop_time - m_setup_start_time; +#endif + } + + /** + * Returns name of measurement to show in diagnostics. + * + * @return Name of measurement to show in diagnostics. + */ + std::string_view name() const { return m_name; } + + /** + * Returns the setup duration in milliseconds as a double. + * + * @return The setup duration in milliseconds as a double. + */ + const std::chrono::duration& duration() const { + return m_setup_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_setup_start_time; + std::chrono::steady_clock::time_point m_setup_stop_time; + std::chrono::duration m_setup_duration{0.0}; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp new file mode 100644 index 0000000000..fb17f1dc39 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp @@ -0,0 +1,105 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +namespace slp { + +/** + * Records the number of profiler measurements (start/stop pairs) and the + * average duration between each start and stop call. + */ +class SolveProfiler { + public: + /** + * Constructs a SolveProfiler. + * + * @param name Name of measurement to show in diagnostics. + */ + explicit SolveProfiler(std::string_view name) : m_name{name} {} + + /** + * Tell the profiler to start measuring solve time. + */ + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_start_time = std::chrono::steady_clock::now(); +#endif + } + + /** + * Tell the profiler to stop measuring solve time, increment the number of + * averages, and incorporate the latest measurement into the average. + */ + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_stop_time = std::chrono::steady_clock::now(); + m_current_solve_duration = + m_current_solve_stop_time - m_current_solve_start_time; + m_total_solve_duration += m_current_solve_duration; + + ++m_num_solves; + m_average_solve_duration = + (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration + + 1.0 / m_num_solves * m_current_solve_duration; +#endif + } + + /** + * Returns name of measurement to show in diagnostics. + * + * @return Name of measurement to show in diagnostics. + */ + std::string_view name() const { return m_name; } + + /** + * Returns the number of solves. + * + * @return The number of solves. + */ + int num_solves() const { return m_num_solves; } + + /** + * Returns the most recent solve duration in seconds. + * + * @return The most recent solve duration in seconds. + */ + const std::chrono::duration& current_duration() const { + return m_current_solve_duration; + } + + /** + * Returns the average solve duration in seconds. + * + * @return The average solve duration in seconds. + */ + const std::chrono::duration& average_duration() const { + return m_average_solve_duration; + } + + /** + * Returns the sum of all solve durations in seconds. + * + * @return The sum of all solve durations in seconds. + */ + const std::chrono::duration& total_duration() const { + return m_total_solve_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_current_solve_start_time; + std::chrono::steady_clock::time_point m_current_solve_stop_time; + std::chrono::duration m_current_solve_duration{0.0}; + std::chrono::duration m_total_solve_duration{0.0}; + + int m_num_solves = 0; + std::chrono::duration m_average_solve_duration{0.0}; +}; + +} // namespace slp diff --git a/wpimath/src/test/native/cpp/SleipnirTest.cpp b/wpimath/src/test/native/cpp/SleipnirTest.cpp index 46cb437fd5..c0eb25d90e 100644 --- a/wpimath/src/test/native/cpp/SleipnirTest.cpp +++ b/wpimath/src/test/native/cpp/SleipnirTest.cpp @@ -3,24 +3,25 @@ // the WPILib BSD license file in the root directory of this project. #include -#include +#include TEST(SleipnirTest, Quartic) { - sleipnir::OptimizationProblem problem; + slp::Problem problem; - auto x = problem.DecisionVariable(); - x.SetValue(20.0); + auto x = problem.decision_variable(); + x.set_value(20.0); - problem.Minimize(sleipnir::pow(x, 4)); + problem.minimize(slp::pow(x, 4)); - problem.SubjectTo(x >= 1); + problem.subject_to(x >= 1); - auto status = problem.Solve({.diagnostics = true}); + EXPECT_EQ(problem.cost_function_type(), slp::ExpressionType::NONLINEAR); + EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::NONE); + EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR); - EXPECT_EQ(status.costFunctionType, sleipnir::ExpressionType::kNonlinear); - EXPECT_EQ(status.equalityConstraintType, sleipnir::ExpressionType::kNone); - EXPECT_EQ(status.inequalityConstraintType, sleipnir::ExpressionType::kLinear); - EXPECT_EQ(status.exitCondition, sleipnir::SolverExitCondition::kSuccess); + auto status = problem.solve({.diagnostics = true}); - EXPECT_NEAR(x.Value(), 1.0, 1e-6); + EXPECT_EQ(status, slp::ExitStatus::SUCCESS); + + EXPECT_NEAR(x.value(), 1.0, 1e-6); } diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp index fe1fb1d7ea..434524cd91 100644 --- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp @@ -33,6 +33,19 @@ TEST(StateSpaceUtilTest, CostArray) { EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); } +TEST(StateSpaceUtilTest, CostDynamic) { + Eigen::MatrixXd mat = frc::MakeCostMatrix(std::vector{1.0, 2.0, 3.0}); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); +} + TEST(StateSpaceUtilTest, CovParameterPack) { constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); @@ -59,6 +72,19 @@ TEST(StateSpaceUtilTest, CovArray) { EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); } +TEST(StateSpaceUtilTest, CovDynamic) { + Eigen::MatrixXd mat = frc::MakeCovMatrix(std::vector{1.0, 2.0, 3.0}); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); +} + TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) { [[maybe_unused]] frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0); @@ -69,6 +95,11 @@ TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) { frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); } +TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) { + [[maybe_unused]] + Eigen::VectorXd vec = frc::MakeWhiteNoiseVector(std::vector{2.0, 3.0}); +} + TEST(StateSpaceUtilTest, IsStabilizable) { frc::Matrixd<2, 1> B{0, 1}; diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp index 5c8e3f15fa..e05720455d 100644 --- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp @@ -2,8 +2,6 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include #include "frc/EigenCore.h" @@ -22,10 +20,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) { } TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) { - std::function(const Vectord<2>&, const Vectord<1>&)> - modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); }; - - frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics, + frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics, 20_ms}; Vectord<2> r{2, 2}; @@ -35,14 +30,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) { } TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) { - std::function(const Vectord<2>&)> modelDynamics = [](auto& x) { - return StateDynamics(x); - }; - - Matrixd<2, 1> B{0, 1}; - - frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics, - B, 20_ms}; + frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ + &StateDynamics, Matrixd<2, 1>{{0.0}, {1.0}}, 20_ms}; Vectord<2> r{2, 2}; Vectord<2> nextR{3, 3}; diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp index d62decda61..1c4bdaa58b 100644 --- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -22,6 +22,21 @@ TEST(AngleStatisticsTest, Mean) { .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3)); } +TEST(AngleStatisticsTest, Mean_DynamicSize) { + Eigen::MatrixXd sigmas{ + {1, 1.2, 0}, + {359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0}, + {1, 2, 0}}; + // Weights need to produce the mean of the sigmas + Eigen::VectorXd weights{3}; + weights.fill(1.0 / sigmas.cols()); + + EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1) + .isApprox(frc::AngleMean( + sigmas, weights, 1), + 1e-3)); +} + TEST(AngleStatisticsTest, Residual) { Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2}; Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1}; @@ -30,9 +45,25 @@ TEST(AngleStatisticsTest, Residual) { Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1})); } +TEST(AngleStatisticsTest, Residual_DynamicSize) { + Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}}; + Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}}; + + EXPECT_TRUE(frc::AngleResidual(a, b, 1).isApprox( + Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}})); +} + TEST(AngleStatisticsTest, Add) { Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2}; Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1}; EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3})); } + +TEST(AngleStatisticsTest, Add_DynamicSize) { + Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}}; + Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}}; + + EXPECT_TRUE(frc::AngleAdd(a, b, 1).isApprox( + Eigen::VectorXd{{2, 0, 3}})); +} diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp index 513684b755..0529fe285d 100644 --- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp @@ -28,6 +28,25 @@ TEST(NumericalJacobianTest, Bu) { EXPECT_TRUE(newB.isApprox(B)); } +Eigen::VectorXd AxBuFn_DynamicSize(const Eigen::VectorXd& x, + const Eigen::VectorXd& u) { + return A * x + B * u; +} + +// Test that we can recover A from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Ax_DynamicSize) { + Eigen::MatrixXd newA = frc::NumericalJacobianX( + AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); + EXPECT_TRUE(newA.isApprox(A)); +} + +// Test that we can recover B from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Bu_DynamicSize) { + Eigen::MatrixXd newB = frc::NumericalJacobianU( + AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); + EXPECT_TRUE(newB.isApprox(B)); +} + frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}}; frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}}; @@ -49,3 +68,20 @@ TEST(NumericalJacobianTest, Du) { CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero()); EXPECT_TRUE(newD.isApprox(D)); } + +Eigen::VectorXd CxDuFn_DynamicSize(const Eigen::VectorXd& x, + const Eigen::VectorXd& u) { + return C * x + D * u; +} + +TEST(NumericalJacobianTest, Cx_DynamicSize) { + Eigen::MatrixXd newC = frc::NumericalJacobianX( + CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2)); + EXPECT_TRUE(newC.isApprox(C)); +} + +TEST(NumericalJacobianTest, Du_DynamicSize) { + Eigen::MatrixXd newD = frc::NumericalJacobianU( + CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2)); + EXPECT_TRUE(newD.isApprox(D)); +} diff --git a/wpinet/BUILD.bazel b/wpinet/BUILD.bazel index 2fe22c2c25..e9974c404a 100644 --- a/wpinet/BUILD.bazel +++ b/wpinet/BUILD.bazel @@ -122,6 +122,9 @@ cc_library( ":tcpsockets-srcs", ] + ["native-srcs"], hdrs = glob(["src/main/native/include/**/*"]), + implementation_deps = [ + ":private_includes", + ], includes = ["src/main/native/include"], linkopts = select({ "@bazel_tools//src/conditions:linux": ["-ldl"], @@ -131,7 +134,6 @@ cc_library( visibility = ["//visibility:public"], deps = [ ":libuv-headers", - ":private_includes", ":tcpsockets-headers", "//wpiutil:wpiutil.static", ], @@ -169,6 +171,7 @@ cc_test( ]), tags = ["no-asan"], deps = [ + ":private_includes", ":wpinet.static", "//thirdparty/googletest:googletest.static", "//wpiutil:wpiutil-testlib", diff --git a/wpinet/src/main/native/cpp/http_parser.cpp b/wpinet/src/main/native/cpp/http_parser.cpp index fcc838e15b..1fa1589814 100644 --- a/wpinet/src/main/native/cpp/http_parser.cpp +++ b/wpinet/src/main/native/cpp/http_parser.cpp @@ -1326,6 +1326,12 @@ reexecute: parser->header_state = h_general; } else if (parser->index == sizeof(TRANSFER_ENCODING)-2) { parser->header_state = h_transfer_encoding; + /* Multiple `Transfer-Encoding` headers should be treated as + * one, but with values separate by a comma. + * + * See: https://tools.ietf.org/html/rfc7230#section-3.2.2 + */ + parser->flags &= ~F_CHUNKED; } break;