Squashed 'third_party/allwpilib/' changes from e4b91005cf..83f1860047
83f1860047 [wpilib] Add/update documentation to PneumaticBase and subclasses (NFC) (#4881)
9872e676d8 [commands] Make Subsystem destructor virtual (#4892)
25db20e49d [hal] Fix segfault in various HAL functions (#4891)
b0c6724eed [glass] Add hamburger menu icon to titlebars (#4874)
f0fa8205ac Add missing compiler flags and fix warnings (#4889)
42fc4cb6bc [wpiutil] SafeThread: Provide start/stop hooks (#4880)
cc166c98d2 [templates] Add Command-based skeleton template (#4861)
3f51f10ad3 [build] Update to 2023v3 image (#4886)
1562eae74a [ntcore] Refactor meta-topic decoding from glass (#4809)
b632b288a3 Fix usages of std::min and std::max to be windows safe (#4887)
c11bd2720f [wpilibc] Add internal function to reset Shuffleboard instance (#4884)
f1151d375f [ntcore] Add method to get server time offset (#4847)
fe1b62647f [hal,wpilib] Update documentation for getComments (NFC) (#4879)
c49a45abbd [build] Fix examples linking in incorrect jni library (#4873)
bc3d01a721 [build] Add platform check to doxygen plugin (#4862)
bc473240ae Add Jetbrains Fleet folder to .gitignore (#4872)
2121bd5fb8 [wpimath] Remove RKF45 (#4870)
835f8470d6 [build] Fix roborio cross-compiler on arm hosts (#4864)
6cfe5de00d [ntcore] Don't deadlock server on early destroy (#4863)
2ac41f3edc [hal, wpilib] Add RobotController.getComments() (#4463)
26bdbf3d41 Java optimization and formatting fixes (#4857)
92149efa11 Spelling and grammar cleanups (#4849)
176fddeb4c [commands] Add functions to HID classes to allow use of axes as BooleanEvents/Triggers (#4762)
87a34af367 [templates] Add bindings to command-based template (#4838)
4534e75787 [examples] Remove redundant MotorControl example (#4837)
1cbebaa2f7 [commands] Remove final semicolon from test macro definition (#4859)
6efb9ee405 [commands] Add constructor for SwerveControllerCommand that takes a HolonomicDriveController (#4785)
1e7fcd5637 [cscore] Change run loop functions to not be mac specific (#4854)
1f940e2b60 [apriltag] Add C++ wrappers, rewrite Java/JNI to match (#4842)
a6d127aedf [build] Add missing task dependency in wpilibjExamples (#4852)
b893b3d6d3 [cscore] Add support for USB cameras on macOS (#4846)
1696a490fa [glass] Add support for alternate NT ports (#4848)
40a22d69bc [glass] Add support for alternate NT ports (#4848)
e84dbfede0 [wpilib] GenericHID: Add rumble both option (#4843)
8aa9dbfa90 [examples] Link apriltag package in examples build.gradle (#4845)
eda2fa8a17 [build] Update Spotless (#4840)
d20594db0d Fix typos (#4839)
dd8ecfdd54 [commands] Fix typo in waitUntil docs (NFC) (#4841)
17ceebfff4 [apriltag] Clean up apriltag JNI (#4823)
8b74ab389d [examples] RapidReactCommandBot: Fix array indices (#4833)
1aad3489c2 [sim] Implement PD total current and power (#4830)
2744991771 [wpimath] Fix docs in SwerveModulePosition (#4825)
ffbf6a1fa2 [commands] Disable regularly failing unit test (#4824)
fbabd0ef15 [commands] Enhance Command Sendable implementations (#4822)
7713f68772 [hal] Use atomic rather then mutex for DS Data updates (#4787)
701995d6cc [examples] Update Command-based starter project (#4778)
bf7068ac27 [wpilibc] Add missing PPS implementation for C++ (#4821)
aae0f52ca6 [ntcore] NetworkTable: fix visibility of get/set value (#4820)
ee02fb7ba7 [hal] Add support for Pulse-Per-Second signal (#4819)
518916ba02 [wpilib] Fix DS mode thread event being manual reset accidentally (#4818)
3997c6635b [hal] Update to new image, use new TCP notify callback and new duty cycle API (#4774)
cc8675a4e5 [examples] Add comment on how to view elevator sim (NFC) (#4482)
fb2c170b6e [ntcore] Simplify local startup (#4803)
7ba8a9ee1f [wpimath] ProfiledPIDController: Add to SendableRegistry (#4656)
c569d8e523 [wpilib] Joystick.getMagnitude(): use hypot() function (#4816)
2a5e89fa97 [apriltag] Improve description of pose coordinates (NFC) (#4810)
cc003c6c38 [apriltag] Fix AprilTagFieldLayout JSON name (#4814)
5522916123 [commands] CommandXBoxController bumper documentation fix (NFC) (#4815)
967b30de3a [glass] Fix NT view UpdateClients() bug (#4808)
3270d4fc86 [wpimath] Rewrite pose estimator docs (#4807)
be39678447 [apriltag] Add test to ensure apriltagjni loads (#4805)
61c75deb2a [commands] Test no-op behavior of scheduling a scheduled command (#4806)
a865f48e96 [ntcore] Pass pub/sub options as a unified PubSubOptions struct (#4794)
f66a667321 [commands] Fix incorrect Trigger docs (NFC) (#4792)
f8d4e9866e [ntcore] Clean up ntcore_test.h (#4804)
7e84ea891f [wpimath] Fix ComputerVisionUtil transform example in parameter docs (NFC) (#4800)
da3ec1be10 [wpimath] Change terminology for ArmFeedforward gravity gain (NFC) (#4791)
944dd7265d [wpilibc] Add C++ Notifier error handling, update java notifier error message (#4795)
6948cea67a [wpiutil] Fix MemoryBuffer initialization (#4797)
a31459bce6 [wpiutil] Fix UnescapeCString overflow when inputSize < 2 (#4796)
4a0ad6b48c [wpimath] Rotation2d: Add reference to angleModulus in docs (NFC) (#4786)
e6552d272e [ntcore] Remove table multi-subscriber (#4789)
bde383f763 [hal] Replace const char* with std::string_view in Driver Station sim functions (#4532)
5a52b51443 [hal] Add RobotController.getSerialNumber() (#4783)
69a66ec5ec [wpilib] Fix multiple motor safety issues (#4784)
989c9fb29a [wpimath] Revert Rotation2D change that limits angles (#4781)
0f5b08ec69 [wpigui] Update imgui to 1.89.1+ (#4780)
fba191099c [examples] AddressableLED: Add unit test (#4779)
b390cad095 [wpilibj] Consistently use ErrorMessages.requireNonNullParam (#4776)
b9772214d9 [wpilib] Sendable: Don't call setter for getter changes
342c375a71 [ntcore] Add subscriber option to exclude single publisher
b0e4053087 [ntcore] Use int for options instead of double
f3e666b7bb [cscore] Convert YUYV and UYVY directly to grayscale (#4777)
b300518bd1 [hal] Add CAN Stream API to Java through JNI bindings (#4193)
be27171236 [wpilibj] Shuffleboard: Check for null sendable (#4772)
4bbdbdfb48 [commands] Move GroupedCommands to CommandScheduler (#4728)
f18fd41ac3 [wpimath] Remove broken and obsoleted ComputerVisionUtil functions (#4775)
2d0faecf4f [glass] DataSource: Add spinlock to protect value (#4771)
348bd107fc [hal] Add CANManufacturer for The Thrifty Bot (#4773)
3149dc64b8 [examples] HatchbotInlined: Use Subsystem factories (#4765)
8618dd4160 [glass, wpilib] Replace remaining references to Speed Controller with Motor Controller (#4769)
72e21a1ed1 [apriltag] Use wpilibsuite fork of apriltag (#4764)
eab0d929e6 [commands] CommandGenericHID POV methods: Fix docs (NFC) (#4760)
6789869663 [wpilib] Call set(0) rather than disable for stopMotor (#4763)
c9dea2968d [cscore] Emit warning that USB Camera isn't supported on OSX (#4766)
8f402645f5 [commands] Fix PIDSubsystem setSetpoint behavior (#4759)
f24ad1d715 [build] Upgrade to googletest 1.12.1 (#4752)
ff88756864 [wpimath] Add new DCMotor functions for alternative calculations and reduction calculation (#4749)
f58873db8e [wpimath] Remove extra terms in matrix for pose estimator docs (#4756)
37e969b41a [wpimath] Add constructors to pose estimators with default standard deviations (#4754)
13cdc29382 [ci] Rename comment command from "/wpiformat" to "/format" (#4755)
6e23985ae6 [examples] Add main include directory to test builds (#4751)
66bb0ffb2c [examples] Add unit testing infrastructure (#4646)
74cc86c4c5 [wpimath] Make transform tests use pose/transform equality operators (#4675)
e22d8cc343 [wpimath] Use Odometry for internal state in Pose Estimation (#4668)
68dba92630 [ci] Update mac and windows builds to Java 17 (#4750)
23bfc2d9ab [sim] Remove unmaintained Gazebo support (#4736)
1f1461e254 [wpilib] Add method to enable/disable LiveWindow in test mode (#4678)
eae68fc165 [wpimath] Add tolerance for Rotation3d rotation matrix special orthogonality (#4744)
4c4545fb4b [apriltag] Suppress warning (#4743)
16ffaa754d [docs] Generate docs for apriltag subproject (#4745)
5e74ff26d8 [apriltag, build] Update native utils, add apriltag impl and JNI (#4733)
53875419a1 [hal] Allow overriding stderr printing by HAL_SendError (#4742)
aa6499e920 [ntcore] Fix special topic multi-subscriber handling (#4740)
df70351107 [build] Fix cmake install of thirdparty includes (#4741)
e9bd50ff9b [glass] NT view: clear meta-topic info on disconnect (#4732)
9b319fd56b [ntcore] Add sub option for local vs remote changes (#4731)
18d28ec5e3 [ntcore] Remove duplicate value checking from ClientImpl
bdfb625211 [ntcore] Send duplicate values to network if necessary
21003e34eb [commands] Update Subsystem factories and example to return CommandBase (#4729)
70080457d5 [commands] Refactor ProxyScheduleCommand, SelectCommand into ProxyCommand (#4534)
e82cd5147b [wpilib] Tweak Color HSV formula and use in AddressableLED (#4724)
ec124bb662 [commands] Allow unsetting a subsystem's default command (#4621)
2b2aa8eef7 [examples] Update all examples to use NWU coordinate conventions (#4725)
cb38bacfe8 [commands] Revert to original Trigger implementation (#4673)
15561338d5 [commands] Remove one more default command isFinished check (#4727)
ca35a2e097 Add simgui files to .gitignore (#4726)
20dbae0cee [examples] Renovate command-based examples (#4409)
1a59737f40 [commands] Add convenience factories (#4460)
42b6d4e3f7 Use defaulted comparison operators in C++ (#4723)
135c13958f [wpigui] Add FontAwesome (#4713)
ffbfc61532 [ntcore] Add NetworkTable table-specific listeners (#4640)
8958b2a4da [commands] Add property tests for command compositions (#4715)
e4ac09077c [wpilib] Add link to MotorSafety article (#4720)
f40de0c120 [commands] Add C++ factory templates (#4686)
51fa3e851f [build] cmake: Use FetchContent instead of ExternalProject (#4714)
1da84b2255 [wpigui] Reload fonts to scale rather than preloading (#4712)
e43e2fbc84 [wpiutil] StringExtras: Add UnescapeCString (#4707)
5804d8fa84 [ntcore] Server: Properly handle multiple subscribers (#4717)
169ef5fabf [glass] Update NT view for topicsOnly and sendAll changes (#4718)
148759ef54 [examples] CANPDP: Expand properties shown (#4687)
58ed112b51 [commands] RepeatCommand: restart on following iteration (#4706)
dd1da77d20 [readme] Fix broken CI badge (#4710)
7cda85df20 [build] Check Gradle plugin repo last to fix CI (#4711)
7ed9b13277 [build] Bump version plugin to fix null tag (#4705)
6b4f26225d [apriltag] Fix pluralization of apriltag artifacts (#4671)
b2d2924b72 [cscore] Add Y16 image support (#4702)
34ec89c041 [wpilibc] Shuffleboard SimpleWidget: Return pointer instead of reference (#4703)
e15200068d [ci] Disable HW testbench runs (#4704)
d5200db6cd [wpimath] Rename HolonomicDriveController.calculate params (#4683)
2ee3d86de4 [wpimath] Clarify Rotation3d roll-pitch-yaw direction (#4699)
9f0a8b930f [cscore] Use MFVideoFormat_L8 for Gray on Windows (#4701)
2bca43779e [cscore] Add UYVY image support (#4700)
4307d0ee8b [glass] Plot: allow for more than 11 plots (#4685)
3fe8d355a1 [examples] StateSpaceDifferentialDriveSimulation: Use encoder reversed constants (#4682)
b44034dadc [ntcore] Allow duplicate client IDs on server (#4676)
52d2c53888 [commands] Rename Java factory wait() to waitSeconds() (#4684)
76e918f71e [build] Fix JNI artifacts linking to incorrect libraries (#4680)
0bee875aff [commands] Change C++ CommandPtr to use CommandBase (#4677)
98e922313b [glass] Don't check IsConnected for NT widgets (#4674)
9a36373b8f [apriltag] Switch 2022 apriltag layout length and width values (#4670)
cf8faa9e67 [wpilib] Update values on controllable sendables (#4667)
5ec067c1f8 [ntcore] Implement keep duplicates pub/sub flag (#4666)
e962fd2916 [ntcore] Allow numeric-compatible value sets (#4620)
88bd67e7de [ci] Update clang repositories to jammy (#4665)
902e8686d3 [wpimath] Rework odometry APIs to improve feature parity (#4645)
e2d49181da Update to native utils 2023.8.0 (#4664)
149bac55b1 [cscore] Add Arducam OV9281 exposure quirk (#4663)
88f7a3ccb9 [wpimath] Fix Pose relativeTo documentation (#4661)
8acce443f0 [examples] Fix swerve examples to use getDistance for turning encoder (#4652)
295a1f8f3b [ntcore] Fix WaitForListenerQueue (#4662)
388e7a4265 [ntcore] Provide mechanism to reset internals of NT instance (#4653)
13aceea8dc [apriltag] Fix FieldDimensions argument order (#4659)
c203f3f0a9 [apriltag] Fix documentation for AprilTagFieldLayout (#4657)
f54d495c90 Fix non initialized hal functionality during motor safety init (#4658)
e6392a1570 [cmd] Change factories return type to CommandBase (#4655)
53904e7cf4 [apriltag] Split AprilTag functionality to a separate library (#4578)
2e88a496c2 [wpimath] Add support for swerve joystick normalization (#4516)
ce4c45df13 [wpimath] Rework function signatures for Pose Estimation / Odometry (#4642)
0401597d3b [readme] Add wpinet to MavenArtifacts.md (#4651)
2e5f9e45bb [wpimath] Remove encoder reset comments on Swerve, Mecanum Odometry and Pose Estimation (#4643)
e4b5795fc7 [docs] Disable Doxygen for memory to fix search (#4636)
03d0ea188c [build] cmake: Add missing wpinet to installed config file (#4637)
3082bd236b [build] Move version file to its own source set (#4638)
b7ca860417 [build] Use build cache for sign step (#4635)
64838e6367 [commands] Remove unsafe default command isFinished check (#4411)
1269d2b901 [myRobot] Disable spotbugs (#4565)
14d8506b72 [wpimath] Fix units docs for LinearSystemId::IdentifyDrivetrainSystem() (#4600)
d1d458db2b [wpimath] Constrain Rotation2d range to -pi to pi (#4611)
f656e99245 [readme] Add links to development build documentation (#4481)
6dd937cef7 [commands] Fix Trigger API docs (NFC) (#4599)
49047c85b9 [commands] Report error on C++ CommandPtr use-after-move (#4575)
d07267fed1 [ci] Upgrade containers to Ubuntu 22.04 and remove libclang installation (#4633)
b53ce1d3f0 [build, wpiutil] Switch macos to universal binaries (#4628)
5a320c326b [upstream_util, wpiutil] Refactor python scripts (#4614)
c4e526d315 [glass] Fix NT Mechanism2D (#4626)
d122e4254f [ci] Run spotlessApply after wpiformat in comment command (#4623)
5a1e7ea036 [wpilibj] FieldObject2d: Add null check to close() (#4619)
179f569113 [ntcore] Notify locally on SetDefault (#4617)
b0f6dc199d [wpilibc] ShuffleboardComponent.WithProperties: Update type (#4615)
7836f661cd [wpimath] Add missing open curly brace to units/base.h (#4613)
dbcc1de37f [wpimath] Add DifferentialDriveFeedforward classes which wrap LinearPlantInversionFeedforward (#4598)
93890c528b [wpimath] Add additional angular acceleration units (#4610)
3d8d5936f9 [wpimath] Add macro for disabling units fmt support (#4609)
2b04159dec [wpimath] Update units/base.h license header (#4608)
2764004fad [wpinet] Fix incorrect jni definitions (#4605)
85f1bb8f2b [wpiutil] Reenable jni check task (#4606)
231ae2c353 [glass] Plot: Fix Y-axis not being saved (#4594)
e92b6dd5f9 [wpilib] Fix AprilTagFieldLayout JSON property name typos (#4597)
2a8e0e1cc8 Update all dependencies that use grgit (#4596)
7d06e517e9 [commands] Move SelectCommand factory impl to header (#4581)
323524fed6 [wpimath] Remove deprecated units/units.h header (#4572)
d426873ed1 [commands] Add missing PS4 triangle methods (#4576)
5be5869b2f [apriltags] Use map as internal data model (#4577)
b1b4c1e9e7 [wpimath] Fix Pose3d transformBy rotation type (#4545)
a4054d702f [commands] Allow composing two triggers directly (#4580)
0190301e09 [wpilibc] Explicitly mark EventLoop as non-copyable/non-movable (#4579)
9d1ce6a6d9 [ntcore] Catch file open error when saving preferences (#4571)
5005e2ca04 [ntcore] Change Java event mask to EnumSet (#4564)
fa44a07938 [upstream-utils][mpack] Add upstream util for mpack (#4500)
4ba16db645 [ntcore] Various fixes and cleanups (#4544)
837415abfd [hal] Fix joysticks either crashing or returning 0 (#4570)
2c20fd0d09 [wpilib] SingleJointedArmSim: Check angle equals limit on wouldHit (#4567)
64a7136e08 [wpimath] SwerveDrivePoseEstimator: Restore comment about encoder reset (#4569)
b2b473b24a [wpilib] Add AprilTag and AprilTagFieldLayout (#4421)
7aab8fa93a [build] Update to Native Utils 2023.6.0 (#4563)
12c2851856 [commands] WrapperCommand: inherit from CommandBase (#4561)
0da169dd84 [wpimath] Remove template argument from ElevatorFeedforward (#4554)
2416827c25 [wpimath] Fix docs for pose estimator local measurement models (#4558)
1177a3522e [wpilib] Fix Xbox/PS4 POV sim for port number constructors (#4548)
102344e27a [commands] HID classes: Add missing methods, tweak return types (#4557)
1831ef3e19 [wpilib] Fix Shuffleboard SuppliedValueWidget (#4559)
a9606ce870 [wpilib] Fix Xbox/PS4 POV sim (#4546)
6c80d5eab3 [wpimath] Remove unused SymbolExports.h include from units/base.h (#4541)
b114006543 [ntcore] Unify listeners (#4536)
32fbfb7da6 [build] cmake: Install ntcore generated include files (#4540)
02465920fb [build] Update native utils to 2023.4.0 (#4539)
3a5a376465 [wpimath] Increase constexpr support in geometry data types (#4231)
1c3c86e9f1 [ntcore] Cache GetEntry(name) values (#4531)
dcda09f90a [command] Rename trigger methods (#4210)
66157397c1 [wpilib] Make drive classes follow NWU axes convention (#4079)
9e22ffbebf [ntcore] Fix null deref in NT3 client (#4530)
648ab6115c [wpigui,dlt,glass,ov] Support arm in GUI tools (#4527)
8bc3b04f5b [wpimath] Make ComputerVisionUtil use 3D geometry classes (#4528)
cfb84a6083 [wpilibc] Don't hang waiting for NT server to start (#4524)
02c47726e1 [wpimath] Remove unused odometry instance from DifferentialDrivePoseEstimator test (#4522)
b2a0093294 [ci] Revert upgrade of github-pages-deploy-action (#4521)
2a98d6b5d7 [wpimath] PIDController: Add getters for position & velocity tolerances (#4458)
9f36301dc8 [ci] Write wpiformat patch to job summary (#4519)
901fc555f4 [wpimath] Position Delta Odometry for Mecanum (#4514)
4170ec6107 [wpimath] Position Delta Odometry for Swerve (#4493)
fe400f68c5 [docs] Add wpinet to docs build (#4517)
794669b346 [ntcore] Revamp listeners (#4511)
dcfa85a5d5 [ci] Build sanitizers with clang-14 (#4518)
15ad855f1d [ntcore] Add UnitTopic<T> (C++ only) (#4497)
11244a49d9 [wpilib] Add IsConnected function to all gyros (#4465)
1d2e8eb153 [build] Update myRobot deployment (#4515)
ad53fb19b4 [hal] Use new HMB api for addressable LED (#4479)
ba850bac3b [hal] Add more shutdown checks and motor safety shutdown (#4510)
023a5989f8 [ntcore] Fix typo in NetworkServer client connect message (#4512)
c970011ccc [docs] Add Doxygen aliases used by Foonathan memory (#4509)
07a43c3d9a [readme] Document clang-format version and /wpiformat (#4503)
a05b212b04 [ci] Revert changes to wpiformat task from #4501 (#4508)
09faf31b67 [commands] Replace Command HID inheritance with delegation (#4470)
9e1f9c1133 [commands] Add command factories (#4476)
f19d2b9b84 [ci] Add NUMBER environment variable to comment command commit script (#4507)
a28f93863c [ci] Push comment command commit directly to PR (#4506)
c9f61669b8 [ci] Fix comment command commit push (#4505)
dcce5ad3b3 [ci] Update github-script API usage (#4504)
6836e5923d [wpilibc] Restore get duty cycle scale factor (#4502)
335188c652 [dlt] Add deselect/select all buttons to download view (#4499)
60a29dcb99 [glass] Field2D: Add "hidden" option for objects (#4498)
b55d5b3034 [ci] Update deprecated github actions (#4501)
10ed4b3969 [ntcore] Various NT4 fixes (#4474)
4a401b89d7 [hal, wpilib] New DS thread model and implementation (#3787)
c195b4fc46 [wpimath] Clean up PoseEstimator nominal dt docs (#4496)
8f2e34c6a3 [build] Remove wpilib prefix from CMake flat install (#4492)
150d692df7 [wpimath] Remove unused private PoseEstimator function (#4495)
3e5bfff1b5 [wpimath] FromFieldRelativeSpeeds: Add ChassisSpeeds overload (#4494)
9c7e66a27d [commands] C++: Add CommandPtr overload for SetDefaultCommand (#4488)
0ca274866b [build] Fix CMake system library opt-ins (#4487)
dc037f8d41 [commands] Remove EndlessCommand (#4483)
16cdc741cf [wpimath] Add Pose3d(Pose2d) constructor (#4485)
9d5055176d [build] cmake: Allow disabling ntcore build (#4486)
d1e66e1296 [build] Compile all java code with inline string concatenation (#4490)
1fc098e696 Enable log macros to work with no args (#4475)
878cc8defb [wpilib] LiveWindow: Add enableAllTelemetry() (#4480)
8153911160 [build] Fix MSVC runtime archiver to grab default runtime (#4478)
fbdc810887 Upgrade to C++20 (#4239)
396143004c [ntcore] Add ntcoreffi binary (#4471)
1f45732700 [build] Update to 2023.2.4 native-utils and new dependencies (#4473)
574cb41c18 [ntcore] Various fixes (#4469)
d9d6c425e7 [build] Force Java 11 source compatibility (#4472)
58b6484dbe Switch away from NI interrupt manager to custom implementation (#3705)
ca43fe2798 [wpimath] Use Units conversions in ComputerVisionUtil docs (NFC) (#4464)
87a64ccedc [hal] Convert DutyCycle Raw output to be a high time measurement (#4466)
89a3d00297 [commands] Add FinallyDo and HandleInterrupt decorators (#4412)
1497665f96 [commands] Add C++ versions of Java-only decorators (#4457)
27b173374e [wpimath] Add minLinearAccel parameter to DifferentialDriveAccelerationLimiter (#4422)
2a13dba8ac [wpilib] TrajectoryUtil: Fix ambiguous documentation (NFC) (#4461)
77301b126c [ntcore] NetworkTables 4 (#3217)
90cfa00115 [build] cmake: Fix libssh include directory order (#4459)
5cf961edb9 [commands] Refactor lambda-based commands to inherit FunctionalCommand (#4451)
b2276e47de [wpimath] Enable continuous angle input for HolonomicDriveController (#4453)
893b46139a [fieldImages] Add utilities to simplify loading of fields (#4456)
60e29627c0 [commands] C++ unique_ptr migration (#4319)
3b81cf6c35 [wpilib] Improve Color.toString (#4450)
5c067d30a0 [wpinet] WebSocket: Add SendFrames() (#4445)
ceaf493811 [wpiutil] MakeJByteArray: Use span<uint8> instead of string_view (#4446)
10e04e2b13 [examples] FrisbeeBot: Fix reference capture (#4449)
726f67c64b [build] Add exeSplitSetup (#4444)
c7b7624c1c [wpiutil] Add MessagePack utility functions (#4448)
d600529ec0 [wpinet] uv::Async: Add UnsafeSend() (#4447)
b53b3526a2 [wpimath] Add CoordinateSystem conversion for Transform3d (#4443)
38bb23eb18 [wpimath] Add scalar multiply and divide operators to all geometry classes (#4438)
3937ff8221 [wpilib] Remove deprecated Controller class (#4440)
abbfe244b5 [wpilib] Improve Color FromHSV (#4439)
4ddb8aa0dd [sim] Provide function that resets all simulation data (#4016)
a791470de7 Clean up Java warning suppressions (#4433)
17f504f548 [hal,wpilib] Fix SPI Mode Setting (#4434)
773198537c [wpiutil] Add wpi::scope_exit (#4432)
5ac658c8f0 [wpiutil] Logger: Conditionalize around WPI_LOG (#4431)
8767e4a941 [wpiutil] DataLog: Fix SetMetadata output (#4430)
8c4af073f4 [wpiutil] Synchronization: shutdown race protection (#4429)
c79f38584a [build] Fix Java integration tests (#4428)
36c08dd97c [build] Fix cmake install of fmtlib (#4426)
69b7b3dd7d [ci] Remove the Windows cmake job (#4425)
738c75fed8 [readme] Fix formatting/linting link (#4423)
4eb1d03fb3 [wpimath] Document C++ LinearFilter exception (#4417)
ba4ec6c967 [build] Fix clang-tidy false positive on Linux (#4406)
97836f0e55 [commands] Fix ProfiledPIDSubsystem setGoal behavior (#4414)
fdfb85f695 [wpimath] Remove Java LQR constructor that takes a controller gain matrix (#4419)
ab1baf4832 [wpimath] Add rotation matrix constructor to Rotation3d (#4413)
9730032866 [wpimath] Document LQR and KalmanFilter exceptions (#4418)
5b656eecf6 [wpimath] Fix HTML5 entity (#4420)
9ae38eaa7c [commands] Add owning overload to ProxyScheduleCommand (#4405)
cb33bd71df [commands] deprecate withInterrupt decorator (#4407)
d9b4e7b8bf [commands] Revert "Change grouping decorator impl to flatten nested group structures (#3335)" (#4402)
0389bf5214 [hal] REVPH: Improve handling of disconnected CAN Bus (#4169)
4267fa08d1 [wpilibc] ADIS IMUs: Fix memory leak (#4170)
65c8fbd452 [wpilib] MotorControllerGroup: Override setVoltage (#4403)
f36162fddc [wpimath] Improve Discretization internal docs (#4400)
5149f7d894 [wpimath] Add two-vector Rotation3d constructor (#4398)
20b5bed1cb [wpimath] Clean up Java Quaternion class (#4399)
f18dd1905d [build] Include all thirdparty sources in distribution (#4397)
aa9d7f1cdc [wpiutil] Import foonathan memory (#4306)
2742662254 [ci] Remove a couple of obsolete clang-tidy checks (#4396)
a5df391166 [hal, wpilib] Fix up DIO pulse API (#4387)
59e6706b75 [glass] Turn on docking by default
8461bb1e03 [glass] Add support for saving docking info
b873e208b4 [wpigui] Add support for imgui config flags
873e72df8c [build] Update imgui to 1.88 docking branch
c8bd6fc5b4 [ci] Fix comment-command (take 2) (#4395)
fed68b83b4 [ci] Fix comment-command action not running runners (#4393)
0ef8a4e1df [wpimath] Support formatting more Eigen types (#4391)
c393b3b367 [build] Update to native utils 2023.1.0 and Gradle 7.5.1 (#4392)
b5a17f762c [wpimath] Add direction to slew rate limiter (#4377)
fafc81ed1a [wpiutil] Upgrade to fmt 9.1.0 (#4389)
cc56bdc787 [wpiutil] SafeThread: Add Synchronization object variant (#4382)
4254438d8d [commands] Mark command group lifecycle methods as final (#4385)
97c15af238 [wpimath] LinearSystemId: Fix docs, move C++ impls out of header (#4388)
d22ff8a158 [wpiutil] Add JNI access to C++ stderr (#4381)
fdb5a2791f [wpiutil] jni_util: Add Mac-friendly MakeJLongArray/JArrayRef (#4383)
c3a93fb995 [commands] Revamp Interruptible (#4192)
f2a8d38d2a [commands] Rename Command.repeat to repeatedly (#4379)
9e24c6eac0 [wpiutil] Logger: paren-protect instance usage in macro (#4384)
fe4d12ce22 [wpimath] Add LTV controller derivations and make enums private (#4380)
eb08486039 [build] Fix MacOS binary rpath generation (#4376)
ccf83c634a [build] Use native-utils platform names instead of raw strings (#4375)
3fd69749e7 [docs] Upgrade to doxygen 1.9.4 (#4370)
594df5fc08 [wpinet] uv/util.h: Pull in ws2_32.lib on Windows for ntohs (#4371)
539070820d [ci] Enable asan for wpinet and wpiutil (#4369)
564a56d99b [wpinet] Fix memory leak in WorkerThreadTest (#4368)
5adf50d93c [upstream_utils] Refactor upstream_utils scripts (#4367)
d80e8039d7 [wpiutil] Suppress fmtlib clang-tidy warning in C++20 consteval contexts (#4364)
0e6d67b23b [upstream_utils] Remove yapf format disable comment (#4366)
be5270697a [build] Suppress enum-enum deprecation warning in OpenCV (#4365)
8d28851263 Add Rosetta install command to build requirements (#4363)
3d2115c93e [wpinet] include-what-you-use in MulticastTest (#4360)
91002ae3cc [wpimath] Upgrade to Drake 1.6.0 (#4361)
148c18e658 [wpinet] Upgrade to libuv 1.44.2 (#4362)
a2a5c926b6 Fix clang-tidy warnings (#4359)
ea6b1d8449 [wpiutil] Remove unused ManagedStatic class (#4358)
ac9be78e27 Use stricter C++ type conversions (#4357)
151dabb2af [wpiutil] Upgrade to fmt 9.0.0 (#4337)
340465c929 [ci] Upgrade to clang-format and clang-tidy 14 (NFC) (#4347)
d45bcddd15 [examples] Add comments to StateSpaceDifferentialDrive (#4341)
0e0786331a Update LLVM libraries to 14.0.6 (#4350)
c5db23f296 [wpimath] Add Eigen sparse matrix and iterative solver support (#4349)
44abc8dfa6 [upstream_utils] Remove git version from upstream patches (#4351)
3fdb2f767d [wpimath] Add comments with Ramsete equations (#4348)
0485f05da9 [wpilibjExamples] Upgrade jacoco to match allwpilib (#4346)
0a5eb65231 [wpinet] Handle empty txt block for mdns announcer (#4072)
19ffebaf3e [wpilib] Add reference to I2C Lockup to API Docs (NFC) (#4340)
ce1a90d639 [hal] Replace SerialHelper "goto done" with continue (#4342)
d25af48797 [ci] Make upstream_utils CI fail on untracked files (#4339)
ebb836dacb [examples] Fix negations in event loop examples (#4334)
d83e202f00 [upstream_utils] Update paths in update_fmt.py (#4338)
3ccf806064 [wpimath] Remove redundant LinearFilter.finiteDifference() argument (#4335)
6f1e01f8bd [wpimath] Document example of online filtering for LinearFilter.finiteDifference() (#4336)
1023c34b1c [readme] Update location of ni-libraries (#4333)
faa29d596c [wpilib] Improve Notifier docs (NFC) (#4326)
add00a96ed [wpimath] Improve DifferentialDriveAccelerationLimiter docs (NFC) (#4323)
82fac41244 [wpimath] Better document trackwidth parameters (NFC) (#4324)
5eb44e22a9 Format Python scripts with black (NFC) (#4325)
2e09fa7325 [build] Fix mpack cmake (#4322)
fe3c24b1ee [command] Add ignoringDisable decorator (#4305)
aa221597bc [build] Add M1 builds, change arm name, update to 2023 deps (#4315)
579a8ee229 [ci] Use one worker for Windows release Gradle build (#4318)
5105c5eab6 [wpilibj] Change "final" to "exit" in the IterativeRobotBase JavaDoc (NFC) (#4317)
787fe6e7a5 [wpiutil] Separate third party libraries (#4190)
6671f8d099 [wpigui] Update portable file dialogs (#4316)
9ac9b69aa2 [command] Reorder Scheduler operations (#4261)
e61028cb18 [build] halsim_gui: Add wpinet dependency (#4313)
661d23eaf5 [glass] Add precision setting for NetworkTable view (#4311)
666040e3e5 [hal] Throw exceptions for invalid sizes in I2C and SPI JNI (#4312)
aebc272449 [build] Upgrade to spotbugs Gradle plugin 5.0.8 (#4310)
fd884581e4 [wpilib] Add BooleanEvent/Trigger factories on HID classes (#4247)
9b1bf5c7f1 [wpimath] Move Drake and Eigen to thirdparty folders (#4307)
c9e620a920 [wpilibc] Change EventLoop data structure to vector (#4304)
41d40dd62f [wpinet] Fix libuv unused variable warning on Mac (#4299)
30f5b68264 [wpinet] Fix JNI loading error (#4295)
f7b3f4b90e [examples] Getting Started: Change Joystick to XboxController (#4194)
a99c11c14c [wpimath] Replace UKF implementation with square root form (#4168)
45b7fc445b [wpilib] Add EventLoop (#4104)
16a4888c52 [wpilib] Default off LiveWindow telemetry (#4301)
17752f1337 [ci] Split debug and release Windows builds (#4277)
abb45a68db [commands] Remove custom test wrappers (#4296)
1280a54ef3 [upstream_utils]: Make work with Python 3.8 (#4298)
f2d243fa68 [build] Change defaults for Java lints (#4300)
a4787130f4 Update using development build to work with 2023 gradlerio (#4294)
af7985e46c [wpiutil] Use invoke_result_t instead of result_of in future.h (#4293)
e9d1b5c2d0 [hal] Remove deprecated SimDevice functions (#4209)
45b598d236 [wpilibj] Add toString() methods to Color and Color8Bit (#4286)
fc37265da5 [wpimath] Add angle measurement convention to ArmFeedforward docs (NFC) (#4285)
a4ec13eb0e [wpilibjexamples] Remove unnecessary voltage desaturation
2fa52007af [wpilibc] Use GetBatteryVoltage() in MotorController::SetVoltage
d9f9cd1140 [wpimath] Reset prev_time on pose estimator reset (#4283)
8b6df88783 [wpilibj] Tachometer.getFrequency(): Fix bug (#4281)
345cff08c0 [wpiutil] Make wpi::array constexpr (#4278)
57428112ac [wpimath] Upgrade to Drake v1.3.0 (#4279)
a18d4ff154 [build] Fix tools not being copied when built with -Ponly* (#4276)
d1cd07b9f3 [wpigui] Add OpenURL (#4273)
e67f8e917a [glass] Use glfwSetKeyCallback for Enter key remap (#4275)
be2fedfe50 [wpimath] Add stdexcept include for std::invalid_argument (IWYU) (#4274)
7ad2be172e [build] Update native-utils to 2023.0.1 (#4272)
abc605c9c9 [ci] Update workflows to 20.04 base image (#4271)
3e94805220 [wpiutil] Reduce llvm collections patches (#4268)
db2e1d170e [upstream_utils] Document how to update thirdparty libraries (#4253)
96ebdcaf16 [wpimath] Remove unused Eigen AutoDiff module (#4267)
553b2a3b12 [upstream_utils] Fix stackwalker (#4265)
3e13ef42eb [wpilibc] Add missing std::array #include (include-what-you-use) (#4266)
d651a1fcec Fix internal deprecation warnings (#4257)
b193b318c1 [commands] Add unless() decorator (#4244)
ef3714223b [commands] Remove docs reference to obsolete interrupted() method (NFC) (#4262)
3d8dbbbac3 [readme] Add quickstart (#4225)
013efdde25 [wpinet] Wrap a number of newer libuv features (#4260)
816aa4e465 [wpilib] Add Pneumatics sim classes (#4033)
046c2c8972 [wpilibc] Rename SpeedControllerGroupTest.cpp (#4258)
d80e9cdf64 [upstream_utils] Use shallow clones for thirdparty repos (#4255)
7576136b4a [upstream_utils] Make update_llvm.py executable (#4254)
c3b223ce60 [wpiutil] Vendor llvm and update to 13.0.0 (#4224)
5aa67f56e6 [wpimath] Clean up math comments (#4252)
fff4d1f44e [wpimath] Extend Eigen warning suppression to GCC 12 (#4251)
0d9956273c [wpimath] Add CoordinateSystem.convert() translation and rotation overloads (#4227)
3fada4e0b4 [wpinet] Update to libuv 1.44.1 (#4232)
65b23ac45e [wpilibc] Fix return value of DriverStation::GetJoystickAxisType() (#4230)
4ac34c0141 [upstream_utils] Cleanup update_libuv.py (#4249)
8bd614bb1e [upstream_utils] Use "git am" instead of "git apply" for patches (#4248)
4253d6d5f0 [upstream_utils] Apply "git am" patches individually (#4250)
6a4752dcdc Fix GCC 12.1 warning false positives (#4246)
5876b40f08 [wpimath] Memoize CoordinateSystem and CoordinateAxis statics (#4241)
5983434a70 [cameraserver] Replace IterativeRobot in comment sample code with TimedRobot (#4238)
a3d44a1e69 [wpimath] Add Translation2d.getAngle() (#4217)
d364bbd5a7 [upstream_utils] Give vendor update scripts execute permissions (#4226)
f341e1b2be [wpimath] Document standard coordinate systems better (NFC) (#4228)
9af389b200 [wpinet] AddrToName: Initialize name (#4229)
2ae4adf2d7 [ci] Add wpiformat command to PRs (#4223)
178b2a1e88 Contributing.md: Correct version of clang-format used (#4222)
18db343cdc [wpiutil, wpinet] Vendor libuv, stack walker (#4219)
f0c821282a [build] Use artifactory mirror (#4220)
d673ead481 [wpinet] Move network portions of wpiutil into new wpinet library (#4077)
b33715db15 [wpimath] Add CoordinateSystem class (#4214)
99424ad562 [sim] Allow creating a PWMSim object from a PWMMotorController (#4039)
dc6f641fd2 [wpimath] PIDController: Reset position and velocity error when reset() is called. (#4064)
f20a20f3f1 [wpimath] Add 3D geometry classes (#4175)
708a4bc3bc [wpimath] Conserve previously calculated swerve module angles when updating states for stationary ChassisSpeeds (#4208)
ef7ed21a9d [wpimath] Improve accuracy of ComputerVisionUtil.calculateDistanceToTarget() (#4215)
b1abf455c1 [wpimath] LTVUnicycleController: Use LUT, provide default hyperparameters (#4213)
d5456cf278 [wpimath] LTVDifferentialDriveController: Remove unused variable (#4212)
99343d40ba [command] Remove old command-based framework (#4211)
ee03a7ad3b Remove most 2022 deprecations (#4205)
ce1a7d698a [wpimath] Refactor WheelVoltages inner class to a separate file (#4203)
87bf70fa8e [wpimath] Add LTV controllers (#4094)
ebd2a303bf [wpimath] Remove deprecated MakeMatrix() function (#4202)
e28776d361 [wpimath] LinearSystemLoop: Add extern templates for common cases
dac1429aa9 [wpimath] LQR: Use extern template instead of Impl class
e767605e94 [wpimath] Add typedefs for common types
97c493241f [wpimath] UnscentedKalmanFilter: Move implementation out-of-line
8ea90d8bc9 [wpimath] ExtendedKalmanFilter: Move implementation out-of-line
ae7b1851ec [wpimath] KalmanFilter: Use extern template instead of Impl class
e3d62c22d3 [wpimath] Add extern templates for common cases
7200c4951d [wpiutil] SymbolExports: Add WPILIB_IMPORTS for dllimport
84056c9347 [wpiutil] SymbolExports: Add EXPORT_TEMPLATE_DECLARE/DEFINE
09cf6eeecb [wpimath] ApplyDeadband: add a scale param (#3865)
03230fc842 [build,ci] Enable artifactory build cache (#4200)
63cf3aaa3f [examples] Don't square ArcadeDrive inputs in auto (#4201)
18ff694f02 [wpimath] Add Rotation2d.fromRadians factory (#4178)
4f79ceedd9 [wpilibc] Add missing #include (#4198)
f7ca72fb41 [command] Rename PerpetualCommand to EndlessCommand (#4177)
a06b3f0307 [hal] Correct documentation on updateNotifierAlarm (#4156)
d926dd1610 [wpimath] Fix pose estimator performance (#4111)
51bc893bc5 [wpiutil] CircularBuffer: Change Java package-private methods to public (#4181)
fbe761f7f6 [build] Increase Gradle JVM heap size (#4172)
5ebe911933 [wpimath] Add DifferentialDriveAccelerationLimiter (#4091)
3919250da2 [wpilibj] Remove finalizers (#4158)
b3aee28388 [commands] Allow BooleanSupplier for Trigger operations (#4103)
9d20ab3024 [wpilib] Allow disabling ElevatorSim gravity (#4145)
aaa69f6717 [ci] Remove 32-bit Windows builds (#4078)
355a11a414 Update Java linters and fix new PMD errors (#4157)
ffc69d406c [examples] Reduce suggested acceleration in Ramsete example (#4171)
922d50079a [wpimath] Units: fix comment in degreesToRotations (NFC) (#4159)
dd163b62ae [wpimath] Rotation2d: Add factory method that uses rotations (#4166)
bd80e220b9 [ci] Upgrade CMake actions (#4161)
aef4b16d4c [wpimath] Remove unnecessary NOLINT in LinearPlantInversionFeedforward (NFC) (#4155)
975171609e [wpilib] Compressor: Rename enabled to isEnabled (#4147)
5bf46a9093 [wpimath] Add ComputerVisionUtil (#4124)
f27a1f9bfb [commands] Fix JoystickButton.getAsBoolean (#4131)
1b26e2d5da [commands] Add RepeatCommand (#4009)
88222daa3d [hal] Fix misspelling in AnalogInput/Output docs (NFC) (#4153)
81c5b41ce1 [wpilibj] Document MechanismLigament2d angle unit (NFC) (#4142)
9650e6733e [wpiutil] DataLog: Document finish and thread safety (NFC) (#4140)
c8905ec29a [wpimath] Remove ImplicitModelFollower dt argument (#4119)
b4620f01f9 [wpimath] Fix Rotation2d interpolation in Java (#4125)
2e462a19d3 [wpimath] Constexprify units unary operators (#4138)
069f932e59 [build] Fix gl3w cmake build (#4139)
126e3de91a [wpilibc] Remove unused SetPriority() call from Ultrasonic (#4123)
ba0dccaae4 [wpimath] Fix reference to Rotation2d.fromRadians() (#4118)
e1b6e5f212 [wpilib] Improve MotorSafety documentation (NFC) (#4120)
8d79dc8738 [wpimath] Add ImplicitModelFollower (#4056)
78108c2aba [wpimath] Fix PIDController having incorrect error after calling SetSetpoint() (#4070)
cdafc723fb [examples] Remove unused LinearPlantInversionFeedforward includes (#4069)
0d70884dce [wpimath] Add InterpolatedTreeMap (#4073)
765efa325e [wpimath] Remove redundant column index from vectors (#4116)
89ffcbbe41 [wpimath] Update TrapezoidProfile class name in comment (NFC) (#4107)
95ae23b0e7 [wpimath] Improve EKF numerical stability (#4093)
d5cb6fed67 [wpimath] Support zero cost entries in MakeCostMatrix() (#4100)
d0fef18378 [wpimath] Remove redundant `this.` from ExtendedKalmanFilter.java (#4115)
d640c0f41f [wpimath] Fix pose estimator local measurement standard deviation docs (NFC) (#4113)
a2fa5e3ff7 [wpilibc] BatterySim: Provide non-initializer list versions of Calculate (#4076)
a3eea9958e [hal] Add link to FRC CAN Spec (NFC) (#4086)
db27331d7b [wpilib] Update DifferentialDrive docs (NFC) (#4085)
fdfb31f164 [dlt] Export boolean[] values (#4082)
f93c3331b3 [wpigui] disable changing directory when initializing on MacOS (#4092)
ab7ac4fbb9 [build] Fix various warnings in cmake builds (#4081)
bc39a1a293 [wpilibc] Fix moved pneumatics objects not destructing properly (#4068)
2668130e70 [wpimath] Remove SwerveDrivePoseEstimator encoder reset warning (#4066)
d27ed3722b [ci] Set actions workflow concurrency (#4060)
dae18308c9 [wpimath] Minor fixes to Rotation2d docs (NFC) (#4055)
d66555e42f [datalogtool] Add datalogtool
9f52d8a3b1 [wpilib] DriverStation: Add DataLog support for modes and joystick data
757ea91932 [wpilib] Add DataLogManager
02a804f1c5 [ntcore] Add DataLog support
9b500df0d9 [wpiutil] Add high speed data logging
5a89575b3a [wpiutil] Import customized LLVM MemoryBuffer
b8c4d7527b [wpiutil] Add MappedFileRegion
ac5d46cfa7 [wpilibc] Fix ProfiledPID SetTolerance default velocity value (#4054)
bc9e96e86f [wpilib] Absolute Encoder API and behavior fixes (#4052)
f88c435dd0 [hal] Add mechanism to cancel all periodic callbacks (#4049)
Change-Id: I49aa5b08abbefc7a045e99e19d48ce2cd8fc4d1b
git-subtree-dir: third_party/allwpilib
git-subtree-split: 83f1860047c86aa3330fcb41caf3b2047e074804
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
new file mode 100644
index 0000000..97968dc
--- /dev/null
+++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
@@ -0,0 +1,19 @@
+// 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 "frc/ComputerVisionUtil.h"
+
+#include "units/math.h"
+
+namespace frc {
+
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+ const frc::Transform3d& cameraToObject,
+ const frc::Transform3d& robotToCamera) {
+ const auto objectToCamera = cameraToObject.Inverse();
+ const auto cameraToRobot = robotToCamera.Inverse();
+ return objectInField + objectToCamera + cameraToRobot;
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/MathUtil.cpp b/wpimath/src/main/native/cpp/MathUtil.cpp
deleted file mode 100644
index 19cd214..0000000
--- a/wpimath/src/main/native/cpp/MathUtil.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-// 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 "frc/MathUtil.h"
-
-#include <cmath>
-
-namespace frc {
-
-double ApplyDeadband(double value, double deadband) {
- if (std::abs(value) > deadband) {
- if (value > 0.0) {
- return (value - deadband) / (1.0 - deadband);
- } else {
- return (value + deadband) / (1.0 - deadband);
- }
- } else {
- return 0.0;
- }
-}
-
-} // namespace frc
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index 8f1145d..fc532db 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -6,33 +6,31 @@
namespace frc {
-Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
- return Eigen::Vector<double, 3>{pose.Translation().X().value(),
- pose.Translation().Y().value(),
- pose.Rotation().Radians().value()};
+Vectord<3> PoseTo3dVector(const Pose2d& pose) {
+ return Vectord<3>{pose.Translation().X().value(),
+ pose.Translation().Y().value(),
+ pose.Rotation().Radians().value()};
}
-Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
- return Eigen::Vector<double, 4>{pose.Translation().X().value(),
- pose.Translation().Y().value(),
- pose.Rotation().Cos(), pose.Rotation().Sin()};
+Vectord<4> PoseTo4dVector(const Pose2d& pose) {
+ return Vectord<4>{pose.Translation().X().value(),
+ pose.Translation().Y().value(), pose.Rotation().Cos(),
+ pose.Rotation().Sin()};
}
template <>
-bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
- const Eigen::Matrix<double, 1, 1>& B) {
+bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
return detail::IsStabilizableImpl<1, 1>(A, B);
}
template <>
-bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 1>& B) {
+bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
return detail::IsStabilizableImpl<2, 1>(A, B);
}
-Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
- return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
- pose.Rotation().Radians().value()};
+Vectord<3> PoseToVector(const Pose2d& pose) {
+ return Vectord<3>{pose.X().value(), pose.Y().value(),
+ pose.Rotation().Radians().value()};
}
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
new file mode 100644
index 0000000..9788023
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
@@ -0,0 +1,85 @@
+// 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 "frc/controller/DifferentialDriveAccelerationLimiter.h"
+
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
+ LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+ units::meters_per_second_squared_t maxLinearAccel,
+ units::radians_per_second_squared_t maxAngularAccel)
+ : DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel,
+ maxLinearAccel, maxAngularAccel) {}
+
+DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
+ LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+ units::meters_per_second_squared_t minLinearAccel,
+ units::meters_per_second_squared_t maxLinearAccel,
+ units::radians_per_second_squared_t maxAngularAccel)
+ : m_system{std::move(system)},
+ m_trackwidth{trackwidth},
+ m_minLinearAccel{minLinearAccel},
+ m_maxLinearAccel{maxLinearAccel},
+ m_maxAngularAccel{maxAngularAccel} {
+ if (minLinearAccel > maxLinearAccel) {
+ throw std::invalid_argument(
+ "maxLinearAccel must be greater than minLinearAccel");
+ }
+}
+
+DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
+ units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+ units::volt_t rightVoltage) {
+ Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
+
+ // Find unconstrained wheel accelerations
+ Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
+ Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
+
+ // Convert from wheel accelerations to linear and angular accelerations
+ //
+ // a = (dxdt(0) + dx/dt(1)) / 2
+ // = 0.5 dxdt(0) + 0.5 dxdt(1)
+ //
+ // α = (dxdt(1) - dxdt(0)) / trackwidth
+ // = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
+ //
+ // [a] = [ 0.5 0.5][dxdt(0)]
+ // [α] [-1/trackwidth 1/trackwidth][dxdt(1)]
+ //
+ // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
+ Matrixd<2, 2> M{{0.5, 0.5},
+ {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
+ Vectord<2> accels = M * dxdt;
+
+ // Constrain the linear and angular accelerations
+ if (accels(0) > m_maxLinearAccel.value()) {
+ accels(0) = m_maxLinearAccel.value();
+ } else if (accels(0) < m_minLinearAccel.value()) {
+ accels(0) = m_minLinearAccel.value();
+ }
+ if (accels(1) > m_maxAngularAccel.value()) {
+ accels(1) = m_maxAngularAccel.value();
+ } else if (accels(1) < -m_maxAngularAccel.value()) {
+ accels(1) = -m_maxAngularAccel.value();
+ }
+
+ // Convert the constrained linear and angular accelerations back to wheel
+ // accelerations
+ dxdt = M.householderQr().solve(accels);
+
+ // Find voltages for the given wheel accelerations
+ //
+ // dx/dt = Ax + Bu
+ // u = B⁻¹(dx/dt - Ax)
+ u = m_system.B().householderQr().solve(dxdt - m_system.A() * x);
+
+ return {units::volt_t{u(0)}, units::volt_t{u(1)}};
+}
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
new file mode 100644
index 0000000..e1b2732
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
@@ -0,0 +1,37 @@
+// 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 "frc/controller/DifferentialDriveFeedforward.h"
+
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_rad_per_s) kVAngular,
+ decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
+ : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+ kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {}
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
+ : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+ kVLinear, kALinear, kVAngular, kAAngular)} {}
+
+DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
+ units::meters_per_second_t currentLeftVelocity,
+ units::meters_per_second_t nextLeftVelocity,
+ units::meters_per_second_t currentRightVelocity,
+ units::meters_per_second_t nextRightVelocity, units::second_t dt) {
+ frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
+
+ frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
+ frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
+ auto u = feedforward.Calculate(r, nextR);
+ return {units::volt_t{u(0)}, units::volt_t{u(1)}};
+}
diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
index 23b22cd..def7234 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -15,7 +15,9 @@
ProfiledPIDController<units::radian> thetaController)
: m_xController(std::move(xController)),
m_yController(std::move(yController)),
- m_thetaController(std::move(thetaController)) {}
+ m_thetaController(std::move(thetaController)) {
+ m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
+}
bool HolonomicDriveController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
@@ -32,8 +34,9 @@
}
ChassisSpeeds HolonomicDriveController::Calculate(
- const Pose2d& currentPose, const Pose2d& poseRef,
- units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+ const Pose2d& currentPose, const Pose2d& trajectoryPose,
+ units::meters_per_second_t desiredLinearVelocity,
+ const Rotation2d& desiredHeading) {
// If this is the first run, then we need to reset the theta controller to the
// current pose's heading.
if (m_firstRun) {
@@ -42,13 +45,13 @@
}
// Calculate feedforward velocities (field-relative)
- auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
- auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
- auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
- currentPose.Rotation().Radians(), angleRef.Radians()));
+ auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
+ auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
+ auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
+ currentPose.Rotation().Radians(), desiredHeading.Radians())};
- m_poseError = poseRef.RelativeTo(currentPose);
- m_rotationError = angleRef - currentPose.Rotation();
+ m_poseError = trajectoryPose.RelativeTo(currentPose);
+ m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
@@ -56,10 +59,10 @@
}
// Calculate feedback velocities (based on position error).
- auto xFeedback = units::meters_per_second_t(
- m_xController.Calculate(currentPose.X().value(), poseRef.X().value()));
- auto yFeedback = units::meters_per_second_t(
- m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value()));
+ auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
+ currentPose.X().value(), trajectoryPose.X().value())};
+ auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
+ currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -68,9 +71,9 @@
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Trajectory::State& desiredState,
- const Rotation2d& angleRef) {
+ const Rotation2d& desiredHeading) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
- angleRef);
+ desiredHeading);
}
void HolonomicDriveController::SetEnabled(bool enabled) {
diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
new file mode 100644
index 0000000..17a0c56
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -0,0 +1,153 @@
+// 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 "frc/controller/LTVDifferentialDriveController.h"
+
+#include <cmath>
+
+#include "frc/MathUtil.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+
+using namespace frc;
+
+namespace {
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+ /// X position in global coordinate frame.
+ [[maybe_unused]] static constexpr int kX = 0;
+
+ /// Y position in global coordinate frame.
+ static constexpr int kY = 1;
+
+ /// Heading in global coordinate frame.
+ static constexpr int kHeading = 2;
+
+ /// Left encoder velocity.
+ [[maybe_unused]] static constexpr int kLeftVelocity = 3;
+
+ /// Right encoder velocity.
+ [[maybe_unused]] static constexpr int kRightVelocity = 4;
+};
+
+} // namespace
+
+LTVDifferentialDriveController::LTVDifferentialDriveController(
+ const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth,
+ const wpi::array<double, 5>& Qelems, const wpi::array<double, 2>& Relems,
+ units::second_t dt)
+ : m_trackwidth{trackwidth} {
+ // Control law derivation is in section 8.7 of
+ // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+ Matrixd<5, 5> A{
+ {0.0, 0.0, 0.0, 0.5, 0.5},
+ {0.0, 0.0, 0.0, 0.0, 0.0},
+ {0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()},
+ {0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)},
+ {0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}};
+ Matrixd<5, 2> B{{0.0, 0.0},
+ {0.0, 0.0},
+ {0.0, 0.0},
+ {plant.B(0, 0), plant.B(0, 1)},
+ {plant.B(1, 0), plant.B(1, 1)}};
+ Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems);
+ Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+
+ // dx/dt = Ax + Bu
+ // 0 = Ax + Bu
+ // Ax = -Bu
+ // x = -A⁻¹Bu
+ units::meters_per_second_t maxV{
+ -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
+
+ for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
+ // The DARE is ill-conditioned if the velocity is close to zero, so don't
+ // let the system stop.
+ if (units::math::abs(velocity) < 1e-4_mps) {
+ m_table.insert(velocity, Matrixd<2, 5>::Zero());
+ } else {
+ A(State::kY, State::kHeading) = velocity.value();
+ m_table.insert(velocity,
+ frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
+ }
+ }
+}
+
+bool LTVDifferentialDriveController::AtReference() const {
+ return std::abs(m_error(0)) < m_tolerance(0) &&
+ std::abs(m_error(1)) < m_tolerance(1) &&
+ std::abs(m_error(2)) < m_tolerance(2) &&
+ std::abs(m_error(3)) < m_tolerance(3) &&
+ std::abs(m_error(4)) < m_tolerance(4);
+}
+
+void LTVDifferentialDriveController::SetTolerance(
+ const Pose2d& poseTolerance,
+ units::meters_per_second_t leftVelocityTolerance,
+ units::meters_per_second_t rightVelocityTolerance) {
+ m_tolerance =
+ Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(),
+ poseTolerance.Rotation().Radians().value(),
+ leftVelocityTolerance.value(), rightVelocityTolerance.value()};
+}
+
+DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
+ const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
+ units::meters_per_second_t leftVelocityRef,
+ units::meters_per_second_t rightVelocityRef) {
+ // This implements the linear time-varying differential drive controller in
+ // theorem 9.6.3 of https://tavsys.net/controls-in-frc.
+ Vectord<5> x{currentPose.X().value(), currentPose.Y().value(),
+ currentPose.Rotation().Radians().value(), leftVelocity.value(),
+ rightVelocity.value()};
+
+ Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity();
+ inRobotFrame(0, 0) = std::cos(x(State::kHeading));
+ inRobotFrame(0, 1) = std::sin(x(State::kHeading));
+ inRobotFrame(1, 0) = -std::sin(x(State::kHeading));
+ inRobotFrame(1, 1) = std::cos(x(State::kHeading));
+
+ Vectord<5> r{poseRef.X().value(), poseRef.Y().value(),
+ poseRef.Rotation().Radians().value(), leftVelocityRef.value(),
+ rightVelocityRef.value()};
+ m_error = r - x;
+ m_error(State::kHeading) =
+ frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value();
+
+ units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
+ const auto& K = m_table[velocity];
+
+ Vectord<2> u = K * inRobotFrame * m_error;
+
+ return DifferentialDriveWheelVoltages{units::volt_t{u(0)},
+ units::volt_t{u(1)}};
+}
+
+DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
+ const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity,
+ const Trajectory::State& desiredState) {
+ // v = (v_r + v_l) / 2 (1)
+ // w = (v_r - v_l) / (2r) (2)
+ // k = w / v (3)
+ //
+ // v_l = v - wr
+ // v_l = v - (vk)r
+ // v_l = v(1 - kr)
+ //
+ // v_r = v + wr
+ // v_r = v + (vk)r
+ // v_r = v(1 + kr)
+ return Calculate(
+ currentPose, leftVelocity, rightVelocity, desiredState.pose,
+ desiredState.velocity *
+ (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
+ desiredState.velocity *
+ (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
+}
diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
new file mode 100644
index 0000000..256b757
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -0,0 +1,133 @@
+// 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 "frc/controller/LTVUnicycleController.h"
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "units/math.h"
+
+using namespace frc;
+
+namespace {
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+ /// X position in global coordinate frame.
+ [[maybe_unused]] static constexpr int kX = 0;
+
+ /// Y position in global coordinate frame.
+ static constexpr int kY = 1;
+
+ /// Heading in global coordinate frame.
+ static constexpr int kHeading = 2;
+};
+
+} // namespace
+
+LTVUnicycleController::LTVUnicycleController(
+ units::second_t dt, units::meters_per_second_t maxVelocity)
+ : LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt, maxVelocity} {
+}
+
+LTVUnicycleController::LTVUnicycleController(
+ const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
+ units::second_t dt, units::meters_per_second_t maxVelocity) {
+ // The change in global pose for a unicycle is defined by the following three
+ // equations.
+ //
+ // ẋ = v cosθ
+ // ẏ = v sinθ
+ // θ̇ = ω
+ //
+ // Here's the model as a vector function where x = [x y θ]ᵀ and u = [v ω]ᵀ.
+ //
+ // [v cosθ]
+ // f(x, u) = [v sinθ]
+ // [ ω ]
+ //
+ // To create an LQR, we need to linearize this.
+ //
+ // [0 0 −v sinθ] [cosθ 0]
+ // ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0]
+ // [0 0 0 ] [ 0 1]
+ //
+ // We're going to make a cross-track error controller, so we'll apply a
+ // clockwise rotation matrix to the global tracking error to transform it into
+ // the robot's coordinate frame. Since the cross-track error is always
+ // measured from the robot's coordinate frame, the model used to compute the
+ // LQR should be linearized around θ = 0 at all times.
+ //
+ // [0 0 −v sin0] [cos0 0]
+ // A = [0 0 v cos0] B = [sin0 0]
+ // [0 0 0 ] [ 0 1]
+ //
+ // [0 0 0] [1 0]
+ // A = [0 0 v] B = [0 0]
+ // [0 0 0] [0 1]
+ Matrixd<3, 3> A = Matrixd<3, 3>::Zero();
+ Matrixd<3, 2> B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
+ Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
+ Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+
+ for (auto velocity = -maxVelocity; velocity < maxVelocity;
+ velocity += 0.01_mps) {
+ // The DARE is ill-conditioned if the velocity is close to zero, so don't
+ // let the system stop.
+ if (units::math::abs(velocity) < 1e-4_mps) {
+ m_table.insert(velocity, Matrixd<2, 3>::Zero());
+ } else {
+ A(State::kY, State::kHeading) = velocity.value();
+ m_table.insert(velocity,
+ frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
+ }
+ }
+}
+
+bool LTVUnicycleController::AtReference() const {
+ const auto& eTranslate = m_poseError.Translation();
+ const auto& eRotate = m_poseError.Rotation();
+ const auto& tolTranslate = m_poseTolerance.Translation();
+ const auto& tolRotate = m_poseTolerance.Rotation();
+ return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+ units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+ units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) {
+ m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds LTVUnicycleController::Calculate(
+ const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef) {
+ if (!m_enabled) {
+ return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+ }
+
+ m_poseError = poseRef.RelativeTo(currentPose);
+
+ const auto& K = m_table[linearVelocityRef];
+ Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(),
+ m_poseError.Rotation().Radians().value()};
+ Vectord<2> u = K * e;
+
+ return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
+ 0_mps,
+ angularVelocityRef + units::radians_per_second_t{u(1)}};
+}
+
+ChassisSpeeds LTVUnicycleController::Calculate(
+ const Pose2d& currentPose, const Trajectory::State& desiredState) {
+ return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+ desiredState.velocity * desiredState.curvature);
+}
+
+void LTVUnicycleController::SetEnabled(bool enabled) {
+ m_enabled = enabled;
+}
diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
index 4d2fbe9..2ab27d1 100644
--- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
+++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
@@ -6,61 +6,11 @@
namespace frc {
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
- const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
- MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
- const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
- units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
- const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
- const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
- const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
- MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
- const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
- units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
- const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
- const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
- const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
- MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
- const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
- units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
- const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
- const Eigen::Matrix<double, 2, 2>& N, units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<2, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<2, 2>;
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 34af2aa..6c89d25 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -24,7 +24,7 @@
period.value());
m_period = 20_ms;
wpi::math::MathSharedStore::ReportWarning(
- "{}", "Controller period defaulted to 20ms.");
+ "Controller period defaulted to 20ms.");
}
static int instances = 0;
instances++;
@@ -65,11 +65,29 @@
}
units::second_t PIDController::GetPeriod() const {
- return units::second_t(m_period);
+ return m_period;
+}
+
+double PIDController::GetPositionTolerance() const {
+ return m_positionTolerance;
+}
+
+double PIDController::GetVelocityTolerance() const {
+ return m_velocityTolerance;
}
void PIDController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
+
+ if (m_continuous) {
+ double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+ m_positionError =
+ frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ } else {
+ m_positionError = m_setpoint - m_measurement;
+ }
+
+ m_velocityError = (m_positionError - m_prevError) / m_period.value();
}
double PIDController::GetSetpoint() const {
@@ -77,19 +95,8 @@
}
bool PIDController::AtSetpoint() const {
- double positionError;
- if (m_continuous) {
- double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
- positionError =
- frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
- } else {
- positionError = m_setpoint - m_measurement;
- }
-
- double velocityError = (positionError - m_prevError) / m_period.value();
-
- return std::abs(positionError) < m_positionTolerance &&
- std::abs(velocityError) < m_velocityTolerance;
+ return std::abs(m_positionError) < m_positionTolerance &&
+ std::abs(m_velocityError) < m_velocityTolerance;
}
void PIDController::EnableContinuousInput(double minimumInput,
@@ -136,7 +143,7 @@
m_positionError =
frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
- m_positionError = m_setpoint - measurement;
+ m_positionError = m_setpoint - m_measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period.value();
@@ -151,14 +158,15 @@
}
double PIDController::Calculate(double measurement, double setpoint) {
- // Set setpoint to provided value
- SetSetpoint(setpoint);
+ m_setpoint = setpoint;
return Calculate(measurement);
}
void PIDController::Reset() {
+ m_positionError = 0;
m_prevError = 0;
m_totalError = 0;
+ m_velocityError = 0;
}
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
diff --git a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
index fa58427..be678cb 100644
--- a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -4,9 +4,7 @@
#include "frc/controller/ProfiledPIDController.h"
-void frc::detail::ReportProfiledPIDController() {
+int frc::detail::IncrementAndGetProfiledPIDControllerInstances() {
static int instances = 0;
- ++instances;
- wpi::math::MathSharedStore::ReportUsage(
- wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+ return ++instances;
}
diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
index 0fea864..576b0a9 100644
--- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
@@ -24,17 +24,13 @@
}
}
-RamseteController::RamseteController(double b, double zeta)
- : RamseteController(units::unit_t<b_unit>{b},
- units::unit_t<zeta_unit>{zeta}) {}
-
RamseteController::RamseteController(units::unit_t<b_unit> b,
units::unit_t<zeta_unit> zeta)
: m_b{b}, m_zeta{zeta} {}
RamseteController::RamseteController()
- : RamseteController(units::unit_t<b_unit>{2.0},
- units::unit_t<zeta_unit>{0.7}) {}
+ : RamseteController{units::unit_t<b_unit>{2.0},
+ units::unit_t<zeta_unit>{0.7}} {}
bool RamseteController::AtReference() const {
const auto& eTranslate = m_poseError.Translation();
@@ -67,10 +63,13 @@
const auto& vRef = linearVelocityRef;
const auto& omegaRef = angularVelocityRef;
+ // k = 2ζ√(ω_ref² + b v_ref²)
auto k = 2.0 * m_zeta *
units::math::sqrt(units::math::pow<2>(omegaRef) +
m_b * units::math::pow<2>(vRef));
+ // v_cmd = v_ref cos(e_θ) + k e_x
+ // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
}
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index c5ed7a1..39e0d8c 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -11,135 +11,154 @@
using namespace frc;
+DifferentialDrivePoseEstimator::InterpolationRecord
+DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
+ DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
+ double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the interpolated left distance.
+ auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
+ // Find the interpolated right distance.
+ auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist =
+ kinematics.ToTwist2d(left - leftDistance, right - rightDistance);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, left, right};
+ }
+}
+
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- const wpi::array<double, 5>& stateStdDevs,
- const wpi::array<double, 3>& localMeasurementStdDevs,
- const wpi::array<double, 3>& visionMeasurmentStdDevs,
- units::second_t nominalDt)
- : m_observer(
- &DifferentialDrivePoseEstimator::F,
- [](const Eigen::Vector<double, 5>& x,
- const Eigen::Vector<double, 3>& u) {
- return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
- },
- stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
- frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
- frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
- m_nominalDt(nominalDt) {
- SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
+ DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ units::meter_t leftDistance, units::meter_t rightDistance,
+ const Pose2d& initialPose)
+ : DifferentialDrivePoseEstimator{
+ kinematics, gyroAngle, leftDistance, rightDistance,
+ initialPose, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}} {}
- // Create correction mechanism for vision measurements.
- m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y) {
- m_observer.Correct<3>(
- u, y,
- [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
- return x.block<3, 1>(0, 0);
- },
- m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
- };
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+ DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ units::meter_t leftDistance, units::meter_t rightDistance,
+ const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
- m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurmentStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+ const wpi::array<double, 3>& visionMeasurementStdDevs) {
+ wpi::array<double, 3> r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
-void DifferentialDrivePoseEstimator::ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle) {
+void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance,
+ const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
- m_latencyCompensator.Reset();
-
- m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
-
- m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
+ m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
+ m_poseBuffer.Clear();
}
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
- return Pose2d(units::meter_t(m_observer.Xhat(0)),
- units::meter_t(m_observer.Xhat(1)),
- Rotation2d(units::radian_t(m_observer.Xhat(2))));
+ return m_odometry.GetPose();
}
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
- m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
- &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
- m_visionCorrect, timestamp);
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: We should not trust the twist entirely, so instead we scale this
+ // twist by a Kalman gain matrix representing how much we trust vision
+ // measurements compared to our current pose.
+ frc::Vectord<3> k_times_twist =
+ m_visionK *
+ frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+ // Step 4: Convert back to Twist2d.
+ Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+ units::meter_t{k_times_twist(1)},
+ units::radian_t{k_times_twist(2)}};
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.ResetPosition(
+ sample.value().gyroAngle, sample.value().leftDistance,
+ sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded
+ // sample to update the pose buffer and correct odometry.
+ auto internal_buf = m_poseBuffer.GetInternalBuffer();
+
+ auto first_newer_record =
+ std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
+ [](const auto& pair, auto t) { return t > pair.first; });
+
+ for (auto entry = first_newer_record + 1; entry != internal_buf.end();
+ entry++) {
+ UpdateWithTime(entry->first, entry->second.gyroAngle,
+ entry->second.leftDistance, entry->second.rightDistance);
+ }
}
-Pose2d DifferentialDrivePoseEstimator::Update(
- const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
- units::meter_t leftDistance, units::meter_t rightDistance) {
+Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- wheelSpeeds, leftDistance, rightDistance);
+ leftDistance, rightDistance);
}
Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance, units::meter_t rightDistance) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ m_odometry.Update(gyroAngle, leftDistance, rightDistance);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+ // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
+ // gyroAngle.Radians(),
+ // leftDistance,
+ // rightDistance,
+ // GetEstimatedPosition().X(),
+ // GetEstimatedPosition().Y(),
+ // GetEstimatedPosition().Rotation().Radians()
+ // );
- auto u = Eigen::Vector<double, 3>{
- (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
-
- m_previousAngle = angle;
-
- auto localY = Eigen::Vector<double, 3>{
- leftDistance.value(), rightDistance.value(), angle.Radians().value()};
-
- m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+ leftDistance, rightDistance});
return GetEstimatedPosition();
}
-
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
- const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
- // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
- // that for us.
- auto& theta = x(2);
- Eigen::Matrix<double, 5, 5> toFieldRotation{
- {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
- {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
- {0.0, 0.0, 1.0, 0.0, 0.0},
- {0.0, 0.0, 0.0, 1.0, 0.0},
- {0.0, 0.0, 0.0, 0.0, 1.0}};
- return toFieldRotation *
- Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
-}
-
-template <int Dim>
-wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
- const Eigen::Vector<double, Dim>& stdDevs) {
- wpi::array<double, Dim> array;
- for (size_t i = 0; i < Dim; ++i) {
- array[i] = stdDevs(i);
- }
- return array;
-}
-
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
- const Pose2d& pose, units::meter_t leftDistance,
- units::meter_t rightDistance) {
- return Eigen::Vector<double, 5>{pose.Translation().X().value(),
- pose.Translation().Y().value(),
- pose.Rotation().Radians().value(),
- leftDistance.value(), rightDistance.value()};
-}
diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
index 1209eae..a56efe4 100644
--- a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
+++ b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
@@ -6,16 +6,7 @@
namespace frc {
-KalmanFilter<1, 1, 1>::KalmanFilter(
- LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
- const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
- : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
- dt} {}
-
-KalmanFilter<2, 1, 1>::KalmanFilter(
- LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
- const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
- : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
- dt} {}
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<2, 1, 1>;
} // namespace frc
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 9d93647..9642e08 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -11,106 +11,159 @@
using namespace frc;
+frc::MecanumDrivePoseEstimator::InterpolationRecord
+frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
+ MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
+ double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distance measurements.
+ MecanumDriveWheelPositions wheels_lerp{
+ wpi::Lerp(this->wheelPositions.frontLeft,
+ endValue.wheelPositions.frontLeft, i),
+ wpi::Lerp(this->wheelPositions.frontRight,
+ endValue.wheelPositions.frontRight, i),
+ wpi::Lerp(this->wheelPositions.rearLeft,
+ endValue.wheelPositions.rearLeft, i),
+ wpi::Lerp(this->wheelPositions.rearRight,
+ endValue.wheelPositions.rearRight, i)};
+
+ // Find the distance between this measurement and the
+ // interpolated measurement.
+ MecanumDriveWheelPositions wheels_delta{
+ wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
+ wheels_lerp.frontRight - this->wheelPositions.frontRight,
+ wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
+ wheels_lerp.rearRight - this->wheelPositions.rearRight};
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist = kinematics.ToTwist2d(wheels_delta);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, wheels_lerp};
+ }
+}
+
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- MecanumDriveKinematics kinematics,
+ MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
+ : MecanumDrivePoseEstimator{kinematics, gyroAngle,
+ wheelPositions, initialPose,
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {}
+
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+ MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
const wpi::array<double, 3>& stateStdDevs,
- const wpi::array<double, 1>& localMeasurementStdDevs,
- const wpi::array<double, 3>& visionMeasurementStdDevs,
- units::second_t nominalDt)
- : m_observer(
- [](const Eigen::Vector<double, 3>& x,
- const Eigen::Vector<double, 3>& u) { return u; },
- [](const Eigen::Vector<double, 3>& x,
- const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
- stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
- frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
- frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
- m_kinematics(kinematics),
- m_nominalDt(nominalDt) {
+ const wpi::array<double, 3>& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
+
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- // Create vision correction mechanism.
- m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y) {
- m_observer.Correct<3>(
- u, y,
- [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
- return x;
- },
- m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
- };
-
- // Set initial state.
- m_observer.SetXhat(PoseTo3dVector(initialPose));
-
- // Calculate offsets.
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
}
void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array<double, 3>& visionMeasurmentStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+ const wpi::array<double, 3>& visionMeasurementStdDevs) {
+ wpi::array<double, 3> r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle) {
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
- m_latencyCompensator.Reset();
-
- m_observer.SetXhat(PoseTo3dVector(pose));
-
- m_gyroOffset = pose.Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
+ m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+ m_poseBuffer.Clear();
}
Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
- return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
- Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+ return m_odometry.GetPose();
}
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
- m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
- &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
- m_visionCorrect, timestamp);
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: We should not trust the twist entirely, so instead we scale this
+ // twist by a Kalman gain matrix representing how much we trust vision
+ // measurements compared to our current pose.
+ frc::Vectord<3> k_times_twist =
+ m_visionK *
+ frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+ // Step 4: Convert back to Twist2d
+ Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+ units::meter_t{k_times_twist(1)},
+ units::radian_t{k_times_twist(2)}};
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.ResetPosition(sample.value().gyroAngle,
+ sample.value().wheelPositions,
+ sample.value().pose.Exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded
+ // sample to update the pose buffer and correct odometry.
+ auto internal_buf = m_poseBuffer.GetInternalBuffer();
+
+ auto upper_bound =
+ std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
+ [](const auto& pair, auto t) { return t > pair.first; });
+
+ for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
+ UpdateWithTime(entry->first, entry->second.gyroAngle,
+ entry->second.wheelPositions);
+ }
}
Pose2d frc::MecanumDrivePoseEstimator::Update(
- const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- wheelSpeeds);
+ wheelPositions);
}
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ const MecanumDriveWheelPositions& wheelPositions) {
+ m_odometry.Update(gyroAngle, wheelPositions);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (angle - m_previousAngle).Radians() / dt;
+ MecanumDriveWheelPositions internalWheelPositions{
+ wheelPositions.frontLeft, wheelPositions.frontRight,
+ wheelPositions.rearLeft, wheelPositions.rearRight};
- auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
- auto fieldRelativeVelocities =
- Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
- .RotateBy(angle);
-
- Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
- fieldRelativeVelocities.Y().value(),
- omega.value()};
-
- Eigen::Vector<double, 1> localY{angle.Radians().value()};
- m_previousAngle = angle;
-
- m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(
+ currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
return GetEstimatedPosition();
}
diff --git a/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp
new file mode 100644
index 0000000..f2ed301
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp
@@ -0,0 +1,12 @@
+// 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 "frc/estimator/SwerveDrivePoseEstimator.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ SwerveDrivePoseEstimator<4>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp
new file mode 100644
index 0000000..d5e869b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp
@@ -0,0 +1,14 @@
+// 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 "frc/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ UnscentedKalmanFilter<3, 3, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ UnscentedKalmanFilter<5, 3, 3>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp
new file mode 100644
index 0000000..1d9c42a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp
@@ -0,0 +1,41 @@
+// 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 "frc/geometry/CoordinateAxis.h"
+
+using namespace frc;
+
+CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} {
+ m_axis /= m_axis.norm();
+}
+
+const CoordinateAxis& CoordinateAxis::N() {
+ static const CoordinateAxis instance{1.0, 0.0, 0.0};
+ return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::S() {
+ static const CoordinateAxis instance{-1.0, 0.0, 0.0};
+ return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::E() {
+ static const CoordinateAxis instance{0.0, -1.0, 0.0};
+ return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::W() {
+ static const CoordinateAxis instance{0.0, 1.0, 0.0};
+ return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::U() {
+ static const CoordinateAxis instance{0.0, 0.0, 1.0};
+ return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::D() {
+ static const CoordinateAxis instance{0.0, 0.0, -1.0};
+ return instance;
+}
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
new file mode 100644
index 0000000..d1ee93d
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
@@ -0,0 +1,73 @@
+// 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 "frc/geometry/CoordinateSystem.h"
+
+#include <cmath>
+#include <stdexcept>
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
+ const CoordinateAxis& positiveY,
+ const CoordinateAxis& positiveZ) {
+ // Construct a change of basis matrix from the source coordinate system to the
+ // NWU coordinate system. Each column vector in the change of basis matrix is
+ // one of the old basis vectors mapped to its representation in the new basis.
+ Matrixd<3, 3> R;
+ R.block<3, 1>(0, 0) = positiveX.m_axis;
+ R.block<3, 1>(0, 1) = positiveY.m_axis;
+ R.block<3, 1>(0, 2) = positiveZ.m_axis;
+
+ // The change of basis matrix should be a pure rotation. The Rotation3d
+ // constructor will verify this by checking for special orthogonality.
+ m_rotation = Rotation3d{R};
+}
+
+const CoordinateSystem& CoordinateSystem::NWU() {
+ static const CoordinateSystem instance{
+ CoordinateAxis::N(), CoordinateAxis::W(), CoordinateAxis::U()};
+ return instance;
+}
+
+const CoordinateSystem& CoordinateSystem::EDN() {
+ static const CoordinateSystem instance{
+ CoordinateAxis::E(), CoordinateAxis::D(), CoordinateAxis::N()};
+ return instance;
+}
+
+const CoordinateSystem& CoordinateSystem::NED() {
+ static const CoordinateSystem instance{
+ CoordinateAxis::N(), CoordinateAxis::E(), CoordinateAxis::D()};
+ return instance;
+}
+
+Translation3d CoordinateSystem::Convert(const Translation3d& translation,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to) {
+ return translation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
+Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to) {
+ return rotation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
+Pose3d CoordinateSystem::Convert(const Pose3d& pose,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to) {
+ return Pose3d{Convert(pose.Translation(), from, to),
+ Convert(pose.Rotation(), from, to)};
+}
+
+Transform3d CoordinateSystem::Convert(const Transform3d& transform,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to) {
+ return Transform3d{Convert(transform.Translation(), from, to),
+ Convert(transform.Rotation(), from, to)};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index b7176cd..2648a90 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -10,32 +10,9 @@
using namespace frc;
-Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
- : m_translation(translation), m_rotation(rotation) {}
-
-Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
- : m_translation(x, y), m_rotation(rotation) {}
-
-Pose2d Pose2d::operator+(const Transform2d& other) const {
- return TransformBy(other);
-}
-
Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
- return Transform2d(pose.Translation(), pose.Rotation());
-}
-
-bool Pose2d::operator==(const Pose2d& other) const {
- return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Pose2d::operator!=(const Pose2d& other) const {
- return !operator==(other);
-}
-
-Pose2d Pose2d::TransformBy(const Transform2d& other) const {
- return {m_translation + (other.Translation().RotateBy(m_rotation)),
- m_rotation + other.Rotation()};
+ return Transform2d{pose.Translation(), pose.Rotation()};
}
Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
@@ -87,7 +64,7 @@
{halfThetaByTanOfHalfDtheta, -halfDtheta}) *
std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
- return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+ return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
void frc::to_json(wpi::json& json, const Pose2d& pose) {
diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
new file mode 100644
index 0000000..4732c4d
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -0,0 +1,155 @@
+// 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 "frc/geometry/Pose3d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+namespace {
+
+/**
+ * Applies the hat operator to a rotation vector.
+ *
+ * It takes a rotation vector and returns the corresponding matrix
+ * representation of the Lie algebra element (a 3x3 rotation matrix).
+ *
+ * @param rotation The rotation vector.
+ * @return The rotation vector as a 3x3 rotation matrix.
+ */
+Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+ // Given a rotation vector <a, b, c>,
+ // [ 0 -c b]
+ // Omega = [ c 0 -a]
+ // [-b a 0]
+ return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
+ {rotation(2), 0.0, -rotation(0)},
+ {-rotation(1), rotation(0), 0.0}};
+}
+} // namespace
+
+Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
+ : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+ Rotation3d rotation)
+ : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(const Pose2d& pose)
+ : m_translation(pose.X(), pose.Y(), 0_m),
+ m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
+
+Pose3d Pose3d::operator+(const Transform3d& other) const {
+ return TransformBy(other);
+}
+
+Transform3d Pose3d::operator-(const Pose3d& other) const {
+ const auto pose = this->RelativeTo(other);
+ return Transform3d{pose.Translation(), pose.Rotation()};
+}
+
+Pose3d Pose3d::operator*(double scalar) const {
+ return Pose3d{m_translation * scalar, m_rotation * scalar};
+}
+
+Pose3d Pose3d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+ return {m_translation + (other.Translation().RotateBy(m_rotation)),
+ other.Rotation() + m_rotation};
+}
+
+Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+ const Transform3d transform{other, *this};
+ return {transform.Translation(), transform.Rotation()};
+}
+
+Pose3d Pose3d::Exp(const Twist3d& twist) const {
+ Matrixd<3, 3> Omega = RotationVectorToMatrix(
+ Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
+ Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+ double thetaSq =
+ (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
+
+ // Get left Jacobian of SO3. See first line in right column of
+ // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+ Matrixd<3, 3> J;
+ if (thetaSq < 1E-9 * 1E-9) {
+ // V = I + 0.5ω
+ J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+ } else {
+ double theta = std::sqrt(thetaSq);
+ // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
+ J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
+ (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+ }
+
+ // Get translation component
+ Vectord<3> translation =
+ J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+
+ const Transform3d transform{Translation3d{units::meter_t{translation(0)},
+ units::meter_t{translation(1)},
+ units::meter_t{translation(2)}},
+ Rotation3d{twist.rx, twist.ry, twist.rz}};
+
+ return *this + transform;
+}
+
+Twist3d Pose3d::Log(const Pose3d& end) const {
+ const auto transform = end.RelativeTo(*this);
+
+ Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+
+ Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
+ Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+ double thetaSq = rotVec.squaredNorm();
+
+ // Get left Jacobian inverse of SO3. See fourth line in right column of
+ // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+ Matrixd<3, 3> Jinv;
+ if (thetaSq < 1E-9 * 1E-9) {
+ // J⁻¹ = I − 0.5ω + 1/12 ω²
+ Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+ } else {
+ double theta = std::sqrt(thetaSq);
+ double halfTheta = 0.5 * theta;
+
+ // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
+ Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
+ (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
+ thetaSq * OmegaSq;
+ }
+
+ // Get dtranslation component
+ Vectord<3> dtranslation =
+ Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
+ transform.Z().value()};
+
+ return Twist3d{
+ units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
+ units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
+ units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
+}
+
+Pose2d Pose3d::ToPose2d() const {
+ return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+}
+
+void frc::to_json(wpi::json& json, const Pose3d& pose) {
+ json = wpi::json{{"translation", pose.Translation()},
+ {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose3d& pose) {
+ pose = Pose3d{json.at("translation").get<Translation3d>(),
+ json.at("rotation").get<Rotation3d>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
new file mode 100644
index 0000000..9c2ceda
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -0,0 +1,94 @@
+// 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 "frc/geometry/Quaternion.h"
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+Quaternion::Quaternion(double w, double x, double y, double z)
+ : m_r{w}, m_v{x, y, z} {}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+ // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+ const auto& r1 = m_r;
+ const auto& v1 = m_v;
+ const auto& r2 = other.m_r;
+ const auto& v2 = other.m_v;
+
+ // v₁ x v₂
+ Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
+ v2(0) * v1(2) - v1(0) * v2(2),
+ v1(0) * v2(1) - v2(0) * v1(1)};
+
+ // v = r₁v₂ + r₂v₁ + v₁ x v₂
+ Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
+
+ return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+}
+
+bool Quaternion::operator==(const Quaternion& other) const {
+ return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+}
+
+Quaternion Quaternion::Inverse() const {
+ return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+}
+
+Quaternion Quaternion::Normalize() const {
+ double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+ if (norm == 0.0) {
+ return Quaternion{};
+ } else {
+ return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+ }
+}
+
+double Quaternion::W() const {
+ return m_r;
+}
+
+double Quaternion::X() const {
+ return m_v(0);
+}
+
+double Quaternion::Y() const {
+ return m_v(1);
+}
+
+double Quaternion::Z() const {
+ return m_v(2);
+}
+
+Eigen::Vector3d Quaternion::ToRotationVector() const {
+ // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+ // Sound State Representation through Encapsulation of Manifolds"
+ //
+ // https://arxiv.org/pdf/1107.1119.pdf
+ double norm = m_v.norm();
+
+ if (norm < 1e-9) {
+ return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
+ } else {
+ if (W() < 0.0) {
+ return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
+ } else {
+ return 2.0 * std::atan2(norm, W()) / norm * m_v;
+ }
+ }
+}
+
+void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
+ json = wpi::json{{"W", quaternion.W()},
+ {"X", quaternion.X()},
+ {"Y", quaternion.Y()},
+ {"Z", quaternion.Z()}};
+}
+
+void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
+ quaternion =
+ Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
+ json.at("Y").get<double>(), json.at("Z").get<double>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 27af5ed..921e1f8 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -12,57 +12,6 @@
using namespace frc;
-Rotation2d::Rotation2d(units::radian_t value)
- : m_value(value),
- m_cos(units::math::cos(value)),
- m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(units::degree_t value)
- : m_value(value),
- m_cos(units::math::cos(value)),
- m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(double x, double y) {
- const auto magnitude = std::hypot(x, y);
- if (magnitude > 1e-6) {
- m_sin = y / magnitude;
- m_cos = x / magnitude;
- } else {
- m_sin = 0.0;
- m_cos = 1.0;
- }
- m_value = units::radian_t(std::atan2(m_sin, m_cos));
-}
-
-Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
- return RotateBy(other);
-}
-
-Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
- return *this + -other;
-}
-
-Rotation2d Rotation2d::operator-() const {
- return Rotation2d(-m_value);
-}
-
-Rotation2d Rotation2d::operator*(double scalar) const {
- return Rotation2d(m_value * scalar);
-}
-
-bool Rotation2d::operator==(const Rotation2d& other) const {
- return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
-}
-
-bool Rotation2d::operator!=(const Rotation2d& other) const {
- return !operator==(other);
-}
-
-Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
- return {Cos() * other.Cos() - Sin() * other.Sin(),
- Cos() * other.Sin() + Sin() * other.Cos()};
-}
-
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
json = wpi::json{{"radians", rotation.Radians().value()}};
}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
new file mode 100644
index 0000000..298f0e6
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -0,0 +1,242 @@
+// 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 "frc/geometry/Rotation3d.h"
+
+#include <cmath>
+#include <numbers>
+
+#include <wpi/json.h>
+
+#include "Eigen/Core"
+#include "Eigen/LU"
+#include "Eigen/QR"
+#include "frc/fmt/Eigen.h"
+#include "units/math.h"
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+Rotation3d::Rotation3d(const Quaternion& q) {
+ m_q = q.Normalize();
+}
+
+Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
+ units::radian_t yaw) {
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+ double cr = units::math::cos(roll * 0.5);
+ double sr = units::math::sin(roll * 0.5);
+
+ double cp = units::math::cos(pitch * 0.5);
+ double sp = units::math::sin(pitch * 0.5);
+
+ double cy = units::math::cos(yaw * 0.5);
+ double sy = units::math::sin(yaw * 0.5);
+
+ m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
+ cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+ double norm = axis.norm();
+ if (norm == 0.0) {
+ return;
+ }
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+ Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+ m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+}
+
+Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+ const auto& R = rotationMatrix;
+
+ // Require that the rotation matrix is special orthogonal. This is true if the
+ // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+ if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
+ std::string msg =
+ fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
+
+ wpi::math::MathSharedStore::ReportError(msg);
+ throw std::domain_error(msg);
+ }
+ if (std::abs(R.determinant() - 1.0) > 1e-9) {
+ std::string msg = fmt::format(
+ "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
+ R);
+
+ wpi::math::MathSharedStore::ReportError(msg);
+ throw std::domain_error(msg);
+ }
+
+ // Turn rotation matrix into a quaternion
+ // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+ double trace = R(0, 0) + R(1, 1) + R(2, 2);
+ double w;
+ double x;
+ double y;
+ double z;
+
+ if (trace > 0.0) {
+ double s = 0.5 / std::sqrt(trace + 1.0);
+ w = 0.25 / s;
+ x = (R(2, 1) - R(1, 2)) * s;
+ y = (R(0, 2) - R(2, 0)) * s;
+ z = (R(1, 0) - R(0, 1)) * s;
+ } else {
+ if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+ double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+ w = (R(2, 1) - R(1, 2)) / s;
+ x = 0.25 * s;
+ y = (R(0, 1) + R(1, 0)) / s;
+ z = (R(0, 2) + R(2, 0)) / s;
+ } else if (R(1, 1) > R(2, 2)) {
+ double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+ w = (R(0, 2) - R(2, 0)) / s;
+ x = (R(0, 1) + R(1, 0)) / s;
+ y = 0.25 * s;
+ z = (R(1, 2) + R(2, 1)) / s;
+ } else {
+ double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+ w = (R(1, 0) - R(0, 1)) / s;
+ x = (R(0, 2) + R(2, 0)) / s;
+ y = (R(1, 2) + R(2, 1)) / s;
+ z = 0.25 * s;
+ }
+ }
+
+ m_q = Quaternion{w, x, y, z};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+ double dot = initial.dot(final);
+ double normProduct = initial.norm() * final.norm();
+ double dotNorm = dot / normProduct;
+
+ if (dotNorm > 1.0 - 1E-9) {
+ // If the dot product is 1, the two vectors point in the same direction so
+ // there's no rotation. The default initialization of m_q will work.
+ return;
+ } else if (dotNorm < -1.0 + 1E-9) {
+ // If the dot product is -1, the two vectors point in opposite directions so
+ // a 180 degree rotation is required. Any orthogonal vector can be used for
+ // it. Q in the QR decomposition is an orthonormal basis, so it contains
+ // orthogonal unit vectors.
+ Eigen::Matrix<double, 3, 1> X = initial;
+ Eigen::HouseholderQR<decltype(X)> qr{X};
+ Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
+
+ // w = std::cos(θ/2) = std::cos(90°) = 0
+ //
+ // For x, y, and z, we use the second column of Q because the first is
+ // parallel instead of orthogonal. The third column would also work.
+ m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
+ } else {
+ // initial x final
+ Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
+ final(0) * initial(2) - initial(0) * final(2),
+ initial(0) * final(1) - final(0) * initial(1)};
+
+ // https://stackoverflow.com/a/11741520
+ m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
+ }
+}
+
+Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
+ return RotateBy(other);
+}
+
+Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
+ return *this + -other;
+}
+
+Rotation3d Rotation3d::operator-() const {
+ return Rotation3d{m_q.Inverse()};
+}
+
+Rotation3d Rotation3d::operator*(double scalar) const {
+ // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+ if (m_q.W() >= 0.0) {
+ return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
+ 2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
+ } else {
+ return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
+ 2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
+ }
+}
+
+Rotation3d Rotation3d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
+ return Rotation3d{other.m_q * m_q};
+}
+
+const Quaternion& Rotation3d::GetQuaternion() const {
+ return m_q;
+}
+
+units::radian_t Rotation3d::X() const {
+ double w = m_q.W();
+ double x = m_q.X();
+ double y = m_q.Y();
+ double z = m_q.Z();
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ return units::radian_t{
+ std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+}
+
+units::radian_t Rotation3d::Y() const {
+ double w = m_q.W();
+ double x = m_q.X();
+ double y = m_q.Y();
+ double z = m_q.Z();
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ double ratio = 2.0 * (w * y - z * x);
+ if (std::abs(ratio) >= 1.0) {
+ return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)};
+ } else {
+ return units::radian_t{std::asin(ratio)};
+ }
+}
+
+units::radian_t Rotation3d::Z() const {
+ double w = m_q.W();
+ double x = m_q.X();
+ double y = m_q.Y();
+ double z = m_q.Z();
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ return units::radian_t{
+ std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+}
+
+Vectord<3> Rotation3d::Axis() const {
+ double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+ if (norm == 0.0) {
+ return {0.0, 0.0, 0.0};
+ } else {
+ return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
+ }
+}
+
+units::radian_t Rotation3d::Angle() const {
+ double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+ return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
+}
+
+Rotation2d Rotation3d::ToRotation2d() const {
+ return Rotation2d{Z()};
+}
+
+void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
+ json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
+ rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 0808f35..25b0590 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -18,24 +18,6 @@
m_rotation = final.Rotation() - initial.Rotation();
}
-Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
- : m_translation(translation), m_rotation(rotation) {}
-
-Transform2d Transform2d::Inverse() const {
- // We are rotating the difference between the translations
- // using a clockwise rotation matrix. This transforms the global
- // delta into a local delta (relative to the initial pose).
- return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
-}
-
Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
-
-bool Transform2d::operator==(const Transform2d& other) const {
- return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Transform2d::operator!=(const Transform2d& other) const {
- return !operator==(other);
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
new file mode 100644
index 0000000..1cfabaa
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -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.
+
+#include "frc/geometry/Transform3d.h"
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+Transform3d::Transform3d(Pose3d initial, Pose3d final) {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ m_translation = (final.Translation() - initial.Translation())
+ .RotateBy(-initial.Rotation());
+
+ m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
+ : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Transform3d Transform3d::Inverse() const {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+Transform3d Transform3d::operator+(const Transform3d& other) const {
+ return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 5a30ec2..d463696 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -10,12 +10,6 @@
using namespace frc;
-Translation2d::Translation2d(units::meter_t x, units::meter_t y)
- : m_x(x), m_y(y) {}
-
-Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
- : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
-
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
@@ -24,40 +18,11 @@
return units::math::hypot(m_x, m_y);
}
-Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
- return {m_x * other.Cos() - m_y * other.Sin(),
- m_x * other.Sin() + m_y * other.Cos()};
-}
-
-Translation2d Translation2d::operator+(const Translation2d& other) const {
- return {X() + other.X(), Y() + other.Y()};
-}
-
-Translation2d Translation2d::operator-(const Translation2d& other) const {
- return *this + -other;
-}
-
-Translation2d Translation2d::operator-() const {
- return {-m_x, -m_y};
-}
-
-Translation2d Translation2d::operator*(double scalar) const {
- return {scalar * m_x, scalar * m_y};
-}
-
-Translation2d Translation2d::operator/(double scalar) const {
- return *this * (1.0 / scalar);
-}
-
bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
-bool Translation2d::operator!=(const Translation2d& other) const {
- return !operator==(other);
-}
-
void frc::to_json(wpi::json& json, const Translation2d& translation) {
json =
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
new file mode 100644
index 0000000..2c53791
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.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 "frc/geometry/Translation3d.h"
+
+#include <wpi/json.h>
+
+#include "units/length.h"
+#include "units/math.h"
+
+using namespace frc;
+
+Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
+ auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+ m_x = rectangular.X();
+ m_y = rectangular.Y();
+ m_z = rectangular.Z();
+}
+
+units::meter_t Translation3d::Distance(const Translation3d& other) const {
+ return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+ units::math::pow<2>(other.m_y - m_y) +
+ units::math::pow<2>(other.m_z - m_z));
+}
+
+units::meter_t Translation3d::Norm() const {
+ return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+}
+
+Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
+ Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+ auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+ return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+ units::meter_t{qprime.Z()}};
+}
+
+bool Translation3d::operator==(const Translation3d& other) const {
+ return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+ units::math::abs(m_y - other.m_y) < 1E-9_m &&
+ units::math::abs(m_z - other.m_z) < 1E-9_m;
+}
+
+void frc::to_json(wpi::json& json, const Translation3d& translation) {
+ json = wpi::json{{"x", translation.X().value()},
+ {"y", translation.Y().value()},
+ {"z", translation.Z().value()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation3d& translation) {
+ translation = Translation3d{units::meter_t{json.at("x").get<double>()},
+ units::meter_t{json.at("y").get<double>()},
+ units::meter_t{json.at("z").get<double>()}};
+}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index b036833..fd8a223 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -8,6 +8,7 @@
#include <wpi/jni_util.h>
+#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
@@ -70,7 +71,7 @@
return elements;
}
-frc::Trajectory CreateTrajectoryFromElements(wpi::span<const double> elements) {
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
// Make sure that the elements have the correct length.
assert(elements.size() % 7 == 0);
@@ -307,4 +308,60 @@
}
}
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+ (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+ jdouble sigma, jboolean lowerTriangular)
+{
+ jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
+ jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
+
+ Eigen::Map<
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+ L{matBody, rows, rows};
+ Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
+
+ if (lowerTriangular == JNI_TRUE) {
+ Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+ } else {
+ Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+ }
+
+ env->ReleaseDoubleArrayElements(mat, matBody, 0);
+ env->ReleaseDoubleArrayElements(vec, vecBody, 0);
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+ (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+ jint Brows, jint Bcols, jdoubleArray dst)
+{
+ jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
+ jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
+
+ Eigen::Map<
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+ Amat{nativeA, Arows, Acols};
+ Eigen::Map<
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+ Bmat{nativeB, Brows, Bcols};
+
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+ Amat.fullPivHouseholderQr().solve(Bmat);
+
+ env->ReleaseDoubleArrayElements(A, nativeA, 0);
+ env->ReleaseDoubleArrayElements(B, nativeB, 0);
+ env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
} // extern "C"
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index c4a4311..1ff7a8a 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -9,8 +9,11 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
- const Rotation2d& gyroAngle, const Pose2d& initialPose)
- : m_pose(initialPose) {
+ const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& initialPose)
+ : m_pose(initialPose),
+ m_prevLeftDistance(leftDistance),
+ m_prevRightDistance(rightDistance) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index c4a71fd..298dd7f 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -25,8 +25,7 @@
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
- Eigen::Vector<double, 4> wheelsVector =
- m_inverseKinematics * chassisSpeedsVector;
+ Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -38,7 +37,7 @@
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
- Eigen::Vector<double, 4> wheelSpeedsVector{
+ Vectord<4> wheelSpeedsVector{
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
@@ -50,13 +49,24 @@
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
+Twist2d MecanumDriveKinematics::ToTwist2d(
+ const MecanumDriveWheelPositions& wheelDeltas) const {
+ Vectord<4> wheelDeltasVector{
+ wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
+ wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
+
+ Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
+
+ return {units::meter_t{twistVector(0)}, // NOLINT
+ units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
+}
+
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
- m_inverseKinematics =
- Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
- {1, 1, (fr.X() - fr.Y()).value()},
- {1, 1, (rl.X() - rl.Y()).value()},
- {1, -1, (-(rr.X() + rr.Y())).value()}};
+ m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
+ {1, 1, (fr.X() - fr.Y()).value()},
+ {1, 1, (rl.X() - rl.Y()).value()},
+ {1, -1, (-(rr.X() + rr.Y())).value()}};
}
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index bbeee58..394adaf 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -8,32 +8,37 @@
using namespace frc;
-MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
- const Rotation2d& gyroAngle,
- const Pose2d& initialPose)
- : m_kinematics(kinematics), m_pose(initialPose) {
+MecanumDriveOdometry::MecanumDriveOdometry(
+ MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
+ : m_kinematics(kinematics),
+ m_pose(initialPose),
+ m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
-const Pose2d& MecanumDriveOdometry::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& gyroAngle,
- MecanumDriveWheelSpeeds wheelSpeeds) {
- units::second_t deltaTime =
- (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
- m_previousTime = currentTime;
-
+const Pose2d& MecanumDriveOdometry::Update(
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
- auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
- static_cast<void>(dtheta);
+ MecanumDriveWheelPositions wheelDeltas{
+ wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
+ wheelPositions.frontRight - m_previousWheelPositions.frontRight,
+ wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
+ wheelPositions.rearRight - m_previousWheelPositions.rearRight,
+ };
- auto newPose = m_pose.Exp(
- {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+ auto twist = m_kinematics.ToTwist2d(wheelDeltas);
+ twist.dtheta = (angle - m_previousAngle).Radians();
+
+ auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
+ m_previousWheelPositions = wheelPositions;
m_pose = {newPose.Translation(), angle};
return m_pose;
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveKinematics.cpp
new file mode 100644
index 0000000..5e6cf28
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveKinematics.cpp
@@ -0,0 +1,12 @@
+// 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 "frc/kinematics/SwerveDriveKinematics.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ SwerveDriveKinematics<4>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp
new file mode 100644
index 0000000..7a9d065
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp
@@ -0,0 +1,11 @@
+// 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 "frc/kinematics/SwerveDriveOdometry.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) SwerveDriveOdometry<4>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index cec620c..e8bbb46 100644
--- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -20,10 +20,10 @@
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
- Translation2d{units::meter_t(xInitial[0]),
- units::meter_t(yInitial[0])});
+ Translation2d{units::meter_t{xInitial[0]},
+ units::meter_t{yInitial[0]}});
waypoints.emplace_back(
- Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+ Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}});
// Populate tridiagonal system for clamped cubic
/* See:
diff --git a/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp b/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp
new file mode 100644
index 0000000..16979d9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp
@@ -0,0 +1,14 @@
+// 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 "frc/system/LinearSystemLoop.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ LinearSystemLoop<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ LinearSystemLoop<2, 1, 1>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
new file mode 100644
index 0000000..3527092
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
@@ -0,0 +1,193 @@
+// 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 "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+
+LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
+ units::kilogram_t mass,
+ units::meter_t radius,
+ double G) {
+ if (mass <= 0_kg) {
+ throw std::domain_error("mass must be greater than zero.");
+ }
+ if (radius <= 0_m) {
+ throw std::domain_error("radius must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ Matrixd<2, 2> A{
+ {0.0, 1.0},
+ {0.0, (-std::pow(G, 2) * motor.Kt /
+ (motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
+ .value()}};
+ Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()};
+ Matrixd<1, 2> C{1.0, 0.0};
+ Matrixd<1, 1> D{0.0};
+
+ return LinearSystem<2, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
+ DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ if (J <= 0_kg_sq_m) {
+ throw std::domain_error("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ Matrixd<2, 2> A{
+ {0.0, 1.0},
+ {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+ Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+ Matrixd<1, 2> C{1.0, 0.0};
+ Matrixd<1, 1> D{0.0};
+
+ return LinearSystem<2, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
+ if (kVLinear <= decltype(kVLinear){0}) {
+ throw std::domain_error("Kv,linear must be greater than zero.");
+ }
+ if (kALinear <= decltype(kALinear){0}) {
+ throw std::domain_error("Ka,linear must be greater than zero.");
+ }
+ if (kVAngular <= decltype(kVAngular){0}) {
+ throw std::domain_error("Kv,angular must be greater than zero.");
+ }
+ if (kAAngular <= decltype(kAAngular){0}) {
+ throw std::domain_error("Ka,angular must be greater than zero.");
+ }
+
+ double A1 = -(kVLinear.value() / kALinear.value() +
+ kVAngular.value() / kAAngular.value());
+ double A2 = -(kVLinear.value() / kALinear.value() -
+ kVAngular.value() / kAAngular.value());
+ double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
+ double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
+
+ Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
+ Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
+ Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+ Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
+
+ return LinearSystem<2, 2, 2>(A, B, C, D);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_rad_per_s) kVAngular,
+ decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) {
+ if (kVLinear <= decltype(kVLinear){0}) {
+ throw std::domain_error("Kv,linear must be greater than zero.");
+ }
+ if (kALinear <= decltype(kALinear){0}) {
+ throw std::domain_error("Ka,linear must be greater than zero.");
+ }
+ if (kVAngular <= decltype(kVAngular){0}) {
+ throw std::domain_error("Kv,angular must be greater than zero.");
+ }
+ if (kAAngular <= decltype(kAAngular){0}) {
+ throw std::domain_error("Ka,angular must be greater than zero.");
+ }
+ if (trackwidth <= 0_m) {
+ throw std::domain_error("r must be greater than zero.");
+ }
+
+ // We want to find a factor to include in Kv,angular that will convert
+ // `u = Kv,angular omega` to `u = Kv,angular v`.
+ //
+ // v = omega r
+ // omega = v/r
+ // omega = 1/r v
+ // omega = 1/(trackwidth/2) v
+ // omega = 2/trackwidth v
+ //
+ // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
+ // to V/(m/s).
+ return IdentifyDrivetrainSystem(kVLinear, kALinear,
+ kVAngular * 2.0 / trackwidth * 1_rad,
+ kAAngular * 2.0 / trackwidth * 1_rad);
+}
+
+LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
+ DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ if (J <= 0_kg_sq_m) {
+ throw std::domain_error("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ Matrixd<1, 1> A{
+ (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
+ Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
+ Matrixd<1, 1> C{1.0};
+ Matrixd<1, 1> D{0.0};
+
+ return LinearSystem<1, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
+ DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ if (J <= 0_kg_sq_m) {
+ throw std::domain_error("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ Matrixd<2, 2> A{
+ {0.0, 1.0},
+ {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+ Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+ Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+ Matrixd<2, 1> D{0.0, 0.0};
+
+ return LinearSystem<2, 1, 2>(A, B, C, D);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
+ const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
+ units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+ if (mass <= 0_kg) {
+ throw std::domain_error("mass must be greater than zero.");
+ }
+ if (r <= 0_m) {
+ throw std::domain_error("r must be greater than zero.");
+ }
+ if (rb <= 0_m) {
+ throw std::domain_error("rb must be greater than zero.");
+ }
+ if (J <= 0_kg_sq_m) {
+ throw std::domain_error("J must be greater than zero.");
+ }
+ if (G <= 0.0) {
+ throw std::domain_error("G must be greater than zero.");
+ }
+
+ auto C1 = -std::pow(G, 2) * motor.Kt /
+ (motor.Kv * motor.R * units::math::pow<2>(r));
+ auto C2 = G * motor.Kt / (motor.R * r);
+
+ Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
+ ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
+ {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(),
+ ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}};
+ Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(),
+ ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()},
+ {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(),
+ ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}};
+ Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+ Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
+
+ return LinearSystem<2, 2, 2>(A, B, C, D);
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index ecb36c2..de47547 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -13,16 +13,6 @@
using namespace frc;
-bool Trajectory::State::operator==(const Trajectory::State& other) const {
- return t == other.t && velocity == other.velocity &&
- acceleration == other.acceleration && pose == other.pose &&
- curvature == other.curvature;
-}
-
-bool Trajectory::State::operator!=(const Trajectory::State& other) const {
- return !operator==(other);
-}
-
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
@@ -46,7 +36,7 @@
const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
// Calculate the change in position.
- // delta_s = v_0 t + 0.5 at^2
+ // delta_s = v_0 t + 0.5at²
const units::meter_t newS =
(velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
(reversing ? -1.0 : 1.0);
@@ -163,11 +153,3 @@
units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
state.curvature = units::curvature_t{json.at("curvature").get<double>()};
}
-
-bool Trajectory::operator==(const Trajectory& other) const {
- return m_states == other.States();
-}
-
-bool Trajectory::operator!=(const Trajectory& other) const {
- return !operator==(other);
-}
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 1884758..c7a7e9a 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -30,7 +30,7 @@
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config) {
- const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+ const Transform2d flip{Translation2d{}, 180_deg};
// Make theta normal for trajectory generation if path is reversed.
// Flip the headings.
@@ -76,7 +76,7 @@
Trajectory TrajectoryGenerator::GenerateTrajectory(
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config) {
- const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+ const Transform2d flip{Translation2d{}, 180_deg};
// Make theta normal for trajectory generation if path is reversed.
if (config.IsReversed()) {
for (auto& vector : controlVectors) {
@@ -112,7 +112,7 @@
Trajectory TrajectoryGenerator::GenerateTrajectory(
const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
auto newWaypoints = waypoints;
- const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+ const Transform2d flip{Translation2d{}, 180_deg};
if (config.IsReversed()) {
for (auto& waypoint : newWaypoints) {
waypoint = waypoint + flip;
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index d397d0c..92d52ed 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -62,7 +62,7 @@
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
- // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+ // acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocity = units::math::min(
maxVelocity,
@@ -126,7 +126,7 @@
while (true) {
// Enforce max velocity limit (reverse)
- // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+ // v_f = √(v_i² + 2ad), where v_i = successor.
units::meters_per_second_t newMaxVelocity =
units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
successor.minAcceleration * ds * 2.0);
@@ -187,10 +187,10 @@
if (i > 0) {
states.at(i - 1).acceleration = reversed ? -accel : accel;
if (units::math::abs(accel) > 1E-6_mps_sq) {
- // v_f = v_0 + a * t
+ // v_f = v_0 + at
dt = (state.maxVelocity - v) / accel;
} else if (units::math::abs(v) > 1E-6_mps) {
- // delta_x = v * t
+ // delta_x = vt
dt = ds / v;
} else {
throw std::runtime_error(fmt::format(
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
index 738d243..40913a8 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -15,12 +15,12 @@
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
- // ac = v^2 / r
- // k (curvature) = 1 / r
+ // ac = v²/r
+ // k (curvature) = 1/r
- // therefore, ac = v^2 * k
- // ac / k = v^2
- // v = std::sqrt(ac / k)
+ // therefore, ac = v²k
+ // ac/k = v²
+ // v = √(ac/k)
// We have to multiply by 1_rad here to get the units to cancel out nicely.
// The units library defines a unit for radians although it is technically
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index 7c10201..d60b4b6 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -23,7 +23,7 @@
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
- return units::meters_per_second_t(std::numeric_limits<double>::max());
+ return units::meters_per_second_t{std::numeric_limits<double>::max()};
}
TrajectoryConstraint::MinMax
@@ -33,8 +33,8 @@
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
- auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
- auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+ auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right);
+ auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
deleted file mode 100644
index 7a4ff46..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
+++ /dev/null
@@ -1,46 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
-//
-// 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_AUTODIFF_MODULE
-#define EIGEN_AUTODIFF_MODULE
-
-namespace Eigen {
-
-/**
- * \defgroup AutoDiff_Module Auto Diff module
- *
- * This module features forward automatic differentation via a simple
- * templated scalar type wrapper AutoDiffScalar.
- *
- * Warning : this should NOT be confused with numerical differentiation, which
- * is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
- *
- * \code
- * #include <unsupported/Eigen/AutoDiff>
- * \endcode
- */
-//@{
-
-}
-#include "../../Eigen/src/Core/util/DisableStupidWarnings.h"
-
-
-#include "src/AutoDiff/AutoDiffScalar.h"
-// #include "src/AutoDiff/AutoDiffVector.h"
-#include "src/AutoDiff/AutoDiffJacobian.h"
-
-#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h"
-
-
-
-namespace Eigen {
-//@}
-}
-
-#endif // EIGEN_AUTODIFF_MODULE
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
deleted file mode 100644
index 33b6c39..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
+++ /dev/null
@@ -1,108 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// 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_AUTODIFF_JACOBIAN_H
-#define EIGEN_AUTODIFF_JACOBIAN_H
-
-namespace Eigen
-{
-
-template<typename Functor> class AutoDiffJacobian : public Functor
-{
-public:
- AutoDiffJacobian() : Functor() {}
- AutoDiffJacobian(const Functor& f) : Functor(f) {}
-
- // forward constructors
-#if EIGEN_HAS_VARIADIC_TEMPLATES
- template<typename... T>
- AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
-#else
- template<typename T0>
- AutoDiffJacobian(const T0& a0) : Functor(a0) {}
- template<typename T0, typename T1>
- AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
- template<typename T0, typename T1, typename T2>
- AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {}
-#endif
-
- typedef typename Functor::InputType InputType;
- typedef typename Functor::ValueType ValueType;
- typedef typename ValueType::Scalar Scalar;
-
- enum {
- InputsAtCompileTime = InputType::RowsAtCompileTime,
- ValuesAtCompileTime = ValueType::RowsAtCompileTime
- };
-
- typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
- typedef typename JacobianType::Index Index;
-
- typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
- typedef AutoDiffScalar<DerivativeType> ActiveScalar;
-
- typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
- typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue;
-
-#if EIGEN_HAS_VARIADIC_TEMPLATES
- // Some compilers don't accept variadic parameters after a default parameter,
- // i.e., we can't just write _jac=0 but we need to overload operator():
- EIGEN_STRONG_INLINE
- void operator() (const InputType& x, ValueType* v) const
- {
- this->operator()(x, v, 0);
- }
- template<typename... ParamsType>
- void operator() (const InputType& x, ValueType* v, JacobianType* _jac,
- const ParamsType&... Params) const
-#else
- void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const
-#endif
- {
- eigen_assert(v!=0);
-
- if (!_jac)
- {
-#if EIGEN_HAS_VARIADIC_TEMPLATES
- Functor::operator()(x, v, Params...);
-#else
- Functor::operator()(x, v);
-#endif
- return;
- }
-
- JacobianType& jac = *_jac;
-
- ActiveInput ax = x.template cast<ActiveScalar>();
- ActiveValue av(jac.rows());
-
- if(InputsAtCompileTime==Dynamic)
- for (Index j=0; j<jac.rows(); j++)
- av[j].derivatives().resize(x.rows());
-
- for (Index i=0; i<jac.cols(); i++)
- ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
-
-#if EIGEN_HAS_VARIADIC_TEMPLATES
- Functor::operator()(ax, &av, Params...);
-#else
- Functor::operator()(ax, &av);
-#endif
-
- for (Index i=0; i<jac.rows(); i++)
- {
- (*v)[i] = av[i].value();
- jac.row(i) = av[i].derivatives();
- }
- }
-};
-
-}
-
-#endif // EIGEN_AUTODIFF_JACOBIAN_H
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
deleted file mode 100644
index 0f166e3..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
+++ /dev/null
@@ -1,730 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// 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_AUTODIFF_SCALAR_H
-#define EIGEN_AUTODIFF_SCALAR_H
-
-namespace Eigen {
-
-namespace internal {
-
-template<typename A, typename B>
-struct make_coherent_impl {
- static void run(A&, B&) {}
-};
-
-// resize a to match b is a.size()==0, and conversely.
-template<typename A, typename B>
-void make_coherent(const A& a, const B&b)
-{
- make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
-}
-
-template<typename DerivativeType, bool Enable> struct auto_diff_special_op;
-
-} // end namespace internal
-
-template<typename DerivativeType> class AutoDiffScalar;
-
-template<typename NewDerType>
-inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
- return AutoDiffScalar<NewDerType>(value,der);
-}
-
-/** \class AutoDiffScalar
- * \brief A scalar type replacement with automatic differentiation capability
- *
- * \param DerivativeType the vector type used to store/represent the derivatives. The base scalar type
- * as well as the number of derivatives to compute are determined from this type.
- * Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
- * if the number of derivatives is not known at compile time, and/or, the number
- * of derivatives is large.
- * Note that DerivativeType can also be a reference (e.g., \c VectorXf&) to wrap a
- * existing vector into an AutoDiffScalar.
- * Finally, DerivativeType can also be any Eigen compatible expression.
- *
- * This class represents a scalar value while tracking its respective derivatives using Eigen's expression
- * template mechanism.
- *
- * It supports the following list of global math function:
- * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
- * - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
- * - internal::conj, internal::real, internal::imag, numext::abs2.
- *
- * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
- * in that case, the expression template mechanism only occurs at the top Matrix level,
- * while derivatives are computed right away.
- *
- */
-
-template<typename DerivativeType>
-class AutoDiffScalar
- : public internal::auto_diff_special_op
- <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
- typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value>
-{
- public:
- typedef internal::auto_diff_special_op
- <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
- typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value> Base;
- typedef typename internal::remove_all<DerivativeType>::type DerType;
- typedef typename internal::traits<DerType>::Scalar Scalar;
- typedef typename NumTraits<Scalar>::Real Real;
-
- using Base::operator+;
- using Base::operator*;
-
- /** Default constructor without any initialization. */
- AutoDiffScalar() {}
-
- /** Constructs an active scalar from its \a value,
- and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
- AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
- : m_value(value), m_derivatives(DerType::Zero(nbDer))
- {
- m_derivatives.coeffRef(derNumber) = Scalar(1);
- }
-
- /** Conversion from a scalar constant to an active scalar.
- * The derivatives are set to zero. */
- /*explicit*/ AutoDiffScalar(const Real& value)
- : m_value(value)
- {
- if(m_derivatives.size()>0)
- m_derivatives.setZero();
- }
-
- /** Constructs an active scalar from its \a value and derivatives \a der */
- AutoDiffScalar(const Scalar& value, const DerType& der)
- : m_value(value), m_derivatives(der)
- {}
-
- template<typename OtherDerType>
- AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
-#ifndef EIGEN_PARSED_BY_DOXYGEN
- , typename internal::enable_if<
- internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
- && internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
-#endif
- )
- : m_value(other.value()), m_derivatives(other.derivatives())
- {}
-
- friend std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
- {
- return s << a.value();
- }
-
- AutoDiffScalar(const AutoDiffScalar& other)
- : m_value(other.value()), m_derivatives(other.derivatives())
- {}
-
- template<typename OtherDerType>
- inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
- {
- m_value = other.value();
- m_derivatives = other.derivatives();
- return *this;
- }
-
- inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
- {
- m_value = other.value();
- m_derivatives = other.derivatives();
- return *this;
- }
-
- inline AutoDiffScalar& operator=(const Scalar& other)
- {
- m_value = other;
- if(m_derivatives.size()>0)
- m_derivatives.setZero();
- return *this;
- }
-
-// inline operator const Scalar& () const { return m_value; }
-// inline operator Scalar& () { return m_value; }
-
- inline const Scalar& value() const { return m_value; }
- inline Scalar& value() { return m_value; }
-
- inline const DerType& derivatives() const { return m_derivatives; }
- inline DerType& derivatives() { return m_derivatives; }
-
- inline bool operator< (const Scalar& other) const { return m_value < other; }
- inline bool operator<=(const Scalar& other) const { return m_value <= other; }
- inline bool operator> (const Scalar& other) const { return m_value > other; }
- inline bool operator>=(const Scalar& other) const { return m_value >= other; }
- inline bool operator==(const Scalar& other) const { return m_value == other; }
- inline bool operator!=(const Scalar& other) const { return m_value != other; }
-
- friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a < b.value(); }
- friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
- friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a > b.value(); }
- friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
- friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
- friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
-
- template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const { return m_value < b.value(); }
- template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const { return m_value <= b.value(); }
- template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const { return m_value > b.value(); }
- template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const { return m_value >= b.value(); }
- template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const { return m_value == b.value(); }
- template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const { return m_value != b.value(); }
-
- inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
- {
- return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
- }
-
- friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
- {
- return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
- }
-
-// inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
-// {
-// return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
-// }
-
-// friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
-// {
-// return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-// }
-
- inline AutoDiffScalar& operator+=(const Scalar& other)
- {
- value() += other;
- return *this;
- }
-
- template<typename OtherDerType>
- inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
- operator+(const AutoDiffScalar<OtherDerType>& other) const
- {
- internal::make_coherent(m_derivatives, other.derivatives());
- return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
- m_value + other.value(),
- m_derivatives + other.derivatives());
- }
-
- template<typename OtherDerType>
- inline AutoDiffScalar&
- operator+=(const AutoDiffScalar<OtherDerType>& other)
- {
- (*this) = (*this) + other;
- return *this;
- }
-
- inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
- {
- return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
- }
-
- friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
- operator-(const Scalar& a, const AutoDiffScalar& b)
- {
- return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
- (a - b.value(), -b.derivatives());
- }
-
- inline AutoDiffScalar& operator-=(const Scalar& other)
- {
- value() -= other;
- return *this;
- }
-
- template<typename OtherDerType>
- inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
- operator-(const AutoDiffScalar<OtherDerType>& other) const
- {
- internal::make_coherent(m_derivatives, other.derivatives());
- return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
- m_value - other.value(),
- m_derivatives - other.derivatives());
- }
-
- template<typename OtherDerType>
- inline AutoDiffScalar&
- operator-=(const AutoDiffScalar<OtherDerType>& other)
- {
- *this = *this - other;
- return *this;
- }
-
- inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
- operator-() const
- {
- return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
- -m_value,
- -m_derivatives);
- }
-
- inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
- operator*(const Scalar& other) const
- {
- return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
- }
-
- friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
- operator*(const Scalar& other, const AutoDiffScalar& a)
- {
- return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
- }
-
-// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-// operator*(const Real& other) const
-// {
-// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-// m_value * other,
-// (m_derivatives * other));
-// }
-//
-// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-// operator*(const Real& other, const AutoDiffScalar& a)
-// {
-// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-// a.value() * other,
-// a.derivatives() * other);
-// }
-
- inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
- operator/(const Scalar& other) const
- {
- return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
- }
-
- friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
- operator/(const Scalar& other, const AutoDiffScalar& a)
- {
- return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
- }
-
-// inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-// operator/(const Real& other) const
-// {
-// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-// m_value / other,
-// (m_derivatives * (Real(1)/other)));
-// }
-//
-// friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-// operator/(const Real& other, const AutoDiffScalar& a)
-// {
-// return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-// other / a.value(),
-// a.derivatives() * (-Real(1)/other));
-// }
-
- template<typename OtherDerType>
- inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
- CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
- const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
- const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
- operator/(const AutoDiffScalar<OtherDerType>& other) const
- {
- internal::make_coherent(m_derivatives, other.derivatives());
- return MakeAutoDiffScalar(
- m_value / other.value(),
- ((m_derivatives * other.value()) - (other.derivatives() * m_value))
- * (Scalar(1)/(other.value()*other.value())));
- }
-
- template<typename OtherDerType>
- inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
- const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
- const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
- operator*(const AutoDiffScalar<OtherDerType>& other) const
- {
- internal::make_coherent(m_derivatives, other.derivatives());
- return MakeAutoDiffScalar(
- m_value * other.value(),
- (m_derivatives * other.value()) + (other.derivatives() * m_value));
- }
-
- inline AutoDiffScalar& operator*=(const Scalar& other)
- {
- *this = *this * other;
- return *this;
- }
-
- template<typename OtherDerType>
- inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
- {
- *this = *this * other;
- return *this;
- }
-
- inline AutoDiffScalar& operator/=(const Scalar& other)
- {
- *this = *this / other;
- return *this;
- }
-
- template<typename OtherDerType>
- inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
- {
- *this = *this / other;
- return *this;
- }
-
- protected:
- Scalar m_value;
- DerType m_derivatives;
-
-};
-
-namespace internal {
-
-template<typename DerivativeType>
-struct auto_diff_special_op<DerivativeType, true>
-// : auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
-// is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
-{
- typedef typename remove_all<DerivativeType>::type DerType;
- typedef typename traits<DerType>::Scalar Scalar;
- typedef typename NumTraits<Scalar>::Real Real;
-
-// typedef auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
-// is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
-
-// using Base::operator+;
-// using Base::operator+=;
-// using Base::operator-;
-// using Base::operator-=;
-// using Base::operator*;
-// using Base::operator*=;
-
- const AutoDiffScalar<DerivativeType>& derived() const { return *static_cast<const AutoDiffScalar<DerivativeType>*>(this); }
- AutoDiffScalar<DerivativeType>& derived() { return *static_cast<AutoDiffScalar<DerivativeType>*>(this); }
-
-
- inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
- {
- return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
- }
-
- friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<DerivativeType>& b)
- {
- return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
- }
-
- inline AutoDiffScalar<DerivativeType>& operator+=(const Real& other)
- {
- derived().value() += other;
- return derived();
- }
-
-
- inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
- operator*(const Real& other) const
- {
- return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
- derived().value() * other,
- derived().derivatives() * other);
- }
-
- friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
- operator*(const Real& other, const AutoDiffScalar<DerivativeType>& a)
- {
- return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
- a.value() * other,
- a.derivatives() * other);
- }
-
- inline AutoDiffScalar<DerivativeType>& operator*=(const Scalar& other)
- {
- *this = *this * other;
- return derived();
- }
-};
-
-template<typename DerivativeType>
-struct auto_diff_special_op<DerivativeType, false>
-{
- void operator*() const;
- void operator-() const;
- void operator+() const;
-};
-
-template<typename BinOp, typename A, typename B, typename RefType>
-void make_coherent_expression(CwiseBinaryOp<BinOp,A,B> xpr, const RefType &ref)
-{
- make_coherent(xpr.const_cast_derived().lhs(), ref);
- make_coherent(xpr.const_cast_derived().rhs(), ref);
-}
-
-template<typename UnaryOp, typename A, typename RefType>
-void make_coherent_expression(const CwiseUnaryOp<UnaryOp,A> &xpr, const RefType &ref)
-{
- make_coherent(xpr.nestedExpression().const_cast_derived(), ref);
-}
-
-// needed for compilation only
-template<typename UnaryOp, typename A, typename RefType>
-void make_coherent_expression(const CwiseNullaryOp<UnaryOp,A> &, const RefType &)
-{}
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
- typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
- static void run(A& a, B& b) {
- if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
- {
- a.resize(b.size());
- a.setZero();
- }
- else if (B::SizeAtCompileTime==Dynamic && a.size()!=0 && b.size()==0)
- {
- make_coherent_expression(b,a);
- }
- }
-};
-
-template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
- typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
- static void run(A& a, B& b) {
- if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
- {
- b.resize(a.size());
- b.setZero();
- }
- else if (A::SizeAtCompileTime==Dynamic && b.size()!=0 && a.size()==0)
- {
- make_coherent_expression(a,b);
- }
- }
-};
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
- typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
- Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
- typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
- typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
- static void run(A& a, B& b) {
- if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
- {
- a.resize(b.size());
- a.setZero();
- }
- else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
- {
- b.resize(a.size());
- b.setZero();
- }
- }
-};
-
-} // end namespace internal
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
-{
- typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
-{
- typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-
-// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
-
-// template<typename DerType, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
-// {
-// enum { Defined = 1 };
-// typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
-// };
-//
-// template<typename DerType1,typename DerType2, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
-// {
-// enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
-// typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
-// };
-
-#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
- template<typename DerType> \
- inline const Eigen::AutoDiffScalar< \
- EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
- FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
- using namespace Eigen; \
- typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
- EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
- CODE; \
- }
-
-template<typename DerType>
-struct CleanedUpDerType {
- typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> type;
-};
-
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x) { return x; }
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x) { return x; }
-template<typename DerType>
-inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&) { return 0.; }
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const T& y) {
- typedef typename CleanedUpDerType<DerType>::type ADS;
- return (x <= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const T& y) {
- typedef typename CleanedUpDerType<DerType>::type ADS;
- return (x >= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (min)(const T& x, const AutoDiffScalar<DerType>& y) {
- typedef typename CleanedUpDerType<DerType>::type ADS;
- return (x < y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (max)(const T& x, const AutoDiffScalar<DerType>& y) {
- typedef typename CleanedUpDerType<DerType>::type ADS;
- return (x > y ? ADS(x) : ADS(y));
-}
-template<typename DerType>
-inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
- return (x.value() < y.value() ? x : y);
-}
-template<typename DerType>
-inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
- return (x.value() >= y.value() ? x : y);
-}
-
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
- using std::abs;
- return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
- using numext::abs2;
- return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
- using std::sqrt;
- Scalar sqrtx = sqrt(x.value());
- return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
- using std::cos;
- using std::sin;
- return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
- using std::sin;
- using std::cos;
- return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
- using std::exp;
- Scalar expx = exp(x.value());
- return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
- using std::log;
- return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
-
-template<typename DerType>
-inline const Eigen::AutoDiffScalar<
-EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
-pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
-{
- using namespace Eigen;
- using std::pow;
- return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
-}
-
-
-template<typename DerTypeA,typename DerTypeB>
-inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
-atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
-{
- using std::atan2;
- typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
- typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
- PlainADS ret;
- ret.value() = atan2(a.value(), b.value());
-
- Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
-
- // if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
- ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
-
- return ret;
-}
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
- using std::tan;
- using std::cos;
- return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
- using std::sqrt;
- using std::asin;
- return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
- using std::sqrt;
- using std::acos;
- return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
- using std::cosh;
- using std::tanh;
- return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
- using std::sinh;
- using std::cosh;
- return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
- using std::sinh;
- using std::cosh;
- return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
-
-#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
-
-template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
- : NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
-{
- typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
- typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
- 0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
- typedef AutoDiffScalar<DerType> NonInteger;
- typedef AutoDiffScalar<DerType> Nested;
- typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
- enum{
- RequireInitialization = 1
- };
-};
-
-}
-
-namespace std {
-
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T> >
- : public numeric_limits<typename T::Scalar> {};
-
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T&> >
- : public numeric_limits<typename T::Scalar> {};
-
-} // namespace std
-
-#endif // EIGEN_AUTODIFF_SCALAR_H
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
deleted file mode 100644
index 8c2d048..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
+++ /dev/null
@@ -1,220 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// 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_AUTODIFF_VECTOR_H
-#define EIGEN_AUTODIFF_VECTOR_H
-
-namespace Eigen {
-
-/* \class AutoDiffScalar
- * \brief A scalar type replacement with automatic differentation capability
- *
- * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
- *
- * This class represents a scalar value while tracking its respective derivatives.
- *
- * It supports the following list of global math function:
- * - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
- * - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
- * - internal::conj, internal::real, internal::imag, numext::abs2.
- *
- * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
- * in that case, the expression template mechanism only occurs at the top Matrix level,
- * while derivatives are computed right away.
- *
- */
-template<typename ValueType, typename JacobianType>
-class AutoDiffVector
-{
- public:
- //typedef typename internal::traits<ValueType>::Scalar Scalar;
- typedef typename internal::traits<ValueType>::Scalar BaseScalar;
- typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
- typedef ActiveScalar Scalar;
- typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
- typedef typename JacobianType::Index Index;
-
- inline AutoDiffVector() {}
-
- inline AutoDiffVector(const ValueType& values)
- : m_values(values)
- {
- m_jacobian.setZero();
- }
-
-
- CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
- const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
- CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
- const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
- CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
- const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
- Index size() const { return m_values.size(); }
-
- // FIXME here we could return an expression of the sum
- Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
-
-
- inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
- : m_values(values), m_jacobian(jac)
- {}
-
- template<typename OtherValueType, typename OtherJacobianType>
- inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
- : m_values(other.values()), m_jacobian(other.jacobian())
- {}
-
- inline AutoDiffVector(const AutoDiffVector& other)
- : m_values(other.values()), m_jacobian(other.jacobian())
- {}
-
- template<typename OtherValueType, typename OtherJacobianType>
- inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
- {
- m_values = other.values();
- m_jacobian = other.jacobian();
- return *this;
- }
-
- inline AutoDiffVector& operator=(const AutoDiffVector& other)
- {
- m_values = other.values();
- m_jacobian = other.jacobian();
- return *this;
- }
-
- inline const ValueType& values() const { return m_values; }
- inline ValueType& values() { return m_values; }
-
- inline const JacobianType& jacobian() const { return m_jacobian; }
- inline JacobianType& jacobian() { return m_jacobian; }
-
- template<typename OtherValueType,typename OtherJacobianType>
- inline const AutoDiffVector<
- typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
- typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
- operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
- {
- return AutoDiffVector<
- typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
- typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
- m_values + other.values(),
- m_jacobian + other.jacobian());
- }
-
- template<typename OtherValueType, typename OtherJacobianType>
- inline AutoDiffVector&
- operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
- {
- m_values += other.values();
- m_jacobian += other.jacobian();
- return *this;
- }
-
- template<typename OtherValueType,typename OtherJacobianType>
- inline const AutoDiffVector<
- typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
- typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
- operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
- {
- return AutoDiffVector<
- typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
- typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
- m_values - other.values(),
- m_jacobian - other.jacobian());
- }
-
- template<typename OtherValueType, typename OtherJacobianType>
- inline AutoDiffVector&
- operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
- {
- m_values -= other.values();
- m_jacobian -= other.jacobian();
- return *this;
- }
-
- inline const AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
- operator-() const
- {
- return AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
- -m_values,
- -m_jacobian);
- }
-
- inline const AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
- operator*(const BaseScalar& other) const
- {
- return AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
- m_values * other,
- m_jacobian * other);
- }
-
- friend inline const AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
- operator*(const Scalar& other, const AutoDiffVector& v)
- {
- return AutoDiffVector<
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
- typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
- v.values() * other,
- v.jacobian() * other);
- }
-
-// template<typename OtherValueType,typename OtherJacobianType>
-// inline const AutoDiffVector<
-// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
-// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
-// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
-// operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
-// {
-// return AutoDiffVector<
-// CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
-// CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
-// CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
-// m_values.cwise() * other.values(),
-// (m_jacobian * other.values()) + (m_values * other.jacobian()));
-// }
-
- inline AutoDiffVector& operator*=(const Scalar& other)
- {
- m_values *= other;
- m_jacobian *= other;
- return *this;
- }
-
- template<typename OtherValueType,typename OtherJacobianType>
- inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
- {
- *this = *this * other;
- return *this;
- }
-
- protected:
- ValueType m_values;
- JacobianType m_jacobian;
-
-};
-
-}
-
-#endif // EIGEN_AUTODIFF_VECTOR_H
diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
new file mode 100644
index 0000000..54f9752
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
@@ -0,0 +1,37 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+/**
+ * Returns the robot's pose in the field coordinate system given an object's
+ * field-relative pose, the transformation from the camera's pose to the
+ * object's pose (obtained via computer vision), and the transformation from the
+ * robot's pose to the camera's pose.
+ *
+ * The object could be a target or a fiducial marker.
+ *
+ * @param objectInField An object's field-relative pose.
+ * @param cameraToObject The transformation from the camera's pose to the
+ * object's pose. This comes from computer vision.
+ * @param robotToCamera The transformation from the robot's pose to the camera's
+ * pose. This can either be a constant for a rigidly mounted camera, or
+ * variable if the camera is mounted to a turret. If the camera was mounted 3
+ * inches in front of the "origin" (usually physical center) of the robot,
+ * this would be frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}.
+ * @return The robot's field-relative pose.
+ */
+WPILIB_DLLEXPORT
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+ const frc::Transform3d& cameraToObject,
+ const frc::Transform3d& robotToCamera);
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h
new file mode 100644
index 0000000..b33e9e2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/EigenCore.h
@@ -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.
+
+#pragma once
+
+#include "Eigen/Core"
+
+namespace frc {
+
+template <int Size>
+using Vectord = Eigen::Vector<double, Size>;
+
+template <int Rows, int Cols,
+ int Options = Eigen::AutoAlign |
+ ((Rows == 1 && Cols != 1) ? Eigen::RowMajor
+ : (Cols == 1 && Rows != 1)
+ ? Eigen::ColMajor
+ : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
+ int MaxRows = Rows, int MaxCols = Cols>
+using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 54a77af..24bf857 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -4,22 +4,84 @@
#pragma once
+#include <numbers>
+
#include <wpi/SymbolExports.h>
-#include <wpi/numbers>
#include "units/angle.h"
+#include "units/base.h"
+#include "units/math.h"
namespace frc {
/**
- * Returns 0.0 if the given value is within the specified range around zero.
- * The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ * Returns 0.0 if the given value is within the specified range around zero. The
+ * remaining range between the deadband and the maximum magnitude is scaled from
+ * 0.0 to the maximum magnitude.
*
- * @param value Value to clip.
+ * @param value Value to clip.
* @param deadband Range around zero.
+ * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can
+ * be infinite.
+ * @return The value after the deadband is applied.
*/
-WPILIB_DLLEXPORT
-double ApplyDeadband(double value, double deadband);
+template <typename T,
+ typename = std::enable_if_t<std::disjunction_v<
+ std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
+ T magnitude;
+ if constexpr (std::is_floating_point_v<T>) {
+ magnitude = std::abs(value);
+ } else {
+ magnitude = units::math::abs(value);
+ }
+
+ if (magnitude > deadband) {
+ if (maxMagnitude / deadband > 1.0E12) {
+ // If max magnitude is sufficiently large, the implementation encounters
+ // roundoff error. Implementing the limiting behavior directly avoids
+ // the problem.
+ return value > T{0.0} ? value - deadband : value + deadband;
+ }
+ if (value > T{0.0}) {
+ // Map deadband to 0 and map max to max.
+ //
+ // y - y₁ = m(x - x₁)
+ // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+ // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+ //
+ // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+ // x₁ = deadband
+ // y₁ = 0
+ // x₂ = max
+ // y₂ = max
+ //
+ // y = (max - 0)/(max - deadband) (x - deadband) + 0
+ // y = max/(max - deadband) (x - deadband)
+ // y = max (x - deadband)/(max - deadband)
+ return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
+ } else {
+ // Map -deadband to 0 and map -max to -max.
+ //
+ // y - y₁ = m(x - x₁)
+ // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+ // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+ //
+ // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+ // x₁ = -deadband
+ // y₁ = 0
+ // x₂ = -max
+ // y₂ = -max
+ //
+ // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+ // y = max/(max - deadband) (x + deadband)
+ // y = max (x + deadband)/(max - deadband)
+ return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
+ }
+ } else {
+ return T{0.0};
+ }
+}
/**
* Returns modulus of input.
@@ -51,8 +113,8 @@
WPILIB_DLLEXPORT
constexpr units::radian_t AngleModulus(units::radian_t angle) {
return InputModulus<units::radian_t>(angle,
- units::radian_t{-wpi::numbers::pi},
- units::radian_t{wpi::numbers::pi});
+ units::radian_t{-std::numbers::pi},
+ units::radian_t{std::numbers::pi});
}
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 5fdd18c..0345b46 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -6,15 +6,16 @@
#include <array>
#include <cmath>
+#include <limits>
#include <random>
#include <type_traits>
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
-#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
@@ -32,7 +33,11 @@
template <typename Matrix, typename T, typename... Ts>
void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
- result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
+ if (elem == std::numeric_limits<double>::infinity()) {
+ result(result.rows() - (sizeof...(Ts) + 1)) = 0.0;
+ } else {
+ result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
+ }
if constexpr (sizeof...(Ts) > 0) {
CostMatrixImpl(result, elems...);
}
@@ -59,9 +64,9 @@
}
template <int States, int Inputs>
-bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B) {
- Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
+bool IsStabilizableImpl(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B) {
+ Eigen::EigenSolver<Matrixd<States, States>> es{A};
for (int i = 0; i < States; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -89,47 +94,23 @@
} // namespace detail
/**
- * Creates a matrix from the given list of elements.
- *
- * The elements of the matrix are filled in in row-major order.
- *
- * @param elems An array of elements in the matrix.
- * @return A matrix containing the given elements.
- * @deprecated Use Eigen::Matrix or Eigen::Vector initializer list constructor.
- */
-template <int Rows, int Cols, typename... Ts,
- typename =
- std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
-WPI_DEPRECATED(
- "Use Eigen::Matrix or Eigen::Vector initializer list constructor")
-Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
- static_assert(
- sizeof...(elems) == Rows * Cols,
- "Number of provided elements doesn't match matrix dimensionality");
-
- Eigen::Matrix<double, Rows, Cols> result;
- detail::MatrixImpl<Rows, Cols>(result, elems...);
- 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 taken and placed on the cost matrix diagonal.
+ * each tolerance is placed on the cost matrix diagonal. If a tolerance is
+ * infinity, its cost matrix entry is set to zero.
*
- * @param costs An array. 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.
+ * @param tolerances An array. 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.
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
-Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
- Ts... costs) {
+Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
- detail::CostMatrixImpl(result.diagonal(), costs...);
+ detail::CostMatrixImpl(result.diagonal(), tolerances...);
return result;
}
@@ -147,8 +128,7 @@
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
-Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
- Ts... stdDevs) {
+Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
return result;
@@ -167,7 +147,7 @@
* @return State excursion or control effort cost matrix.
*/
template <size_t N>
-Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
+Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -189,8 +169,7 @@
* @return Process noise or measurement noise covariance matrix.
*/
template <size_t N>
-Eigen::Matrix<double, N, N> MakeCovMatrix(
- const std::array<double, N>& stdDevs) {
+Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -201,8 +180,8 @@
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
-Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
- Eigen::Matrix<double, sizeof...(Ts), 1> result;
+Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
+ Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
return result;
}
@@ -216,12 +195,11 @@
* @return White noise vector.
*/
template <int N>
-Eigen::Vector<double, N> MakeWhiteNoiseVector(
- const std::array<double, N>& stdDevs) {
+Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
- Eigen::Vector<double, N> result;
+ Vectord<N> result;
for (int i = 0; i < N; ++i) {
// Passing a standard deviation of 0.0 to std::normal_distribution is
// undefined behavior
@@ -243,7 +221,7 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
+Vectord<3> PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -253,7 +231,7 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
+Vectord<4> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -268,8 +246,8 @@
* @param B Input matrix.
*/
template <int States, int Inputs>
-bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B) {
+bool IsStabilizable(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B) {
return detail::IsStabilizableImpl<States, Inputs>(A, B);
}
@@ -286,8 +264,8 @@
* @param C Output matrix.
*/
template <int States, int Outputs>
-bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, Outputs, States>& C) {
+bool IsDetectable(const Matrixd<States, States>& A,
+ const Matrixd<Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
}
@@ -295,14 +273,14 @@
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
-WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
- const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
+WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
+ const Matrixd<1, 1>& B);
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
-WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
- const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
+WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
+ const Matrixd<2, 1>& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
@@ -312,7 +290,7 @@
* @return The vector.
*/
WPILIB_DLLEXPORT
-Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
+Vectord<3> PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.
@@ -324,11 +302,10 @@
* @return Clamped input vector.
*/
template <int Inputs>
-Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
- const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Inputs>& umin,
- const Eigen::Vector<double, Inputs>& umax) {
- Eigen::Vector<double, Inputs> result;
+Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
+ const Vectord<Inputs>& umin,
+ const Vectord<Inputs>& umax) {
+ Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result(i) = std::clamp(u(i), umin(i), umax(i));
}
@@ -345,8 +322,8 @@
* @return The normalizedInput
*/
template <int Inputs>
-Eigen::Vector<double, Inputs> DesaturateInputVector(
- const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
+Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
+ double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
if (maxValue > maxMagnitude) {
diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
index 1f0e407..5621803 100644
--- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -37,7 +37,7 @@
* @param kS The static gain, in volts.
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per radian.
- * @param kA The acceleration gain, in volt seconds^2 per radian.
+ * @param kA The acceleration gain, in volt seconds² per radian.
*/
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -47,9 +47,13 @@
/**
* Calculates the feedforward from the gains and setpoints.
*
- * @param angle The angle setpoint, in radians.
+ * @param angle The angle setpoint, in radians. This angle should be
+ * measured from the horizontal (i.e. if the provided
+ * angle is 0, the arm should be parallel to the floor).
+ * If your encoder does not follow this convention, an
+ * offset should be added.
* @param velocity The velocity setpoint, in radians per second.
- * @param acceleration The acceleration setpoint, in radians per second^2.
+ * @param acceleration The acceleration setpoint, in radians per second².
* @return The computed feedforward, in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> angle,
@@ -70,8 +74,12 @@
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
- * @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle of the arm
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm. This angle should be measured
+ * from the horizontal (i.e. if the provided angle is 0,
+ * the arm should be parallel to the floor). If your
+ * encoder does not follow this convention, an offset
+ * should be added.
* @param acceleration The acceleration of the arm.
* @return The maximum possible velocity at the given acceleration and angle.
*/
@@ -91,8 +99,12 @@
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
- * @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle of the arm
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm. This angle should be measured
+ * from the horizontal (i.e. if the provided angle is 0,
+ * the arm should be parallel to the floor). If your
+ * encoder does not follow this convention, an offset
+ * should be added.
* @param acceleration The acceleration of the arm.
* @return The minimum possible velocity at the given acceleration and angle.
*/
@@ -113,8 +125,12 @@
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle of the arm
- * @param velocity The velocity of the arm.
+ * @param angle The angle of the arm. This angle should be measured
+ * from the horizontal (i.e. if the provided angle is 0,
+ * the arm should be parallel to the floor). If your
+ * encoder does not follow this convention, an offset
+ * should be added.
+ * @param velocity The velocity of the arm.
* @return The maximum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MaxAchievableAcceleration(
@@ -133,8 +149,12 @@
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle of the arm
- * @param velocity The velocity of the arm.
+ * @param angle The angle of the arm. This angle should be measured
+ * from the horizontal (i.e. if the provided angle is 0,
+ * the arm should be parallel to the floor). If your
+ * encoder does not follow this convention, an offset
+ * should be added.
+ * @param velocity The velocity of the arm.
* @return The minimum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MinAchievableAcceleration(
diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 656f767..21aad25 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -7,8 +7,8 @@
#include <array>
#include <functional>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
@@ -39,6 +39,9 @@
template <int States, int Inputs>
class ControlAffinePlantInversionFeedforward {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+
/**
* Constructs a feedforward with given model dynamics as a function
* of state and input.
@@ -50,15 +53,11 @@
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
- std::function<
- Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- f,
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
units::second_t dt)
: m_dt(dt), m_f(f) {
- m_B = NumericalJacobianU<States, States, Inputs>(
- f, Eigen::Vector<double, States>::Zero(),
- Eigen::Vector<double, Inputs>::Zero());
+ m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
+ InputVector::Zero());
Reset();
}
@@ -73,14 +72,12 @@
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
- std::function<
- Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
- f,
- const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+ std::function<StateVector(const StateVector&)> f,
+ const Matrixd<States, Inputs>& B, units::second_t dt)
: m_B(B), m_dt(dt) {
- m_f = [=](const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, Inputs>& u)
- -> Eigen::Vector<double, States> { return f(x); };
+ m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
+ return f(x);
+ };
Reset();
}
@@ -95,7 +92,7 @@
*
* @return The calculated feedforward.
*/
- const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
+ const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -104,14 +101,14 @@
*
* @return The row of the calculated feedforward.
*/
- double Uff(int i) const { return m_uff(i, 0); }
+ double Uff(int i) const { return m_uff(i); }
/**
* Returns the current reference vector r.
*
* @return The current reference vector.
*/
- const Eigen::Vector<double, States>& R() const { return m_r; }
+ const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -120,14 +117,14 @@
*
* @return The row of the current reference vector.
*/
- double R(int i) const { return m_r(i, 0); }
+ double R(int i) const { return m_r(i); }
/**
* Resets the feedforward with a specified initial state vector.
*
* @param initialState The initial state vector.
*/
- void Reset(const Eigen::Vector<double, States>& initialState) {
+ void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -146,15 +143,14 @@
* reference.
*
* If this method is used the initial state of the system is the one set using
- * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+ * Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& nextR) {
+ InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -166,36 +162,30 @@
*
* @return The calculated feedforward.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& r,
- const Eigen::Vector<double, States>& nextR) {
- Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
+ InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+ StateVector rDot = (nextR - r) / m_dt.value();
- m_uff = m_B.householderQr().solve(
- rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
+ m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
m_r = nextR;
return m_uff;
}
private:
- Eigen::Matrix<double, States, Inputs> m_B;
+ Matrixd<States, Inputs> m_B;
units::second_t m_dt;
/**
* The model dynamics.
*/
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- m_f;
+ std::function<StateVector(const StateVector&, const InputVector&)> m_f;
// Current reference
- Eigen::Vector<double, States> m_r;
+ StateVector m_r;
// Computed feedforward
- Eigen::Vector<double, Inputs> m_uff;
+ InputVector m_uff;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
new file mode 100644
index 0000000..3a5148a
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
@@ -0,0 +1,83 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "Eigen/Core"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/length.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and
+ * angular acceleration.
+ *
+ * The differential drive model can be created via the functions in
+ * LinearSystemId.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
+ public:
+ /**
+ * Constructs a DifferentialDriveAccelerationLimiter.
+ *
+ * @param system The differential drive dynamics.
+ * @param trackwidth The distance between the differential drive's left and
+ * right wheels.
+ * @param maxLinearAccel The maximum linear acceleration.
+ * @param maxAngularAccel The maximum angular acceleration.
+ */
+ DifferentialDriveAccelerationLimiter(
+ LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+ units::meters_per_second_squared_t maxLinearAccel,
+ units::radians_per_second_squared_t maxAngularAccel);
+
+ /**
+ * Constructs a DifferentialDriveAccelerationLimiter.
+ *
+ * @param system The differential drive dynamics.
+ * @param trackwidth The distance between the differential drive's left and
+ * right wheels.
+ * @param minLinearAccel The minimum (most negative) linear acceleration.
+ * @param maxLinearAccel The maximum (most positive) linear acceleration.
+ * @param maxAngularAccel The maximum angular acceleration.
+ * @throws std::invalid_argument if minimum linear acceleration is greater
+ * than maximum linear acceleration
+ */
+ DifferentialDriveAccelerationLimiter(
+ LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+ units::meters_per_second_squared_t minLinearAccel,
+ units::meters_per_second_squared_t maxLinearAccel,
+ units::radians_per_second_squared_t maxAngularAccel);
+
+ /**
+ * Returns the next voltage pair subject to acceleraiton constraints.
+ *
+ * @param leftVelocity The left wheel velocity.
+ * @param rightVelocity The right wheel velocity.
+ * @param leftVoltage The unconstrained left motor voltage.
+ * @param rightVoltage The unconstrained right motor voltage.
+ * @return The constrained wheel voltages.
+ */
+ DifferentialDriveWheelVoltages Calculate(
+ units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+ units::volt_t rightVoltage);
+
+ private:
+ LinearSystem<2, 2, 2> m_system;
+ units::meter_t m_trackwidth;
+ units::meters_per_second_squared_t m_minLinearAccel;
+ units::meters_per_second_squared_t m_maxLinearAccel;
+ units::radians_per_second_squared_t m_maxAngularAccel;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
new file mode 100644
index 0000000..716bc78
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
@@ -0,0 +1,83 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class which computes the feedforward outputs for a differential
+ * drive drivetrain.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
+ frc::LinearSystem<2, 2, 2> m_plant;
+
+ public:
+ /**
+ * Creates a new DifferentialDriveFeedforward with the specified parameters.
+ *
+ * @param kVLinear The linear velocity gain in volts per (meters per second).
+ * @param kALinear The linear acceleration gain in volts per (meters per
+ * second squared).
+ * @param kVAngular The angular velocity gain in volts per (radians per
+ * second).
+ * @param kAAngular The angular acceleration gain in volts per (radians per
+ * second squared).
+ * @param trackwidth The distance between the differential drive's left and
+ * right wheels, in meters.
+ */
+ DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+ decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_rad_per_s) kVAngular,
+ decltype(1_V / 1_rad_per_s_sq) kAAngular,
+ units::meter_t trackwidth);
+
+ /**
+ * Creates a new DifferentialDriveFeedforward with the specified parameters.
+ *
+ * @param kVLinear The linear velocity gain in volts per (meters per second).
+ * @param kALinear The linear acceleration gain in volts per (meters per
+ * second squared).
+ * @param kVAngular The angular velocity gain in volts per (meters per
+ * second).
+ * @param kAAngular The angular acceleration gain in volts per (meters per
+ * second squared).
+ */
+ DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+ decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_mps) kVAngular,
+ decltype(1_V / 1_mps_sq) kAAngular);
+
+ /**
+ * Calculates the differential drive feedforward inputs given velocity
+ * setpoints.
+ *
+ * @param currentLeftVelocity The current left velocity of the differential
+ * drive in meters/second.
+ * @param nextLeftVelocity The next left velocity of the differential drive in
+ * meters/second.
+ * @param currentRightVelocity The current right velocity of the differential
+ * drive in meters/second.
+ * @param nextRightVelocity The next right velocity of the differential drive
+ * in meters/second.
+ * @param dt Discretization timestep.
+ */
+ DifferentialDriveWheelVoltages Calculate(
+ units::meters_per_second_t currentLeftVelocity,
+ units::meters_per_second_t nextLeftVelocity,
+ units::meters_per_second_t currentRightVelocity,
+ units::meters_per_second_t nextRightVelocity, units::second_t dt);
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
new file mode 100644
index 0000000..48f341e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
@@ -0,0 +1,19 @@
+// 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
+
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Motor voltages for a differential drive.
+ */
+struct DifferentialDriveWheelVoltages {
+ units::volt_t left = 0_V;
+ units::volt_t right = 0_V;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index 269a7e6..bbe7720 100644
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -6,6 +6,7 @@
#include <wpi/MathExtras.h>
+#include "units/length.h"
#include "units/time.h"
#include "units/voltage.h"
@@ -14,9 +15,9 @@
* A helper class that computes feedforward outputs for a simple elevator
* (modeled as a motor acting against the force of gravity).
*/
-template <class Distance>
class ElevatorFeedforward {
public:
+ using Distance = units::meters;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Acceleration =
@@ -33,7 +34,7 @@
* @param kS The static gain, in volts.
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
- * @param kA The acceleration gain, in volt seconds^2 per distance.
+ * @param kA The acceleration gain, in volt seconds² per distance.
*/
constexpr ElevatorFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -44,7 +45,7 @@
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint, in distance per second.
- * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 398a872..9a749c6 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -45,6 +45,12 @@
frc2::PIDController xController, frc2::PIDController yController,
ProfiledPIDController<units::radian> thetaController);
+ HolonomicDriveController(const HolonomicDriveController&) = default;
+ HolonomicDriveController& operator=(const HolonomicDriveController&) =
+ default;
+ HolonomicDriveController(HolonomicDriveController&&) = default;
+ HolonomicDriveController& operator=(HolonomicDriveController&&) = default;
+
/**
* Returns true if the pose error is within tolerance of the reference.
*/
@@ -61,32 +67,32 @@
/**
* Returns the next output of the holonomic drive controller.
*
- * The reference pose, linear velocity, and angular velocity should come from
- * a drivetrain trajectory.
- *
- * @param currentPose The current pose.
- * @param poseRef The desired pose.
- * @param linearVelocityRef The desired linear velocity.
- * @param angleRef The desired ending angle.
+ * @param currentPose The current pose, as measured by odometry or pose
+ * estimator.
+ * @param trajectoryPose The desired trajectory pose, as sampled for the
+ * current timestep.
+ * @param desiredLinearVelocity The desired linear velocity.
+ * @param desiredHeading The desired heading.
+ * @return The next output of the holonomic drive controller.
*/
- ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
- units::meters_per_second_t linearVelocityRef,
- const Rotation2d& angleRef);
+ ChassisSpeeds Calculate(const Pose2d& currentPose,
+ const Pose2d& trajectoryPose,
+ units::meters_per_second_t desiredLinearVelocity,
+ const Rotation2d& desiredHeading);
/**
* Returns the next output of the holonomic drive controller.
*
- * The reference pose, linear velocity, and angular velocity should come from
- * a drivetrain trajectory.
- *
- * @param currentPose The current pose.
- * @param desiredState The desired pose, linear velocity, and angular velocity
- * from a trajectory.
- * @param angleRef The desired ending angle.
+ * @param currentPose The current pose, as measured by odometry or pose
+ * estimator.
+ * @param desiredState The desired trajectory pose, as sampled for the current
+ * timestep.
+ * @param desiredHeading The desired heading.
+ * @return The next output of the holonomic drive controller.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState,
- const Rotation2d& angleRef);
+ const Rotation2d& desiredHeading);
/**
* Enables and disables the controller for troubleshooting purposes. When
diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
new file mode 100644
index 0000000..eef1fc0
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
@@ -0,0 +1,123 @@
+// 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
+
+#include <frc/system/LinearSystem.h>
+
+#include "Eigen/QR"
+#include "frc/EigenCore.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Contains the controller coefficients and logic for an implicit model
+ * follower.
+ *
+ * Implicit model following lets us design a feedback controller that erases the
+ * dynamics of our system and makes it behave like some other system. This can
+ * be used to make a drivetrain more controllable during teleop driving by
+ * making it behave like a slower or more benign drivetrain.
+ *
+ * For more on the underlying math, read appendix B.3 in
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs>
+class ImplicitModelFollower {
+ public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+
+ /**
+ * Constructs a controller with the given coefficients and plant.
+ *
+ * @param plant The plant being controlled.
+ * @param plantRef The plant whose dynamics should be followed.
+ */
+ template <int Outputs>
+ ImplicitModelFollower(const LinearSystem<States, Inputs, Outputs>& plant,
+ const LinearSystem<States, Inputs, Outputs>& plantRef)
+ : ImplicitModelFollower<States, Inputs>(plant.A(), plant.B(),
+ plantRef.A(), plantRef.B()) {}
+
+ /**
+ * Constructs a controller with the given coefficients and plant.
+ *
+ * @param A Continuous system matrix of the plant being controlled.
+ * @param B Continuous input matrix of the plant being controlled.
+ * @param Aref Continuous system matrix whose dynamics should be followed.
+ * @param Bref Continuous input matrix whose dynamics should be followed.
+ */
+ ImplicitModelFollower(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Aref,
+ const Matrixd<States, Inputs>& Bref) {
+ // Find u_imf that makes real model match reference model.
+ //
+ // dx/dt = Ax + Bu_imf
+ // dz/dt = A_ref z + B_ref u
+ //
+ // Let x = z.
+ //
+ // dx/dt = dz/dt
+ // Ax + Bu_imf = Aref x + B_ref u
+ // Bu_imf = A_ref x - Ax + B_ref u
+ // Bu_imf = (A_ref - A)x + B_ref u
+ // u_imf = B⁻¹((A_ref - A)x + Bref u)
+ // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
+
+ // The first term makes the open-loop poles that of the reference
+ // system, and the second term makes the input behave like that of the
+ // reference system.
+ m_A = -B.householderQr().solve(A - Aref);
+ m_B = B.householderQr().solve(Bref);
+
+ Reset();
+ }
+
+ /**
+ * Returns the control input vector u.
+ *
+ * @return The control input.
+ */
+ const InputVector& U() const { return m_u; }
+
+ /**
+ * Returns an element of the control input vector u.
+ *
+ * @param i Row of u.
+ *
+ * @return The row of the control input vector.
+ */
+ double U(int i) const { return m_u(i); }
+
+ /**
+ * Resets the controller.
+ */
+ void Reset() { m_u.setZero(); }
+
+ /**
+ * Returns the next output of the controller.
+ *
+ * @param x The current state x.
+ * @param u The current input for the original model.
+ */
+ InputVector Calculate(const StateVector& x, const InputVector& u) {
+ m_u = m_A * x + m_B * u;
+ return m_u;
+ }
+
+ private:
+ // Computed controller output
+ InputVector m_u;
+
+ // State space conversion gain
+ Matrixd<Inputs, States> m_A;
+
+ // Input space conversion gain
+ Matrixd<Inputs, Inputs> m_B;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
new file mode 100644
index 0000000..0a336aa
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
@@ -0,0 +1,125 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+#include <wpi/interpolating_map.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying differential drive controller has a similar form to
+ * the LQR, but the model used to compute the controller gain is the nonlinear
+ * model linearized around the drivetrain's current state. We precomputed gains
+ * for important places in our state-space, then interpolated between them with
+ * a LUT to save computational resources.
+ *
+ * See section 8.7 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.7.4.
+ */
+class WPILIB_DLLEXPORT LTVDifferentialDriveController {
+ public:
+ /**
+ * Constructs a linear time-varying differential drive controller.
+ *
+ * @param plant The differential drive velocity plant.
+ * @param trackwidth The distance between the differential drive's left and
+ * right wheels.
+ * @param Qelems The maximum desired error tolerance for each state.
+ * @param Relems The maximum desired control effort for each input.
+ * @param dt Discretization timestep.
+ */
+ LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
+ units::meter_t trackwidth,
+ const wpi::array<double, 5>& Qelems,
+ const wpi::array<double, 2>& Relems,
+ units::second_t dt);
+
+ /**
+ * Move constructor.
+ */
+ LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
+
+ /**
+ * Move assignment operator.
+ */
+ LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
+ default;
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ */
+ bool AtReference() const;
+
+ /**
+ * Sets the pose error which is considered tolerable for use with
+ * AtReference().
+ *
+ * @param poseTolerance Pose error which is tolerable.
+ * @param leftVelocityTolerance Left velocity error which is tolerable.
+ * @param rightVelocityTolerance Right velocity error which is tolerable.
+ */
+ void SetTolerance(const Pose2d& poseTolerance,
+ units::meters_per_second_t leftVelocityTolerance,
+ units::meters_per_second_t rightVelocityTolerance);
+
+ /**
+ * Returns the left and right output voltages of the LTV controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param leftVelocity The current left velocity.
+ * @param rightVelocity The current right velocity.
+ * @param poseRef The desired pose.
+ * @param leftVelocityRef The desired left velocity.
+ * @param rightVelocityRef The desired right velocity.
+ */
+ DifferentialDriveWheelVoltages Calculate(
+ const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
+ units::meters_per_second_t leftVelocityRef,
+ units::meters_per_second_t rightVelocityRef);
+
+ /**
+ * Returns the left and right output voltages of the LTV controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param leftVelocity The left velocity.
+ * @param rightVelocity The right velocity.
+ * @param desiredState The desired pose, linear velocity, and angular velocity
+ * from a trajectory.
+ */
+ DifferentialDriveWheelVoltages Calculate(
+ const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+ units::meters_per_second_t rightVelocity,
+ const Trajectory::State& desiredState);
+
+ private:
+ units::meter_t m_trackwidth;
+
+ // LUT from drivetrain linear velocity to LQR gain
+ wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
+
+ Vectord<5> m_error;
+ Vectord<5> m_tolerance;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
new file mode 100644
index 0000000..83cfe4b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
@@ -0,0 +1,123 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+#include <wpi/interpolating_map.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying unicycle controller has a similar form to the LQR,
+ * but the model used to compute the controller gain is the nonlinear model
+ * linearized around the drivetrain's current state.
+ *
+ * See section 8.9 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.9.1.
+ */
+class WPILIB_DLLEXPORT LTVUnicycleController {
+ public:
+ /**
+ * Constructs a linear time-varying unicycle controller with default maximum
+ * desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum
+ * desired control effort of (1 m/s, 2 rad/s).
+ *
+ * @param dt Discretization timestep.
+ * @param maxVelocity The maximum velocity for the controller gain lookup
+ * table.
+ */
+ explicit LTVUnicycleController(
+ units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
+
+ /**
+ * Constructs a linear time-varying unicycle controller.
+ *
+ * @param Qelems The maximum desired error tolerance for each state.
+ * @param Relems The maximum desired control effort for each input.
+ * @param dt Discretization timestep.
+ * @param maxVelocity The maximum velocity for the controller gain lookup
+ * table.
+ */
+ LTVUnicycleController(const wpi::array<double, 3>& Qelems,
+ const wpi::array<double, 2>& Relems, units::second_t dt,
+ units::meters_per_second_t maxVelocity = 9_mps);
+
+ /**
+ * Move constructor.
+ */
+ LTVUnicycleController(LTVUnicycleController&&) = default;
+
+ /**
+ * Move assignment operator.
+ */
+ LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ */
+ bool AtReference() const;
+
+ /**
+ * Sets the pose error which is considered tolerable for use with
+ * AtReference().
+ *
+ * @param poseTolerance Pose error which is tolerable.
+ */
+ void SetTolerance(const Pose2d& poseTolerance);
+
+ /**
+ * Returns the linear and angular velocity outputs of the LTV controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param poseRef The desired pose.
+ * @param linearVelocityRef The desired linear velocity.
+ * @param angularVelocityRef The desired angular velocity.
+ */
+ ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef);
+
+ /**
+ * Returns the linear and angular velocity outputs of the LTV controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param desiredState The desired pose, linear velocity, and angular velocity
+ * from a trajectory.
+ */
+ ChassisSpeeds Calculate(const Pose2d& currentPose,
+ const Trajectory::State& desiredState);
+
+ /**
+ * Enables and disables the controller for troubleshooting purposes.
+ *
+ * @param enabled If the controller is enabled or not.
+ */
+ void SetEnabled(bool enabled);
+
+ private:
+ // LUT from drivetrain linear velocity to LQR gain
+ wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 3>> m_table;
+
+ Pose2d m_poseError;
+ Pose2d m_poseTolerance;
+ bool m_enabled = true;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index 519368d..d4cc3c4 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -7,8 +7,8 @@
#include <array>
#include <functional>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -31,6 +31,9 @@
template <int States, int Inputs>
class LinearPlantInversionFeedforward {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+
/**
* Constructs a feedforward with the given plant.
*
@@ -50,9 +53,9 @@
* @param B Continuous input matrix of the plant being controlled.
* @param dt Discretization timestep.
*/
- LinearPlantInversionFeedforward(
- const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+ LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ units::second_t dt)
: m_dt(dt) {
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
Reset();
@@ -63,7 +66,7 @@
*
* @return The calculated feedforward.
*/
- const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
+ const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -72,14 +75,14 @@
*
* @return The row of the calculated feedforward.
*/
- double Uff(int i) const { return m_uff(i, 0); }
+ double Uff(int i) const { return m_uff(i); }
/**
* Returns the current reference vector r.
*
* @return The current reference vector.
*/
- const Eigen::Vector<double, States>& R() const { return m_r; }
+ const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -88,14 +91,14 @@
*
* @return The row of the current reference vector.
*/
- double R(int i) const { return m_r(i, 0); }
+ double R(int i) const { return m_r(i); }
/**
* Resets the feedforward with a specified initial state vector.
*
* @param initialState The initial state vector.
*/
- void Reset(const Eigen::Vector<double, States>& initialState) {
+ void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -114,16 +117,15 @@
* reference.
*
* If this method is used the initial state of the system is the one set using
- * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+ * Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& nextR) {
- return Calculate(m_r, nextR); // NOLINT
+ InputVector Calculate(const StateVector& nextR) {
+ return Calculate(m_r, nextR);
}
/**
@@ -134,25 +136,23 @@
*
* @return The calculated feedforward.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& r,
- const Eigen::Vector<double, States>& nextR) {
+ InputVector Calculate(const StateVector& r, const StateVector& nextR) {
m_uff = m_B.householderQr().solve(nextR - (m_A * r));
m_r = nextR;
return m_uff;
}
private:
- Eigen::Matrix<double, States, States> m_A;
- Eigen::Matrix<double, States, Inputs> m_B;
+ Matrixd<States, States> m_A;
+ Matrixd<States, Inputs> m_B;
units::second_t m_dt;
// Current reference
- Eigen::Vector<double, States> m_r;
+ StateVector m_r;
// Computed feedforward
- Eigen::Vector<double, Inputs> m_uff;
+ InputVector m_uff;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 44a8501..50d6566 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -4,26 +4,14 @@
#pragma once
-#include <frc/fmt/Eigen.h>
-
-#include <string>
-
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
+#include "frc/EigenCore.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
-#include "wpimath/MathShared.h"
namespace frc {
-namespace detail {
/**
* Contains the controller coefficients and logic for a linear-quadratic
@@ -37,8 +25,14 @@
* @tparam Inputs Number of inputs.
*/
template <int States, int Inputs>
-class LinearQuadraticRegulatorImpl {
+class LinearQuadraticRegulator {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+
+ using StateArray = wpi::array<double, States>;
+ using InputArray = wpi::array<double, Inputs>;
+
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -46,14 +40,12 @@
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
+ * @throws std::invalid_argument If the system is uncontrollable.
*/
template <int Outputs>
- LinearQuadraticRegulatorImpl(
- const LinearSystem<States, Inputs, Outputs>& plant,
- const wpi::array<double, States>& Qelems,
- const wpi::array<double, Inputs>& Relems, units::second_t dt)
- : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
- }
+ LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
+ const StateArray& Qelems, const InputArray& Relems,
+ units::second_t dt);
/**
* Constructs a controller with the given coefficients and plant.
@@ -63,14 +55,12 @@
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
+ * @throws std::invalid_argument If the system is uncontrollable.
*/
- LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const wpi::array<double, States>& Qelems,
- const wpi::array<double, Inputs>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
- MakeCostMatrix(Relems), dt) {}
+ LinearQuadraticRegulator(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ const StateArray& Qelems, const InputArray& Relems,
+ units::second_t dt);
/**
* Constructs a controller with the given coefficients and plant.
@@ -80,36 +70,13 @@
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dt Discretization timestep.
+ * @throws std::invalid_argument If the system is uncontrollable.
*/
- LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const Eigen::Matrix<double, States, States>& Q,
- const Eigen::Matrix<double, Inputs, Inputs>& R,
- units::second_t dt) {
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, Inputs> discB;
- DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
-
- if (!IsStabilizable<States, Inputs>(discA, discB)) {
- std::string msg = fmt::format(
- "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
- "=\n{}\n",
- discA, discB);
-
- wpi::math::MathSharedStore::ReportError(msg);
- throw std::invalid_argument(msg);
- }
-
- Eigen::Matrix<double, States, States> S =
- drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
-
- // K = (BᵀSB + R)⁻¹BᵀSA
- m_K = (discB.transpose() * S * discB + R)
- .llt()
- .solve(discB.transpose() * S * discA);
-
- Reset();
- }
+ LinearQuadraticRegulator(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Q,
+ const Matrixd<Inputs, Inputs>& R,
+ units::second_t dt);
/**
* Constructs a controller with the given coefficients and plant.
@@ -120,36 +87,22 @@
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
+ * @throws std::invalid_argument If the system is uncontrollable.
*/
- LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const Eigen::Matrix<double, States, States>& Q,
- const Eigen::Matrix<double, Inputs, Inputs>& R,
- const Eigen::Matrix<double, States, Inputs>& N,
- units::second_t dt) {
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, Inputs> discB;
- DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+ LinearQuadraticRegulator(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Q,
+ const Matrixd<Inputs, Inputs>& R,
+ const Matrixd<States, Inputs>& N,
+ units::second_t dt);
- Eigen::Matrix<double, States, States> S =
- drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
-
- // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
- m_K = (discB.transpose() * S * discB + R)
- .llt()
- .solve(discB.transpose() * S * discA + N.transpose());
-
- Reset();
- }
-
- LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
- LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
- default;
+ LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+ LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
/**
* Returns the controller matrix K.
*/
- const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
+ const Matrixd<Inputs, States>& K() const { return m_K; }
/**
* Returns an element of the controller matrix K.
@@ -164,7 +117,7 @@
*
* @return The reference vector.
*/
- const Eigen::Vector<double, States>& R() const { return m_r; }
+ const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -173,14 +126,14 @@
*
* @return The row of the reference vector.
*/
- double R(int i) const { return m_r(i, 0); }
+ double R(int i) const { return m_r(i); }
/**
* Returns the control input vector u.
*
* @return The control input.
*/
- const Eigen::Vector<double, Inputs>& U() const { return m_u; }
+ const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -189,7 +142,7 @@
*
* @return The row of the control input vector.
*/
- double U(int i) const { return m_u(i, 0); }
+ double U(int i) const { return m_u(i); }
/**
* Resets the controller.
@@ -204,11 +157,7 @@
*
* @param x The current state x.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& x) {
- m_u = m_K * (m_r - x);
- return m_u;
- }
+ InputVector Calculate(const StateVector& x);
/**
* Returns the next output of the controller.
@@ -216,12 +165,7 @@
* @param x The current state x.
* @param nextR The next reference vector r.
*/
- Eigen::Vector<double, Inputs> Calculate(
- const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, States>& nextR) {
- m_r = nextR;
- return Calculate(x);
- }
+ InputVector Calculate(const StateVector& x, const StateVector& nextR);
/**
* Adjusts LQR controller gain to compensate for a pure time delay in the
@@ -241,209 +185,26 @@
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
- units::second_t dt, units::second_t inputDelay) {
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, Inputs> discB;
- DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
-
- m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
- }
+ units::second_t dt, units::second_t inputDelay);
private:
// Current reference
- Eigen::Vector<double, States> m_r;
+ StateVector m_r;
// Computed controller output
- Eigen::Vector<double, Inputs> m_u;
+ InputVector m_u;
// Controller gain
- Eigen::Matrix<double, Inputs, States> m_K;
+ Matrixd<Inputs, States> m_K;
};
-} // namespace detail
-
-template <int States, int Inputs>
-class LinearQuadraticRegulator
- : public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
- public:
- /**
- * Constructs a controller with the given coefficients and plant.
- *
- * @tparam Outputs The number of outputs.
- * @param plant The plant being controlled.
- * @param Qelems The maximum desired error tolerance for each state.
- * @param Relems The maximum desired control effort for each input.
- * @param dt Discretization timestep.
- */
- template <int Outputs>
- LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
- const wpi::array<double, States>& Qelems,
- const wpi::array<double, Inputs>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
- /**
- * Constructs a controller with the given coefficients and plant.
- *
- * @param A Continuous system matrix of the plant being controlled.
- * @param B Continuous input matrix of the plant being controlled.
- * @param Qelems The maximum desired error tolerance for each state.
- * @param Relems The maximum desired control effort for each input.
- * @param dt Discretization timestep.
- */
- LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const wpi::array<double, States>& Qelems,
- const wpi::array<double, Inputs>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
- MakeCostMatrix(Relems), dt) {}
-
- /**
- * Constructs a controller with the given coefficients and plant.
- *
- * @param A Continuous system matrix of the plant being controlled.
- * @param B Continuous input matrix of the plant being controlled.
- * @param Q The state cost matrix.
- * @param R The input cost matrix.
- * @param dt Discretization timestep.
- */
- LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const Eigen::Matrix<double, States, States>& Q,
- const Eigen::Matrix<double, Inputs, Inputs>& R,
- units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
-
- /**
- * Constructs a controller with the given coefficients and plant.
- *
- * @param A Continuous system matrix of the plant being controlled.
- * @param B Continuous input matrix of the plant being controlled.
- * @param Q The state cost matrix.
- * @param R The input cost matrix.
- * @param N The state-input cross-term cost matrix.
- * @param dt Discretization timestep.
- */
- LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const Eigen::Matrix<double, States, States>& Q,
- const Eigen::Matrix<double, Inputs, Inputs>& R,
- const Eigen::Matrix<double, States, Inputs>& N,
- units::second_t dt)
- : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
- R, N, dt} {}
-
- LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
- LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
- : public detail::LinearQuadraticRegulatorImpl<1, 1> {
- public:
- template <int Outputs>
- LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
- const wpi::array<double, 1>& Qelems,
- const wpi::array<double, 1>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
- const Eigen::Matrix<double, 1, 1>& B,
- const wpi::array<double, 1>& Qelems,
- const wpi::array<double, 1>& Relems,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
- const Eigen::Matrix<double, 1, 1>& B,
- const Eigen::Matrix<double, 1, 1>& Q,
- const Eigen::Matrix<double, 1, 1>& R,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
- const Eigen::Matrix<double, 1, 1>& B,
- const Eigen::Matrix<double, 1, 1>& Q,
- const Eigen::Matrix<double, 1, 1>& R,
- const Eigen::Matrix<double, 1, 1>& N,
- units::second_t dt);
-
- LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
- LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
- : public detail::LinearQuadraticRegulatorImpl<2, 1> {
- public:
- template <int Outputs>
- LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
- const wpi::array<double, 2>& Qelems,
- const wpi::array<double, 1>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 1>& B,
- const wpi::array<double, 2>& Qelems,
- const wpi::array<double, 1>& Relems,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 1>& B,
- const Eigen::Matrix<double, 2, 2>& Q,
- const Eigen::Matrix<double, 1, 1>& R,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 1>& B,
- const Eigen::Matrix<double, 2, 2>& Q,
- const Eigen::Matrix<double, 1, 1>& R,
- const Eigen::Matrix<double, 2, 1>& N,
- units::second_t dt);
-
- LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
- LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
- : public detail::LinearQuadraticRegulatorImpl<2, 2> {
- public:
- template <int Outputs>
- LinearQuadraticRegulator(const LinearSystem<2, 2, Outputs>& plant,
- const wpi::array<double, 2>& Qelems,
- const wpi::array<double, 2>& Relems,
- units::second_t dt)
- : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 2>& B,
- const wpi::array<double, 2>& Qelems,
- const wpi::array<double, 2>& Relems,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 2>& B,
- const Eigen::Matrix<double, 2, 2>& Q,
- const Eigen::Matrix<double, 2, 2>& R,
- units::second_t dt);
-
- LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
- const Eigen::Matrix<double, 2, 2>& B,
- const Eigen::Matrix<double, 2, 2>& Q,
- const Eigen::Matrix<double, 2, 2>& R,
- const Eigen::Matrix<double, 2, 2>& N,
- units::second_t dt);
-
- LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
- LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<2, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ LinearQuadraticRegulator<2, 2>;
} // namespace frc
+
+#include "LinearQuadraticRegulator.inc"
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
new file mode 100644
index 0000000..87d37ec
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
@@ -0,0 +1,113 @@
+// 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
+
+#include <stdexcept>
+#include <string>
+
+#include "Eigen/Cholesky"
+#include "Eigen/Eigenvalues"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/fmt/Eigen.h"
+#include "frc/system/Discretization.h"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+template <int States, int Inputs>
+template <int Outputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+ const LinearSystem<States, Inputs, Outputs>& plant,
+ const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+ : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+ const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+ const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+ : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+ MakeCostMatrix(Relems), dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+ const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+ units::second_t dt) {
+ Matrixd<States, States> discA;
+ Matrixd<States, Inputs> discB;
+ DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+ if (!IsStabilizable<States, Inputs>(discA, discB)) {
+ std::string msg = fmt::format(
+ "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
+ "=\n{}\n",
+ discA, discB);
+
+ wpi::math::MathSharedStore::ReportError(msg);
+ throw std::invalid_argument(msg);
+ }
+
+ Matrixd<States, States> S =
+ drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_K = (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA);
+
+ Reset();
+}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+ const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+ const Matrixd<States, Inputs>& N, units::second_t dt) {
+ Matrixd<States, States> discA;
+ Matrixd<States, Inputs> discB;
+ DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+ Matrixd<States, States> S =
+ drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+
+ // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+ m_K = (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA + N.transpose());
+
+ Reset();
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x) {
+ m_u = m_K * (m_r - x);
+ return m_u;
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x,
+ const StateVector& nextR) {
+ m_r = nextR;
+ return Calculate(x);
+}
+
+template <int States, int Inputs>
+template <int Outputs>
+void LinearQuadraticRegulator<States, Inputs>::LatencyCompensate(
+ const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt,
+ units::second_t inputDelay) {
+ Matrixd<States, States> discA;
+ Matrixd<States, Inputs> discB;
+ DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+ m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index 98625f8..d6a41d1 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -102,6 +102,20 @@
units::second_t GetPeriod() const;
/**
+ * Gets the position tolerance of this controller.
+ *
+ * @return The position tolerance of the controller.
+ */
+ double GetPositionTolerance() const;
+
+ /**
+ * Gets the velocity tolerance of this controller.
+ *
+ * @return The velocity tolerance of the controller.
+ */
+ double GetVelocityTolerance() const;
+
+ /**
* Sets the setpoint for the PIDController.
*
* @param setpoint The desired setpoint.
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index e0b10c7..8491118 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -22,7 +22,7 @@
namespace frc {
namespace detail {
WPILIB_DLLEXPORT
-void ReportProfiledPIDController();
+int IncrementAndGetProfiledPIDControllerInstances();
} // namespace detail
/**
@@ -59,7 +59,10 @@
ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints, units::second_t period = 20_ms)
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
- detail::ReportProfiledPIDController();
+ int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
+ wpi::math::MathSharedStore::ReportUsage(
+ wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+ wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
}
~ProfiledPIDController() override = default;
@@ -132,6 +135,24 @@
units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
/**
+ * Gets the position tolerance of this controller.
+ *
+ * @return The position tolerance of the controller.
+ */
+ double GetPositionTolerance() const {
+ return m_controller.GetPositionTolerance();
+ }
+
+ /**
+ * Gets the velocity tolerance of this controller.
+ *
+ * @return The velocity tolerance of the controller.
+ */
+ double GetVelocityTolerance() const {
+ return m_controller.GetVelocityTolerance();
+ }
+
+ /**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
@@ -143,7 +164,7 @@
*
* @param goal The desired unprofiled setpoint.
*/
- void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
+ void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; }
/**
* Gets the goal for the ProfiledPIDController.
@@ -224,9 +245,9 @@
* @param positionTolerance Position error which is tolerable.
* @param velocityTolerance Velocity error which is tolerable.
*/
- void SetTolerance(
- Distance_t positionTolerance,
- Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+ void SetTolerance(Distance_t positionTolerance,
+ Velocity_t velocityTolerance = Velocity_t{
+ std::numeric_limits<double>::infinity()}) {
m_controller.SetTolerance(positionTolerance.value(),
velocityTolerance.value());
}
@@ -237,14 +258,14 @@
* @return The error.
*/
Distance_t GetPositionError() const {
- return Distance_t(m_controller.GetPositionError());
+ return Distance_t{m_controller.GetPositionError()};
}
/**
* Returns the change in error per second.
*/
Velocity_t GetVelocityError() const {
- return Velocity_t(m_controller.GetVelocityError());
+ return Velocity_t{m_controller.GetVelocityError()};
}
/**
@@ -339,7 +360,7 @@
* velocity is assumed to be zero.
*/
void Reset(Distance_t measuredPosition) {
- Reset(measuredPosition, Velocity_t(0));
+ Reset(measuredPosition, Velocity_t{0});
}
void InitSendable(wpi::SendableBuilder& builder) override {
diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h
index bb23f46..d031101 100644
--- a/wpimath/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h
@@ -5,7 +5,6 @@
#pragma once
#include <wpi/SymbolExports.h>
-#include <wpi/deprecated.h>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -58,17 +57,6 @@
* @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
* values provide more damping in response.
*/
- WPI_DEPRECATED("Use unit-safe constructor instead")
- RamseteController(double b, double zeta);
-
- /**
- * Construct a Ramsete unicycle controller.
- *
- * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make
- * convergence more aggressive like a proportional term.
- * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
- * values provide more damping in response.
- */
RamseteController(units::unit_t<b_unit> b, units::unit_t<zeta_unit> zeta);
/**
diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index df1a52c..86b40ea 100644
--- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -6,7 +6,7 @@
#include <wpi/MathExtras.h>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
@@ -35,7 +35,7 @@
*
* @param kS The static gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
- * @param kA The acceleration gain, in volt seconds^2 per distance.
+ * @param kA The acceleration gain, in volt seconds² per distance.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
@@ -46,7 +46,7 @@
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint, in distance per second.
- * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
@@ -70,8 +70,8 @@
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
- Eigen::Vector<double, 1> r{currentVelocity.value()};
- Eigen::Vector<double, 1> nextR{nextVelocity.value()};
+ Vectord<1> r{currentVelocity.value()};
+ Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 3ddabc1..0512c68c 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -4,9 +4,9 @@
#pragma once
-#include <wpi/numbers>
+#include <numbers>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/MathUtil.h"
namespace frc {
@@ -21,10 +21,9 @@
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
-Eigen::Vector<double, States> AngleResidual(
- const Eigen::Vector<double, States>& a,
- const Eigen::Vector<double, States>& b, int angleStateIdx) {
- Eigen::Vector<double, States> ret = a - b;
+Vectord<States> AngleResidual(const Vectord<States>& a,
+ const Vectord<States>& b, int angleStateIdx) {
+ Vectord<States> ret = a - b;
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
return ret;
@@ -38,8 +37,7 @@
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
-std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleResidual(int angleStateIdx) {
return [=](auto a, auto b) {
return AngleResidual<States>(a, b, angleStateIdx);
@@ -56,12 +54,11 @@
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
-Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
- const Eigen::Vector<double, States>& b,
- int angleStateIdx) {
- Eigen::Vector<double, States> ret = a + b;
+Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
+ int angleStateIdx) {
+ Vectord<States> ret = a + b;
ret[angleStateIdx] =
- InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
+ InputModulus(ret[angleStateIdx], -std::numbers::pi, std::numbers::pi);
return ret;
}
@@ -73,8 +70,7 @@
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
-std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleAdd(int angleStateIdx) {
return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
}
@@ -91,17 +87,21 @@
* @param angleStatesIdx The row containing the angles.
*/
template <int CovDim, int States>
-Eigen::Vector<double, CovDim> AngleMean(
- const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
- const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
- double sumSin = sigmas.row(angleStatesIdx)
- .unaryExpr([](auto it) { return std::sin(it); })
+Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
+ const Vectord<2 * States + 1>& Wm,
+ int angleStatesIdx) {
+ double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+ return std::sin(it);
+ }) *
+ Wm)
.sum();
- double sumCos = sigmas.row(angleStatesIdx)
- .unaryExpr([](auto it) { return std::cos(it); })
+ double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+ return std::cos(it);
+ }) *
+ Wm)
.sum();
- Eigen::Vector<double, CovDim> ret = sigmas * Wm;
+ Vectord<CovDim> ret = sigmas * Wm;
ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
return ret;
}
@@ -116,9 +116,8 @@
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
-std::function<Eigen::Vector<double, CovDim>(
- const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
+std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
AngleMean(int angleStateIdx) {
return [=](auto sigmas, auto Wm) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 12810fa..339ccc9 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -7,17 +7,18 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Differential Drive Odometry to fuse latency-compensated
* vision measurements with differential drive encoder measurements. It will
* correct for noisy vision measurements and encoder drift. It is intended to be
* an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
@@ -25,77 +26,68 @@
* same as DifferentialDriveOdometry.
*
* Update() should be called every robot loop (if your robot loops are faster or
- * slower than the default, then you should change the nominal delta time via
- * the constructor).
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time via the constructor).
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate
- * system containing x position, y position, heading, left encoder distance,
- * and right encoder distance.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * NB: Using velocities make things considerably easier, because it means that
- * teams don't have to worry about getting an accurate model. Basically, we
- * suspect that it's easier for teams to get good encoder data than it is for
- * them to perform system identification well enough to get a good model.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong>y = [dist_l, dist_r, theta] </strong>
- * containing left encoder position, right encoder position, and gyro heading.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
public:
/**
- * Constructs a DifferentialDrivePoseEstimator.
+ * Constructs a DifferentialDrivePoseEstimator with default standard
+ * deviations for the model and vision measurements.
*
- * @param gyroAngle The gyro angle of the robot.
- * @param initialPose The estimated initial pose.
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form
- * [x, y, theta, dist_l, dist_r]ᵀ,
- * with units in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro
- * measurements. Increase these numbers to
- * trust sensor readings from
- * encoders and gyros less.
- * This matrix is in the form
- * [dist_l, dist_r, theta]ᵀ, with units in
- * meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from
- * vision less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
- * @param nominalDt The period of the loop calling Update().
+ * The default standard deviations of the model states are
+ * 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading.
+ * The default standard deviations of the vision measurements are
+ * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The gyro angle of the robot.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
+ * @param initialPose The estimated initial pose.
*/
- DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- const wpi::array<double, 5>& stateStdDevs,
- const wpi::array<double, 3>& localMeasurementStdDevs,
- const wpi::array<double, 3>& visionMeasurementStdDevs,
- units::second_t nominalDt = 0.02_s);
+ DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
+ const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance,
+ const Pose2d& initialPose);
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Constructs a DifferentialDrivePoseEstimator.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The gyro angle of the robot.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
+ * @param initialPose The estimated initial pose.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ DifferentialDrivePoseEstimator(
+ DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ units::meter_t leftDistance, units::meter_t rightDistance,
+ const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+ /**
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
@@ -103,74 +95,71 @@
/**
* Resets the robot's position on the field.
*
- * You NEED to reset your encoders to zero when calling this method. The
- * gyroscope angle does not need to be reset here on the user's robot code.
- * The library automatically takes care of offsetting the gyro angle.
- *
- * @param pose The estimated pose of the robot on the field.
* @param gyroAngle The current gyro angle.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
+ * @param pose The estimated pose of the robot on the field.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& pose);
/**
- * Returns the pose of the robot at the current time as estimated by the
- * Unscented Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup (i.e. the
- * epoch of this timestamp is the same epoch as
- * frc::Timer::GetFPGATimestamp(). This means that
- * you should use frc::Timer::GetFPGATimestamp() as
- * your time source in this case.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -180,27 +169,24 @@
}
/**
- * Updates the Unscented Kalman Filter using only wheel encoder information.
- * Note that this should be called every loop iteration.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
* @return The estimated pose of the robot.
*/
- Pose2d Update(const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
- units::meter_t leftDistance, units::meter_t rightDistance);
+ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance);
/**
- * Updates the Unscented Kalman Filter using only wheel encoder information.
- * Note that this should be called every loop iteration.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
@@ -208,35 +194,62 @@
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance,
units::meter_t rightDistance);
private:
- UnscentedKalmanFilter<5, 3, 3> m_observer;
- KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
- m_latencyCompensator;
- std::function<void(const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y)>
- m_visionCorrect;
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
- Eigen::Matrix<double, 3, 3> m_visionContR;
+ // The current gyro angle.
+ Rotation2d gyroAngle;
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
+ // The distance traveled by the left encoder.
+ units::meter_t leftDistance;
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ // The distance traveled by the right encoder.
+ units::meter_t rightDistance;
- template <int Dim>
- static wpi::array<double, Dim> StdDevMatrixToArray(
- const Eigen::Vector<double, Dim>& stdDevs);
+ /**
+ * Checks equality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const InterpolationRecord& other) const = default;
- static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
- const Eigen::Vector<double, 3>& u);
- static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
- units::meter_t leftDistance,
- units::meter_t rightDistance);
+ /**
+ * Checks inequality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const InterpolationRecord& other) const = default;
+
+ /**
+ * Interpolates between two InterpolationRecords.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ *
+ * @return The interpolated state.
+ */
+ InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
+ InterpolationRecord endValue,
+ double i) const;
+ };
+
+ DifferentialDriveKinematics& m_kinematics;
+ DifferentialDriveOdometry m_odometry;
+ wpi::array<double, 3> m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index 3e5edb8..b09e8d9 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -8,13 +8,7 @@
#include <wpi/array.h>
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
-#include "frc/system/NumericalIntegration.h"
-#include "frc/system/NumericalJacobian.h"
+#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -46,6 +40,15 @@
template <int States, int Inputs, int Outputs>
class ExtendedKalmanFilter {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+ using OutputVector = Vectord<Outputs>;
+
+ using StateArray = wpi::array<double, States>;
+ using OutputArray = wpi::array<double, Outputs>;
+
+ using StateMatrix = Matrixd<States, States>;
+
/**
* Constructs an extended Kalman filter.
*
@@ -57,52 +60,11 @@
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
- ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- f,
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- units::second_t dt)
- : m_f(f), m_h(h) {
- m_contQ = MakeCovMatrix(stateStdDevs);
- m_contR = MakeCovMatrix(measurementStdDevs);
- m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
- return a - b;
- };
- m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a + b;
- };
- m_dt = dt;
-
- Reset();
-
- Eigen::Matrix<double, States, States> contA =
- NumericalJacobianX<States, States, Inputs>(
- m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
- Eigen::Matrix<double, Outputs, States> C =
- NumericalJacobianX<Outputs, States, Inputs>(
- m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, States> discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
- Eigen::Matrix<double, Outputs, Outputs> discR =
- DiscretizeR<Outputs>(m_contR, dt);
-
- if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
- m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
- } else {
- m_initP = Eigen::Matrix<double, States, States>::Zero();
- }
- m_P = m_initP;
- }
+ ExtendedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ units::second_t dt);
/**
* Constructs an extended Kalman filter.
@@ -118,59 +80,20 @@
* @param addFuncX A function that adds two state vectors.
* @param dt Nominal discretization timestep.
*/
- ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- f,
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, Outputs>&,
- const Eigen::Vector<double, Outputs>&)>
- residualFuncY,
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>&)>
- addFuncX,
- units::second_t dt)
- : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
- m_contQ = MakeCovMatrix(stateStdDevs);
- m_contR = MakeCovMatrix(measurementStdDevs);
- m_dt = dt;
-
- Reset();
-
- Eigen::Matrix<double, States, States> contA =
- NumericalJacobianX<States, States, Inputs>(
- m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
- Eigen::Matrix<double, Outputs, States> C =
- NumericalJacobianX<Outputs, States, Inputs>(
- m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, States> discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
- Eigen::Matrix<double, Outputs, Outputs> discR =
- DiscretizeR<Outputs>(m_contR, dt);
-
- if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
- m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
- } else {
- m_initP = Eigen::Matrix<double, States, States>::Zero();
- }
- m_P = m_initP;
- }
+ ExtendedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ addFuncX,
+ units::second_t dt);
/**
* Returns the error covariance matrix P.
*/
- const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+ const StateMatrix& P() const { return m_P; }
/**
* Returns an element of the error covariance matrix P.
@@ -185,26 +108,26 @@
*
* @param P The error covariance matrix P.
*/
- void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+ void SetP(const StateMatrix& P) { m_P = P; }
/**
* Returns the state estimate x-hat.
*/
- const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+ const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
*
* @param i Row of x-hat.
*/
- double Xhat(int i) const { return m_xHat(i, 0); }
+ double Xhat(int i) const { return m_xHat(i); }
/**
* Set initial state estimate x-hat.
*
* @param xHat The state estimate x-hat.
*/
- void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+ void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -212,7 +135,7 @@
* @param i Row of x-hat.
* @param value Value for element of x-hat.
*/
- void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+ void SetXhat(int i, double value) { m_xHat(i) = value; }
/**
* Resets the observer.
@@ -228,23 +151,7 @@
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
- void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
- // Find continuous A
- Eigen::Matrix<double, States, States> contA =
- NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
-
- // Find discrete A and Q
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, States> discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
- m_xHat = RK4(m_f, m_xHat, u, dt);
-
- // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
- m_P = discA * m_P * discA.transpose() + discQ;
-
- m_dt = dt;
- }
+ void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -252,27 +159,15 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Outputs>& y) {
+ void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
}
template <int Rows>
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Rows>& y,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const Eigen::Matrix<double, Rows, Rows>& R) {
- auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
- return a - b;
- };
- auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a + b;
- };
- Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
- }
+ void Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -291,73 +186,30 @@
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Rows>& y,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const Eigen::Matrix<double, Rows, Rows>& R,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, Rows>&,
- const Eigen::Vector<double, Rows>&)>
- residualFuncY,
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>)>
- addFuncX) {
- const Eigen::Matrix<double, Rows, States> C =
- NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
- const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
-
- Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
-
- // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
- // efficiently.
- //
- // K = PCᵀS⁻¹
- // KS = PCᵀ
- // (KS)ᵀ = (PCᵀ)ᵀ
- // SᵀKᵀ = CPᵀ
- //
- // The solution of Ax = b can be found via x = A.solve(b).
- //
- // Kᵀ = Sᵀ.solve(CPᵀ)
- // K = (Sᵀ.solve(CPᵀ))ᵀ
- Eigen::Matrix<double, States, Rows> K =
- S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
-
- // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
- m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
-
- // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
- m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
- }
+ void Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R,
+ std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ addFuncX);
private:
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- m_f;
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- m_h;
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, Outputs>&,
- const Eigen::Vector<double, Outputs>)>
+ std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+ std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>)>
- m_addFuncX;
- Eigen::Vector<double, States> m_xHat;
- Eigen::Matrix<double, States, States> m_P;
- Eigen::Matrix<double, States, States> m_contQ;
- Eigen::Matrix<double, Outputs, Outputs> m_contR;
+ std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+ StateVector m_xHat;
+ StateMatrix m_P;
+ StateMatrix m_contQ;
+ Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
- Eigen::Matrix<double, States, States> m_initP;
+ StateMatrix m_initP;
};
} // namespace frc
+
+#include "ExtendedKalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
new file mode 100644
index 0000000..a56b8b5
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
@@ -0,0 +1,160 @@
+// 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
+
+#include "Eigen/Cholesky"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/ExtendedKalmanFilter.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ units::second_t dt)
+ : m_f(f), m_h(h) {
+ m_contQ = MakeCovMatrix(stateStdDevs);
+ m_contR = MakeCovMatrix(measurementStdDevs);
+ m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+ m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+ m_dt = dt;
+
+ Reset();
+
+ StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+ m_f, m_xHat, InputVector::Zero());
+ Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+ m_h, m_xHat, InputVector::Zero());
+
+ StateMatrix discA;
+ StateMatrix discQ;
+ DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+ Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
+
+ if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
+ m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
+ discA.transpose(), C.transpose(), discQ, discR);
+ } else {
+ m_initP = StateMatrix::Zero();
+ }
+ m_P = m_initP;
+}
+
+template <int States, int Inputs, int Outputs>
+ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
+ units::second_t dt)
+ : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
+ m_contQ = MakeCovMatrix(stateStdDevs);
+ m_contR = MakeCovMatrix(measurementStdDevs);
+ m_dt = dt;
+
+ Reset();
+
+ StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+ m_f, m_xHat, InputVector::Zero());
+ Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+ m_h, m_xHat, InputVector::Zero());
+
+ StateMatrix discA;
+ StateMatrix discQ;
+ DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+ Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
+
+ if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
+ m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
+ discA.transpose(), C.transpose(), discQ, discR);
+ } else {
+ m_initP = StateMatrix::Zero();
+ }
+ m_P = m_initP;
+}
+
+template <int States, int Inputs, int Outputs>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
+ const InputVector& u, units::second_t dt) {
+ // Find continuous A
+ StateMatrix contA =
+ NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+
+ // Find discrete A and Q
+ StateMatrix discA;
+ StateMatrix discQ;
+ DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+ m_xHat = RK4(m_f, m_xHat, u, dt);
+
+ // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+ m_P = discA * m_P * discA.transpose() + discQ;
+
+ m_dt = dt;
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R) {
+ auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+ auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+ Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R,
+ std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ addFuncX) {
+ const Matrixd<Rows, States> C =
+ NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
+ const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+
+ Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
+
+ // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+ // efficiently.
+ //
+ // K = PCᵀS⁻¹
+ // KS = PCᵀ
+ // (KS)ᵀ = (PCᵀ)ᵀ
+ // SᵀKᵀ = CPᵀ
+ //
+ // The solution of Ax = b can be found via x = A.solve(b).
+ //
+ // Kᵀ = Sᵀ.solve(CPᵀ)
+ // K = (Sᵀ.solve(CPᵀ))ᵀ
+ Matrixd<States, Rows> K =
+ S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
+
+ // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+ m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
+
+ // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+ // Use Joseph form for numerical stability
+ m_P = (StateMatrix::Identity() - K * C) * m_P *
+ (StateMatrix::Identity() - K * C).transpose() +
+ K * discR * K.transpose();
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 3aa4dbd..2121284 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -4,25 +4,14 @@
#pragma once
-#include <frc/fmt/Eigen.h>
-
-#include <cmath>
-#include <string>
-
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
+#include "frc/EigenCore.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
-#include "wpimath/MathShared.h"
namespace frc {
-namespace detail {
/**
* A Kalman filter combines predictions from a model and measurements to give an
@@ -45,8 +34,15 @@
* @tparam Outputs The number of outputs.
*/
template <int States, int Inputs, int Outputs>
-class KalmanFilterImpl {
+class KalmanFilter {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+ using OutputVector = Vectord<Outputs>;
+
+ using StateArray = wpi::array<double, States>;
+ using OutputArray = wpi::array<double, Outputs>;
+
/**
* Constructs a state-space observer with the given plant.
*
@@ -54,65 +50,19 @@
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
+ * @throws std::invalid_argument If the system is unobservable.
*/
- KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- units::second_t dt) {
- m_plant = &plant;
+ KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+ const StateArray& stateStdDevs,
+ const OutputArray& measurementStdDevs, units::second_t dt);
- auto contQ = MakeCovMatrix(stateStdDevs);
- auto contR = MakeCovMatrix(measurementStdDevs);
-
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, States> discQ;
- DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
-
- auto discR = DiscretizeR<Outputs>(contR, dt);
-
- const auto& C = plant.C();
-
- if (!IsDetectable<States, Outputs>(discA, C)) {
- std::string msg = fmt::format(
- "The system passed to the Kalman filter is "
- "unobservable!\n\nA =\n{}\nC =\n{}\n",
- discA, C);
-
- wpi::math::MathSharedStore::ReportError(msg);
- throw std::invalid_argument(msg);
- }
-
- Eigen::Matrix<double, States, States> P =
- drake::math::DiscreteAlgebraicRiccatiEquation(
- discA.transpose(), C.transpose(), discQ, discR);
-
- // S = CPCᵀ + R
- Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
-
- // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
- // efficiently.
- //
- // K = PCᵀS⁻¹
- // KS = PCᵀ
- // (KS)ᵀ = (PCᵀ)ᵀ
- // SᵀKᵀ = CPᵀ
- //
- // The solution of Ax = b can be found via x = A.solve(b).
- //
- // Kᵀ = Sᵀ.solve(CPᵀ)
- // K = (Sᵀ.solve(CPᵀ))ᵀ
- m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
-
- Reset();
- }
-
- KalmanFilterImpl(KalmanFilterImpl&&) = default;
- KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default;
+ KalmanFilter(KalmanFilter&&) = default;
+ KalmanFilter& operator=(KalmanFilter&&) = default;
/**
* Returns the steady-state Kalman gain matrix K.
*/
- const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
+ const Matrixd<States, Outputs>& K() const { return m_K; }
/**
* Returns an element of the steady-state Kalman gain matrix K.
@@ -125,7 +75,7 @@
/**
* Returns the state estimate x-hat.
*/
- const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+ const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -139,7 +89,7 @@
*
* @param xHat The state estimate x-hat.
*/
- void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+ void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -160,9 +110,7 @@
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
- void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
- m_xHat = m_plant->CalculateX(m_xHat, u, dt);
- }
+ void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -170,11 +118,7 @@
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Outputs>& y) {
- // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
- m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
- }
+ void Correct(const InputVector& u, const OutputVector& y);
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -182,66 +126,19 @@
/**
* The steady-state Kalman gain matrix.
*/
- Eigen::Matrix<double, States, Outputs> m_K;
+ Matrixd<States, Outputs> m_K;
/**
* The state estimate.
*/
- Eigen::Vector<double, States> m_xHat;
+ StateVector m_xHat;
};
-} // namespace detail
-
-template <int States, int Inputs, int Outputs>
-class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
- public:
- /**
- * Constructs a state-space observer with the given plant.
- *
- * @param plant The plant used for the prediction step.
- * @param stateStdDevs Standard deviations of model states.
- * @param measurementStdDevs Standard deviations of measurements.
- * @param dt Nominal discretization timestep.
- */
- KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- units::second_t dt)
- : detail::KalmanFilterImpl<States, Inputs, Outputs>{
- plant, stateStdDevs, measurementStdDevs, dt} {}
-
- KalmanFilter(KalmanFilter&&) = default;
- KalmanFilter& operator=(KalmanFilter&&) = default;
-};
-
-// Template specializations are used here to make common state-input-output
-// triplets compile faster.
-template <>
-class WPILIB_DLLEXPORT KalmanFilter<1, 1, 1>
- : public detail::KalmanFilterImpl<1, 1, 1> {
- public:
- KalmanFilter(LinearSystem<1, 1, 1>& plant,
- const wpi::array<double, 1>& stateStdDevs,
- const wpi::array<double, 1>& measurementStdDevs,
- units::second_t dt);
-
- KalmanFilter(KalmanFilter&&) = default;
- KalmanFilter& operator=(KalmanFilter&&) = default;
-};
-
-// Template specializations are used here to make common state-input-output
-// triplets compile faster.
-template <>
-class WPILIB_DLLEXPORT KalmanFilter<2, 1, 1>
- : public detail::KalmanFilterImpl<2, 1, 1> {
- public:
- KalmanFilter(LinearSystem<2, 1, 1>& plant,
- const wpi::array<double, 2>& stateStdDevs,
- const wpi::array<double, 1>& measurementStdDevs,
- units::second_t dt);
-
- KalmanFilter(KalmanFilter&&) = default;
- KalmanFilter& operator=(KalmanFilter&&) = default;
-};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ KalmanFilter<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ KalmanFilter<2, 1, 1>;
} // namespace frc
+
+#include "KalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
new file mode 100644
index 0000000..ca4f37c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
@@ -0,0 +1,86 @@
+// 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
+
+#include <frc/fmt/Eigen.h>
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include "Eigen/Cholesky"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/KalmanFilter.h"
+#include "frc/system/Discretization.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
+ LinearSystem<States, Inputs, Outputs>& plant,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ units::second_t dt) {
+ m_plant = &plant;
+
+ auto contQ = MakeCovMatrix(stateStdDevs);
+ auto contR = MakeCovMatrix(measurementStdDevs);
+
+ Matrixd<States, States> discA;
+ Matrixd<States, States> discQ;
+ DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+
+ auto discR = DiscretizeR<Outputs>(contR, dt);
+
+ const auto& C = plant.C();
+
+ if (!IsDetectable<States, Outputs>(discA, C)) {
+ std::string msg = fmt::format(
+ "The system passed to the Kalman filter is "
+ "unobservable!\n\nA =\n{}\nC =\n{}\n",
+ discA, C);
+
+ wpi::math::MathSharedStore::ReportError(msg);
+ throw std::invalid_argument(msg);
+ }
+
+ Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
+ discA.transpose(), C.transpose(), discQ, discR);
+
+ // S = CPCᵀ + R
+ Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
+
+ // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+ // efficiently.
+ //
+ // K = PCᵀS⁻¹
+ // KS = PCᵀ
+ // (KS)ᵀ = (PCᵀ)ᵀ
+ // SᵀKᵀ = CPᵀ
+ //
+ // The solution of Ax = b can be found via x = A.solve(b).
+ //
+ // Kᵀ = Sᵀ.solve(CPᵀ)
+ // K = (Sᵀ.solve(CPᵀ))ᵀ
+ m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
+
+ Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
+ units::second_t dt) {
+ m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
+ const OutputVector& y) {
+ // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+ m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index 5cb8eb2..d6e4127 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -10,7 +10,7 @@
#include <utility>
#include <vector>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "units/math.h"
#include "units/time.h"
@@ -20,16 +20,15 @@
class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
- Eigen::Vector<double, States> xHat;
- Eigen::Matrix<double, States, States> errorCovariances;
- Eigen::Vector<double, Inputs> inputs;
- Eigen::Vector<double, Outputs> localMeasurements;
+ Vectord<States> xHat;
+ Matrixd<States, States> squareRootErrorCovariances;
+ Vectord<Inputs> inputs;
+ Vectord<Outputs> localMeasurements;
- ObserverSnapshot(const KalmanFilterType& observer,
- const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Outputs>& localY)
+ ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
+ const Vectord<Outputs>& localY)
: xHat(observer.Xhat()),
- errorCovariances(observer.P()),
+ squareRootErrorCovariances(observer.S()),
inputs(u),
localMeasurements(localY) {}
};
@@ -47,10 +46,8 @@
* @param localY The local output at the timestamp
* @param timestamp The timesnap of the state.
*/
- void AddObserverState(const KalmanFilterType& observer,
- Eigen::Vector<double, Inputs> u,
- Eigen::Vector<double, Outputs> localY,
- units::second_t timestamp) {
+ void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
+ Vectord<Outputs> localY, units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -74,10 +71,8 @@
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
- KalmanFilterType* observer, units::second_t nominalDt,
- Eigen::Vector<double, Rows> y,
- std::function<void(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Rows>& y)>
+ KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
+ std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
units::second_t timestamp) {
if (m_pastObserverSnapshots.size() == 0) {
@@ -140,7 +135,7 @@
auto& [key, snapshot] = m_pastObserverSnapshots[i];
if (i == indexOfClosestEntry) {
- observer->SetP(snapshot.errorCovariances);
+ observer->SetS(snapshot.squareRootErrorCovariances);
observer->SetXhat(snapshot.xHat);
}
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index e9817ef..d1967e9 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -9,89 +9,82 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
#include "units/time.h"
namespace frc {
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Mecanum Drive Odometry to fuse latency-compensated
* vision measurements with mecanum drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for MecanumDriveOdometry.
+ * easy drop-in for MecanumDriveOdometry.
*
* Update() should be called every robot loop. If your loops are faster or
- * slower than the default of 0.02s, then you should change the nominal delta
+ * slower than the default of 20 ms, then you should change the nominal delta
* time by specifying it in the constructor.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave mostly like regular encoder
* odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
- * containing x position, y position, and heading.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
public:
/**
- * Constructs a MecanumDrivePoseEstimator.
+ * Constructs a MecanumDrivePoseEstimator with default standard deviations
+ * for the model and vision measurements.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPose The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form [x, y, theta]ᵀ, with units
- * in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro
- * measurements. Increase these numbers to
- * trust sensor readings from encoders
- * and gyros less. This matrix is in the form
- * [theta], with units in radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
- * @param nominalDt The time in seconds between each robot
- * loop.
+ * The default standard deviations of the model states are
+ * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+ * The default standard deviations of the vision measurements are
+ * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distance measured by each wheel.
+ * @param initialPose The starting pose estimate.
*/
- MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- MecanumDriveKinematics kinematics,
- const wpi::array<double, 3>& stateStdDevs,
- const wpi::array<double, 1>& localMeasurementStdDevs,
- const wpi::array<double, 3>& visionMeasurementStdDevs,
- units::second_t nominalDt = 0.02_s);
+ MecanumDrivePoseEstimator(MecanumDriveKinematics& kinematics,
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& initialPose);
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Constructs a MecanumDrivePoseEstimator.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distance measured by each wheel.
+ * @param initialPose The starting pose estimate.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ MecanumDrivePoseEstimator(
+ MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+ /**
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
@@ -99,75 +92,74 @@
/**
* Resets the robot's position on the field.
*
- * <p>You NEED to reset your encoders (to zero) when calling this method.
- *
- * <p>The gyroscope angle does not need to be reset in the user's robot code.
+ * The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The distances measured at each wheel.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& pose);
/**
- * Gets the pose of the robot at the current time as estimated by the Extended
- * Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct
+ * Add a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime() then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the same
- * epoch as Timer#GetFPGATimestamp.) This means
- * that you should use Timer#GetFPGATimestamp as your
- * time source or sync the epochs.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime()
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp().) This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -177,57 +169,79 @@
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+ * @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds);
+ const MecanumDriveWheelPositions& wheelPositions);
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+ * @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds);
+ const MecanumDriveWheelPositions& wheelPositions);
private:
- UnscentedKalmanFilter<3, 3, 1> m_observer;
- MecanumDriveKinematics m_kinematics;
- KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
- m_latencyCompensator;
- std::function<void(const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y)>
- m_visionCorrect;
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
- Eigen::Matrix3d m_visionContR;
+ // The current gyroscope angle.
+ Rotation2d gyroAngle;
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
+ // The distances measured at each wheel.
+ MecanumDriveWheelPositions wheelPositions;
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ /**
+ * Checks equality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const InterpolationRecord& other) const = default;
- template <int Dim>
- static wpi::array<double, Dim> StdDevMatrixToArray(
- const Eigen::Vector<double, Dim>& vector) {
- wpi::array<double, Dim> array;
- for (size_t i = 0; i < Dim; ++i) {
- array[i] = vector(i);
- }
- return array;
- }
+ /**
+ * Checks inequality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const InterpolationRecord& other) const = default;
+
+ /**
+ * Interpolates between two InterpolationRecords.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ *
+ * @return The interpolated state.
+ */
+ InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
+ InterpolationRecord endValue,
+ double i) const;
+ };
+
+ MecanumDriveKinematics& m_kinematics;
+ MecanumDriveOdometry m_odometry;
+ wpi::array<double, 3> m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index 42f5593..b2e0e45 100644
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -6,8 +6,7 @@
#include <cmath>
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
namespace frc {
@@ -52,24 +51,23 @@
/**
* Computes the sigma points for an unscented Kalman filter given the mean
- * (x) and covariance(P) of the filter.
+ * (x) and square-root covariance(S) of the filter.
*
* @param x An array of the means.
- * @param P Covariance of the filter.
+ * @param S Square-root covariance of the filter.
*
* @return Two dimensional array of sigma points. Each column contains all of
* the sigmas for one dimension in the problem space. Ordered by
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*
*/
- Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
- const Eigen::Vector<double, States>& x,
- const Eigen::Matrix<double, States, States>& P) {
+ Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
+ const Vectord<States>& x, const Matrixd<States, States>& S) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
- Eigen::Matrix<double, States, States> U =
- ((lambda + States) * P).llt().matrixL();
+ double eta = std::sqrt(lambda + States);
+ Matrixd<States, States> U = eta * S;
- Eigen::Matrix<double, States, 2 * States + 1> sigmas;
+ Matrixd<States, 2 * States + 1> sigmas;
sigmas.template block<States, 1>(0, 0) = x;
for (int k = 0; k < States; ++k) {
sigmas.template block<States, 1>(0, k + 1) =
@@ -84,7 +82,7 @@
/**
* Returns the weight for each sigma point for the mean.
*/
- const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
+ const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
/**
* Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +94,7 @@
/**
* Returns the weight for each sigma point for the covariance.
*/
- const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
+ const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
/**
* Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +104,8 @@
double Wc(int i) const { return m_Wc(i, 0); }
private:
- Eigen::Vector<double, 2 * States + 1> m_Wm;
- Eigen::Vector<double, 2 * States + 1> m_Wc;
+ Vectord<2 * States + 1> m_Wm;
+ Vectord<2 * States + 1> m_Wc;
double m_alpha;
int m_kappa;
@@ -120,8 +118,8 @@
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
double c = 0.5 / (States + lambda);
- m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
- m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
+ m_Wm = Vectord<2 * States + 1>::Constant(c);
+ m_Wc = Vectord<2 * States + 1>::Constant(c);
m_Wm(0) = lambda / (States + lambda);
m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 7422a3c..d07bafe 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -4,224 +4,240 @@
#pragma once
-#include <limits>
+#include <cmath>
+#include <fmt/format.h>
+#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/timestamp.h>
-#include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
#include "units/time.h"
namespace frc {
+
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
- * vision measurements with swerve drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for SwerveDriveOdometry.
+ * This class wraps Swerve Drive Odometry to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is
+ * intended to be a drop-in for SwerveDriveOdometry.
*
- * Update() should be called every robot loop. If your loops are faster or
- * slower than the default of 0.02s, then you should change the nominal delta
- * time by specifying it in the constructor.
+ * Update() should be called every robot loop.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
- * never call it, then this class will behave mostly like regular encoder
+ * never call it, then this class will behave as regular encoder
* odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
- * containing x position, y position, and heading.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
public:
/**
- * Constructs a SwerveDrivePoseEstimator.
+ * Constructs a SwerveDrivePoseEstimator with default standard deviations
+ * for the model and vision measurements.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPose The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
- * @param stateStdDevs Standard deviations of model states.
- * Increase these numbers to trust your
- * model's state estimates less. This matrix
- * is in the form [x, y, theta]ᵀ, with units
- * in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro
- * measurements. Increase these numbers to
- * trust sensor readings from encoders
- * and gyros less. This matrix is in the form
- * [theta], with units in radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
- * @param nominalDt The time in seconds between each robot
- * loop.
+ * The default standard deviations of the model states are
+ * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+ * The default standard deviations of the vision measurements are
+ * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @param initialPose The starting pose estimate.
*/
SwerveDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
SwerveDriveKinematics<NumModules>& kinematics,
- const wpi::array<double, 3>& stateStdDevs,
- const wpi::array<double, 1>& localMeasurementStdDevs,
- const wpi::array<double, 3>& visionMeasurementStdDevs,
- units::second_t nominalDt = 0.02_s)
- : m_observer([](const Eigen::Vector<double, 3>& x,
- const Eigen::Vector<double, 3>& u) { return u; },
- [](const Eigen::Vector<double, 3>& x,
- const Eigen::Vector<double, 3>& u) {
- return x.block<1, 1>(2, 0);
- },
- stateStdDevs, localMeasurementStdDevs,
- frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
- frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
- frc::AngleAdd<3>(2), nominalDt),
- m_kinematics(kinematics),
- m_nominalDt(nominalDt) {
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& initialPose)
+ : SwerveDrivePoseEstimator{kinematics, gyroAngle,
+ modulePositions, initialPose,
+ {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {}
+
+ /**
+ * Constructs a SwerveDrivePoseEstimator.
+ *
+ * @param kinematics A correctly-configured kinematics object for your
+ * drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @param initialPose The starting pose estimate.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in
+ * meters, y position in meters, and heading in radians). Increase these
+ * numbers to trust your state estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
+ */
+ SwerveDrivePoseEstimator(
+ SwerveDriveKinematics<NumModules>& kinematics,
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
+ const wpi::array<double, 3>& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
+
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- // Create correction mechanism for vision measurements.
- m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y) {
- m_observer.Correct<3>(
- u, y,
- [](const Eigen::Vector<double, 3>& x,
- const Eigen::Vector<double, 3>& u) { return x; },
- m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
- };
-
- // Set initial state.
- m_observer.SetXhat(PoseTo3dVector(initialPose));
-
- // Calculate offsets.
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
}
/**
* Resets the robot's position on the field.
*
- * You NEED to reset your encoders (to zero) when calling this method.
- *
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
- * @param gyroAngle The angle reported by the gyroscope.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+ void ResetPosition(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
- m_latencyCompensator.Reset();
-
- m_observer.SetXhat(PoseTo3dVector(pose));
-
- m_gyroOffset = pose.Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
+ m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
+ m_poseBuffer.Clear();
}
/**
- * Gets the pose of the robot at the current time as estimated by the Extended
- * Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
- Pose2d GetEstimatedPosition() const {
- return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
- Rotation2d(units::radian_t{m_observer.Xhat(2)}));
- }
+ Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
- * Sets the pose estimator's trust of global measurements. This might be used
+ * Sets the pose estimator's trust in vision measurements. This might be used
* to change trust in vision measurements after the autonomous period, or to
* change trust as distance to a vision target increases.
*
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs);
+ wpi::array<double, 3> r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the
+ * odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* @param visionRobotPose The pose of the robot as measured by the vision
- * camera.
- * @param timestamp The timestamp of the vision measurement in seconds.
- * Note that if you don't use your own time source by
- * calling UpdateWithTime() then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the same
- * epoch as Timer#GetFPGATimestamp.) This means
- * that you should use Timer#GetFPGATimestamp as your
- * time source or sync the epochs.
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime()
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp().) This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
- m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
- &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
- m_visionCorrect, timestamp);
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: We should not trust the twist entirely, so instead we scale this
+ // twist by a Kalman gain matrix representing how much we trust vision
+ // measurements compared to our current pose.
+ frc::Vectord<3> k_times_twist =
+ m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
+ twist.dtheta.value()};
+
+ // Step 4: Convert back to Twist2d
+ Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+ units::meter_t{k_times_twist(1)},
+ units::radian_t{k_times_twist(2)}};
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.ResetPosition(sample.value().gyroAngle,
+ sample.value().modulePostions,
+ sample.value().pose.Exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded
+ // sample to update the pose buffer and correct odometry.
+ auto internal_buf = m_poseBuffer.GetInternalBuffer();
+
+ auto upper_bound = std::lower_bound(
+ internal_buf.begin(), internal_buf.end(), timestamp,
+ [](const auto& pair, auto t) { return t > pair.first; });
+
+ for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
+ UpdateWithTime(entry->first, entry->second.gyroAngle,
+ entry->second.modulePostions);
+ }
}
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the
+ * odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
*
+ * To promote stability of the pose estimate and make it robust to bad vision
+ * data, we recommend only adding vision measurements that are already within
+ * one meter or so of the current pose estimate.
+ *
* Note that the vision measurement standard deviations passed into this
* method will continue to apply to future measurements until a subsequent
* call to SetVisionMeasurementStdDevs() or this method.
*
- * @param visionRobotPose The pose of the robot as measured by the
- * vision camera.
- * @param timestamp The timestamp of the vision measurement in
- * seconds. Note that if you don't use your
- * own time source by calling
- * UpdateWithTime(), then you must use a
- * timestamp with an epoch since FPGA startup
- * (i.e. the epoch of this timestamp is the
- * same epoch as
- * frc::Timer::GetFPGATimestamp(). This means
- * that you should use
- * frc::Timer::GetFPGATimestamp() as your
- * time source in this case.
- * @param visionMeasurementStdDevs Standard deviations of the vision
- * measurements. Increase these numbers to
- * trust global measurements from vision
- * less. This matrix is in the form
- * [x, y, theta]ᵀ, with units in meters and
- * radians.
+ * @param visionRobotPose The pose of the robot as measured by the vision
+ * camera.
+ * @param timestamp The timestamp of the vision measurement in seconds. Note
+ * that if you don't use your own time source by calling UpdateWithTime(),
+ * then you must use a timestamp with an epoch since FPGA startup (i.e.,
+ * the epoch of this timestamp is the same epoch as
+ * frc::Timer::GetFPGATimestamp(). This means that you should use
+ * frc::Timer::GetFPGATimestamp() as your time source in this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose
+ * measurement (x position in meters, y position in meters, and heading in
+ * radians). Increase these numbers to trust the vision pose measurement
+ * less.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
@@ -231,87 +247,140 @@
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
- * @param gyroAngle The current gyro angle.
- * @param moduleStates The current velocities and rotations of the swerve
- * modules.
- * @return The estimated pose of the robot in meters.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of
+ * the swerve modules.
+ * @return The estimated robot pose in meters.
*/
- template <typename... ModuleState>
- Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
+ Pose2d Update(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- moduleStates...);
+ modulePositions);
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the pose estimator with wheel encoder and gyro information. This
+ * should be called every loop.
*
- * @param currentTime Time at which this method was called, in seconds.
- * @param gyroAngle The current gyroscope angle.
- * @param moduleStates The current velocities and rotations of the swerve
- * modules.
- * @return The estimated pose of the robot in meters.
+ * @param currentTime Time at which this method was called, in seconds.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance traveled and rotations of
+ * the swerve modules.
+ * @return The estimated robot pose in meters.
*/
- template <typename... ModuleState>
- Pose2d UpdateWithTime(units::second_t currentTime,
- const Rotation2d& gyroAngle,
- ModuleState&&... moduleStates) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ Pose2d UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+ m_odometry.Update(gyroAngle, modulePositions);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (angle - m_previousAngle).Radians() / dt;
+ wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
+ wpi::empty_array};
- auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
- auto fieldRelativeSpeeds =
- Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
- .RotateBy(angle);
+ for (size_t i = 0; i < NumModules; i++) {
+ internalModulePositions[i].distance = modulePositions[i].distance;
+ internalModulePositions[i].angle = modulePositions[i].angle;
+ }
- Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
- fieldRelativeSpeeds.Y().value(), omega.value()};
-
- Eigen::Vector<double, 1> localY{angle.Radians().value()};
- m_previousAngle = angle;
-
- m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+ internalModulePositions});
return GetEstimatedPosition();
}
private:
- UnscentedKalmanFilter<3, 3, 1> m_observer;
- SwerveDriveKinematics<NumModules>& m_kinematics;
- KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
- m_latencyCompensator;
- std::function<void(const Eigen::Vector<double, 3>& u,
- const Eigen::Vector<double, 3>& y)>
- m_visionCorrect;
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
- Eigen::Matrix3d m_visionContR;
+ // The current gyroscope angle.
+ Rotation2d gyroAngle;
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
+ // The distances traveled and rotations meaured at each module.
+ wpi::array<SwerveModulePosition, NumModules> modulePostions;
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ /**
+ * Checks equality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const InterpolationRecord& other) const = default;
- template <int Dim>
- static wpi::array<double, Dim> StdDevMatrixToArray(
- const Eigen::Vector<double, Dim>& vector) {
- wpi::array<double, Dim> array;
- for (size_t i = 0; i < Dim; ++i) {
- array[i] = vector(i);
+ /**
+ * Checks inequality between this InterpolationRecord and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const InterpolationRecord& other) const = default;
+
+ /**
+ * Interpolates between two InterpolationRecords.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ *
+ * @return The interpolated state.
+ */
+ InterpolationRecord Interpolate(
+ SwerveDriveKinematics<NumModules>& kinematics,
+ InterpolationRecord endValue, double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the new module distances.
+ wpi::array<SwerveModulePosition, NumModules> modulePositions{
+ wpi::empty_array};
+ // Find the distance between this measurement and the
+ // interpolated measurement.
+ wpi::array<SwerveModulePosition, NumModules> modulesDelta{
+ wpi::empty_array};
+
+ for (size_t i = 0; i < NumModules; i++) {
+ modulePositions[i].distance =
+ wpi::Lerp(this->modulePostions[i].distance,
+ endValue.modulePostions[i].distance, i);
+ modulePositions[i].angle =
+ wpi::Lerp(this->modulePostions[i].angle,
+ endValue.modulePostions[i].angle, i);
+
+ modulesDelta[i].distance =
+ modulePositions[i].distance - this->modulePostions[i].distance;
+ modulesDelta[i].angle = modulePositions[i].angle;
+ }
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist = kinematics.ToTwist2d(modulesDelta);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, modulePositions};
+ }
}
- return array;
- }
+ };
+
+ SwerveDriveKinematics<NumModules>& m_kinematics;
+ SwerveDriveOdometry<NumModules> m_odometry;
+ wpi::array<double, 3> m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ SwerveDrivePoseEstimator<4>;
+
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 3aa3e59..39ce615 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -6,16 +6,11 @@
#include <functional>
+#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
+#include "frc/EigenCore.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
-#include "frc/estimator/UnscentedTransform.h"
-#include "frc/system/Discretization.h"
-#include "frc/system/NumericalIntegration.h"
-#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
namespace frc {
@@ -40,6 +35,10 @@
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
* "Stochastic control theory".
*
+ * <p> This class implements a square-root-form unscented Kalman filter
+ * (SR-UKF). For more information about the SR-UKF, see
+ * https://www.researchgate.net/publication/3908304.
+ *
* @tparam States The number of states.
* @tparam Inputs The number of inputs.
* @tparam Outputs The number of outputs.
@@ -47,6 +46,15 @@
template <int States, int Inputs, int Outputs>
class UnscentedKalmanFilter {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+ using OutputVector = Vectord<Outputs>;
+
+ using StateArray = wpi::array<double, States>;
+ using OutputArray = wpi::array<double, Outputs>;
+
+ using StateMatrix = Matrixd<States, States>;
+
/**
* Constructs an unscented Kalman filter.
*
@@ -58,39 +66,11 @@
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
- UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- f,
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- units::second_t dt)
- : m_f(f), m_h(h) {
- m_contQ = MakeCovMatrix(stateStdDevs);
- m_contR = MakeCovMatrix(measurementStdDevs);
- m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
- return sigmas * Wm;
- };
- m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
- return sigmas * Wc;
- };
- m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a - b;
- };
- m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
- return a - b;
- };
- m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a + b;
- };
- m_dt = dt;
-
- Reset();
- }
+ UnscentedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ units::second_t dt);
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
@@ -117,89 +97,74 @@
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(
- std::function<
- Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- f,
- std::function<
- Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const wpi::array<double, States>& stateStdDevs,
- const wpi::array<double, Outputs>& measurementStdDevs,
- std::function<Eigen::Vector<double, States>(
- const Eigen::Matrix<double, States, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
meanFuncX,
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
+ std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
meanFuncY,
- std::function<
- Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>&)>
+ std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
- std::function<
- Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
- const Eigen::Vector<double, Outputs>&)>
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
- std::function<
- Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>&)>
+ std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
- units::second_t dt)
- : m_f(f),
- m_h(h),
- m_meanFuncX(meanFuncX),
- m_meanFuncY(meanFuncY),
- m_residualFuncX(residualFuncX),
- m_residualFuncY(residualFuncY),
- m_addFuncX(addFuncX) {
- m_contQ = MakeCovMatrix(stateStdDevs);
- m_contR = MakeCovMatrix(measurementStdDevs);
- m_dt = dt;
-
- Reset();
- }
+ units::second_t dt);
/**
- * Returns the error covariance matrix P.
+ * Returns the square-root error covariance matrix S.
*/
- const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+ const StateMatrix& S() const { return m_S; }
/**
- * Returns an element of the error covariance matrix P.
+ * Returns an element of the square-root error covariance matrix S.
*
- * @param i Row of P.
- * @param j Column of P.
+ * @param i Row of S.
+ * @param j Column of S.
*/
- double P(int i, int j) const { return m_P(i, j); }
+ double S(int i, int j) const { return m_S(i, j); }
/**
- * Set the current error covariance matrix P.
+ * Set the current square-root error covariance matrix S.
+ *
+ * @param S The square-root error covariance matrix S.
+ */
+ void SetS(const StateMatrix& S) { m_S = S; }
+
+ /**
+ * Returns the reconstructed error covariance matrix P.
+ */
+ StateMatrix P() const { return m_S.transpose() * m_S; }
+
+ /**
+ * Set the current square-root error covariance matrix S by taking the square
+ * root of P.
*
* @param P The error covariance matrix P.
*/
- void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+ void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
/**
* Returns the state estimate x-hat.
*/
- const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+ const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
*
* @param i Row of x-hat.
*/
- double Xhat(int i) const { return m_xHat(i, 0); }
+ double Xhat(int i) const { return m_xHat(i); }
/**
* Set initial state estimate x-hat.
*
* @param xHat The state estimate x-hat.
*/
- void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+ void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -207,14 +172,14 @@
* @param i Row of x-hat.
* @param value Value for element of x-hat.
*/
- void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+ void SetXhat(int i, double value) { m_xHat(i) = value; }
/**
* Resets the observer.
*/
void Reset() {
m_xHat.setZero();
- m_P.setZero();
+ m_S.setZero();
m_sigmasF.setZero();
}
@@ -224,31 +189,7 @@
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
- void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
- m_dt = dt;
-
- // Discretize Q before projecting mean and covariance forward
- Eigen::Matrix<double, States, States> contA =
- NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, States> discQ;
- DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
- Eigen::Matrix<double, States, 2 * States + 1> sigmas =
- m_pts.SigmaPoints(m_xHat, m_P);
-
- for (int i = 0; i < m_pts.NumSigmas(); ++i) {
- Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
- m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
- }
-
- auto ret = UnscentedTransform<States, States>(
- m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
- m_xHat = std::get<0>(ret);
- m_P = std::get<1>(ret);
-
- m_P += discQ;
- }
+ void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -256,8 +197,7 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Outputs>& y) {
+ void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
m_residualFuncX, m_addFuncX);
}
@@ -276,28 +216,10 @@
* @param R Measurement noise covariance matrix (continuous-time).
*/
template <int Rows>
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Rows>& y,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const Eigen::Matrix<double, Rows, Rows>& R) {
- auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
- return sigmas * Wc;
- };
- auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a - b;
- };
- auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
- return a - b;
- };
- auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
- return a + b;
- };
- Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX,
- addFuncX);
- }
+ void Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -320,109 +242,49 @@
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
- void Correct(const Eigen::Vector<double, Inputs>& u,
- const Eigen::Vector<double, Rows>& y,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- h,
- const Eigen::Matrix<double, Rows, Rows>& R,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Matrix<double, Rows, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
- meanFuncY,
- std::function<Eigen::Vector<double, Rows>(
- const Eigen::Vector<double, Rows>&,
- const Eigen::Vector<double, Rows>&)>
- residualFuncY,
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>&)>
- residualFuncX,
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>)>
- addFuncX) {
- const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
-
- // Transform sigma points into measurement space
- Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
- Eigen::Matrix<double, States, 2 * States + 1> sigmas =
- m_pts.SigmaPoints(m_xHat, m_P);
- for (int i = 0; i < m_pts.NumSigmas(); ++i) {
- sigmasH.template block<Rows, 1>(0, i) =
- h(sigmas.template block<States, 1>(0, i), u);
- }
-
- // Mean and covariance of prediction passed through UT
- auto [yHat, Py] = UnscentedTransform<Rows, States>(
- sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
- Py += discR;
-
- // Compute cross covariance of the state and the measurements
- Eigen::Matrix<double, States, Rows> Pxy;
- Pxy.setZero();
- for (int i = 0; i < m_pts.NumSigmas(); ++i) {
- // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
- Pxy +=
- m_pts.Wc(i) *
- (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
- (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
- .transpose();
- }
-
- // K = P_{xy} P_y⁻¹
- // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
- // P_yᵀKᵀ = P_{xy}ᵀ
- // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
- // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
- Eigen::Matrix<double, States, Rows> K =
- Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
-
- // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
- m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
-
- // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
- m_P -= K * Py * K.transpose();
- }
+ void Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R,
+ std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
+ meanFuncY,
+ std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ residualFuncX,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ addFuncX);
private:
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- m_f;
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, Inputs>&)>
- m_h;
- std::function<Eigen::Vector<double, States>(
- const Eigen::Matrix<double, States, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
+ std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+ std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+ std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
m_meanFuncX;
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
+ std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
m_meanFuncY;
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>&)>
+ std::function<StateVector(const StateVector&, const StateVector&)>
m_residualFuncX;
- std::function<Eigen::Vector<double, Outputs>(
- const Eigen::Vector<double, Outputs>&,
- const Eigen::Vector<double, Outputs>)>
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
- std::function<Eigen::Vector<double, States>(
- const Eigen::Vector<double, States>&,
- const Eigen::Vector<double, States>)>
- m_addFuncX;
- Eigen::Vector<double, States> m_xHat;
- Eigen::Matrix<double, States, States> m_P;
- Eigen::Matrix<double, States, States> m_contQ;
- Eigen::Matrix<double, Outputs, Outputs> m_contR;
- Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
+ std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+ StateVector m_xHat;
+ StateMatrix m_S;
+ StateMatrix m_contQ;
+ Matrixd<Outputs, Outputs> m_contR;
+ Matrixd<States, 2 * States + 1> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;
};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ UnscentedKalmanFilter<3, 3, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ UnscentedKalmanFilter<5, 3, 3>;
+
} // namespace frc
+
+#include "UnscentedKalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
new file mode 100644
index 0000000..a6744bf
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -0,0 +1,174 @@
+// 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
+
+#include "Eigen/Cholesky"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/estimator/UnscentedTransform.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ units::second_t dt)
+ : m_f(f), m_h(h) {
+ m_contQ = MakeCovMatrix(stateStdDevs);
+ m_contR = MakeCovMatrix(measurementStdDevs);
+ m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
+ m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
+ return sigmas * Wc;
+ };
+ m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+ m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+ m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+ m_dt = dt;
+
+ Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
+ std::function<StateVector(const StateVector&, const InputVector&)> f,
+ std::function<OutputVector(const StateVector&, const InputVector&)> h,
+ const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+ std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
+ meanFuncX,
+ std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
+ meanFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ residualFuncX,
+ std::function<OutputVector(const OutputVector&, const OutputVector&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
+ units::second_t dt)
+ : m_f(f),
+ m_h(h),
+ m_meanFuncX(meanFuncX),
+ m_meanFuncY(meanFuncY),
+ m_residualFuncX(residualFuncX),
+ m_residualFuncY(residualFuncY),
+ m_addFuncX(addFuncX) {
+ m_contQ = MakeCovMatrix(stateStdDevs);
+ m_contR = MakeCovMatrix(measurementStdDevs);
+ m_dt = dt;
+
+ Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
+ const InputVector& u, units::second_t dt) {
+ m_dt = dt;
+
+ // Discretize Q before projecting mean and covariance forward
+ StateMatrix contA =
+ NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+ StateMatrix discA;
+ StateMatrix discQ;
+ DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+ Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
+
+ Matrixd<States, 2 * States + 1> sigmas =
+ m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+
+ for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+ StateVector x = sigmas.template block<States, 1>(0, i);
+ m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
+ }
+
+ auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
+ m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
+ discQ.template triangularView<Eigen::Lower>());
+ m_xHat = xHat;
+ m_S = S;
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R) {
+ auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
+ return sigmas * Wc;
+ };
+ auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+ auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+ auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+ Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
+ const InputVector& u, const Vectord<Rows>& y,
+ std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+ const Matrixd<Rows, Rows>& R,
+ std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
+ meanFuncY,
+ std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+ residualFuncY,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ residualFuncX,
+ std::function<StateVector(const StateVector&, const StateVector&)>
+ addFuncX) {
+ Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+ Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
+
+ // Transform sigma points into measurement space
+ Matrixd<Rows, 2 * States + 1> sigmasH;
+ Matrixd<States, 2 * States + 1> sigmas =
+ m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+ for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+ sigmasH.template block<Rows, 1>(0, i) =
+ h(sigmas.template block<States, 1>(0, i), u);
+ }
+
+ // Mean and covariance of prediction passed through UT
+ auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
+ sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
+ discR.template triangularView<Eigen::Lower>());
+
+ // Compute cross covariance of the state and the measurements
+ Matrixd<States, Rows> Pxy;
+ Pxy.setZero();
+ for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+ // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
+ Pxy += m_pts.Wc(i) *
+ (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
+ (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
+ .transpose();
+ }
+
+ // K = (P_{xy} / S_yᵀ) / S_y
+ // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+ // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+ Matrixd<States, Rows> K =
+ Sy.transpose()
+ .fullPivHouseholderQr()
+ .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
+ .transpose();
+
+ // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+ m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
+
+ Matrixd<States, Rows> U = K * Sy;
+ for (int i = 0; i < Rows; i++) {
+ Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+ m_S, U.template block<States, 1>(0, i), -1);
+ }
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index 2df0a47..e28f094 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -6,15 +6,17 @@
#include <tuple>
-#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/EigenCore.h"
namespace frc {
/**
* Computes unscented transform of a set of sigma points and weights. CovDim
- * returns the mean and covariance in a tuple.
+ * returns the mean and square-root covariance of the sigma points in a tuple.
*
- * This works in conjunction with the UnscentedKalmanFilter class.
+ * This works in conjunction with the UnscentedKalmanFilter class. For use with
+ * square-root form UKFs.
*
* @tparam CovDim Dimension of covariance of sigma points after passing
* through the transform.
@@ -26,41 +28,48 @@
* vectors using a given set of weights.
* @param residualFunc A function that computes the residual of two state
* vectors (i.e. it subtracts them.)
+ * @param squareRootR Square-root of the noise covaraince of the sigma points.
*
- * @return Tuple of x, mean of sigma points; P, covariance of sigma points after
- * passing through the transform.
+ * @return Tuple of x, mean of sigma points; S, square-root covariance of
+ * sigmas.
*/
template <int CovDim, int States>
-std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
-UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
- const Eigen::Vector<double, 2 * States + 1>& Wm,
- const Eigen::Vector<double, 2 * States + 1>& Wc,
- std::function<Eigen::Vector<double, CovDim>(
- const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
- const Eigen::Vector<double, 2 * States + 1>&)>
- meanFunc,
- std::function<Eigen::Vector<double, CovDim>(
- const Eigen::Vector<double, CovDim>&,
- const Eigen::Vector<double, CovDim>&)>
- residualFunc) {
+std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
+SquareRootUnscentedTransform(
+ const Matrixd<CovDim, 2 * States + 1>& sigmas,
+ const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
+ std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+ const Vectord<2 * States + 1>&)>
+ meanFunc,
+ std::function<Vectord<CovDim>(const Vectord<CovDim>&,
+ const Vectord<CovDim>&)>
+ residualFunc,
+ const Matrixd<CovDim, CovDim>& squareRootR) {
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
- Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
+ Vectord<CovDim> x = meanFunc(sigmas, Wm);
- // New covariance is the sum of the outer product of the residuals times the
- // weights
- Eigen::Matrix<double, CovDim, 2 * States + 1> y;
- for (int i = 0; i < 2 * States + 1; ++i) {
- // y[:, i] = sigmas[:, i] - x
- y.template block<CovDim, 1>(0, i) =
- residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
+ Matrixd<CovDim, States * 2 + CovDim> Sbar;
+ for (int i = 0; i < States * 2; i++) {
+ Sbar.template block<CovDim, 1>(0, i) =
+ std::sqrt(Wc[1]) *
+ residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
}
- Eigen::Matrix<double, CovDim, CovDim> P =
- y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
+ Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
- return std::make_tuple(x, P);
+ // Merwe defines the QR decomposition as Aᵀ = QR
+ Matrixd<CovDim, CovDim> S = Sbar.transpose()
+ .householderQr()
+ .matrixQR()
+ .template block<CovDim, CovDim>(0, 0)
+ .template triangularView<Eigen::Upper>();
+
+ Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+ S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
+
+ return std::make_tuple(x, S);
}
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h
index 7307102..a929f37 100644
--- a/wpimath/src/main/native/include/frc/filter/Debouncer.h
+++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h
@@ -4,6 +4,7 @@
#pragma once
+#include <wpi/SymbolExports.h>
#include <wpi/timestamp.h>
#include "units/time.h"
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 6295327..0fb4f48 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -7,15 +7,15 @@
#include <algorithm>
#include <cmath>
#include <initializer_list>
+#include <span>
#include <stdexcept>
#include <vector>
#include <wpi/array.h>
#include <wpi/circular_buffer.h>
-#include <wpi/span.h>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -80,7 +80,7 @@
* @param ffGains The "feedforward" or FIR gains.
* @param fbGains The "feedback" or IIR gains.
*/
- LinearFilter(wpi::span<const double> ffGains, wpi::span<const double> fbGains)
+ LinearFilter(std::span<const double> ffGains, std::span<const double> fbGains)
: m_inputs(ffGains.size()),
m_outputs(fbGains.size()),
m_inputGains(ffGains.begin(), ffGains.end()),
@@ -157,6 +157,7 @@
*
* @param taps The number of samples to average over. Higher = smoother but
* slower
+ * @throws std::runtime_error if number of taps is less than 1.
*/
static LinearFilter<T> MovingAverage(int taps) {
if (taps <= 0) {
@@ -175,12 +176,12 @@
* difference. 0 is the current sample, -1 is the previous sample, -2 is the
* sample before that, etc. Don't use positive stencil points (samples from
* the future) if the LinearFilter will be used for stream-based online
- * filtering.
+ * filtering (e.g., taking derivative of encoder samples in real-time).
*
* @tparam Derivative The order of the derivative to compute.
* @tparam Samples The number of samples to use to compute the given
* derivative. This must be one more than the order of
- * derivative or higher.
+ * the derivative or higher.
* @param stencil List of stencil points.
* @param period The period in seconds between samples taken by the user.
*/
@@ -209,7 +210,7 @@
static_assert(Derivative < Samples,
"Order of derivative must be less than number of samples.");
- Eigen::Matrix<double, Samples, Samples> S;
+ Matrixd<Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
S(row, col) = std::pow(stencil[col], row);
@@ -217,12 +218,12 @@
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
- Eigen::Vector<double, Samples> d;
+ Vectord<Samples> d;
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
}
- Eigen::Vector<double, Samples> a =
+ Vectord<Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list
diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index f99c1af..542cd94 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -6,6 +6,7 @@
#include <algorithm>
+#include <wpi/deprecated.h>
#include <wpi/timestamp.h>
#include "units/time.h"
@@ -28,15 +29,45 @@
using Rate_t = units::unit_t<Rate>;
/**
- * Creates a new SlewRateLimiter with the given rate limit and initial value.
+ * Creates a new SlewRateLimiter with the given positive and negative rate
+ * limits and initial value.
+ *
+ * @param positiveRateLimit The rate-of-change limit in the positive
+ * direction, in units per second. This is expected
+ * to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative
+ * direction, in units per second. This is expected
+ * to be negative.
+ * @param initialValue The initial value of the input.
+ */
+ SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit,
+ Unit_t initialValue = Unit_t{0})
+ : m_positiveRateLimit{positiveRateLimit},
+ m_negativeRateLimit{negativeRateLimit},
+ m_prevVal{initialValue},
+ m_prevTime{units::microsecond_t(wpi::Now())} {}
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and
+ * negative rate limit of -rateLimit.
+ *
+ * @param rateLimit The rate-of-change limit.
+ */
+ explicit SlewRateLimiter(Rate_t rateLimit)
+ : SlewRateLimiter(rateLimit, -rateLimit) {}
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and
+ * negative rate limit of -rateLimit and initial value.
*
* @param rateLimit The rate-of-change limit.
* @param initialValue The initial value of the input.
*/
- explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
- : m_rateLimit{rateLimit},
- m_prevVal{initialValue},
- m_prevTime{units::microsecond_t(wpi::Now())} {}
+ WPI_DEPRECATED(
+ "Use SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, "
+ "Unit_t initalValue) instead")
+ SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue)
+ : SlewRateLimiter(rateLimit, -rateLimit, initialValue) {}
/**
* Filters the input to limit its slew rate.
@@ -48,8 +79,9 @@
Unit_t Calculate(Unit_t input) {
units::second_t currentTime = units::microsecond_t(wpi::Now());
units::second_t elapsedTime = currentTime - m_prevTime;
- m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime,
- m_rateLimit * elapsedTime);
+ m_prevVal +=
+ std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
+ m_positiveRateLimit * elapsedTime);
m_prevTime = currentTime;
return m_prevVal;
}
@@ -66,7 +98,8 @@
}
private:
- Rate_t m_rateLimit;
+ Rate_t m_positiveRateLimit;
+ Rate_t m_negativeRateLimit;
Unit_t m_prevVal;
units::second_t m_prevTime;
};
diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
index f6cc7b6..c6b2ee6 100644
--- a/wpimath/src/main/native/include/frc/fmt/Eigen.h
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -7,9 +7,10 @@
#include <fmt/format.h>
#include "Eigen/Core"
+#include "Eigen/SparseCore"
/**
- * Formatter for Eigen::Matrix<>.
+ * Formatter for Eigen::Matrix<double, Rows, Cols>.
*
* @tparam Rows Number of rows.
* @tparam Cols Number of columns.
@@ -51,12 +52,68 @@
auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
FormatContext& ctx) {
auto out = ctx.out();
- for (int i = 0; i < Rows; ++i) {
- for (int j = 0; j < Cols; ++j) {
+ for (int i = 0; i < mat.rows(); ++i) {
+ for (int j = 0; j < mat.cols(); ++j) {
out = fmt::format_to(out, " {:f}", mat(i, j));
}
- if (i < Rows - 1) {
+ if (i < mat.rows() - 1) {
+ out = fmt::format_to(out, "\n");
+ }
+ }
+
+ return out;
+ }
+};
+
+/**
+ * Formatter for Eigen::SparseMatrix<double>.
+ *
+ * @tparam Options Union of bit flags controlling the storage scheme.
+ * @tparam StorageIndex The type of the indices.
+ */
+template <int Options, typename StorageIndex>
+struct fmt::formatter<Eigen::SparseMatrix<double, Options, StorageIndex>> {
+ /**
+ * Storage for format specifier.
+ */
+ char presentation = 'f';
+
+ /**
+ * Format string parser.
+ *
+ * @param ctx Format string context.
+ */
+ constexpr auto parse(fmt::format_parse_context& ctx) {
+ auto it = ctx.begin(), end = ctx.end();
+ if (it != end && (*it == 'f' || *it == 'e')) {
+ presentation = *it++;
+ }
+
+ if (it != end && *it != '}') {
+ throw fmt::format_error("invalid format");
+ }
+
+ return it;
+ }
+
+ /**
+ * Writes out a formatted matrix.
+ *
+ * @tparam FormatContext Format string context type.
+ * @param mat Matrix to format.
+ * @param ctx Format string context.
+ */
+ template <typename FormatContext>
+ auto format(const Eigen::SparseMatrix<double, Options, StorageIndex>& mat,
+ FormatContext& ctx) {
+ auto out = ctx.out();
+ for (int i = 0; i < mat.rows(); ++i) {
+ for (int j = 0; j < mat.cols(); ++j) {
+ out = fmt::format_to(out, " {:f}", mat.coeff(i, j));
+ }
+
+ if (i < mat.rows() - 1) {
out = fmt::format_to(out, "\n");
}
}
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
new file mode 100644
index 0000000..58b966a
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
@@ -0,0 +1,73 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A class representing a coordinate system axis within the NWU coordinate
+ * system.
+ */
+class WPILIB_DLLEXPORT CoordinateAxis {
+ public:
+ /**
+ * Constructs a coordinate system axis within the NWU coordinate system and
+ * normalizes it.
+ *
+ * @param x The x component.
+ * @param y The y component.
+ * @param z The z component.
+ */
+ CoordinateAxis(double x, double y, double z);
+
+ CoordinateAxis(const CoordinateAxis&) = default;
+ CoordinateAxis& operator=(const CoordinateAxis&) = default;
+
+ CoordinateAxis(CoordinateAxis&&) = default;
+ CoordinateAxis& operator=(CoordinateAxis&&) = default;
+
+ /**
+ * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+ */
+ static const CoordinateAxis& N();
+
+ /**
+ * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+ */
+ static const CoordinateAxis& S();
+
+ /**
+ * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+ */
+ static const CoordinateAxis& E();
+
+ /**
+ * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+ */
+ static const CoordinateAxis& W();
+
+ /**
+ * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+ */
+ static const CoordinateAxis& U();
+
+ /**
+ * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+ */
+ static const CoordinateAxis& D();
+
+ private:
+ friend class CoordinateSystem;
+
+ Vectord<3> m_axis;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
new file mode 100644
index 0000000..232455f
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
@@ -0,0 +1,114 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/CoordinateAxis.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation3d.h"
+
+namespace frc {
+
+/**
+ * A helper class that converts Pose3d objects between different standard
+ * coordinate frames.
+ */
+class WPILIB_DLLEXPORT CoordinateSystem {
+ public:
+ /**
+ * Constructs a coordinate system with the given cardinal directions for each
+ * axis.
+ *
+ * @param positiveX The cardinal direction of the positive x-axis.
+ * @param positiveY The cardinal direction of the positive y-axis.
+ * @param positiveZ The cardinal direction of the positive z-axis.
+ * @throws std::domain_error if the coordinate system isn't special orthogonal
+ */
+ CoordinateSystem(const CoordinateAxis& positiveX,
+ const CoordinateAxis& positiveY,
+ const CoordinateAxis& positiveZ);
+
+ CoordinateSystem(const CoordinateSystem&) = default;
+ CoordinateSystem& operator=(const CoordinateSystem&) = default;
+ CoordinateSystem(CoordinateSystem&&) = default;
+ CoordinateSystem& operator=(CoordinateSystem&&) = default;
+
+ /**
+ * Returns an instance of the North-West-Up (NWU) coordinate system.
+ *
+ * The +X axis is north, the +Y axis is west, and the +Z axis is up.
+ */
+ static const CoordinateSystem& NWU();
+
+ /**
+ * Returns an instance of the East-Down-North (EDN) coordinate system.
+ *
+ * The +X axis is east, the +Y axis is down, and the +Z axis is north.
+ */
+ static const CoordinateSystem& EDN();
+
+ /**
+ * Returns an instance of the NED coordinate system.
+ *
+ * The +X axis is north, the +Y axis is east, and the +Z axis is down.
+ */
+ static const CoordinateSystem& NED();
+
+ /**
+ * Converts the given translation from one coordinate system to another.
+ *
+ * @param translation The translation to convert.
+ * @param from The coordinate system the translation starts in.
+ * @param to The coordinate system to which to convert.
+ * @return The given translation in the desired coordinate system.
+ */
+ static Translation3d Convert(const Translation3d& translation,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to);
+
+ /**
+ * Converts the given rotation from one coordinate system to another.
+ *
+ * @param rotation The rotation to convert.
+ * @param from The coordinate system the rotation starts in.
+ * @param to The coordinate system to which to convert.
+ * @return The given rotation in the desired coordinate system.
+ */
+ static Rotation3d Convert(const Rotation3d& rotation,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to);
+
+ /**
+ * Converts the given pose from one coordinate system to another.
+ *
+ * @param pose The pose to convert.
+ * @param from The coordinate system the pose starts in.
+ * @param to The coordinate system to which to convert.
+ * @return The given pose in the desired coordinate system.
+ */
+ static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
+ const CoordinateSystem& to);
+
+ /**
+ * Converts the given transform from one coordinate system to another.
+ *
+ * @param transform The transform to convert.
+ * @param from The coordinate system the transform starts in.
+ * @param to The coordinate system to which to convert.
+ * @return The given transform in the desired coordinate system.
+ */
+ static Transform3d Convert(const Transform3d& transform,
+ const CoordinateSystem& from,
+ const CoordinateSystem& to);
+
+ private:
+ // Rotation from this coordinate system to NWU coordinate system
+ Rotation3d m_rotation;
+};
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index ebaa7c1..d096e8c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -17,13 +17,12 @@
namespace frc {
/**
- * Represents a 2d pose containing translational and rotational elements.
+ * Represents a 2D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose2d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
- * (Translation2d{0, 0} and Rotation{0})
*/
constexpr Pose2d() = default;
@@ -33,31 +32,33 @@
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
- Pose2d(Translation2d translation, Rotation2d rotation);
+ constexpr Pose2d(Translation2d translation, Rotation2d rotation);
/**
- * Convenience constructors that takes in x and y values directly instead of
- * having to construct a Translation2d.
+ * Constructs a pose with x and y translations instead of a separate
+ * Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
- Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
+ constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
+ * <pre>
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
- * [t_new] [0, 0, 1][transform.t]
+ * [t_new] [ 0, 0, 1][transform.t]
+ * </pre>
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
- Pose2d operator+(const Transform2d& other) const;
+ constexpr Pose2d operator+(const Transform2d& other) const;
/**
* Returns the Transform2d that maps the one pose to another.
@@ -69,47 +70,54 @@
/**
* Checks equality between this Pose2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
*/
- bool operator==(const Pose2d& other) const;
-
- /**
- * Checks inequality between this Pose2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const Pose2d& other) const;
+ bool operator==(const Pose2d&) const = default;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
- const Translation2d& Translation() const { return m_translation; }
+ constexpr const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
- units::meter_t X() const { return m_translation.X(); }
+ constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
- units::meter_t Y() const { return m_translation.Y(); }
+ constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
- const Rotation2d& Rotation() const { return m_rotation; }
+ constexpr const Rotation2d& Rotation() const { return m_rotation; }
+
+ /**
+ * Multiplies the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Pose2d.
+ */
+ constexpr Pose2d operator*(double scalar) const;
+
+ /**
+ * Divides the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Pose2d.
+ */
+ constexpr Pose2d operator/(double scalar) const;
/**
* Transforms the pose by the given transformation and returns the new pose.
@@ -119,10 +127,10 @@
*
* @return The transformed pose.
*/
- Pose2d TransformBy(const Transform2d& other) const;
+ constexpr Pose2d TransformBy(const Transform2d& other) const;
/**
- * Returns the other pose relative to the current pose.
+ * Returns the current pose relative to the given pose.
*
* This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
@@ -152,7 +160,7 @@
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
- * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
*
* @return The new pose of the robot.
*/
@@ -180,3 +188,5 @@
void from_json(const wpi::json& json, Pose2d& pose);
} // namespace frc
+
+#include "Pose2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
new file mode 100644
index 0000000..c549f26
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
@@ -0,0 +1,39 @@
+// 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
+
+#include <utility>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+ : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
+ Rotation2d rotation)
+ : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
+ return TransformBy(other);
+}
+
+constexpr Pose2d Pose2d::operator*(double scalar) const {
+ return Pose2d{m_translation * scalar, m_rotation * scalar};
+}
+
+constexpr Pose2d Pose2d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+ return {m_translation + (other.Translation().RotateBy(m_rotation)),
+ other.Rotation() + m_rotation};
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
new file mode 100644
index 0000000..8c7ace3
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -0,0 +1,204 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "Pose2d.h"
+#include "Transform3d.h"
+#include "Translation3d.h"
+#include "Twist3d.h"
+
+namespace wpi {
+class json;
+} // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a 3D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose3d {
+ public:
+ /**
+ * Constructs a pose at the origin facing toward the positive X axis.
+ */
+ constexpr Pose3d() = default;
+
+ /**
+ * Constructs a pose with the specified translation and rotation.
+ *
+ * @param translation The translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ Pose3d(Translation3d translation, Rotation3d rotation);
+
+ /**
+ * Constructs a pose with x, y, and z translations instead of a separate
+ * Translation3d.
+ *
+ * @param x The x component of the translational component of the pose.
+ * @param y The y component of the translational component of the pose.
+ * @param z The z component of the translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+ Rotation3d rotation);
+
+ /**
+ * Constructs a 3D pose from a 2D pose in the X-Y plane.
+ *
+ * @param pose The 2D pose.
+ */
+ explicit Pose3d(const Pose2d& pose);
+
+ /**
+ * Transforms the pose by the given transformation and returns the new
+ * transformed pose.
+ *
+ * @param other The transform to transform the pose by.
+ *
+ * @return The transformed pose.
+ */
+ Pose3d operator+(const Transform3d& other) const;
+
+ /**
+ * Returns the Transform3d that maps the one pose to another.
+ *
+ * @param other The initial pose of the transformation.
+ * @return The transform that maps the other pose to the current pose.
+ */
+ Transform3d operator-(const Pose3d& other) const;
+
+ /**
+ * Checks equality between this Pose3d and another object.
+ */
+ bool operator==(const Pose3d&) const = default;
+
+ /**
+ * Returns the underlying translation.
+ *
+ * @return Reference to the translational component of the pose.
+ */
+ const Translation3d& Translation() const { return m_translation; }
+
+ /**
+ * Returns the X component of the pose's translation.
+ *
+ * @return The x component of the pose's translation.
+ */
+ units::meter_t X() const { return m_translation.X(); }
+
+ /**
+ * Returns the Y component of the pose's translation.
+ *
+ * @return The y component of the pose's translation.
+ */
+ units::meter_t Y() const { return m_translation.Y(); }
+
+ /**
+ * Returns the Z component of the pose's translation.
+ *
+ * @return The z component of the pose's translation.
+ */
+ units::meter_t Z() const { return m_translation.Z(); }
+
+ /**
+ * Returns the underlying rotation.
+ *
+ * @return Reference to the rotational component of the pose.
+ */
+ const Rotation3d& Rotation() const { return m_rotation; }
+
+ /**
+ * Multiplies the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Pose2d.
+ */
+ Pose3d operator*(double scalar) const;
+
+ /**
+ * Divides the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Pose2d.
+ */
+ Pose3d operator/(double scalar) const;
+
+ /**
+ * Transforms the pose by the given transformation and returns the new pose.
+ * See + operator for the matrix multiplication performed.
+ *
+ * @param other The transform to transform the pose by.
+ *
+ * @return The transformed pose.
+ */
+ Pose3d TransformBy(const Transform3d& other) const;
+
+ /**
+ * Returns the current pose relative to the given pose.
+ *
+ * This function can often be used for trajectory tracking or pose
+ * stabilization algorithms to get the error between the reference and the
+ * current pose.
+ *
+ * @param other The pose that is the origin of the new coordinate frame that
+ * the current pose will be converted into.
+ *
+ * @return The current pose relative to the new origin pose.
+ */
+ Pose3d RelativeTo(const Pose3d& other) const;
+
+ /**
+ * Obtain a new Pose3d from a (constant curvature) velocity.
+ *
+ * The twist is a change in pose in the robot's coordinate frame since the
+ * previous pose update. When the user runs exp() on the previous known
+ * field-relative pose with the argument being the twist, the user will
+ * receive the new field-relative pose.
+ *
+ * "Exp" represents the pose exponential, which is solving a differential
+ * equation moving the pose forward in time.
+ *
+ * @param twist The change in pose in the robot's coordinate frame since the
+ * previous pose update. For example, if a non-holonomic robot moves forward
+ * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+ * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
+ * 0.5_deg}}.
+ *
+ * @return The new pose of the robot.
+ */
+ Pose3d Exp(const Twist3d& twist) const;
+
+ /**
+ * Returns a Twist3d that maps this pose to the end pose. If c is the output
+ * of a.Log(b), then a.Exp(c) would yield b.
+ *
+ * @param end The end pose for the transformation.
+ *
+ * @return The twist that maps this to end.
+ */
+ Twist3d Log(const Pose3d& end) const;
+
+ /**
+ * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+ */
+ Pose2d ToPose2d() const;
+
+ private:
+ Translation3d m_translation;
+ Rotation3d m_rotation;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
new file mode 100644
index 0000000..b5a318b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
@@ -0,0 +1,97 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+
+namespace wpi {
+class json;
+} // namespace wpi
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Quaternion {
+ public:
+ /**
+ * Constructs a quaternion with a default angle of 0 degrees.
+ */
+ Quaternion() = default;
+
+ /**
+ * Constructs a quaternion with the given components.
+ *
+ * @param w W component of the quaternion.
+ * @param x X component of the quaternion.
+ * @param y Y component of the quaternion.
+ * @param z Z component of the quaternion.
+ */
+ Quaternion(double w, double x, double y, double z);
+
+ /**
+ * Multiply with another quaternion.
+ *
+ * @param other The other quaternion.
+ */
+ Quaternion operator*(const Quaternion& other) const;
+
+ /**
+ * Checks equality between this Quaternion and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Quaternion& other) const;
+
+ /**
+ * Returns the inverse of the quaternion.
+ */
+ Quaternion Inverse() const;
+
+ /**
+ * Normalizes the quaternion.
+ */
+ Quaternion Normalize() const;
+
+ /**
+ * Returns W component of the quaternion.
+ */
+ double W() const;
+
+ /**
+ * Returns X component of the quaternion.
+ */
+ double X() const;
+
+ /**
+ * Returns Y component of the quaternion.
+ */
+ double Y() const;
+
+ /**
+ * Returns Z component of the quaternion.
+ */
+ double Z() const;
+
+ /**
+ * Returns the rotation vector representation of this quaternion.
+ *
+ * This is also the log operator of SO(3).
+ */
+ Eigen::Vector3d ToRotationVector() const;
+
+ private:
+ double m_r = 1.0;
+ Eigen::Vector3d m_v{0.0, 0.0, 0.0};
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 94a17fc..406ef3c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -15,8 +15,13 @@
namespace frc {
/**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle
* (cosine and sine).
+ *
+ * The angle is continuous, that is if a Rotation2d is constructed with 361
+ * degrees, it will return 361 degrees. This allows algorithms that wouldn't
+ * want to see a discontinuity in the rotations as it sweeps past from 360 to 0
+ * on the second time around.
*/
class WPILIB_DLLEXPORT Rotation2d {
public:
@@ -30,14 +35,14 @@
*
* @param value The value of the angle in radians.
*/
- Rotation2d(units::radian_t value); // NOLINT
+ constexpr Rotation2d(units::radian_t value); // NOLINT
/**
* Constructs a Rotation2d with the given degree value.
*
* @param value The value of the angle in degrees.
*/
- Rotation2d(units::degree_t value); // NOLINT
+ constexpr Rotation2d(units::degree_t value); // NOLINT
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -46,33 +51,33 @@
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
- Rotation2d(double x, double y);
+ constexpr Rotation2d(double x, double y);
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
*
- * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
- * Rotation2d{-pi/2}
+ * For example, <code>Rotation2d{30_deg} + Rotation2d{60_deg}</code> equals
+ * <code>Rotation2d{units::radian_t{std::numbers::pi/2.0}}</code>
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
- Rotation2d operator+(const Rotation2d& other) const;
+ constexpr Rotation2d operator+(const Rotation2d& other) const;
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
- * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
- * Rotation2d{-pi/2}
+ * For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
+ * <code>Rotation2d{units::radian_t{-std::numbers::pi/2.0}}</code>
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
*/
- Rotation2d operator-(const Rotation2d& other) const;
+ constexpr Rotation2d operator-(const Rotation2d& other) const;
/**
* Takes the inverse of the current rotation. This is simply the negative of
@@ -80,15 +85,25 @@
*
* @return The inverse of the current rotation.
*/
- Rotation2d operator-() const;
+ constexpr Rotation2d operator-() const;
/**
* Multiplies the current rotation by a scalar.
+ *
* @param scalar The scalar.
*
* @return The new scaled Rotation2d.
*/
- Rotation2d operator*(double scalar) const;
+ constexpr Rotation2d operator*(double scalar) const;
+
+ /**
+ * Divides the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Rotation2d.
+ */
+ constexpr Rotation2d operator/(double scalar) const;
/**
* Checks equality between this Rotation2d and another object.
@@ -96,15 +111,7 @@
* @param other The other object.
* @return Whether the two objects are equal.
*/
- bool operator==(const Rotation2d& other) const;
-
- /**
- * Checks inequality between this Rotation2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const Rotation2d& other) const;
+ constexpr bool operator==(const Rotation2d& other) const;
/**
* Adds the new rotation to the current rotation using a rotation matrix.
@@ -119,42 +126,44 @@
*
* @return The new rotated Rotation2d.
*/
- Rotation2d RotateBy(const Rotation2d& other) const;
+ constexpr Rotation2d RotateBy(const Rotation2d& other) const;
/**
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
+ * @see AngleModulus to constrain the angle within (-pi, pi]
*/
- units::radian_t Radians() const { return m_value; }
+ constexpr units::radian_t Radians() const { return m_value; }
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
+ * @see InputModulus to constrain the angle within (-180, 180]
*/
- units::degree_t Degrees() const { return m_value; }
+ constexpr units::degree_t Degrees() const { return m_value; }
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
- double Cos() const { return m_cos; }
+ constexpr double Cos() const { return m_cos; }
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
- double Sin() const { return m_sin; }
+ constexpr double Sin() const { return m_sin; }
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
- double Tan() const { return m_sin / m_cos; }
+ constexpr double Tan() const { return Sin() / Cos(); }
private:
units::radian_t m_value = 0_rad;
@@ -169,3 +178,5 @@
void from_json(const wpi::json& json, Rotation2d& rotation);
} // namespace frc
+
+#include "Rotation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
new file mode 100644
index 0000000..eb31ebd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -0,0 +1,70 @@
+// 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
+
+#include <type_traits>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gcem.hpp"
+#include "units/angle.h"
+
+namespace frc {
+
+constexpr Rotation2d::Rotation2d(units::radian_t value)
+ : m_value(value),
+ m_cos(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
+ : std::cos(value.to<double>())),
+ m_sin(std::is_constant_evaluated() ? gcem::sin(value.to<double>())
+ : std::sin(value.to<double>())) {}
+
+constexpr Rotation2d::Rotation2d(units::degree_t value)
+ : Rotation2d(units::radian_t{value}) {}
+
+constexpr Rotation2d::Rotation2d(double x, double y) {
+ const auto magnitude = gcem::hypot(x, y);
+ if (magnitude > 1e-6) {
+ m_sin = y / magnitude;
+ m_cos = x / magnitude;
+ } else {
+ m_sin = 0.0;
+ m_cos = 1.0;
+ }
+ m_value =
+ units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos)
+ : std::atan2(m_sin, m_cos)};
+}
+
+constexpr Rotation2d Rotation2d::operator-() const {
+ return Rotation2d{-m_value};
+}
+
+constexpr Rotation2d Rotation2d::operator*(double scalar) const {
+ return Rotation2d(m_value * scalar);
+}
+
+constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+ return RotateBy(other);
+}
+
+constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+ return *this + -other;
+}
+
+constexpr Rotation2d Rotation2d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
+ return (std::is_constant_evaluated()
+ ? gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin())
+ : std::hypot(Cos() - other.Cos(), Sin() - other.Sin())) < 1E-9;
+}
+
+constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+ return {Cos() * other.Cos() - Sin() * other.Sin(),
+ Cos() * other.Sin() + Sin() * other.Cos()};
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
new file mode 100644
index 0000000..7c1a60d
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -0,0 +1,186 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "Quaternion.h"
+#include "Rotation2d.h"
+#include "frc/EigenCore.h"
+#include "units/angle.h"
+
+namespace wpi {
+class json;
+} // namespace wpi
+
+namespace frc {
+
+/**
+ * A rotation in a 3D coordinate frame represented by a quaternion.
+ */
+class WPILIB_DLLEXPORT Rotation3d {
+ public:
+ /**
+ * Constructs a Rotation3d with a default angle of 0 degrees.
+ */
+ Rotation3d() = default;
+
+ /**
+ * Constructs a Rotation3d from a quaternion.
+ *
+ * @param q The quaternion.
+ */
+ explicit Rotation3d(const Quaternion& q);
+
+ /**
+ * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+ *
+ * Extrinsic rotations occur in that order around the axes in the fixed global
+ * frame rather than the body frame.
+ *
+ * Angles are measured counterclockwise with the rotation axis pointing "out
+ * of the page". If you point your right thumb along the positive axis
+ * direction, your fingers curl in the direction of positive rotation.
+ *
+ * @param roll The counterclockwise rotation angle around the X axis (roll).
+ * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+ * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+ */
+ Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
+
+ /**
+ * Constructs a Rotation3d with the given axis-angle representation. The axis
+ * doesn't have to be normalized.
+ *
+ * @param axis The rotation axis.
+ * @param angle The rotation around the axis.
+ */
+ Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+
+ /**
+ * Constructs a Rotation3d from a rotation matrix.
+ *
+ * @param rotationMatrix The rotation matrix.
+ * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+ */
+ explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+
+ /**
+ * Constructs a Rotation3d that rotates the initial vector onto the final
+ * vector.
+ *
+ * This is useful for turning a 3D vector (final) into an orientation relative
+ * to a coordinate system vector (initial).
+ *
+ * @param initial The initial vector.
+ * @param final The final vector.
+ */
+ Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
+
+ /**
+ * Adds two rotations together.
+ *
+ * @param other The rotation to add.
+ *
+ * @return The sum of the two rotations.
+ */
+ Rotation3d operator+(const Rotation3d& other) const;
+
+ /**
+ * Subtracts the new rotation from the current rotation and returns the new
+ * rotation.
+ *
+ * @param other The rotation to subtract.
+ *
+ * @return The difference between the two rotations.
+ */
+ Rotation3d operator-(const Rotation3d& other) const;
+
+ /**
+ * Takes the inverse of the current rotation.
+ *
+ * @return The inverse of the current rotation.
+ */
+ Rotation3d operator-() const;
+
+ /**
+ * Multiplies the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Rotation3d.
+ */
+ Rotation3d operator*(double scalar) const;
+
+ /**
+ * Divides the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Rotation3d.
+ */
+ Rotation3d operator/(double scalar) const;
+
+ /**
+ * Checks equality between this Rotation3d and another object.
+ */
+ bool operator==(const Rotation3d&) const = default;
+
+ /**
+ * Adds the new rotation to the current rotation.
+ *
+ * @param other The rotation to rotate by.
+ *
+ * @return The new rotated Rotation3d.
+ */
+ Rotation3d RotateBy(const Rotation3d& other) const;
+
+ /**
+ * Returns the quaternion representation of the Rotation3d.
+ */
+ const Quaternion& GetQuaternion() const;
+
+ /**
+ * Returns the counterclockwise rotation angle around the X axis (roll).
+ */
+ units::radian_t X() const;
+
+ /**
+ * Returns the counterclockwise rotation angle around the Y axis (pitch).
+ */
+ units::radian_t Y() const;
+
+ /**
+ * Returns the counterclockwise rotation angle around the Z axis (yaw).
+ */
+ units::radian_t Z() const;
+
+ /**
+ * Returns the axis in the axis-angle representation of this rotation.
+ */
+ Vectord<3> Axis() const;
+
+ /**
+ * Returns the angle in the axis-angle representation of this rotation.
+ */
+ units::radian_t Angle() const;
+
+ /**
+ * Returns a Rotation2d representing this Rotation3d projected into the X-Y
+ * plane.
+ */
+ Rotation2d ToRotation2d() const;
+
+ private:
+ Quaternion m_q;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 3d5e1d6..3c21ec1 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -31,7 +31,7 @@
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
- Transform2d(Translation2d translation, Rotation2d rotation);
+ constexpr Transform2d(Translation2d translation, Rotation2d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -43,47 +43,57 @@
*
* @return Reference to the translational component of the transform.
*/
- const Translation2d& Translation() const { return m_translation; }
+ constexpr const Translation2d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
- units::meter_t X() const { return m_translation.X(); }
+ constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
- units::meter_t Y() const { return m_translation.Y(); }
+ constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
- const Rotation2d& Rotation() const { return m_rotation; }
+ constexpr const Rotation2d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
- Transform2d Inverse() const;
+ constexpr Transform2d Inverse() const;
/**
- * Scales the transform by the scalar.
+ * Multiplies the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
- Transform2d operator*(double scalar) const {
+ constexpr Transform2d operator*(double scalar) const {
return Transform2d(m_translation * scalar, m_rotation * scalar);
}
/**
+ * Divides the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform2d.
+ */
+ constexpr Transform2d operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+ }
+
+ /**
* Composes two transformations.
*
* @param other The transform to compose with this one.
@@ -93,22 +103,13 @@
/**
* Checks equality between this Transform2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
*/
- bool operator==(const Transform2d& other) const;
-
- /**
- * Checks inequality between this Transform2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const Transform2d& other) const;
+ bool operator==(const Transform2d&) const = default;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc
+
+#include "Transform2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
new file mode 100644
index 0000000..f851a05
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
@@ -0,0 +1,26 @@
+// 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
+
+#include <utility>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+
+namespace frc {
+
+constexpr Transform2d::Transform2d(Translation2d translation,
+ Rotation2d rotation)
+ : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+constexpr Transform2d Transform2d::Inverse() const {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
new file mode 100644
index 0000000..5f50ec2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -0,0 +1,118 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "Translation3d.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Pose3d;
+
+/**
+ * Represents a transformation for a Pose3d.
+ */
+class WPILIB_DLLEXPORT Transform3d {
+ public:
+ /**
+ * Constructs the transform that maps the initial pose to the final pose.
+ *
+ * @param initial The initial pose for the transformation.
+ * @param final The final pose for the transformation.
+ */
+ Transform3d(Pose3d initial, Pose3d final);
+
+ /**
+ * Constructs a transform with the given translation and rotation components.
+ *
+ * @param translation Translational component of the transform.
+ * @param rotation Rotational component of the transform.
+ */
+ Transform3d(Translation3d translation, Rotation3d rotation);
+
+ /**
+ * Constructs the identity transform -- maps an initial pose to itself.
+ */
+ constexpr Transform3d() = default;
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return Reference to the translational component of the transform.
+ */
+ const Translation3d& Translation() const { return m_translation; }
+
+ /**
+ * Returns the X component of the transformation's translation.
+ *
+ * @return The x component of the transformation's translation.
+ */
+ units::meter_t X() const { return m_translation.X(); }
+
+ /**
+ * Returns the Y component of the transformation's translation.
+ *
+ * @return The y component of the transformation's translation.
+ */
+ units::meter_t Y() const { return m_translation.Y(); }
+
+ /**
+ * Returns the Z component of the transformation's translation.
+ *
+ * @return The z component of the transformation's translation.
+ */
+ units::meter_t Z() const { return m_translation.Z(); }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return Reference to the rotational component of the transform.
+ */
+ const Rotation3d& Rotation() const { return m_rotation; }
+
+ /**
+ * Invert the transformation. This is useful for undoing a transformation.
+ *
+ * @return The inverted transformation.
+ */
+ Transform3d Inverse() const;
+
+ /**
+ * Multiplies the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform3d.
+ */
+ Transform3d operator*(double scalar) const {
+ return Transform3d(m_translation * scalar, m_rotation * scalar);
+ }
+
+ /**
+ * Divides the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform3d.
+ */
+ Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
+
+ /**
+ * Composes two transformations.
+ *
+ * @param other The transform to compose with this one.
+ * @return The composition of the two transformations.
+ */
+ Transform3d operator+(const Transform3d& other) const;
+
+ /**
+ * Checks equality between this Transform3d and another object.
+ */
+ bool operator==(const Transform3d&) const = default;
+
+ private:
+ Translation3d m_translation;
+ Rotation3d m_rotation;
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 204da30..e168510 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -16,12 +16,12 @@
namespace frc {
/**
- * Represents a translation in 2d space.
+ * Represents a translation in 2D space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
+ * When the robot is at the origin facing in the positive X direction, forward
+ * is positive X and left is positive Y.
*/
class WPILIB_DLLEXPORT Translation2d {
public:
@@ -37,7 +37,7 @@
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
- Translation2d(units::meter_t x, units::meter_t y);
+ constexpr Translation2d(units::meter_t x, units::meter_t y);
/**
* Constructs a Translation2d with the provided distance and angle. This is
@@ -46,13 +46,12 @@
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
- Translation2d(units::meter_t distance, const Rotation2d& angle);
+ constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
/**
- * Calculates the distance between two translations in 2d space.
+ * Calculates the distance between two translations in 2D space.
*
- * This function uses the pythagorean theorem to calculate the distance.
- * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+ * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
*
* @param other The translation to compute the distance to.
*
@@ -63,16 +62,16 @@
/**
* Returns the X component of the translation.
*
- * @return The x component of the translation.
+ * @return The X component of the translation.
*/
- units::meter_t X() const { return m_x; }
+ constexpr units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
- * @return The y component of the translation.
+ * @return The Y component of the translation.
*/
- units::meter_t Y() const { return m_y; }
+ constexpr units::meter_t Y() const { return m_y; }
/**
* Returns the norm, or distance from the origin to the translation.
@@ -82,79 +81,86 @@
units::meter_t Norm() const;
/**
- * Applies a rotation to the translation in 2d space.
+ * Returns the angle this translation forms with the positive X axis.
+ *
+ * @return The angle of the translation
+ */
+ constexpr Rotation2d Angle() const;
+
+ /**
+ * Applies a rotation to the translation in 2D space.
*
* This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
*
+ * <pre>
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
+ * </pre>
*
- * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
- * Translation2d of {0, 2}.
+ * For example, rotating a Translation2d of <2, 0> by 90 degrees will
+ * return a Translation2d of <0, 2>.
*
* @param other The rotation to rotate the translation by.
*
* @return The new rotated translation.
*/
- Translation2d RotateBy(const Rotation2d& other) const;
+ constexpr Translation2d RotateBy(const Rotation2d& other) const;
/**
- * Adds two translations in 2d space and returns the sum. This is similar to
- * vector addition.
+ * Returns the sum of two translations in 2D space.
*
- * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
- * Translation2d{3.0, 8.0}
+ * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
+ * Translation3d{3.0, 8.0}.
*
* @param other The translation to add.
*
* @return The sum of the translations.
*/
- Translation2d operator+(const Translation2d& other) const;
+ constexpr Translation2d operator+(const Translation2d& other) const;
/**
- * Subtracts the other translation from the other translation and returns the
- * difference.
+ * Returns the difference between two translations.
*
* For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
- * Translation2d{4.0, 2.0}
+ * Translation2d{4.0, 2.0}.
*
* @param other The translation to subtract.
*
* @return The difference between the two translations.
*/
- Translation2d operator-(const Translation2d& other) const;
+ constexpr Translation2d operator-(const Translation2d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
- * rotating by 180 degrees, flipping the point over both axes, or simply
- * negating both components of the translation.
+ * rotating by 180 degrees, flipping the point over both axes, or negating all
+ * components of the translation.
*
* @return The inverse of the current translation.
*/
- Translation2d operator-() const;
+ constexpr Translation2d operator-() const;
/**
- * Multiplies the translation by a scalar and returns the new translation.
+ * Returns the translation multiplied by a scalar.
*
- * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+ * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
*
* @param scalar The scalar to multiply by.
*
* @return The scaled translation.
*/
- Translation2d operator*(double scalar) const;
+ constexpr Translation2d operator*(double scalar) const;
/**
- * Divides the translation by a scalar and returns the new translation.
+ * Returns the translation divided by a scalar.
*
- * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+ * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
- Translation2d operator/(double scalar) const;
+ constexpr Translation2d operator/(double scalar) const;
/**
* Checks equality between this Translation2d and another object.
@@ -164,14 +170,6 @@
*/
bool operator==(const Translation2d& other) const;
- /**
- * Checks inequality between this Translation2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const Translation2d& other) const;
-
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
@@ -184,3 +182,5 @@
void from_json(const wpi::json& json, Translation2d& state);
} // namespace frc
+
+#include "Translation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
new file mode 100644
index 0000000..3be897f
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -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.
+
+#pragma once
+
+#include "frc/geometry/Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+ : m_x(x), m_y(y) {}
+
+constexpr Translation2d::Translation2d(units::meter_t distance,
+ const Rotation2d& angle)
+ : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+
+constexpr Rotation2d Translation2d::Angle() const {
+ return Rotation2d{m_x.value(), m_y.value()};
+}
+
+constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+ return {m_x * other.Cos() - m_y * other.Sin(),
+ m_x * other.Sin() + m_y * other.Cos()};
+}
+
+constexpr Translation2d Translation2d::operator+(
+ const Translation2d& other) const {
+ return {X() + other.X(), Y() + other.Y()};
+}
+
+constexpr Translation2d Translation2d::operator-(
+ const Translation2d& other) const {
+ return *this + -other;
+}
+
+constexpr Translation2d Translation2d::operator-() const {
+ return {-m_x, -m_y};
+}
+
+constexpr Translation2d Translation2d::operator*(double scalar) const {
+ return {scalar * m_x, scalar * m_y};
+}
+
+constexpr Translation2d Translation2d::operator/(double scalar) const {
+ return operator*(1.0 / scalar);
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
new file mode 100644
index 0000000..ab641fa
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -0,0 +1,189 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "Rotation3d.h"
+#include "Translation2d.h"
+#include "units/length.h"
+
+namespace wpi {
+class json;
+} // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a translation in 3D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes. When the
+ * robot is at the origin facing in the positive X direction, forward is
+ * positive X, left is positive Y, and up is positive Z.
+ */
+class WPILIB_DLLEXPORT Translation3d {
+ public:
+ /**
+ * Constructs a Translation3d with X, Y, and Z components equal to zero.
+ */
+ constexpr Translation3d() = default;
+
+ /**
+ * Constructs a Translation3d with the X, Y, and Z components equal to the
+ * provided values.
+ *
+ * @param x The x component of the translation.
+ * @param y The y component of the translation.
+ * @param z The z component of the translation.
+ */
+ constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
+
+ /**
+ * Constructs a Translation3d with the provided distance and angle. This is
+ * essentially converting from polar coordinates to Cartesian coordinates.
+ *
+ * @param distance The distance from the origin to the end of the translation.
+ * @param angle The angle between the x-axis and the translation vector.
+ */
+ Translation3d(units::meter_t distance, const Rotation3d& angle);
+
+ /**
+ * Calculates the distance between two translations in 3D space.
+ *
+ * The distance between translations is defined as
+ * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+ *
+ * @param other The translation to compute the distance to.
+ *
+ * @return The distance between the two translations.
+ */
+ units::meter_t Distance(const Translation3d& other) const;
+
+ /**
+ * Returns the X component of the translation.
+ *
+ * @return The Z component of the translation.
+ */
+ constexpr units::meter_t X() const { return m_x; }
+
+ /**
+ * Returns the Y component of the translation.
+ *
+ * @return The Y component of the translation.
+ */
+ constexpr units::meter_t Y() const { return m_y; }
+
+ /**
+ * Returns the Z component of the translation.
+ *
+ * @return The Z component of the translation.
+ */
+ constexpr units::meter_t Z() const { return m_z; }
+
+ /**
+ * Returns the norm, or distance from the origin to the translation.
+ *
+ * @return The norm of the translation.
+ */
+ units::meter_t Norm() const;
+
+ /**
+ * Applies a rotation to the translation in 3D space.
+ *
+ * For example, rotating a Translation3d of <2, 0, 0> by 90 degrees
+ * around the Z axis will return a Translation3d of <0, 2, 0>.
+ *
+ * @param other The rotation to rotate the translation by.
+ *
+ * @return The new rotated translation.
+ */
+ Translation3d RotateBy(const Rotation3d& other) const;
+
+ /**
+ * Returns a Translation2d representing this Translation3d projected into the
+ * X-Y plane.
+ */
+ constexpr Translation2d ToTranslation2d() const;
+
+ /**
+ * Returns the sum of two translations in 3D space.
+ *
+ * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
+ * Translation3d{3.0, 8.0, 11.0}.
+ *
+ * @param other The translation to add.
+ *
+ * @return The sum of the translations.
+ */
+ constexpr Translation3d operator+(const Translation3d& other) const;
+
+ /**
+ * Returns the difference between two translations.
+ *
+ * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
+ * Translation3d{4.0, 2.0, 0.0}.
+ *
+ * @param other The translation to subtract.
+ *
+ * @return The difference between the two translations.
+ */
+ constexpr Translation3d operator-(const Translation3d& other) const;
+
+ /**
+ * Returns the inverse of the current translation. This is equivalent to
+ * negating all components of the translation.
+ *
+ * @return The inverse of the current translation.
+ */
+ constexpr Translation3d operator-() const;
+
+ /**
+ * Returns the translation multiplied by a scalar.
+ *
+ * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
+ * 9.0}.
+ *
+ * @param scalar The scalar to multiply by.
+ *
+ * @return The scaled translation.
+ */
+ constexpr Translation3d operator*(double scalar) const;
+
+ /**
+ * Returns the translation divided by a scalar.
+ *
+ * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
+ * 2.25}.
+ *
+ * @param scalar The scalar to divide by.
+ *
+ * @return The scaled translation.
+ */
+ constexpr Translation3d operator/(double scalar) const;
+
+ /**
+ * Checks equality between this Translation3d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Translation3d& other) const;
+
+ private:
+ units::meter_t m_x = 0_m;
+ units::meter_t m_y = 0_m;
+ units::meter_t m_z = 0_m;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
+} // namespace frc
+
+#include "Translation3d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
new file mode 100644
index 0000000..8ab6e94
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -0,0 +1,44 @@
+// 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
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+ units::meter_t z)
+ : m_x(x), m_y(y), m_z(z) {}
+
+constexpr Translation2d Translation3d::ToTranslation2d() const {
+ return Translation2d{m_x, m_y};
+}
+
+constexpr Translation3d Translation3d::operator+(
+ const Translation3d& other) const {
+ return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+constexpr Translation3d Translation3d::operator-(
+ const Translation3d& other) const {
+ return operator+(-other);
+}
+
+constexpr Translation3d Translation3d::operator-() const {
+ return {-m_x, -m_y, -m_z};
+}
+
+constexpr Translation3d Translation3d::operator*(double scalar) const {
+ return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+constexpr Translation3d Translation3d::operator/(double scalar) const {
+ return operator*(1.0 / scalar);
+}
+
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 6ad2b38..620b688 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -12,9 +12,9 @@
namespace frc {
/**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
+ * A change in distance along a 2D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose2ds from a Twist2d and
+ * vise versa.
*
* A Twist can be used to represent a difference between two poses.
*/
@@ -47,20 +47,12 @@
}
/**
- * Checks inequality between this Twist2d and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const Twist2d& other) const { return !operator==(other); }
-
- /**
* Scale this by a given factor.
*
* @param factor The factor by which to scale.
* @return The scaled Twist2d.
*/
- Twist2d operator*(double factor) const {
+ constexpr Twist2d operator*(double factor) const {
return Twist2d{dx * factor, dy * factor, dtheta * factor};
}
};
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
new file mode 100644
index 0000000..3040ab3
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
@@ -0,0 +1,79 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use
+ * ideas from differential calculus to create new Pose3ds from a Twist3d and
+ * vise versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist3d {
+ /**
+ * Linear "dx" component
+ */
+ units::meter_t dx = 0_m;
+
+ /**
+ * Linear "dy" component
+ */
+ units::meter_t dy = 0_m;
+
+ /**
+ * Linear "dz" component
+ */
+ units::meter_t dz = 0_m;
+
+ /**
+ * Rotation vector x component.
+ */
+ units::radian_t rx = 0_rad;
+
+ /**
+ * Rotation vector y component.
+ */
+ units::radian_t ry = 0_rad;
+
+ /**
+ * Rotation vector z component.
+ */
+ units::radian_t rz = 0_rad;
+
+ /**
+ * Checks equality between this Twist3d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Twist3d& other) const {
+ return units::math::abs(dx - other.dx) < 1E-9_m &&
+ units::math::abs(dy - other.dy) < 1E-9_m &&
+ units::math::abs(dz - other.dz) < 1E-9_m &&
+ units::math::abs(rx - other.rx) < 1E-9_rad &&
+ units::math::abs(ry - other.ry) < 1E-9_rad &&
+ units::math::abs(rz - other.rz) < 1E-9_rad;
+ }
+
+ /**
+ * Scale this by a given factor.
+ *
+ * @param factor The factor by which to scale.
+ * @return The scaled Twist3d.
+ */
+ constexpr Twist3d operator*(double factor) const {
+ return Twist3d{dx * factor, dy * factor, dz * factor,
+ rx * factor, ry * factor, rz * factor};
+ }
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index a049e40..771fe84 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -4,9 +4,11 @@
#pragma once
+#include <algorithm>
#include <array>
#include <functional>
#include <map>
+#include <optional>
#include <utility>
#include <vector>
@@ -81,12 +83,16 @@
void Clear() { m_pastSnapshots.clear(); }
/**
- * Sample the buffer at the given time. If there are no elements in the
- * buffer, calling this function results in undefined behavior.
+ * Sample the buffer at the given time. If the buffer is empty, an empty
+ * optional is returned.
*
* @param time The time at which to sample the buffer.
*/
- T Sample(units::second_t time) {
+ std::optional<T> Sample(units::second_t time) {
+ if (m_pastSnapshots.empty()) {
+ return {};
+ }
+
// We will perform a binary search to find the index of the element in the
// vector that has a timestamp that is equal to or greater than the vision
// measurement timestamp.
@@ -114,6 +120,14 @@
return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
}
+ /**
+ * Grant access to the internal sample buffer. Used in Pose Estimation to
+ * replay odometry inputs stored within this buffer.
+ */
+ std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
+ return m_pastSnapshots;
+ }
+
private:
units::second_t m_historySize;
std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 7414dec..37fe768 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -60,5 +60,26 @@
return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
}
+
+ /**
+ * Converts a user provided field-relative ChassisSpeeds object into a
+ * robot-relative ChassisSpeeds object.
+ *
+ * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds
+ * in the field frame of reference. Positive x is away from your alliance
+ * wall. Positive y is to your left when standing behind the alliance wall.
+ * @param robotAngle The angle of the robot as measured by a gyroscope. The
+ * robot's angle is considered to be zero when it is facing directly away
+ * from your alliance station wall. Remember that this should be CCW
+ * positive.
+ * @return ChassisSpeeds object representing the speeds in the robot's frame
+ * of reference.
+ */
+ static ChassisSpeeds FromFieldRelativeSpeeds(
+ const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
+ return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
+ fieldRelativeSpeeds.vy,
+ fieldRelativeSpeeds.omega, robotAngle);
+ }
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 4bf27ff..930c7a6 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
+#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/angle.h"
@@ -64,6 +65,20 @@
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
+ /**
+ * Returns a twist from left and right distance deltas using
+ * forward kinematics.
+ *
+ * @param leftDistance The distance measured by the left encoder.
+ * @param rightDistance The distance measured by the right encoder.
+ * @return The resulting Twist2d.
+ */
+ constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
+ const units::meter_t rightDistance) const {
+ return {(leftDistance + rightDistance) / 2, 0_m,
+ (rightDistance - leftDistance) / trackWidth * 1_rad};
+ }
+
units::meter_t trackWidth;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index 8e992ef..cc198ac 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -27,30 +27,41 @@
/**
* Constructs a DifferentialDriveOdometry object.
*
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero).
+ *
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
- const Pose2d& initialPose = Pose2d());
+ units::meter_t leftDistance,
+ units::meter_t rightDistance,
+ const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
- * You NEED to reset your encoders (to zero) when calling this method.
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero).
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
- m_prevLeftDistance = 0_m;
- m_prevRightDistance = 0_m;
+ m_prevLeftDistance = leftDistance;
+ m_prevRightDistance = rightDistance;
}
/**
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 9a7cef9..2880cef 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -6,10 +6,12 @@
#include <wpi/SymbolExports.h>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "wpimath/MathShared.h"
@@ -99,7 +101,7 @@
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
- const Translation2d& centerOfRotation = Translation2d()) const;
+ const Translation2d& centerOfRotation = Translation2d{}) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -114,9 +116,21 @@
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+ /**
+ * Performs forward kinematics to return the resulting Twist2d from the
+ * given wheel position deltas. This method is often used for odometry --
+ * determining the robot's position on the field using data from the
+ * distance driven by each wheel on the robot.
+ *
+ * @param wheelDeltas The change in distance driven by each wheel.
+ *
+ * @return The resulting chassis speed.
+ */
+ Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
+
private:
- mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
- Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
+ mutable Matrixd<4, 3> m_inverseKinematics;
+ Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index bdd1239..5e949ca 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -30,11 +30,13 @@
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
- explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
- const Rotation2d& gyroAngle,
- const Pose2d& initialPose = Pose2d());
+ explicit MecanumDriveOdometry(
+ MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -42,13 +44,17 @@
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The current distances measured by each wheel.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ m_previousWheelPositions = wheelPositions;
}
/**
@@ -59,45 +65,23 @@
/**
* Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method takes in the current time as
- * a parameter to calculate period (difference between two timestamps). The
- * period is used to calculate the change in distance from a velocity. This
- * also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
- *
- * @param currentTime The current time.
- * @param gyroAngle The angle reported by the gyroscope.
- * @param wheelSpeeds The current wheel speeds.
- *
- * @return The new pose of the robot.
- */
- const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& gyroAngle,
- MecanumDriveWheelSpeeds wheelSpeeds);
-
- /**
- * Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method automatically calculates
- * the current time to calculate period (difference between two timestamps).
- * The period is used to calculate the change in distance from a velocity.
- * This also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
+ * integration of the pose over time. This method takes in an angle parameter
+ * which is used instead of the angular rate that is calculated from forward
+ * kinematics, in addition to the current distance measurement at each wheel.
*
* @param gyroAngle The angle reported by the gyroscope.
- * @param wheelSpeeds The current wheel speeds.
+ * @param wheelPositions The current distances measured by each wheel.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
- MecanumDriveWheelSpeeds wheelSpeeds) {
- return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
- }
+ const MecanumDriveWheelPositions& wheelPositions);
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
- units::second_t m_previousTime = -1_s;
+ MecanumDriveWheelPositions m_previousWheelPositions;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
new file mode 100644
index 0000000..b69aceb
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -0,0 +1,53 @@
+// 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
+
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a mecanum drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
+ /**
+ * Distance driven by the front-left wheel.
+ */
+ units::meter_t frontLeft = 0_m;
+
+ /**
+ * Distance driven by the front-right wheel.
+ */
+ units::meter_t frontRight = 0_m;
+
+ /**
+ * Distance driven by the rear-left wheel.
+ */
+ units::meter_t rearLeft = 0_m;
+
+ /**
+ * Distance driven by the rear-right wheel.
+ */
+ units::meter_t rearRight = 0_m;
+
+ /**
+ * Checks equality between this MecanumDriveWheelPositions and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const MecanumDriveWheelPositions& other) const = default;
+
+ /**
+ * Checks inequality between this MecanumDriveWheelPositions and another
+ * object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const MecanumDriveWheelPositions& other) const = default;
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 9801092..97ee233 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -6,13 +6,16 @@
#include <cstddef>
+#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
@@ -58,7 +61,7 @@
*/
template <typename... Wheels>
explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
- : m_modules{wheel, wheels...} {
+ : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
static_assert(sizeof...(wheels) >= 1,
"A swerve drive requires at least two modules");
@@ -78,7 +81,7 @@
explicit SwerveDriveKinematics(
const wpi::array<Translation2d, NumModules>& wheels)
- : m_modules{wheels} {
+ : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -106,6 +109,9 @@
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
+ * In the case that the desired chassis speeds are zero (i.e. the robot will
+ * be stationary), the previously calculated module angle will be maintained.
+ *
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
@@ -125,7 +131,7 @@
*/
wpi::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
- const Translation2d& centerOfRotation = Translation2d()) const;
+ const Translation2d& centerOfRotation = Translation2d{}) const;
/**
* Performs forward kinematics to return the resulting chassis state from the
@@ -159,6 +165,38 @@
wpi::array<SwerveModuleState, NumModules> moduleStates) const;
/**
+ * Performs forward kinematics to return the resulting Twist2d from the
+ * given module position deltas. This method is often used for odometry --
+ * determining the robot's position on the field using data from the
+ * real-world position delta and angle of each module on the robot.
+ *
+ * @param wheelDeltas The latest change in position of the modules (as a
+ * SwerveModulePosition type) as measured from respective encoders and gyros.
+ * The order of the swerve module states should be same as passed into the
+ * constructor of this class.
+ *
+ * @return The resulting Twist2d.
+ */
+ template <typename... ModuleDeltas>
+ Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+
+ /**
+ * Performs forward kinematics to return the resulting Twist2d from the
+ * given module position deltas. This method is often used for odometry --
+ * determining the robot's position on the field using data from the
+ * real-world position delta and angle of each module on the robot.
+ *
+ * @param wheelDeltas The latest change in position of the modules (as a
+ * SwerveModulePosition type) as measured from respective encoders and gyros.
+ * The order of the swerve module states should be same as passed into the
+ * constructor of this class.
+ *
+ * @return The resulting Twist2d.
+ */
+ Twist2d ToTwist2d(
+ wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+
+ /**
* Renormalizes the wheel speeds if any individual speed is above the
* specified maximum.
*
@@ -177,14 +215,46 @@
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed);
+ /**
+ * Renormalizes the wheel speeds if any individual speed is above the
+ * specified maximum, as well as getting rid of joystick saturation at edges
+ * of joystick.
+ *
+ * Sometimes, after inverse kinematics, the requested speed
+ * from one or more modules may be above the max attainable speed for the
+ * driving motor on that module. To fix this issue, one can reduce all the
+ * wheel speeds to make sure that all requested module speeds are at-or-below
+ * the absolute threshold, while maintaining the ratio of speeds between
+ * modules.
+ *
+ * @param moduleStates Reference to array of module states. The array will be
+ * mutated with the normalized speeds!
+ * @param currentChassisSpeed Current speed of the robot
+ * @param attainableMaxModuleSpeed The absolute max speed a module can reach
+ * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
+ * can reach while translating
+ * @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
+ * reach while rotating
+ */
+ static void DesaturateWheelSpeeds(
+ wpi::array<SwerveModuleState, NumModules>* moduleStates,
+ ChassisSpeeds currentChassisSpeed,
+ units::meters_per_second_t attainableMaxModuleSpeed,
+ units::meters_per_second_t attainableMaxRobotTranslationSpeed,
+ units::radians_per_second_t attainableMaxRobotRotationSpeed);
+
private:
- mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
- Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
- m_forwardKinematics;
+ mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
+ Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
+ mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
mutable Translation2d m_previousCoR;
};
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ SwerveDriveKinematics<4>;
+
} // namespace frc
#include "SwerveDriveKinematics.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 7b958c1..c7f26e0 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -5,7 +5,9 @@
#pragma once
#include <algorithm>
+#include <utility>
+#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "units/math.h"
@@ -20,12 +22,21 @@
SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation) const {
+ if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
+ chassisSpeeds.omega == 0_rad_per_s) {
+ for (size_t i = 0; i < NumModules; i++) {
+ m_moduleStates[i].speed = 0_mps;
+ }
+
+ return m_moduleStates;
+ }
+
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation != m_previousCoR) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
- Eigen::Matrix<double, 2, 3>{
+ Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
@@ -37,21 +48,20 @@
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
- Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
+ Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
- wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
- units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
- units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
+ units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
+ units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
- moduleStates[i] = {speed, rotation};
+ m_moduleStates[i] = {speed, rotation};
}
- return moduleStates;
+ return m_moduleStates;
}
template <size_t NumModules>
@@ -70,17 +80,16 @@
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
wpi::array<SwerveModuleState, NumModules> moduleStates) const {
- Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
+ Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
- moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
- moduleStatesMatrix(i * 2 + 1, 0) =
- module.speed.value() * module.angle.Sin();
+ moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
+ moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
- m_forwardKinematics.solve(moduleStatesMatrix);
+ m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},
@@ -88,6 +97,39 @@
}
template <size_t NumModules>
+template <typename... ModuleDeltas>
+Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
+ ModuleDeltas&&... wheelDeltas) const {
+ static_assert(sizeof...(wheelDeltas) == NumModules,
+ "Number of modules is not consistent with number of wheel "
+ "locations provided in constructor.");
+
+ wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
+
+ return this->ToTwist2d(moduleDeltas);
+}
+
+template <size_t NumModules>
+Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
+ wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
+ Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
+
+ for (size_t i = 0; i < NumModules; ++i) {
+ SwerveModulePosition module = moduleDeltas[i];
+ moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos();
+ moduleDeltaMatrix(i * 2 + 1, 0) =
+ module.distance.value() * module.angle.Sin();
+ }
+
+ Eigen::Vector3d chassisDeltaVector =
+ m_forwardKinematics.solve(moduleDeltaMatrix);
+
+ return {units::meter_t{chassisDeltaVector(0)},
+ units::meter_t{chassisDeltaVector(1)},
+ units::radian_t{chassisDeltaVector(2)}};
+}
+
+template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
@@ -106,4 +148,41 @@
}
}
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
+ wpi::array<SwerveModuleState, NumModules>* moduleStates,
+ ChassisSpeeds currentChassisSpeed,
+ units::meters_per_second_t attainableMaxModuleSpeed,
+ units::meters_per_second_t attainableMaxRobotTranslationSpeed,
+ units::radians_per_second_t attainableMaxRobotRotationSpeed) {
+ auto& states = *moduleStates;
+
+ auto realMaxSpeed = std::max_element(states.begin(), states.end(),
+ [](const auto& a, const auto& b) {
+ return units::math::abs(a.speed) <
+ units::math::abs(b.speed);
+ })
+ ->speed;
+
+ if (attainableMaxRobotTranslationSpeed == 0_mps ||
+ attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
+ return;
+ }
+
+ auto translationalK =
+ units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) /
+ attainableMaxRobotTranslationSpeed;
+
+ auto rotationalK = units::math::abs(currentChassisSpeed.omega) /
+ attainableMaxRobotRotationSpeed;
+
+ auto k = units::math::max(translationalK, rotationalK);
+
+ auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
+ units::scalar_t(1));
+ for (auto& module : states) {
+ module.speed = module.speed * scale;
+ }
+}
+
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index d1a4958..015c2c0 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -8,10 +8,11 @@
#include <cstddef>
#include <ctime>
+#include <wpi/SymbolExports.h>
#include <wpi/timestamp.h>
#include "SwerveDriveKinematics.h"
-#include "SwerveModuleState.h"
+#include "SwerveModulePosition.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
@@ -34,11 +35,13 @@
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The wheel positions reported by each module.
* @param initialPose The starting position of the robot on the field.
*/
- SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
- const Rotation2d& gyroAngle,
- const Pose2d& initialPose = Pose2d());
+ SwerveDriveOdometry(
+ SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -46,14 +49,14 @@
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The wheel positions reported by each module.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
- m_pose = pose;
- m_previousAngle = pose.Rotation();
- m_gyroOffset = m_pose.Rotation() - gyroAngle;
- }
+ void ResetPosition(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& pose);
/**
* Returns the position of the robot on the field.
@@ -63,55 +66,35 @@
/**
* Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method takes in the current time as
- * a parameter to calculate period (difference between two timestamps). The
- * period is used to calculate the change in distance from a velocity. This
- * also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
+ * integration of the pose over time. This also takes in an angle parameter
+ * which is used instead of the angular rate that is calculated from forward
+ * kinematics.
*
- * @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
- * @param moduleStates The current state of all swerve modules. Please provide
- * the states in the same order in which you instantiated
- * your SwerveDriveKinematics.
+ * @param modulePositions The current position of all swerve modules. Please
+ * provide the positions in the same order in which you instantiated your
+ * SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
- template <typename... ModuleStates>
- const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& gyroAngle,
- ModuleStates&&... moduleStates);
-
- /**
- * Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method automatically calculates
- * the current time to calculate period (difference between two timestamps).
- * The period is used to calculate the change in distance from a velocity.
- * This also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
- *
- * @param gyroAngle The angle reported by the gyroscope.
- * @param moduleStates The current state of all swerve modules. Please provide
- * the states in the same order in which you instantiated
- * your SwerveDriveKinematics.
- *
- * @return The new pose of the robot.
- */
- template <typename... ModuleStates>
- const Pose2d& Update(const Rotation2d& gyroAngle,
- ModuleStates&&... moduleStates) {
- return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
- }
+ const Pose2d& Update(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
- units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
+
+ wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
+ wpi::empty_array};
};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ SwerveDriveOdometry<4>;
+
} // namespace frc
#include "SwerveDriveOdometry.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 96db930..64b46c1 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -11,30 +11,56 @@
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ m_previousModulePositions[i] = {modulePositions[i].distance,
+ modulePositions[i].angle};
+ }
+
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
template <size_t NumModules>
-template <typename... ModuleStates>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& gyroAngle,
- ModuleStates&&... moduleStates) {
- units::second_t deltaTime =
- (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
- m_previousTime = currentTime;
+void SwerveDriveOdometry<NumModules>::ResetPosition(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+ const Pose2d& pose) {
+ m_pose = pose;
+ m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ m_previousModulePositions[i].distance = modulePositions[i].distance;
+ }
+}
+
+template <size_t NumModules>
+const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
+ const Rotation2d& gyroAngle,
+ const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+ auto moduleDeltas =
+ wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+ for (size_t index = 0; index < NumModules; index++) {
+ auto lastPosition = m_previousModulePositions[index];
+ auto currentPosition = modulePositions[index];
+ moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
+ currentPosition.angle};
+
+ m_previousModulePositions[index].distance = modulePositions[index].distance;
+ }
auto angle = gyroAngle + m_gyroOffset;
- auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
- static_cast<void>(dtheta);
+ auto twist = m_kinematics.ToTwist2d(moduleDeltas);
+ twist.dtheta = (angle - m_previousAngle).Radians();
- auto newPose = m_pose.Exp(
- {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+ auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
new file mode 100644
index 0000000..18ed464
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * Represents the position of one swerve module.
+ */
+struct WPILIB_DLLEXPORT SwerveModulePosition {
+ /**
+ * Distance the wheel of a module has traveled
+ */
+ units::meter_t distance = 0_m;
+
+ /**
+ * Angle of the module.
+ */
+ Rotation2d angle;
+};
+} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index 8126349..0636707 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,44 +40,41 @@
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
- Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
- return m_coefficients;
- }
+ Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
private:
- Eigen::Matrix<double, 6, 4> m_coefficients =
- Eigen::Matrix<double, 6, 4>::Zero();
+ Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
- static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+ static Matrixd<4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
- // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+ // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
//
- // P(i) = P(0) = a0
- // P'(i) = P'(0) = a1
- // P(i+1) = P(1) = a3 + a2 + a1 + a0
- // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+ // P(i) = P(0) = a₀
+ // P'(i) = P'(0) = a₁
+ // P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀
+ // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
//
- // [ P(i) ] = [ 0 0 0 1 ][ a3 ]
- // [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
- // [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
- // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+ // [P(i) ] = [0 0 0 1][a₃]
+ // [P'(i) ] = [0 0 1 0][a₂]
+ // [P(i+1) ] = [1 1 1 1][a₁]
+ // [P'(i+1)] = [3 2 1 0][a₀]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
- // [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
- // [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
- // [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
- // [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
+ // [a₃] = [ 2 1 -2 1][P(i) ]
+ // [a₂] = [-3 -2 3 -1][P'(i) ]
+ // [a₁] = [ 0 1 0 0][P(i+1) ]
+ // [a₀] = [ 1 0 0 0][P'(i+1)]
- static const Eigen::Matrix<double, 4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
- {-3.0, -2.0, +3.0, -1.0},
- {+0.0, +1.0, +0.0, +0.0},
- {+1.0, +0.0, +0.0, +0.0}};
+ static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
+ {-3.0, -2.0, +3.0, -1.0},
+ {+0.0, +1.0, +0.0, +0.0},
+ {+1.0, +0.0, +0.0, +0.0}};
return basis;
}
diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 5ba3e2a..ad03a23 100644
--- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,48 +40,45 @@
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
- Eigen::Matrix<double, 6, 6> Coefficients() const override {
- return m_coefficients;
- }
+ Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
private:
- Eigen::Matrix<double, 6, 6> m_coefficients =
- Eigen::Matrix<double, 6, 6>::Zero();
+ Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
- static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
- // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
- // vectors, we want to find the coefficients of the spline
- // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+ static Matrixd<6, 6> MakeHermiteBasis() {
+ // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors,
+ // we want to find the coefficients of the spline
+ // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀.
//
- // P(i) = P(0) = a0
- // P'(i) = P'(0) = a1
- // P''(i) = P''(0) = 2 * a2
- // P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
- // P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
- // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+ // P(i) = P(0) = a₀
+ // P'(i) = P'(0) = a₁
+ // P''(i) = P"(0) = 2a₂
+ // P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀
+ // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁
+ // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂
//
- // [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
- // [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
- // [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
- // [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
- // [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
- // [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
+ // [P(i) ] = [ 0 0 0 0 0 1][a₅]
+ // [P'(i) ] = [ 0 0 0 0 1 0][a₄]
+ // [P"(i) ] = [ 0 0 0 2 0 0][a₃]
+ // [P(i+1) ] = [ 1 1 1 1 1 1][a₂]
+ // [P'(i+1)] = [ 5 4 3 2 1 0][a₁]
+ // [P"(i+1)] = [20 12 6 2 0 0][a₀]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
- // [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
- // [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
- // [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
- // [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
- // [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
- // [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
+ // [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ]
+ // [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ]
+ // [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ]
+ // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ]
+ // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)]
+ // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)]
- static const Eigen::Matrix<double, 6, 6> basis{
+ static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
@@ -100,11 +97,10 @@
*
* @return The control vector matrix for a dimension.
*/
- static Eigen::Vector<double, 6> ControlVectorFromArrays(
- wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
- return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
- initialVector[2], finalVector[0],
- finalVector[1], finalVector[2]};
+ static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
+ wpi::array<double, 3> finalVector) {
+ return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
+ finalVector[0], finalVector[1], finalVector[2]};
}
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h
index 2dd248a..b5ff024 100644
--- a/wpimath/src/main/native/include/frc/spline/Spline.h
+++ b/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -9,7 +9,7 @@
#include <wpi/array.h>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
#include "units/length.h"
@@ -55,7 +55,7 @@
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
- Eigen::Vector<double, Degree + 1> polynomialBases;
+ Vectord<Degree + 1> polynomialBases;
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@
// This simply multiplies by the coefficients. We need to divide out t some
// n number of times where n is the derivative we want to take.
- Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
+ Vectord<6> combined = Coefficients() * polynomialBases;
double dx, dy, ddx, ddy;
@@ -90,8 +90,8 @@
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
return {
- {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
- units::curvature_t(curvature)};
+ {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
+ units::curvature_t{curvature}};
}
protected:
@@ -100,7 +100,7 @@
*
* @return The coefficients of the spline.
*/
- virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+ virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
/**
* Converts a Translation2d into a vector that is compatible with Eigen.
@@ -119,7 +119,7 @@
* @return The Translation2d.
*/
static Translation2d FromVector(const Eigen::Vector2d& vector) {
- return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+ return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}};
}
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
index 722bb5f..957b875 100644
--- a/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -4,7 +4,7 @@
#pragma once
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "units/time.h"
#include "unsupported/Eigen/MatrixFunctions"
@@ -19,9 +19,9 @@
* @param discA Storage for discrete system matrix.
*/
template <int States>
-void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
- units::second_t dt,
- Eigen::Matrix<double, States, States>* discA) {
+void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
+ Matrixd<States, States>* discA) {
+ // A_d = eᴬᵀ
*discA = (contA * dt.value()).exp();
}
@@ -37,21 +37,23 @@
* @param discB Storage for discrete input matrix.
*/
template <int States, int Inputs>
-void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
- const Eigen::Matrix<double, States, Inputs>& contB,
- units::second_t dt,
- Eigen::Matrix<double, States, States>* discA,
- Eigen::Matrix<double, States, Inputs>* discB) {
- // Matrices are blocked here to minimize matrix exponentiation calculations
- Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
- Mcont.setZero();
- Mcont.template block<States, States>(0, 0) = contA * dt.value();
- Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
+void DiscretizeAB(const Matrixd<States, States>& contA,
+ const Matrixd<States, Inputs>& contB, units::second_t dt,
+ Matrixd<States, States>* discA,
+ Matrixd<States, Inputs>* discB) {
+ // M = [A B]
+ // [0 0]
+ Matrixd<States + Inputs, States + Inputs> M;
+ M.setZero();
+ M.template block<States, States>(0, 0) = contA;
+ M.template block<States, Inputs>(0, States) = contB;
- // Discretize A and B with the given timestep
- Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
- *discA = Mdisc.template block<States, States>(0, 0);
- *discB = Mdisc.template block<States, Inputs>(0, States);
+ // ϕ = eᴹᵀ = [A_d B_d]
+ // [ 0 I ]
+ Matrixd<States + Inputs, States + Inputs> phi = (M * dt.value()).exp();
+
+ *discA = phi.template block<States, States>(0, 0);
+ *discB = phi.template block<States, Inputs>(0, States);
}
/**
@@ -65,29 +67,30 @@
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
-void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
- const Eigen::Matrix<double, States, States>& contQ,
- units::second_t dt,
- Eigen::Matrix<double, States, States>* discA,
- Eigen::Matrix<double, States, States>* discQ) {
+void DiscretizeAQ(const Matrixd<States, States>& contA,
+ const Matrixd<States, States>& contQ, units::second_t dt,
+ Matrixd<States, States>* discA,
+ Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
- Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+ Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
- // Set up the matrix M = [[-A, Q], [0, A.T]]
- Eigen::Matrix<double, 2 * States, 2 * States> M;
+ // M = [−A Q ]
+ // [ 0 Aᵀ]
+ Matrixd<2 * States, 2 * States> M;
M.template block<States, States>(0, 0) = -contA;
M.template block<States, States>(0, States) = Q;
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
- Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
+ // ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d]
+ // [ 0 A_dᵀ ]
+ Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp();
- // Phi12 = phi[0:States, States:2*States]
- // Phi22 = phi[States:2*States, States:2*States]
- Eigen::Matrix<double, States, States> phi12 =
- phi.block(0, States, States, States);
- Eigen::Matrix<double, States, States> phi22 =
- phi.block(States, States, States, States);
+ // ϕ₁₂ = A_d⁻¹Q_d
+ Matrixd<States, States> phi12 = phi.block(0, States, States, States);
+
+ // ϕ₂₂ = A_dᵀ
+ Matrixd<States, States> phi22 = phi.block(States, States, States, States);
*discA = phi22.transpose();
@@ -104,10 +107,12 @@
* (which is expensive), we take advantage of the structure of the block matrix
* of A and Q.
*
- * 1) The exponential of A*t, which is only N x N, is relatively cheap.
- * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
- * using a taylor series to several terms and still be substantially cheaper
- * than taking the big exponential.
+ * <ul>
+ * <li>eᴬᵀ, which is only N x N, is relatively cheap.
+ * <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate
+ * using a taylor series to several terms and still be substantially
+ * cheaper than taking the big exponential.
+ * </ul>
*
* @tparam States Number of states.
* @param contA Continuous system matrix.
@@ -117,30 +122,64 @@
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
-void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
- const Eigen::Matrix<double, States, States>& contQ,
- units::second_t dt,
- Eigen::Matrix<double, States, States>* discA,
- Eigen::Matrix<double, States, States>* discQ) {
- // Make continuous Q symmetric if it isn't already
- Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
+ const Matrixd<States, States>& contQ,
+ units::second_t dt, Matrixd<States, States>* discA,
+ Matrixd<States, States>* discQ) {
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
+ //
+ // M = [−A Q ]
+ // [ 0 Aᵀ]
+ // ϕ = eᴹᵀ
+ // ϕ₁₂ = A_d⁻¹Q_d
+ //
+ // Taylor series of ϕ:
+ //
+ // ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
+ // ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
+ //
+ // Taylor series of ϕ expanded for ϕ₁₂:
+ //
+ // ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
+ //
+ // ```
+ // lastTerm = Q
+ // lastCoeff = T
+ // ATn = Aᵀ
+ // ϕ₁₂ = lastTerm lastCoeff = QT
+ //
+ // for i in range(2, 6):
+ // // i = 2
+ // lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
+ // lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
+ // ATn *= Aᵀ = Aᵀ²
+ //
+ // // i = 3
+ // lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
+ // …
+ // ```
- Eigen::Matrix<double, States, States> lastTerm = Q;
+ // Make continuous Q symmetric if it isn't already
+ Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
+
+ Matrixd<States, States> lastTerm = Q;
double lastCoeff = dt.value();
// Aᵀⁿ
- Eigen::Matrix<double, States, States> Atn = contA.transpose();
+ Matrixd<States, States> ATn = contA.transpose();
- Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
+ Matrixd<States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
- lastTerm = -contA * lastTerm + Q * Atn;
+ lastTerm = -contA * lastTerm + Q * ATn;
lastCoeff *= dt.value() / static_cast<double>(i);
phi12 += lastTerm * lastCoeff;
- Atn *= contA.transpose();
+ ATn *= contA.transpose();
}
DiscretizeA<States>(contA, dt, discA);
@@ -159,8 +198,9 @@
* @param dt Discretization timestep.
*/
template <int Outputs>
-Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
- const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
+Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
+ units::second_t dt) {
+ // R_d = 1/T R
return R / dt.value();
}
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h
index bd3ff40..dff2433 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystem.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -8,7 +8,7 @@
#include <functional>
#include <stdexcept>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
@@ -30,6 +30,10 @@
template <int States, int Inputs, int Outputs>
class LinearSystem {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+ using OutputVector = Vectord<Outputs>;
+
/**
* Constructs a discrete plant with the given continuous system coefficients.
*
@@ -39,10 +43,10 @@
* @param D Feedthrough matrix.
* @throws std::domain_error if any matrix element isn't finite.
*/
- LinearSystem(const Eigen::Matrix<double, States, States>& A,
- const Eigen::Matrix<double, States, Inputs>& B,
- const Eigen::Matrix<double, Outputs, States>& C,
- const Eigen::Matrix<double, Outputs, Inputs>& D) {
+ LinearSystem(const Matrixd<States, States>& A,
+ const Matrixd<States, Inputs>& B,
+ const Matrixd<Outputs, States>& C,
+ const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
throw std::domain_error(
"Elements of A aren't finite. This is usually due to model "
@@ -78,7 +82,7 @@
/**
* Returns the system matrix A.
*/
- const Eigen::Matrix<double, States, States>& A() const { return m_A; }
+ const Matrixd<States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
@@ -91,7 +95,7 @@
/**
* Returns the input matrix B.
*/
- const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
+ const Matrixd<States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
@@ -104,7 +108,7 @@
/**
* Returns the output matrix C.
*/
- const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
+ const Matrixd<Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
@@ -117,7 +121,7 @@
/**
* Returns the feedthrough matrix D.
*/
- const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
+ const Matrixd<Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
@@ -137,11 +141,10 @@
* @param clampedU The control input.
* @param dt Timestep for model update.
*/
- Eigen::Vector<double, States> CalculateX(
- const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, Inputs> discB;
+ StateVector CalculateX(const StateVector& x, const InputVector& clampedU,
+ units::second_t dt) const {
+ Matrixd<States, States> discA;
+ Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
return discA * x + discB * clampedU;
@@ -156,9 +159,8 @@
* @param x The current state.
* @param clampedU The control input.
*/
- Eigen::Vector<double, Outputs> CalculateY(
- const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, Inputs>& clampedU) const {
+ OutputVector CalculateY(const StateVector& x,
+ const InputVector& clampedU) const {
return m_C * x + m_D * clampedU;
}
@@ -166,22 +168,22 @@
/**
* Continuous system matrix.
*/
- Eigen::Matrix<double, States, States> m_A;
+ Matrixd<States, States> m_A;
/**
* Continuous input matrix.
*/
- Eigen::Matrix<double, States, Inputs> m_B;
+ Matrixd<States, Inputs> m_B;
/**
* Output matrix.
*/
- Eigen::Matrix<double, Outputs, States> m_C;
+ Matrixd<Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
- Eigen::Matrix<double, Outputs, Inputs> m_D;
+ Matrixd<Outputs, Inputs> m_D;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index a18c03b..1300a82 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -4,7 +4,9 @@
#pragma once
-#include "Eigen/Core"
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -35,6 +37,10 @@
template <int States, int Inputs, int Outputs>
class LinearSystemLoop {
public:
+ using StateVector = Vectord<States>;
+ using InputVector = Vectord<Inputs>;
+ using OutputVector = Vectord<Outputs>;
+
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
@@ -53,7 +59,7 @@
units::volt_t maxVoltage, units::second_t dt)
: LinearSystemLoop(
plant, controller, observer,
- [=](const Eigen::Vector<double, Inputs>& u) {
+ [=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -73,9 +79,7 @@
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
KalmanFilter<States, Inputs, Outputs>& observer,
- std::function<Eigen::Vector<double, Inputs>(
- const Eigen::Vector<double, Inputs>&)>
- clampFunction,
+ std::function<InputVector(const InputVector&)> clampFunction,
units::second_t dt)
: LinearSystemLoop(
controller,
@@ -97,11 +101,10 @@
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
- : LinearSystemLoop(controller, feedforward, observer,
- [=](const Eigen::Vector<double, Inputs>& u) {
- return frc::DesaturateInputVector<Inputs>(
- u, maxVoltage.value());
- }) {}
+ : LinearSystemLoop(
+ controller, feedforward, observer, [=](const InputVector& u) {
+ return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
+ }) {}
/**
* Constructs a state-space loop with the given controller, feedforward,
@@ -117,9 +120,7 @@
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
- std::function<
- Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
- clampFunction)
+ std::function<InputVector(const InputVector&)> clampFunction)
: m_controller(&controller),
m_feedforward(feedforward),
m_observer(&observer),
@@ -134,9 +135,7 @@
/**
* Returns the observer's state estimate x-hat.
*/
- const Eigen::Vector<double, States>& Xhat() const {
- return m_observer->Xhat();
- }
+ const StateVector& Xhat() const { return m_observer->Xhat(); }
/**
* Returns an element of the observer's state estimate x-hat.
@@ -148,7 +147,7 @@
/**
* Returns the controller's next reference r.
*/
- const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
+ const StateVector& NextR() const { return m_nextR; }
/**
* Returns an element of the controller's next reference r.
@@ -160,7 +159,7 @@
/**
* Returns the controller's calculated control input u.
*/
- Eigen::Vector<double, Inputs> U() const {
+ InputVector U() const {
return ClampInput(m_controller->U() + m_feedforward.Uff());
}
@@ -176,9 +175,7 @@
*
* @param xHat The initial state estimate x-hat.
*/
- void SetXhat(const Eigen::Vector<double, States>& xHat) {
- m_observer->SetXhat(xHat);
- }
+ void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); }
/**
* Set an element of the initial state estimate x-hat.
@@ -193,7 +190,7 @@
*
* @param nextR Next reference.
*/
- void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
+ void SetNextR(const StateVector& nextR) { m_nextR = nextR; }
/**
* Return the controller used internally.
@@ -215,7 +212,7 @@
* Return the observer used internally.
*/
const KalmanFilter<States, Inputs, Outputs>& Observer() const {
- return m_observer;
+ return *m_observer;
}
/**
@@ -225,7 +222,7 @@
*
* @param initialState The initial state.
*/
- void Reset(const Eigen::Vector<double, States>& initialState) {
+ void Reset(const StateVector& initialState) {
m_nextR.setZero();
m_controller->Reset();
m_feedforward.Reset(initialState);
@@ -235,18 +232,14 @@
/**
* Returns difference between reference r and current state x-hat.
*/
- Eigen::Vector<double, States> Error() const {
- return m_controller->R() - m_observer->Xhat();
- }
+ StateVector Error() const { return m_controller->R() - m_observer->Xhat(); }
/**
* Correct the state estimate x-hat using the measurements in y.
*
* @param y Measurement vector.
*/
- void Correct(const Eigen::Vector<double, Outputs>& y) {
- m_observer->Correct(U(), y);
- }
+ void Correct(const OutputVector& y) { m_observer->Correct(U(), y); }
/**
* Sets new controller output, projects model forward, and runs observer
@@ -258,7 +251,7 @@
* @param dt Timestep for model update.
*/
void Predict(units::second_t dt) {
- Eigen::Vector<double, Inputs> u =
+ InputVector u =
ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
m_feedforward.Calculate(m_nextR));
m_observer->Predict(u, dt);
@@ -270,10 +263,7 @@
* @param u Input vector to clamp.
* @return Clamped input vector.
*/
- Eigen::Vector<double, Inputs> ClampInput(
- const Eigen::Vector<double, Inputs>& u) const {
- return m_clampFunc(u);
- }
+ InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); }
protected:
LinearQuadraticRegulator<States, Inputs>* m_controller;
@@ -283,12 +273,10 @@
/**
* Clamping function.
*/
- std::function<Eigen::Vector<double, Inputs>(
- const Eigen::Vector<double, Inputs>&)>
- m_clampFunc;
+ std::function<InputVector(const InputVector&)> m_clampFunc;
// Reference to go to in the next cycle (used by feedforward controller).
- Eigen::Vector<double, States> m_nextR;
+ StateVector m_nextR;
// These are accessible from non-templated subclasses.
static constexpr int kStates = States;
@@ -296,4 +284,9 @@
static constexpr int kOutputs = Outputs;
};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ LinearSystemLoop<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+ LinearSystemLoop<2, 1, 1>;
+
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index 68d047f..bb856ec 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -54,80 +54,6 @@
}
/**
- * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described
- * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
- *
- * @param f The function to integrate. It must take two arguments x and
- * u.
- * @param x The initial value of x.
- * @param u The value u held constant over the integration period.
- * @param dt The time over which to integrate.
- * @param maxError The maximum acceptable truncation error. Usually a small
- * number like 1e-6.
- */
-template <typename F, typename T, typename U>
-T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
- // See
- // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
- // for the Butcher tableau the following arrays came from.
- constexpr int kDim = 6;
-
- // clang-format off
- constexpr double A[kDim - 1][kDim - 1]{
- { 1.0 / 4.0},
- { 3.0 / 32.0, 9.0 / 32.0},
- {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
- { 439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0},
- { -8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}};
- // clang-format on
-
- constexpr std::array<double, kDim> b1{16.0 / 135.0, 0.0,
- 6656.0 / 12825.0, 28561.0 / 56430.0,
- -9.0 / 50.0, 2.0 / 55.0};
- constexpr std::array<double, kDim> b2{
- 25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
-
- T newX;
- double truncationError;
-
- double dtElapsed = 0.0;
- double h = dt.value();
-
- // Loop until we've gotten to our desired dt
- while (dtElapsed < dt.value()) {
- do {
- // Only allow us to advance up to the dt remaining
- h = std::min(h, dt.value() - dtElapsed);
-
- // Notice how the derivative in the Wikipedia notation is dy/dx.
- // That means their y is our x and their x is our t
- // clang-format off
- T k1 = f(x, u);
- T k2 = f(x + h * (A[0][0] * k1), u);
- T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
- T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
- T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
- T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
- // clang-format on
-
- newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
- b1[4] * k5 + b1[5] * k6);
- truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
- (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
- (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6))
- .norm();
-
- h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
- } while (truncationError > maxError);
-
- dtElapsed += h;
- x = newX;
- }
-
- return x;
-}
-
-/**
* Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and
@@ -173,7 +99,7 @@
while (dtElapsed < dt.value()) {
do {
// Only allow us to advance up to the dt remaining
- h = std::min(h, dt.value() - dtElapsed);
+ h = (std::min)(h, dt.value() - dtElapsed);
// clang-format off
T k1 = f(x, u);
diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
index 5f1bc78..c4cb695 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -4,7 +4,7 @@
#pragma once
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
namespace frc {
@@ -17,16 +17,16 @@
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
-auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
+auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
- Eigen::Matrix<double, Rows, Cols> result;
+ Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
- Eigen::Vector<double, Cols> dX_plus = x;
+ Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
- Eigen::Vector<double, Cols> dX_minus = x;
+ Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
@@ -48,12 +48,10 @@
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, Inputs>& u,
- Args&&... args) {
+auto NumericalJacobianX(F&& f, const Vectord<States>& x,
+ const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
- [&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
- x);
+ [&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
@@ -70,12 +68,10 @@
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
- const Eigen::Vector<double, Inputs>& u,
- Args&&... args) {
+auto NumericalJacobianU(F&& f, const Vectord<States>& x,
+ const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
- [&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
- u);
+ [&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index a519b0e..831f532 100644
--- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -76,6 +76,49 @@
}
/**
+ * Returns torque produced by the motor with a given current.
+ *
+ * @param current The current drawn by the motor.
+ */
+ constexpr units::newton_meter_t Torque(units::ampere_t current) const {
+ return current * Kt;
+ }
+
+ /**
+ * Returns the voltage provided to the motor for a given torque and
+ * angular velocity.
+ *
+ * @param torque The torque produced by the motor.
+ * @param speed The current angular velocity of the motor.
+ */
+ constexpr units::volt_t Voltage(units::newton_meter_t torque,
+ units::radians_per_second_t speed) const {
+ return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
+ }
+
+ /**
+ * Returns the speed produced by the motor at a given torque and input
+ * voltage.
+ *
+ * @param torque The torque produced by the motor.
+ * @param inputVoltage The input voltage provided to the motor.
+ */
+ constexpr units::radians_per_second_t Speed(
+ units::newton_meter_t torque, units::volt_t inputVoltage) const {
+ return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
+ }
+
+ /**
+ * Returns a copy of this motor with the given gearbox reduction applied.
+ *
+ * @param gearboxReduction The gearbox reduction.
+ */
+ constexpr DCMotor WithReduction(double gearboxReduction) {
+ return DCMotor(nominalVoltage, stallTorque * gearboxReduction, stallCurrent,
+ freeCurrent, freeSpeed / gearboxReduction);
+ }
+
+ /**
* Returns instance of CIM.
*/
static constexpr DCMotor CIM(int numMotors = 1) {
diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 26142bf..3e69545 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -31,94 +31,49 @@
units::inverse<units::seconds>>>;
/**
- * Constructs the state-space model for an elevator.
+ * Create a state-space model of the elevator system. The states of the system
+ * are [position, velocity], inputs are [voltage], and outputs are [position].
*
- * States: [[position], [velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[position]]
- *
- * @param motor Instance of DCMotor.
- * @param m Carriage mass.
- * @param r Pulley radius.
+ * @param motor The motor (or gearbox) attached to the carriage.
+ * @param mass The mass of the elevator carriage, in kilograms.
+ * @param radius The radius of the elevator's driving drum, in meters.
* @param G Gear ratio from motor to carriage.
- * @throws std::domain_error if m <= 0, r <= 0, or G <= 0.
+ * @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
- units::kilogram_t m,
- units::meter_t r, double G) {
- if (m <= 0_kg) {
- throw std::domain_error("m must be greater than zero.");
- }
- if (r <= 0_m) {
- throw std::domain_error("r must be greater than zero.");
- }
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
- }
-
- Eigen::Matrix<double, 2, 2> A{
- {0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt /
- (motor.R * units::math::pow<2>(r) * m * motor.Kv))
- .value()}};
- Eigen::Matrix<double, 2, 1> B{0.0,
- (G * motor.Kt / (motor.R * r * m)).value()};
- Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
- Eigen::Matrix<double, 1, 1> D{0.0};
-
- return LinearSystem<2, 1, 1>(A, B, C, D);
- }
+ units::kilogram_t mass,
+ units::meter_t radius, double G);
/**
- * Constructs the state-space model for a single-jointed arm.
+ * Create a state-space model of a single-jointed arm system.The states of the
+ * system are [angle, angular velocity], inputs are [voltage], and outputs are
+ * [angle].
*
- * States: [[angle], [angular velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[angle]]
- *
- * @param motor Instance of DCMotor.
- * @param J Moment of inertia.
- * @param G Gear ratio from motor to carriage.
+ * @param motor The motor (or gearbox) attached to the arm.
+ * @param J The moment of inertia J of the arm.
+ * @param G Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
- DCMotor motor, units::kilogram_square_meter_t J, double G) {
- if (J <= 0_kg_sq_m) {
- throw std::domain_error("J must be greater than zero.");
- }
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
- }
-
- Eigen::Matrix<double, 2, 2> A{
- {0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
- Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
- Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
- Eigen::Matrix<double, 1, 1> D{0.0};
-
- return LinearSystem<2, 1, 1>(A, B, C, D);
- }
+ DCMotor motor, units::kilogram_square_meter_t J, double G);
/**
- * Constructs the state-space model for a 1 DOF velocity-only system from
- * system identification data.
+ * Create a state-space model for a 1 DOF velocity system from its kV
+ * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
+ * found using SysId. The states of the system are [velocity], inputs are
+ * [voltage], and outputs are [velocity].
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
* actual method arguments; they will automatically be converted to SI
* internally.
*
- * States: [[velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[velocity]]
- *
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
- * @param kV The velocity gain, in volt seconds per distance.
- * @param kA The acceleration gain, in volt seconds^2 per distance.
+ * @param kV The velocity gain, in volts/(unit/sec).
+ * @param kA The acceleration gain, in volts/(unit/sec²).
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<
@@ -134,33 +89,32 @@
throw std::domain_error("Ka must be greater than zero.");
}
- Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
- Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
- Eigen::Matrix<double, 1, 1> C{1.0};
- Eigen::Matrix<double, 1, 1> D{0.0};
+ Matrixd<1, 1> A{-kV.value() / kA.value()};
+ Matrixd<1, 1> B{1.0 / kA.value()};
+ Matrixd<1, 1> C{1.0};
+ Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
- * Constructs the state-space model for a 1 DOF position system from system
- * identification data.
+ * Create a state-space model for a 1 DOF position system from its kV
+ * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
+ * found using SysId. the states of the system are [position, velocity],
+ * inputs are [voltage], and outputs are [position].
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
* actual method arguments; they will automatically be converted to SI
* internally.
*
- * States: [[position], [velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[position]]
- *
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
- * @param kV The velocity gain, in volt seconds per distance.
- * @param kA The acceleration gain, in volt seconds^2 per distance.
+ * @param kV The velocity gain, in volts/(unit/sec).
+ * @param kA The acceleration gain, in volts/(unit/sec²).
+ *
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<
@@ -176,236 +130,111 @@
throw std::domain_error("Ka must be greater than zero.");
}
- Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
- Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
- Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
- Eigen::Matrix<double, 1, 1> D{0.0};
+ Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
+ Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
+ Matrixd<1, 2> C{1.0, 0.0};
+ Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
- * Constructs the state-space model for a 2 DOF drivetrain velocity system
- * from system identification data.
+ * Identify a differential drive drivetrain given the drivetrain's kV and kA
+ * in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
+ * {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be
+ * found using SysId.
*
- * States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
+ * States: [[left velocity], [right velocity]]<br>
+ * Inputs: [[left voltage], [right voltage]]<br>
* Outputs: [[left velocity], [right velocity]]
*
- * @param kVlinear The linear velocity gain in volts per (meter per second).
- * @param kAlinear The linear acceleration gain in volts per (meter per
+ * @param kVLinear The linear velocity gain in volts per (meters per second).
+ * @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
- * @param kVangular The angular velocity gain in volts per (meter per second).
- * @param kAangular The angular acceleration gain in volts per (meter per
+ * @param kVAngular The angular velocity gain in volts per (meters per
+ * second).
+ * @param kAAngular The angular acceleration gain in volts per (meters per
* second squared).
- * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
- * or kAangular <= 0.
+ * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
+ * or kAAngular <= 0.
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
- decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
- decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
- if (kVlinear <= decltype(kVlinear){0}) {
- throw std::domain_error("Kv,linear must be greater than zero.");
- }
- if (kAlinear <= decltype(kAlinear){0}) {
- throw std::domain_error("Ka,linear must be greater than zero.");
- }
- if (kVangular <= decltype(kVangular){0}) {
- throw std::domain_error("Kv,angular must be greater than zero.");
- }
- if (kAangular <= decltype(kAangular){0}) {
- throw std::domain_error("Ka,angular must be greater than zero.");
- }
-
- double A1 = -(kVlinear.value() / kAlinear.value() +
- kVangular.value() / kAangular.value());
- double A2 = -(kVlinear.value() / kAlinear.value() -
- kVangular.value() / kAangular.value());
- double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
- double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
-
- Eigen::Matrix<double, 2, 2> A =
- 0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
- Eigen::Matrix<double, 2, 2> B =
- 0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
- Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
- Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
-
- return LinearSystem<2, 2, 2>(A, B, C, D);
- }
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular);
/**
- * Constructs the state-space model for a 2 DOF drivetrain velocity system
- * from system identification data.
+ * Identify a differential drive drivetrain given the drivetrain's kV and kA
+ * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
+ * {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found
+ * using SysId.
*
- * States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
+ * States: [[left velocity], [right velocity]]<br>
+ * Inputs: [[left voltage], [right voltage]]<br>
* Outputs: [[left velocity], [right velocity]]
*
- * @param kVlinear The linear velocity gain in volts per (meter per second).
- * @param kAlinear The linear acceleration gain in volts per (meter per
+ * @param kVLinear The linear velocity gain in volts per (meters per
+ * second).
+ * @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
- * @param kVangular The angular velocity gain in volts per (radian per
+ * @param kVAngular The angular velocity gain in volts per (radians per
* second).
- * @param kAangular The angular acceleration gain in volts per (radian per
+ * @param kAAngular The angular acceleration gain in volts per (radians per
* second squared).
- * @param trackwidth The width of the drivetrain.
- * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
- * kAangular <= 0, or trackwidth <= 0.
+ * @param trackwidth The distance between the differential drive's left and
+ * right wheels, in meters.
+ * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
+ * kAAngular <= 0, or trackwidth <= 0.
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
- decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
- decltype(1_V / 1_rad_per_s) kVangular,
- decltype(1_V / 1_rad_per_s_sq) kAangular, units::meter_t trackwidth) {
- if (kVlinear <= decltype(kVlinear){0}) {
- throw std::domain_error("Kv,linear must be greater than zero.");
- }
- if (kAlinear <= decltype(kAlinear){0}) {
- throw std::domain_error("Ka,linear must be greater than zero.");
- }
- if (kVangular <= decltype(kVangular){0}) {
- throw std::domain_error("Kv,angular must be greater than zero.");
- }
- if (kAangular <= decltype(kAangular){0}) {
- throw std::domain_error("Ka,angular must be greater than zero.");
- }
- if (trackwidth <= 0_m) {
- throw std::domain_error("r must be greater than zero.");
- }
-
- // We want to find a factor to include in Kv,angular that will convert
- // `u = Kv,angular omega` to `u = Kv,angular v`.
- //
- // v = omega r
- // omega = v/r
- // omega = 1/r v
- // omega = 1/(trackwidth/2) v
- // omega = 2/trackwidth v
- //
- // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
- // to V/(m/s).
- return IdentifyDrivetrainSystem(kVlinear, kAlinear,
- kVangular * 2.0 / trackwidth * 1_rad,
- kAangular * 2.0 / trackwidth * 1_rad);
- }
+ decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+ decltype(1_V / 1_rad_per_s) kVAngular,
+ decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth);
/**
- * Constructs the state-space model for a flywheel.
+ * Create a state-space model of a flywheel system, the states of the system
+ * are [angular velocity], inputs are [voltage], and outputs are [angular
+ * velocity].
*
- * States: [[angular velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[angular velocity]]
- *
- * @param motor Instance of DCMotor.
- * @param J Moment of inertia.
- * @param G Gear ratio from motor to carriage.
+ * @param motor The motor (or gearbox) attached to the flywheel.
+ * @param J The moment of inertia J of the flywheel.
+ * @param G Gear ratio from motor to flywheel.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
units::kilogram_square_meter_t J,
- double G) {
- if (J <= 0_kg_sq_m) {
- throw std::domain_error("J must be greater than zero.");
- }
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
- }
-
- Eigen::Matrix<double, 1, 1> A{
- (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
- Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
- Eigen::Matrix<double, 1, 1> C{1.0};
- Eigen::Matrix<double, 1, 1> D{0.0};
-
- return LinearSystem<1, 1, 1>(A, B, C, D);
- }
+ double G);
/**
- * Constructs the state-space model for a DC motor motor.
+ * Create a state-space model of a DC motor system. The states of the system
+ * are [angular position, angular velocity], inputs are [voltage], and outputs
+ * are [angular position, angular velocity].
*
- * States: [[angular position, angular velocity]]
- * Inputs: [[voltage]]
- * Outputs: [[angular position, angular velocity]]
- *
- * @param motor Instance of DCMotor.
- * @param J Moment of inertia.
- * @param G Gear ratio from motor to carriage.
+ * @param motor The motor (or gearbox) attached to the system.
+ * @param J the moment of inertia J of the DC motor.
+ * @param G Gear ratio from motor to output.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
units::kilogram_square_meter_t J,
- double G) {
- if (J <= 0_kg_sq_m) {
- throw std::domain_error("J must be greater than zero.");
- }
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
- }
-
- Eigen::Matrix<double, 2, 2> A{
- {0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
- Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
- Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
- Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
-
- return LinearSystem<2, 1, 2>(A, B, C, D);
- }
-
+ double G);
/**
- * Constructs the state-space model for a drivetrain.
+ * Create a state-space model of differential drive drivetrain. In this model,
+ * the states are [left velocity, right velocity], the inputs are [left
+ * voltage, right voltage], and the outputs are [left velocity, right
+ * velocity]
*
- * States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
- * Outputs: [[left velocity], [right velocity]]
- *
- * @param motor Instance of DCMotor.
- * @param m Drivetrain mass.
- * @param r Wheel radius.
- * @param rb Robot radius.
- * @param J Moment of inertia.
+ * @param motor The motor (or gearbox) driving the drivetrain.
+ * @param mass The mass of the robot in kilograms.
+ * @param r The radius of the wheels in meters.
+ * @param rb The radius of the base (half of the track width), in meters.
+ * @param J The moment of inertia of the robot.
* @param G Gear ratio from motor to wheel.
- * @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or
+ * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
* G <= 0.
*/
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
- const DCMotor& motor, units::kilogram_t m, units::meter_t r,
- units::meter_t rb, units::kilogram_square_meter_t J, double G) {
- if (m <= 0_kg) {
- throw std::domain_error("m must be greater than zero.");
- }
- if (r <= 0_m) {
- throw std::domain_error("r must be greater than zero.");
- }
- if (rb <= 0_m) {
- throw std::domain_error("rb must be greater than zero.");
- }
- if (J <= 0_kg_sq_m) {
- throw std::domain_error("J must be greater than zero.");
- }
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
- }
-
- auto C1 = -std::pow(G, 2) * motor.Kt /
- (motor.Kv * motor.R * units::math::pow<2>(r));
- auto C2 = G * motor.Kt / (motor.R * r);
-
- Eigen::Matrix<double, 2, 2> A{
- {((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
- ((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
- {((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
- ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
- Eigen::Matrix<double, 2, 2> B{
- {((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
- ((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
- {((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
- ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
- Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
- Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
-
- return LinearSystem<2, 2, 2>(A, B, C, D);
- }
+ const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
+ units::meter_t rb, units::kilogram_square_meter_t J, double G);
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index d071ca3..f5ee79b 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -48,19 +48,8 @@
/**
* Checks equality between this State and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
*/
- bool operator==(const State& other) const;
-
- /**
- * Checks inequality between this State and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const State& other) const;
+ bool operator==(const State&) const = default;
/**
* Interpolates between two States.
@@ -140,19 +129,8 @@
/**
* Checks equality between this Trajectory and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
*/
- bool operator==(const Trajectory& other) const;
-
- /**
- * Checks inequality between this Trajectory and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are inequal.
- */
- bool operator!=(const Trajectory& other) const;
+ bool operator==(const Trajectory&) const = default;
private:
std::vector<State> m_states;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
index eea1c07..807b315 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
@@ -81,7 +81,7 @@
* the trajectory, max velocity, min acceleration and max acceleration.
*/
struct ConstrainedState {
- PoseWithCurvature pose = {Pose2d(), units::curvature_t(0.0)};
+ PoseWithCurvature pose = {Pose2d{}, units::curvature_t{0.0}};
units::meter_t distance = 0_m;
units::meters_per_second_t maxVelocity = 0_mps;
units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
index 6e52a09..3ba882f 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -25,7 +25,7 @@
static void ToPathweaverJson(const Trajectory& trajectory,
std::string_view path);
/**
- * Imports a Trajectory from a PathWeaver-style JSON file.
+ * Imports a Trajectory from a JSON file exported from PathWeaver.
*
* @param path The path of the json file to import from.
*
@@ -34,7 +34,7 @@
static Trajectory FromPathweaverJson(std::string_view path);
/**
- * Deserializes a Trajectory from PathWeaver-style JSON.
+ * Deserializes a Trajectory from JSON exported from PathWeaver.
*
* @param trajectory the trajectory to export
*
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 0e623eb..24a8253 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -19,14 +19,14 @@
*
* Initialization:
* @code{.cpp}
- * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
+ * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
* double previousProfiledReference = initialReference;
* @endcode
*
* Run on update:
* @code{.cpp}
- * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
- * previousProfiledReference};
+ * TrapezoidProfile profile{constraints, unprofiledReference,
+ * previousProfiledReference};
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
* @endcode
*
@@ -68,10 +68,7 @@
public:
Distance_t position{0};
Velocity_t velocity{0};
- bool operator==(const State& rhs) const {
- return position == rhs.position && velocity == rhs.velocity;
- }
- bool operator!=(const State& rhs) const { return !(*this == rhs); }
+ bool operator==(const State&) const = default;
};
/**
@@ -82,7 +79,7 @@
* @param initial The initial state (usually the current state).
*/
TrapezoidProfile(Constraints constraints, State goal,
- State initial = State{Distance_t(0), Velocity_t(0)});
+ State initial = State{Distance_t{0}, Velocity_t{0}});
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 4ec0f42..19eb1f3 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -46,10 +46,10 @@
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
- if (fullSpeedDist < Distance_t(0)) {
+ if (fullSpeedDist < Distance_t{0}) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
- fullSpeedDist = Distance_t(0);
+ fullSpeedDist = Distance_t{0};
}
m_endAccel = accelerationTime - cutoffBegin;
@@ -110,7 +110,7 @@
Distance_t distToTarget = units::math::abs(target - position);
- if (distToTarget < Distance_t(1e-6)) {
+ if (distToTarget < Distance_t{1e-6}) {
return 0_s;
}
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index e2ef37b..f9f0d2e 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -44,8 +44,8 @@
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
- return units::meters_per_second_t(
- std::numeric_limits<double>::infinity());
+ return units::meters_per_second_t{
+ std::numeric_limits<double>::infinity()};
}
}
@@ -66,11 +66,16 @@
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const {
- // The region (disk) bounded by the ellipse is given by the equation:
- // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+ // The region bounded by the ellipse is given by the equation:
+ //
+ // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
+ //
+ // Multiply by Rx²Ry² for efficiency reasons:
+ //
+ // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
+ //
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
- // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
return units::math::pow<2>(pose.X() - m_center.X()) *
units::math::pow<2>(m_radii.Y()) +
units::math::pow<2>(pose.Y() - m_center.Y()) *
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index c5bc559..18522fe 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -41,8 +41,8 @@
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
- return units::meters_per_second_t(
- std::numeric_limits<double>::infinity());
+ return units::meters_per_second_t{
+ std::numeric_limits<double>::infinity()};
}
}
diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
index 6a411c4..632982b 100644
--- a/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -25,6 +25,16 @@
UNIT_ADD(angular_acceleration, degrees_per_second_squared,
degrees_per_second_squared, deg_per_s_sq,
compound_unit<angle::degrees, inverse<squared<time::seconds>>>)
+UNIT_ADD(angular_acceleration, turns_per_second_squared,
+ turns_per_second_squared, tr_per_s_sq,
+ compound_unit<angle::turns, inverse<squared<time::seconds>>>)
+UNIT_ADD(angular_acceleration, revolutions_per_minute_squared,
+ revolutions_per_minute_squared, rev_per_m_sq,
+ compound_unit<angle::turns, inverse<squared<time::minutes>>>)
+UNIT_ADD(angular_acceleration, revolutions_per_minute_per_second,
+ revolutions_per_minute_per_second, rev_per_m_per_s,
+ compound_unit<angle::turns, compound_unit<inverse<time::minutes>,
+ inverse<time::seconds>>>)
UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)
diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index bc9fb44..9d021b1 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
+// 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) 2016 Nic Holthaus
//
@@ -78,14 +75,13 @@
#include <iostream>
#include <locale>
#include <string>
-#else
+#endif
+#if !defined(UNIT_LIB_DISABLE_FMT)
#include <locale>
#include <string>
#include <fmt/format.h>
#endif
-#include <wpi/SymbolExports.h>
-
//------------------------------
// STRING FORMATTER
//------------------------------
@@ -180,7 +176,7 @@
* @param abbrev - abbreviated unit name, e.g. 'm'
* @note When UNIT_LIB_ENABLE_IOSTREAM isn't defined, the macro does not generate any code
*/
-#if !defined(UNIT_LIB_ENABLE_IOSTREAM)
+#if !defined(UNIT_LIB_DISABLE_FMT)
#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
}\
template <>\
@@ -205,7 +201,8 @@
return units::detail::to_string(obj()) + std::string(" "#abbrev);\
}\
}
-#else
+#endif
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
namespace namespaceName\
{\
@@ -872,7 +869,7 @@
* - A `std::ratio` defining the conversion factor to the base unit type. (e.g. `std::ratio<1,12>` for inches to feet)
* - A base unit that the unit is derived from (or a unit category. Must be of type `unit` or `base_unit`)
* - An exponent representing factors of PI required by the conversion. (e.g. `std::ratio<-1>` for a radians to degrees conversion)
- * - a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a farenheit to celsius conversion)
+ * - a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a fahrenheit to celsius conversion)
*
* Typically, a specific unit, like `meters`, would be implemented as a type alias
* of `unit`, i.e. `using meters = unit<std::ratio<1>, units::category::length_unit`, or
@@ -2328,7 +2325,7 @@
// unary addition: +T
template<class Units, typename T, template<typename> class NonLinearScale>
- inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
+ constexpr inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
{
return u;
}
@@ -2352,7 +2349,7 @@
// unary addition: -T
template<class Units, typename T, template<typename> class NonLinearScale>
- inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
+ constexpr inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
{
return unit_t<Units, T, NonLinearScale>(-u());
}
@@ -2869,13 +2866,16 @@
namespace dimensionless
{
typedef unit_t<scalar, UNIT_LIB_DEFAULT_TYPE, decibel_scale> dB_t;
-#if defined(UNIT_LIB_ENABLE_IOSTREAM)
- inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
typedef dB_t dBi_t;
}
-#else
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
+ namespace dimensionless
+ {
+ inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
+ }
+#endif
}
-}
+#if !defined(UNIT_LIB_DISABLE_FMT)
template <>
struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
{
@@ -2888,13 +2888,9 @@
return fmt::format_to(out, " dB");
}
};
-
-namespace units {
-namespace dimensionless {
- typedef dB_t dBi_t;
- }
#endif
+namespace units {
//------------------------------
// DECIBEL ARITHMETIC
//------------------------------
@@ -3444,4 +3440,6 @@
using namespace units::literals;
#endif // UNIT_HAS_LITERAL_SUPPORT
+#if !defined(UNIT_LIB_DISABLE_FMT)
#include "frc/fmt/Units.h"
+#endif
diff --git a/wpimath/src/main/native/include/units/units.h b/wpimath/src/main/native/include/units/units.h
deleted file mode 100644
index 8d47679..0000000
--- a/wpimath/src/main/native/include/units/units.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message("warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead.")
-#else
-#warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
-#endif
-
-// clang-format on
-
-#include "units/acceleration.h"
-#include "units/angle.h"
-#include "units/angular_acceleration.h"
-#include "units/angular_velocity.h"
-#include "units/area.h"
-#include "units/capacitance.h"
-#include "units/charge.h"
-#include "units/concentration.h"
-#include "units/conductance.h"
-#include "units/constants.h"
-#include "units/current.h"
-#include "units/curvature.h"
-#include "units/data.h"
-#include "units/data_transfer_rate.h"
-#include "units/density.h"
-#include "units/dimensionless.h"
-#include "units/energy.h"
-#include "units/force.h"
-#include "units/frequency.h"
-#include "units/illuminance.h"
-#include "units/impedance.h"
-#include "units/inductance.h"
-#include "units/length.h"
-#include "units/luminous_flux.h"
-#include "units/luminous_intensity.h"
-#include "units/magnetic_field_strength.h"
-#include "units/magnetic_flux.h"
-#include "units/mass.h"
-#include "units/math.h"
-#include "units/moment_of_inertia.h"
-#include "units/power.h"
-#include "units/pressure.h"
-#include "units/radiation.h"
-#include "units/solid_angle.h"
-#include "units/substance.h"
-#include "units/temperature.h"
-#include "units/time.h"
-#include "units/torque.h"
-#include "units/velocity.h"
-#include "units/voltage.h"
-#include "units/volume.h"
diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
index f4a1795..3b9b3cd 100644
--- a/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -34,12 +34,12 @@
template <typename S, typename... Args>
inline void ReportError(const S& format, Args&&... args) {
- ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+ ReportErrorV(format, fmt::make_format_args(args...));
}
template <typename S, typename... Args>
inline void ReportWarning(const S& format, Args&&... args) {
- ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
+ ReportWarningV(format, fmt::make_format_args(args...));
}
};
@@ -55,7 +55,7 @@
template <typename S, typename... Args>
static inline void ReportError(const S& format, Args&&... args) {
- ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+ ReportErrorV(format, fmt::make_format_args(args...));
}
static void ReportWarningV(fmt::string_view format, fmt::format_args args) {
@@ -64,7 +64,7 @@
template <typename S, typename... Args>
static inline void ReportWarning(const S& format, Args&&... args) {
- ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
+ ReportWarningV(format, fmt::make_format_args(args...));
}
static void ReportUsage(MathUsageId id, int count) {
diff --git a/wpimath/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
similarity index 97%
rename from wpimath/src/main/native/include/drake/common/drake_assert.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
index e9c3aa2..47097ed 100644
--- a/wpimath/src/main/native/include/drake/common/drake_assert.h
+++ b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
@@ -92,9 +92,9 @@
namespace assert {
// Allows for specialization of how to bool-convert Conditions used in
// assertions, in case they are not intrinsically convertible. See
-// symbolic_formula.h for an example use. This is a public interface to
-// extend; it is intended to be specialized by unusual Scalar types that
-// require special handling.
+// common/symbolic/expression/formula.h for an example use. This is a public
+// interface to extend; it is intended to be specialized by unusual Scalar
+// types that require special handling.
template <typename Condition>
struct ConditionTraits {
static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
diff --git a/wpimath/src/main/native/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_assertion_error.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
diff --git a/wpimath/src/main/native/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_copyable.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
diff --git a/wpimath/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_throw.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
diff --git a/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
diff --git a/wpimath/src/main/native/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/never_destroyed.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
diff --git a/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
similarity index 100%
rename from wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
rename to wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
similarity index 100%
rename from wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
rename to wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Cholesky b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Cholesky
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Core b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Core
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Householder b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Householder
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
new file mode 100644
index 0000000..957d575
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_ITERATIVELINEARSOLVERS_MODULE_H
+#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/**
+ * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
+ *
+ * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
+ * Those solvers are accessible via the following classes:
+ * - ConjugateGradient for selfadjoint (hermitian) matrices,
+ * - LeastSquaresConjugateGradient for rectangular least-square problems,
+ * - BiCGSTAB for general square matrices.
+ *
+ * These iterative solvers are associated with some preconditioners:
+ * - IdentityPreconditioner - not really useful
+ * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices.
+ * - IncompleteLUT - incomplete LU factorization with dual thresholding
+ *
+ * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
+ *
+ \code
+ #include <Eigen/IterativeLinearSolvers>
+ \endcode
+ */
+
+#include "src/IterativeLinearSolvers/SolveWithGuess.h"
+#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
+#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
+#include "src/IterativeLinearSolvers/ConjugateGradient.h"
+#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h"
+#include "src/IterativeLinearSolvers/BiCGSTAB.h"
+#include "src/IterativeLinearSolvers/IncompleteLUT.h"
+#include "src/IterativeLinearSolvers/IncompleteCholesky.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Jacobi b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Jacobi
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/LU b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/LU
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
new file mode 100644
index 0000000..29691a6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
@@ -0,0 +1,70 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_ORDERINGMETHODS_MODULE_H
+#define EIGEN_ORDERINGMETHODS_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/**
+ * \defgroup OrderingMethods_Module OrderingMethods module
+ *
+ * This module is currently for internal use only
+ *
+ * It defines various built-in and external ordering methods for sparse matrices.
+ * They are typically used to reduce the number of elements during
+ * the sparse matrix decomposition (LLT, LU, QR).
+ * Precisely, in a preprocessing step, a permutation matrix P is computed using
+ * those ordering methods and applied to the columns of the matrix.
+ * Using for instance the sparse Cholesky decomposition, it is expected that
+ * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
+ *
+ *
+ * Usage :
+ * \code
+ * #include <Eigen/OrderingMethods>
+ * \endcode
+ *
+ * A simple usage is as a template parameter in the sparse decomposition classes :
+ *
+ * \code
+ * SparseLU<MatrixType, COLAMDOrdering<int> > solver;
+ * \endcode
+ *
+ * \code
+ * SparseQR<MatrixType, COLAMDOrdering<int> > solver;
+ * \endcode
+ *
+ * It is possible as well to call directly a particular ordering method for your own purpose,
+ * \code
+ * AMDOrdering<int> ordering;
+ * PermutationMatrix<Dynamic, Dynamic, int> perm;
+ * SparseMatrix<double> A;
+ * //Fill the matrix ...
+ *
+ * ordering(A, perm); // Call AMD
+ * \endcode
+ *
+ * \note Some of these methods (like AMD or METIS), need the sparsity pattern
+ * of the input matrix to be symmetric. When the matrix is structurally unsymmetric,
+ * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
+ * If your matrix is already symmetric (at leat in structure), you can avoid that
+ * by calling the method with a SelfAdjointView type.
+ *
+ * \code
+ * // Call the ordering on the pattern of the lower triangular matrix A
+ * ordering(A.selfadjointView<Lower>(), perm);
+ * \endcode
+ */
+
+#include "src/OrderingMethods/Amd.h"
+#include "src/OrderingMethods/Ordering.h"
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ORDERINGMETHODS_MODULE_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/QR b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/QR
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/SVD b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/SVD
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
new file mode 100644
index 0000000..d2b1f12
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
@@ -0,0 +1,37 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSECHOLESKY_MODULE_H
+#define EIGEN_SPARSECHOLESKY_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/**
+ * \defgroup SparseCholesky_Module SparseCholesky module
+ *
+ * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices.
+ * Those decompositions are accessible via the following classes:
+ * - SimplicialLLt,
+ * - SimplicialLDLt
+ *
+ * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
+ *
+ * \code
+ * #include <Eigen/SparseCholesky>
+ * \endcode
+ */
+
+#include "src/SparseCholesky/SimplicialCholesky.h"
+#include "src/SparseCholesky/SimplicialCholesky_impl.h"
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECHOLESKY_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
new file mode 100644
index 0000000..76966c4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
@@ -0,0 +1,69 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_SPARSECORE_MODULE_H
+#define EIGEN_SPARSECORE_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <algorithm>
+
+/**
+ * \defgroup SparseCore_Module SparseCore module
+ *
+ * This module provides a sparse matrix representation, and basic associated matrix manipulations
+ * and operations.
+ *
+ * See the \ref TutorialSparse "Sparse tutorial"
+ *
+ * \code
+ * #include <Eigen/SparseCore>
+ * \endcode
+ *
+ * This module depends on: Core.
+ */
+
+#include "src/SparseCore/SparseUtil.h"
+#include "src/SparseCore/SparseMatrixBase.h"
+#include "src/SparseCore/SparseAssign.h"
+#include "src/SparseCore/CompressedStorage.h"
+#include "src/SparseCore/AmbiVector.h"
+#include "src/SparseCore/SparseCompressedBase.h"
+#include "src/SparseCore/SparseMatrix.h"
+#include "src/SparseCore/SparseMap.h"
+#include "src/SparseCore/MappedSparseMatrix.h"
+#include "src/SparseCore/SparseVector.h"
+#include "src/SparseCore/SparseRef.h"
+#include "src/SparseCore/SparseCwiseUnaryOp.h"
+#include "src/SparseCore/SparseCwiseBinaryOp.h"
+#include "src/SparseCore/SparseTranspose.h"
+#include "src/SparseCore/SparseBlock.h"
+#include "src/SparseCore/SparseDot.h"
+#include "src/SparseCore/SparseRedux.h"
+#include "src/SparseCore/SparseView.h"
+#include "src/SparseCore/SparseDiagonalProduct.h"
+#include "src/SparseCore/ConservativeSparseSparseProduct.h"
+#include "src/SparseCore/SparseSparseProductWithPruning.h"
+#include "src/SparseCore/SparseProduct.h"
+#include "src/SparseCore/SparseDenseProduct.h"
+#include "src/SparseCore/SparseSelfAdjointView.h"
+#include "src/SparseCore/SparseTriangularView.h"
+#include "src/SparseCore/TriangularSolver.h"
+#include "src/SparseCore/SparsePermutation.h"
+#include "src/SparseCore/SparseFuzzy.h"
+#include "src/SparseCore/SparseSolverBase.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECORE_MODULE_H
+
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
new file mode 100644
index 0000000..37c4a5c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
@@ -0,0 +1,50 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_MODULE_H
+#define EIGEN_SPARSELU_MODULE_H
+
+#include "SparseCore"
+
+/**
+ * \defgroup SparseLU_Module SparseLU module
+ * This module defines a supernodal factorization of general sparse matrices.
+ * The code is fully optimized for supernode-panel updates with specialized kernels.
+ * Please, see the documentation of the SparseLU class for more details.
+ */
+
+// Ordering interface
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "src/SparseLU/SparseLU_gemm_kernel.h"
+
+#include "src/SparseLU/SparseLU_Structs.h"
+#include "src/SparseLU/SparseLU_SupernodalMatrix.h"
+#include "src/SparseLU/SparseLUImpl.h"
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseLU/SparseLU_Memory.h"
+#include "src/SparseLU/SparseLU_heap_relax_snode.h"
+#include "src/SparseLU/SparseLU_relax_snode.h"
+#include "src/SparseLU/SparseLU_pivotL.h"
+#include "src/SparseLU/SparseLU_panel_dfs.h"
+#include "src/SparseLU/SparseLU_kernel_bmod.h"
+#include "src/SparseLU/SparseLU_panel_bmod.h"
+#include "src/SparseLU/SparseLU_column_dfs.h"
+#include "src/SparseLU/SparseLU_column_bmod.h"
+#include "src/SparseLU/SparseLU_copy_to_ucol.h"
+#include "src/SparseLU/SparseLU_pruneL.h"
+#include "src/SparseLU/SparseLU_Utils.h"
+#include "src/SparseLU/SparseLU.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSELU_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
new file mode 100644
index 0000000..f5fc5fa
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
@@ -0,0 +1,36 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_SPARSEQR_MODULE_H
+#define EIGEN_SPARSEQR_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup SparseQR_Module SparseQR module
+ * \brief Provides QR decomposition for sparse matrices
+ *
+ * This module provides a simplicial version of the left-looking Sparse QR decomposition.
+ * The columns of the input matrix should be reordered to limit the fill-in during the
+ * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end.
+ * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list
+ * of built-in and external ordering methods.
+ *
+ * \code
+ * #include <Eigen/SparseQR>
+ * \endcode
+ *
+ *
+ */
+
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseQR/SparseQR.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdDeque b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdDeque
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdList b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdList
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdVector b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdVector
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
similarity index 95%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index 3bec072..d973255 100644
--- a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -71,10 +71,17 @@
// See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
#pragma GCC diagnostic ignored "-Wattributes"
#endif
- #if __GNUC__==11
+ #if __GNUC__>=8
+ #pragma GCC diagnostic ignored "-Wclass-memaccess"
+ #endif
+ #if __GNUC__>=11
// This warning is a false positive
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
+ #if __GNUC__==12
+ // This warning is a false positive
+ #pragma GCC diagnostic ignored "-Warray-bounds"
+ #endif
#endif
#if defined __NVCC__
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
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
new file mode 100644
index 0000000..a117fc1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_BASIC_PRECONDITIONERS_H
+#define EIGEN_BASIC_PRECONDITIONERS_H
+
+namespace Eigen {
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief A preconditioner based on the digonal entries
+ *
+ * This class allows to approximately solve for A.x = b problems assuming A is a diagonal matrix.
+ * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for:
+ \code
+ A.diagonal().asDiagonal() . x = b
+ \endcode
+ *
+ * \tparam _Scalar the type of the scalar.
+ *
+ * \implsparsesolverconcept
+ *
+ * This preconditioner is suitable for both selfadjoint and general problems.
+ * The diagonal entries are pre-inverted and stored into a dense vector.
+ *
+ * \note A variant that has yet to be implemented would attempt to preserve the norm of each column.
+ *
+ * \sa class LeastSquareDiagonalPreconditioner, class ConjugateGradient
+ */
+template <typename _Scalar>
+class DiagonalPreconditioner
+{
+ typedef _Scalar Scalar;
+ typedef Matrix<Scalar,Dynamic,1> Vector;
+ public:
+ typedef typename Vector::StorageIndex StorageIndex;
+ enum {
+ ColsAtCompileTime = Dynamic,
+ MaxColsAtCompileTime = Dynamic
+ };
+
+ DiagonalPreconditioner() : m_isInitialized(false) {}
+
+ template<typename MatType>
+ explicit DiagonalPreconditioner(const MatType& mat) : m_invdiag(mat.cols())
+ {
+ compute(mat);
+ }
+
+ EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+ EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+
+ template<typename MatType>
+ DiagonalPreconditioner& analyzePattern(const MatType& )
+ {
+ return *this;
+ }
+
+ template<typename MatType>
+ DiagonalPreconditioner& factorize(const MatType& mat)
+ {
+ m_invdiag.resize(mat.cols());
+ for(int j=0; j<mat.outerSize(); ++j)
+ {
+ typename MatType::InnerIterator it(mat,j);
+ while(it && it.index()!=j) ++it;
+ if(it && it.index()==j && it.value()!=Scalar(0))
+ m_invdiag(j) = Scalar(1)/it.value();
+ else
+ m_invdiag(j) = Scalar(1);
+ }
+ m_isInitialized = true;
+ return *this;
+ }
+
+ template<typename MatType>
+ DiagonalPreconditioner& compute(const MatType& mat)
+ {
+ return factorize(mat);
+ }
+
+ /** \internal */
+ template<typename Rhs, typename Dest>
+ void _solve_impl(const Rhs& b, Dest& x) const
+ {
+ x = m_invdiag.array() * b.array() ;
+ }
+
+ template<typename Rhs> inline const Solve<DiagonalPreconditioner, Rhs>
+ solve(const MatrixBase<Rhs>& b) const
+ {
+ eigen_assert(m_isInitialized && "DiagonalPreconditioner is not initialized.");
+ eigen_assert(m_invdiag.size()==b.rows()
+ && "DiagonalPreconditioner::solve(): invalid number of rows of the right hand side matrix b");
+ return Solve<DiagonalPreconditioner, Rhs>(*this, b.derived());
+ }
+
+ ComputationInfo info() { return Success; }
+
+ protected:
+ Vector m_invdiag;
+ bool m_isInitialized;
+};
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief Jacobi preconditioner for LeastSquaresConjugateGradient
+ *
+ * This class allows to approximately solve for A' A x = A' b problems assuming A' A is a diagonal matrix.
+ * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for:
+ \code
+ (A.adjoint() * A).diagonal().asDiagonal() * x = b
+ \endcode
+ *
+ * \tparam _Scalar the type of the scalar.
+ *
+ * \implsparsesolverconcept
+ *
+ * The diagonal entries are pre-inverted and stored into a dense vector.
+ *
+ * \sa class LeastSquaresConjugateGradient, class DiagonalPreconditioner
+ */
+template <typename _Scalar>
+class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<_Scalar>
+{
+ typedef _Scalar Scalar;
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+ typedef DiagonalPreconditioner<_Scalar> Base;
+ using Base::m_invdiag;
+ public:
+
+ LeastSquareDiagonalPreconditioner() : Base() {}
+
+ template<typename MatType>
+ explicit LeastSquareDiagonalPreconditioner(const MatType& mat) : Base()
+ {
+ compute(mat);
+ }
+
+ template<typename MatType>
+ LeastSquareDiagonalPreconditioner& analyzePattern(const MatType& )
+ {
+ return *this;
+ }
+
+ template<typename MatType>
+ LeastSquareDiagonalPreconditioner& factorize(const MatType& mat)
+ {
+ // Compute the inverse squared-norm of each column of mat
+ m_invdiag.resize(mat.cols());
+ if(MatType::IsRowMajor)
+ {
+ m_invdiag.setZero();
+ for(Index j=0; j<mat.outerSize(); ++j)
+ {
+ for(typename MatType::InnerIterator it(mat,j); it; ++it)
+ m_invdiag(it.index()) += numext::abs2(it.value());
+ }
+ for(Index j=0; j<mat.cols(); ++j)
+ if(numext::real(m_invdiag(j))>RealScalar(0))
+ m_invdiag(j) = RealScalar(1)/numext::real(m_invdiag(j));
+ }
+ else
+ {
+ for(Index j=0; j<mat.outerSize(); ++j)
+ {
+ RealScalar sum = mat.col(j).squaredNorm();
+ if(sum>RealScalar(0))
+ m_invdiag(j) = RealScalar(1)/sum;
+ else
+ m_invdiag(j) = RealScalar(1);
+ }
+ }
+ Base::m_isInitialized = true;
+ return *this;
+ }
+
+ template<typename MatType>
+ LeastSquareDiagonalPreconditioner& compute(const MatType& mat)
+ {
+ return factorize(mat);
+ }
+
+ ComputationInfo info() { return Success; }
+
+ protected:
+};
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief A naive preconditioner which approximates any matrix as the identity matrix
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class DiagonalPreconditioner
+ */
+class IdentityPreconditioner
+{
+ public:
+
+ IdentityPreconditioner() {}
+
+ template<typename MatrixType>
+ explicit IdentityPreconditioner(const MatrixType& ) {}
+
+ template<typename MatrixType>
+ IdentityPreconditioner& analyzePattern(const MatrixType& ) { return *this; }
+
+ template<typename MatrixType>
+ IdentityPreconditioner& factorize(const MatrixType& ) { return *this; }
+
+ template<typename MatrixType>
+ IdentityPreconditioner& compute(const MatrixType& ) { return *this; }
+
+ template<typename Rhs>
+ inline const Rhs& solve(const Rhs& b) const { return b; }
+
+ ComputationInfo info() { return Success; }
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_BASIC_PRECONDITIONERS_H
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
new file mode 100644
index 0000000..153acef
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
@@ -0,0 +1,212 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_BICGSTAB_H
+#define EIGEN_BICGSTAB_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal Low-level bi conjugate gradient stabilized algorithm
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ * approximation of Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ * \return false in the case of numerical issue, for example a break down of BiCGSTAB.
+ */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
+ const Preconditioner& precond, Index& iters,
+ typename Dest::RealScalar& tol_error)
+{
+ using std::sqrt;
+ using std::abs;
+ typedef typename Dest::RealScalar RealScalar;
+ typedef typename Dest::Scalar Scalar;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+ RealScalar tol = tol_error;
+ Index maxIters = iters;
+
+ Index n = mat.cols();
+ VectorType r = rhs - mat * x;
+ VectorType r0 = r;
+
+ RealScalar r0_sqnorm = r0.squaredNorm();
+ RealScalar rhs_sqnorm = rhs.squaredNorm();
+ if(rhs_sqnorm == 0)
+ {
+ x.setZero();
+ return true;
+ }
+ Scalar rho = 1;
+ Scalar alpha = 1;
+ Scalar w = 1;
+
+ VectorType v = VectorType::Zero(n), p = VectorType::Zero(n);
+ VectorType y(n), z(n);
+ VectorType kt(n), ks(n);
+
+ VectorType s(n), t(n);
+
+ RealScalar tol2 = tol*tol*rhs_sqnorm;
+ RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
+ Index i = 0;
+ Index restarts = 0;
+
+ while ( r.squaredNorm() > tol2 && i<maxIters )
+ {
+ Scalar rho_old = rho;
+
+ rho = r0.dot(r);
+ if (abs(rho) < eps2*r0_sqnorm)
+ {
+ // 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();
+ if(restarts++ == 0)
+ i = 0;
+ }
+ Scalar beta = (rho/rho_old) * (alpha / w);
+ p = r + beta * (p - w * v);
+
+ y = precond.solve(p);
+
+ v.noalias() = mat * y;
+
+ alpha = rho / r0.dot(v);
+ s = r - alpha * v;
+
+ z = precond.solve(s);
+ t.noalias() = mat * z;
+
+ RealScalar tmp = t.squaredNorm();
+ if(tmp>RealScalar(0))
+ w = t.dot(s) / tmp;
+ else
+ w = Scalar(0);
+ x += alpha * y + w * z;
+ r = s - w * t;
+ ++i;
+ }
+ tol_error = sqrt(r.squaredNorm()/rhs_sqnorm);
+ iters = i;
+ return true;
+}
+
+}
+
+template< typename _MatrixType,
+ typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+class BiCGSTAB;
+
+namespace internal {
+
+template< typename _MatrixType, typename _Preconditioner>
+struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief A bi conjugate gradient stabilized solver for sparse square problems
+ *
+ * This class allows to solve for A.x = b sparse linear problems using a bi conjugate gradient
+ * stabilized algorithm. The vectors x and b can be either dense or sparse.
+ *
+ * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix.
+ * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
+ *
+ * \implsparsesolverconcept
+ *
+ * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+ * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+ * and NumTraits<Scalar>::epsilon() for the tolerance.
+ *
+ * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
+ *
+ * \b Performance: when using sparse matrices, best performance is achied for a row-major sparse matrix format.
+ * Moreover, in this case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
+ * See \ref TopicMultiThreading for details.
+ *
+ * This class can be used as the direct solver classes. Here is a typical usage example:
+ * \include BiCGSTAB_simple.cpp
+ *
+ * By default the iterations start with x=0 as an initial guess of the solution.
+ * One can control the start using the solveWithGuess() method.
+ *
+ * BiCGSTAB can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+ *
+ * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+ */
+template< typename _MatrixType, typename _Preconditioner>
+class BiCGSTAB : public IterativeSolverBase<BiCGSTAB<_MatrixType,_Preconditioner> >
+{
+ typedef IterativeSolverBase<BiCGSTAB> Base;
+ using Base::matrix;
+ using Base::m_error;
+ using Base::m_iterations;
+ using Base::m_info;
+ using Base::m_isInitialized;
+public:
+ typedef _MatrixType MatrixType;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef _Preconditioner Preconditioner;
+
+public:
+
+ /** Default constructor. */
+ BiCGSTAB() : Base() {}
+
+ /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+ *
+ * This constructor is a shortcut for the default constructor followed
+ * by a call to compute().
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ explicit BiCGSTAB(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+ ~BiCGSTAB() {}
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+ {
+ m_iterations = Base::maxIterations();
+ m_error = Base::m_tolerance;
+
+ bool ret = internal::bicgstab(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
+
+ m_info = (!ret) ? NumericalIssue
+ : m_error <= Base::m_tolerance ? Success
+ : NoConvergence;
+ }
+
+protected:
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_BICGSTAB_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
new file mode 100644
index 0000000..5d8c6b4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
@@ -0,0 +1,229 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_CONJUGATE_GRADIENT_H
+#define EIGEN_CONJUGATE_GRADIENT_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal Low-level conjugate gradient algorithm
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ * approximation of Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE
+void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
+ const Preconditioner& precond, Index& iters,
+ typename Dest::RealScalar& tol_error)
+{
+ using std::sqrt;
+ using std::abs;
+ typedef typename Dest::RealScalar RealScalar;
+ typedef typename Dest::Scalar Scalar;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+
+ RealScalar tol = tol_error;
+ Index maxIters = iters;
+
+ Index n = mat.cols();
+
+ VectorType residual = rhs - mat * x; //initial residual
+
+ RealScalar rhsNorm2 = rhs.squaredNorm();
+ if(rhsNorm2 == 0)
+ {
+ x.setZero();
+ iters = 0;
+ tol_error = 0;
+ return;
+ }
+ const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
+ RealScalar threshold = numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero);
+ RealScalar residualNorm2 = residual.squaredNorm();
+ if (residualNorm2 < threshold)
+ {
+ iters = 0;
+ tol_error = sqrt(residualNorm2 / rhsNorm2);
+ return;
+ }
+
+ VectorType p(n);
+ p = precond.solve(residual); // initial search direction
+
+ VectorType z(n), tmp(n);
+ RealScalar absNew = numext::real(residual.dot(p)); // the square of the absolute value of r scaled by invM
+ Index i = 0;
+ while(i < maxIters)
+ {
+ tmp.noalias() = mat * p; // the bottleneck of the algorithm
+
+ Scalar alpha = absNew / p.dot(tmp); // the amount we travel on dir
+ x += alpha * p; // update solution
+ residual -= alpha * tmp; // update residual
+
+ residualNorm2 = residual.squaredNorm();
+ if(residualNorm2 < threshold)
+ break;
+
+ z = precond.solve(residual); // approximately solve for "A z = residual"
+
+ RealScalar absOld = absNew;
+ absNew = numext::real(residual.dot(z)); // update the absolute value of r
+ RealScalar beta = absNew / absOld; // calculate the Gram-Schmidt value used to create the new search direction
+ p = z + beta * p; // update search direction
+ i++;
+ }
+ tol_error = sqrt(residualNorm2 / rhsNorm2);
+ iters = i;
+}
+
+}
+
+template< typename _MatrixType, int _UpLo=Lower,
+ typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+class ConjugateGradient;
+
+namespace internal {
+
+template< typename _MatrixType, int _UpLo, typename _Preconditioner>
+struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief A conjugate gradient solver for sparse (or dense) self-adjoint problems
+ *
+ * This class allows to solve for A.x = b linear problems using an iterative conjugate gradient algorithm.
+ * The matrix A must be selfadjoint. The matrix A and the vectors x and b can be either dense or sparse.
+ *
+ * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
+ * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower,
+ * \c Upper, or \c Lower|Upper in which the full matrix entries will be considered.
+ * Default is \c Lower, best performance is \c Lower|Upper.
+ * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
+ *
+ * \implsparsesolverconcept
+ *
+ * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+ * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+ * and NumTraits<Scalar>::epsilon() for the tolerance.
+ *
+ * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
+ *
+ * \b Performance: Even though the default value of \c _UpLo is \c Lower, significantly higher performance is
+ * achieved when using a complete matrix and \b Lower|Upper as the \a _UpLo template parameter. Moreover, in this
+ * case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
+ * See \ref TopicMultiThreading for details.
+ *
+ * This class can be used as the direct solver classes. Here is a typical usage example:
+ \code
+ int n = 10000;
+ VectorXd x(n), b(n);
+ SparseMatrix<double> A(n,n);
+ // fill A and b
+ ConjugateGradient<SparseMatrix<double>, Lower|Upper> cg;
+ cg.compute(A);
+ x = cg.solve(b);
+ std::cout << "#iterations: " << cg.iterations() << std::endl;
+ std::cout << "estimated error: " << cg.error() << std::endl;
+ // update b, and solve again
+ x = cg.solve(b);
+ \endcode
+ *
+ * By default the iterations start with x=0 as an initial guess of the solution.
+ * One can control the start using the solveWithGuess() method.
+ *
+ * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+ *
+ * \sa class LeastSquaresConjugateGradient, class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+ */
+template< typename _MatrixType, int _UpLo, typename _Preconditioner>
+class ConjugateGradient : public IterativeSolverBase<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
+{
+ typedef IterativeSolverBase<ConjugateGradient> Base;
+ using Base::matrix;
+ using Base::m_error;
+ using Base::m_iterations;
+ using Base::m_info;
+ using Base::m_isInitialized;
+public:
+ typedef _MatrixType MatrixType;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef _Preconditioner Preconditioner;
+
+ enum {
+ UpLo = _UpLo
+ };
+
+public:
+
+ /** Default constructor. */
+ ConjugateGradient() : Base() {}
+
+ /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+ *
+ * This constructor is a shortcut for the default constructor followed
+ * by a call to compute().
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ explicit ConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+ ~ConjugateGradient() {}
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+ {
+ typedef typename Base::MatrixWrapper MatrixWrapper;
+ typedef typename Base::ActualMatrixType ActualMatrixType;
+ enum {
+ TransposeInput = (!MatrixWrapper::MatrixFree)
+ && (UpLo==(Lower|Upper))
+ && (!MatrixType::IsRowMajor)
+ && (!NumTraits<Scalar>::IsComplex)
+ };
+ typedef typename internal::conditional<TransposeInput,Transpose<const ActualMatrixType>, ActualMatrixType const&>::type RowMajorWrapper;
+ EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(MatrixWrapper::MatrixFree,UpLo==(Lower|Upper)),MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY);
+ typedef typename internal::conditional<UpLo==(Lower|Upper),
+ RowMajorWrapper,
+ typename MatrixWrapper::template ConstSelfAdjointViewReturnType<UpLo>::Type
+ >::type SelfAdjointWrapper;
+
+ m_iterations = Base::maxIterations();
+ m_error = Base::m_tolerance;
+
+ RowMajorWrapper row_mat(matrix());
+ internal::conjugate_gradient(SelfAdjointWrapper(row_mat), b, x, Base::m_preconditioner, m_iterations, m_error);
+ m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
+ }
+
+protected:
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CONJUGATE_GRADIENT_H
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
new file mode 100644
index 0000000..7803fd8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
@@ -0,0 +1,394 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_INCOMPLETE_CHOlESKY_H
+#define EIGEN_INCOMPLETE_CHOlESKY_H
+
+#include <vector>
+#include <list>
+
+namespace Eigen {
+/**
+ * \brief Modified Incomplete Cholesky with dual threshold
+ *
+ * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
+ * Limited memory, SIAM J. Sci. Comput. 21(1), pp. 24-45, 1999
+ *
+ * \tparam Scalar the scalar type of the input matrices
+ * \tparam _UpLo The triangular part that will be used for the computations. It can be Lower
+ * or Upper. Default is Lower.
+ * \tparam _OrderingType The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<int>,
+ * unless EIGEN_MPL2_ONLY is defined, in which case the default is NaturalOrdering<int>.
+ *
+ * \implsparsesolverconcept
+ *
+ * It performs the following incomplete factorization: \f$ S P A P' S \approx L L' \f$
+ * where L is a lower triangular factor, S is a diagonal scaling matrix, and P is a
+ * fill-in reducing permutation as computed by the ordering method.
+ *
+ * \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. Otherwise, the factorization is performed on the shifted matrix \f$ B + (\sigma+|\beta| I \f$ where
+ * \f$ \sigma \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ \sigma = 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.
+ *
+ */
+template <typename Scalar, int _UpLo = Lower, typename _OrderingType = AMDOrdering<int> >
+class IncompleteCholesky : public SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> >
+{
+ protected:
+ typedef SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> > Base;
+ using Base::m_isInitialized;
+ public:
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+ typedef _OrderingType OrderingType;
+ typedef typename OrderingType::PermutationType PermutationType;
+ typedef typename PermutationType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> FactorType;
+ typedef Matrix<Scalar,Dynamic,1> VectorSx;
+ typedef Matrix<RealScalar,Dynamic,1> VectorRx;
+ typedef Matrix<StorageIndex,Dynamic, 1> VectorIx;
+ typedef std::vector<std::list<StorageIndex> > VectorList;
+ enum { UpLo = _UpLo };
+ enum {
+ ColsAtCompileTime = Dynamic,
+ MaxColsAtCompileTime = Dynamic
+ };
+ public:
+
+ /** Default constructor leaving the object in a partly non-initialized stage.
+ *
+ * You must call compute() or the pair analyzePattern()/factorize() to make it valid.
+ *
+ * \sa IncompleteCholesky(const MatrixType&)
+ */
+ IncompleteCholesky() : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false) {}
+
+ /** Constructor computing the incomplete factorization for the given matrix \a matrix.
+ */
+ template<typename MatrixType>
+ IncompleteCholesky(const MatrixType& matrix) : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false)
+ {
+ compute(matrix);
+ }
+
+ /** \returns number of rows of the factored matrix */
+ EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_L.rows(); }
+
+ /** \returns number of columns of the factored matrix */
+ EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_L.cols(); }
+
+
+ /** \brief Reports whether previous computation was successful.
+ *
+ * It triggers an assertion if \c *this has not been initialized through the respective constructor,
+ * or a call to compute() or analyzePattern().
+ *
+ * \returns \c Success if computation was successful,
+ * \c NumericalIssue if the matrix appears to be negative.
+ */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "IncompleteCholesky is not initialized.");
+ return m_info;
+ }
+
+ /** \brief Set the initial shift parameter \f$ \sigma \f$.
+ */
+ void setInitialShift(RealScalar shift) { m_initialShift = shift; }
+
+ /** \brief Computes the fill reducing permutation vector using the sparsity pattern of \a mat
+ */
+ template<typename MatrixType>
+ void analyzePattern(const MatrixType& mat)
+ {
+ OrderingType ord;
+ PermutationType pinv;
+ ord(mat.template selfadjointView<UpLo>(), pinv);
+ if(pinv.size()>0) m_perm = pinv.inverse();
+ else m_perm.resize(0);
+ m_L.resize(mat.rows(), mat.cols());
+ m_analysisIsOk = true;
+ m_isInitialized = true;
+ m_info = Success;
+ }
+
+ /** \brief Performs the numerical factorization of the input matrix \a mat
+ *
+ * The method analyzePattern() or compute() must have been called beforehand
+ * with a matrix having the same pattern.
+ *
+ * \sa compute(), analyzePattern()
+ */
+ template<typename MatrixType>
+ void factorize(const MatrixType& mat);
+
+ /** Computes or re-computes the incomplete Cholesky factorization of the input matrix \a mat
+ *
+ * It is a shortcut for a sequential call to the analyzePattern() and factorize() methods.
+ *
+ * \sa analyzePattern(), factorize()
+ */
+ template<typename MatrixType>
+ void compute(const MatrixType& mat)
+ {
+ analyzePattern(mat);
+ factorize(mat);
+ }
+
+ // internal
+ template<typename Rhs, typename Dest>
+ void _solve_impl(const Rhs& b, Dest& x) const
+ {
+ eigen_assert(m_factorizationIsOk && "factorize() should be called first");
+ if (m_perm.rows() == b.rows()) x = m_perm * b;
+ else x = b;
+ x = m_scale.asDiagonal() * x;
+ x = m_L.template triangularView<Lower>().solve(x);
+ x = m_L.adjoint().template triangularView<Upper>().solve(x);
+ x = m_scale.asDiagonal() * x;
+ if (m_perm.rows() == b.rows())
+ x = m_perm.inverse() * x;
+ }
+
+ /** \returns the sparse lower triangular factor L */
+ const FactorType& matrixL() const { eigen_assert("m_factorizationIsOk"); return m_L; }
+
+ /** \returns a vector representing the scaling factor S */
+ const VectorRx& scalingS() const { eigen_assert("m_factorizationIsOk"); return m_scale; }
+
+ /** \returns the fill-in reducing permutation P (can be empty for a natural ordering) */
+ const PermutationType& permutationP() const { eigen_assert("m_analysisIsOk"); return m_perm; }
+
+ protected:
+ FactorType m_L; // The lower part stored in CSC
+ VectorRx m_scale; // The vector for scaling the matrix
+ RealScalar m_initialShift; // The initial shift parameter
+ bool m_analysisIsOk;
+ bool m_factorizationIsOk;
+ ComputationInfo m_info;
+ PermutationType m_perm;
+
+ private:
+ inline void updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol);
+};
+
+// Based on the following paper:
+// C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
+// Limited memory, SIAM J. Sci. Comput. 21(1), pp. 24-45, 1999
+// http://ftp.mcs.anl.gov/pub/tech_reports/reports/P682.pdf
+template<typename Scalar, int _UpLo, typename OrderingType>
+template<typename _MatrixType>
+void IncompleteCholesky<Scalar,_UpLo, OrderingType>::factorize(const _MatrixType& mat)
+{
+ using std::sqrt;
+ eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
+
+ // Dropping strategy : Keep only the p largest elements per column, where p is the number of elements in the column of the original matrix. Other strategies will be added
+
+ // Apply the fill-reducing permutation computed in analyzePattern()
+ if (m_perm.rows() == mat.rows() ) // To detect the null permutation
+ {
+ // The temporary is needed to make sure that the diagonal entry is properly sorted
+ FactorType tmp(mat.rows(), mat.cols());
+ tmp = mat.template selfadjointView<_UpLo>().twistedBy(m_perm);
+ m_L.template selfadjointView<Lower>() = tmp.template selfadjointView<Lower>();
+ }
+ else
+ {
+ m_L.template selfadjointView<Lower>() = mat.template selfadjointView<_UpLo>();
+ }
+
+ Index n = m_L.cols();
+ Index nnz = m_L.nonZeros();
+ Map<VectorSx> vals(m_L.valuePtr(), nnz); //values
+ Map<VectorIx> rowIdx(m_L.innerIndexPtr(), nnz); //Row indices
+ Map<VectorIx> colPtr( m_L.outerIndexPtr(), n+1); // Pointer to the beginning of each row
+ VectorIx firstElt(n-1); // for each j, points to the next entry in vals that will be used in the factorization
+ VectorList listCol(n); // listCol(j) is a linked list of columns to update column j
+ VectorSx col_vals(n); // Store a nonzero values in each column
+ VectorIx col_irow(n); // Row indices of nonzero elements in each column
+ VectorIx col_pattern(n);
+ col_pattern.fill(-1);
+ StorageIndex col_nnz;
+
+
+ // Computes the scaling factors
+ m_scale.resize(n);
+ m_scale.setZero();
+ for (Index j = 0; j < n; j++)
+ for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
+ {
+ m_scale(j) += numext::abs2(vals(k));
+ if(rowIdx[k]!=j)
+ m_scale(rowIdx[k]) += numext::abs2(vals(k));
+ }
+
+ m_scale = m_scale.cwiseSqrt().cwiseSqrt();
+
+ for (Index j = 0; j < n; ++j)
+ if(m_scale(j)>(std::numeric_limits<RealScalar>::min)())
+ m_scale(j) = RealScalar(1)/m_scale(j);
+ else
+ m_scale(j) = 1;
+
+ // TODO disable scaling if not needed, i.e., if it is roughly uniform? (this will make solve() faster)
+
+ // Scale and compute the shift for the matrix
+ RealScalar mindiag = NumTraits<RealScalar>::highest();
+ for (Index j = 0; j < n; j++)
+ {
+ for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
+ vals[k] *= (m_scale(j)*m_scale(rowIdx[k]));
+ eigen_internal_assert(rowIdx[colPtr[j]]==j && "IncompleteCholesky: only the lower triangular part must be stored");
+ mindiag = numext::mini(numext::real(vals[colPtr[j]]), mindiag);
+ }
+
+ FactorType L_save = m_L;
+
+ RealScalar shift = 0;
+ if(mindiag <= RealScalar(0.))
+ shift = m_initialShift - mindiag;
+
+ m_info = NumericalIssue;
+
+ // Try to perform the incomplete factorization using the current shift
+ int iter = 0;
+ do
+ {
+ // Apply the shift to the diagonal elements of the matrix
+ for (Index j = 0; j < n; j++)
+ vals[colPtr[j]] += shift;
+
+ // jki version of the Cholesky factorization
+ Index j=0;
+ for (; j < n; ++j)
+ {
+ // Left-looking factorization of the j-th column
+ // First, load the j-th column into col_vals
+ Scalar diag = vals[colPtr[j]]; // It is assumed that only the lower part is stored
+ col_nnz = 0;
+ for (Index i = colPtr[j] + 1; i < colPtr[j+1]; i++)
+ {
+ StorageIndex l = rowIdx[i];
+ col_vals(col_nnz) = vals[i];
+ col_irow(col_nnz) = l;
+ col_pattern(l) = col_nnz;
+ col_nnz++;
+ }
+ {
+ typename std::list<StorageIndex>::iterator k;
+ // Browse all previous columns that will update column j
+ for(k = listCol[j].begin(); k != listCol[j].end(); k++)
+ {
+ Index jk = firstElt(*k); // First element to use in the column
+ eigen_internal_assert(rowIdx[jk]==j);
+ Scalar v_j_jk = numext::conj(vals[jk]);
+
+ jk += 1;
+ for (Index i = jk; i < colPtr[*k+1]; i++)
+ {
+ StorageIndex l = rowIdx[i];
+ if(col_pattern[l]<0)
+ {
+ col_vals(col_nnz) = vals[i] * v_j_jk;
+ col_irow[col_nnz] = l;
+ col_pattern(l) = col_nnz;
+ col_nnz++;
+ }
+ else
+ col_vals(col_pattern[l]) -= vals[i] * v_j_jk;
+ }
+ updateList(colPtr,rowIdx,vals, *k, jk, firstElt, listCol);
+ }
+ }
+
+ // Scale the current column
+ if(numext::real(diag) <= 0)
+ {
+ if(++iter>=10)
+ return;
+
+ // increase shift
+ shift = numext::maxi(m_initialShift,RealScalar(2)*shift);
+ // restore m_L, col_pattern, and listCol
+ vals = Map<const VectorSx>(L_save.valuePtr(), nnz);
+ rowIdx = Map<const VectorIx>(L_save.innerIndexPtr(), nnz);
+ colPtr = Map<const VectorIx>(L_save.outerIndexPtr(), n+1);
+ col_pattern.fill(-1);
+ for(Index i=0; i<n; ++i)
+ listCol[i].clear();
+
+ break;
+ }
+
+ RealScalar rdiag = sqrt(numext::real(diag));
+ vals[colPtr[j]] = rdiag;
+ for (Index k = 0; k<col_nnz; ++k)
+ {
+ Index i = col_irow[k];
+ //Scale
+ col_vals(k) /= rdiag;
+ //Update the remaining diagonals with col_vals
+ vals[colPtr[i]] -= numext::abs2(col_vals(k));
+ }
+ // Select the largest p elements
+ // p is the original number of elements in the column (without the diagonal)
+ Index p = colPtr[j+1] - colPtr[j] - 1 ;
+ Ref<VectorSx> cvals = col_vals.head(col_nnz);
+ Ref<VectorIx> cirow = col_irow.head(col_nnz);
+ internal::QuickSplit(cvals,cirow, p);
+ // Insert the largest p elements in the matrix
+ Index cpt = 0;
+ for (Index i = colPtr[j]+1; i < colPtr[j+1]; i++)
+ {
+ vals[i] = col_vals(cpt);
+ rowIdx[i] = col_irow(cpt);
+ // restore col_pattern:
+ col_pattern(col_irow(cpt)) = -1;
+ cpt++;
+ }
+ // Get the first smallest row index and put it after the diagonal element
+ Index jk = colPtr(j)+1;
+ updateList(colPtr,rowIdx,vals,j,jk,firstElt,listCol);
+ }
+
+ if(j==n)
+ {
+ m_factorizationIsOk = true;
+ m_info = Success;
+ }
+ } while(m_info!=Success);
+}
+
+template<typename Scalar, int _UpLo, typename OrderingType>
+inline void IncompleteCholesky<Scalar,_UpLo, OrderingType>::updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol)
+{
+ if (jk < colPtr(col+1) )
+ {
+ Index p = colPtr(col+1) - jk;
+ Index minpos;
+ rowIdx.segment(jk,p).minCoeff(&minpos);
+ minpos += jk;
+ if (rowIdx(minpos) != rowIdx(jk))
+ {
+ //Swap
+ std::swap(rowIdx(jk),rowIdx(minpos));
+ std::swap(vals(jk),vals(minpos));
+ }
+ firstElt(col) = internal::convert_index<StorageIndex,Index>(jk);
+ listCol[rowIdx(jk)].push_back(internal::convert_index<StorageIndex,Index>(col));
+ }
+}
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
new file mode 100644
index 0000000..cdcf709
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
@@ -0,0 +1,453 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_INCOMPLETE_LUT_H
+#define EIGEN_INCOMPLETE_LUT_H
+
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal
+ * Compute a quick-sort split of a vector
+ * On output, the vector row is permuted such that its elements satisfy
+ * abs(row(i)) >= abs(row(ncut)) if i<ncut
+ * abs(row(i)) <= abs(row(ncut)) if i>ncut
+ * \param row The vector of values
+ * \param ind The array of index for the elements in @p row
+ * \param ncut The number of largest elements to keep
+ **/
+template <typename VectorV, typename VectorI>
+Index QuickSplit(VectorV &row, VectorI &ind, Index ncut)
+{
+ typedef typename VectorV::RealScalar RealScalar;
+ using std::swap;
+ using std::abs;
+ Index mid;
+ Index n = row.size(); /* length of the vector */
+ Index first, last ;
+
+ ncut--; /* to fit the zero-based indices */
+ first = 0;
+ last = n-1;
+ if (ncut < first || ncut > last ) return 0;
+
+ do {
+ mid = first;
+ RealScalar abskey = abs(row(mid));
+ for (Index j = first + 1; j <= last; j++) {
+ if ( abs(row(j)) > abskey) {
+ ++mid;
+ swap(row(mid), row(j));
+ swap(ind(mid), ind(j));
+ }
+ }
+ /* Interchange for the pivot element */
+ swap(row(mid), row(first));
+ swap(ind(mid), ind(first));
+
+ if (mid > ncut) last = mid - 1;
+ else if (mid < ncut ) first = mid + 1;
+ } while (mid != ncut );
+
+ return 0; /* mid is equal to ncut */
+}
+
+}// end namespace internal
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \class IncompleteLUT
+ * \brief Incomplete LU factorization with dual-threshold strategy
+ *
+ * \implsparsesolverconcept
+ *
+ * During the numerical factorization, two dropping rules are used :
+ * 1) any element whose magnitude is less than some tolerance is dropped.
+ * This tolerance is obtained by multiplying the input tolerance @p droptol
+ * by the average magnitude of all the original elements in the current row.
+ * 2) After the elimination of the row, only the @p fill largest elements in
+ * the L part and the @p fill largest elements in the U part are kept
+ * (in addition to the diagonal element ). Note that @p fill is computed from
+ * the input parameter @p fillfactor which is used the ratio to control the fill_in
+ * relatively to the initial number of nonzero elements.
+ *
+ * The two extreme cases are when @p droptol=0 (to keep all the @p fill*2 largest elements)
+ * and when @p fill=n/2 with @p droptol being different to zero.
+ *
+ * References : Yousef Saad, ILUT: A dual threshold incomplete LU factorization,
+ * Numerical Linear Algebra with Applications, 1(4), pp 387-402, 1994.
+ *
+ * NOTE : The following implementation is derived from the ILUT implementation
+ * in the SPARSKIT package, Copyright (C) 2005, the Regents of the University of Minnesota
+ * released under the terms of the GNU LGPL:
+ * http://www-users.cs.umn.edu/~saad/software/SPARSKIT/README
+ * However, Yousef Saad gave us permission to relicense his ILUT code to MPL2.
+ * See the Eigen mailing list archive, thread: ILUT, date: July 8, 2012:
+ * http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/07/msg00064.html
+ * alternatively, on GMANE:
+ * http://comments.gmane.org/gmane.comp.lib.eigen/3302
+ */
+template <typename _Scalar, typename _StorageIndex = int>
+class IncompleteLUT : public SparseSolverBase<IncompleteLUT<_Scalar, _StorageIndex> >
+{
+ protected:
+ typedef SparseSolverBase<IncompleteLUT> Base;
+ using Base::m_isInitialized;
+ public:
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+ typedef Matrix<Scalar,Dynamic,1> Vector;
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ typedef SparseMatrix<Scalar,RowMajor,StorageIndex> FactorType;
+
+ enum {
+ ColsAtCompileTime = Dynamic,
+ MaxColsAtCompileTime = Dynamic
+ };
+
+ public:
+
+ IncompleteLUT()
+ : m_droptol(NumTraits<Scalar>::dummy_precision()), m_fillfactor(10),
+ m_analysisIsOk(false), m_factorizationIsOk(false)
+ {}
+
+ template<typename MatrixType>
+ explicit IncompleteLUT(const MatrixType& mat, const RealScalar& droptol=NumTraits<Scalar>::dummy_precision(), int fillfactor = 10)
+ : m_droptol(droptol),m_fillfactor(fillfactor),
+ m_analysisIsOk(false),m_factorizationIsOk(false)
+ {
+ eigen_assert(fillfactor != 0);
+ compute(mat);
+ }
+
+ EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+
+ EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
+
+ /** \brief Reports whether previous computation was successful.
+ *
+ * \returns \c Success if computation was successful,
+ * \c NumericalIssue if the matrix.appears to be negative.
+ */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
+ return m_info;
+ }
+
+ template<typename MatrixType>
+ void analyzePattern(const MatrixType& amat);
+
+ template<typename MatrixType>
+ void factorize(const MatrixType& amat);
+
+ /**
+ * Compute an incomplete LU factorization with dual threshold on the matrix mat
+ * No pivoting is done in this version
+ *
+ **/
+ template<typename MatrixType>
+ IncompleteLUT& compute(const MatrixType& amat)
+ {
+ analyzePattern(amat);
+ factorize(amat);
+ return *this;
+ }
+
+ void setDroptol(const RealScalar& droptol);
+ void setFillfactor(int fillfactor);
+
+ template<typename Rhs, typename Dest>
+ void _solve_impl(const Rhs& b, Dest& x) const
+ {
+ x = m_Pinv * b;
+ x = m_lu.template triangularView<UnitLower>().solve(x);
+ x = m_lu.template triangularView<Upper>().solve(x);
+ x = m_P * x;
+ }
+
+protected:
+
+ /** keeps off-diagonal entries; drops diagonal entries */
+ struct keep_diag {
+ inline bool operator() (const Index& row, const Index& col, const Scalar&) const
+ {
+ return row!=col;
+ }
+ };
+
+protected:
+
+ FactorType m_lu;
+ RealScalar m_droptol;
+ int m_fillfactor;
+ bool m_analysisIsOk;
+ bool m_factorizationIsOk;
+ ComputationInfo m_info;
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P; // Fill-reducing permutation
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv; // Inverse permutation
+};
+
+/**
+ * Set control parameter droptol
+ * \param droptol Drop any element whose magnitude is less than this tolerance
+ **/
+template<typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar,StorageIndex>::setDroptol(const RealScalar& droptol)
+{
+ this->m_droptol = droptol;
+}
+
+/**
+ * Set control parameter fillfactor
+ * \param fillfactor This is used to compute the number @p fill_in of largest elements to keep on each row.
+ **/
+template<typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar,StorageIndex>::setFillfactor(int fillfactor)
+{
+ this->m_fillfactor = fillfactor;
+}
+
+template <typename Scalar, typename StorageIndex>
+template<typename _MatrixType>
+void IncompleteLUT<Scalar,StorageIndex>::analyzePattern(const _MatrixType& amat)
+{
+ // Compute the Fill-reducing permutation
+ // Since ILUT does not perform any numerical pivoting,
+ // it is highly preferable to keep the diagonal through symmetric permutations.
+ // To this end, let's symmetrize the pattern and perform AMD on it.
+ SparseMatrix<Scalar,ColMajor, StorageIndex> mat1 = amat;
+ SparseMatrix<Scalar,ColMajor, StorageIndex> mat2 = amat.transpose();
+ // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice.
+ // on the other hand for a really non-symmetric pattern, mat2*mat1 should be preferred...
+ SparseMatrix<Scalar,ColMajor, StorageIndex> AtA = mat2 + mat1;
+ AMDOrdering<StorageIndex> ordering;
+ ordering(AtA,m_P);
+ m_Pinv = m_P.inverse(); // cache the inverse permutation
+ m_analysisIsOk = true;
+ m_factorizationIsOk = false;
+ m_isInitialized = true;
+}
+
+template <typename Scalar, typename StorageIndex>
+template<typename _MatrixType>
+void IncompleteLUT<Scalar,StorageIndex>::factorize(const _MatrixType& amat)
+{
+ using std::sqrt;
+ using std::swap;
+ using std::abs;
+ using internal::convert_index;
+
+ eigen_assert((amat.rows() == amat.cols()) && "The factorization should be done on a square matrix");
+ Index n = amat.cols(); // Size of the matrix
+ m_lu.resize(n,n);
+ // Declare Working vectors and variables
+ Vector u(n) ; // real values of the row -- maximum size is n --
+ VectorI ju(n); // column position of the values in u -- maximum size is n
+ VectorI jr(n); // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1
+
+ // Apply the fill-reducing permutation
+ eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+ SparseMatrix<Scalar,RowMajor, StorageIndex> mat;
+ mat = amat.twistedBy(m_Pinv);
+
+ // Initialization
+ jr.fill(-1);
+ ju.fill(0);
+ u.fill(0);
+
+ // number of largest elements to keep in each row:
+ Index fill_in = (amat.nonZeros()*m_fillfactor)/n + 1;
+ if (fill_in > n) fill_in = n;
+
+ // number of largest nonzero elements to keep in the L and the U part of the current row:
+ Index nnzL = fill_in/2;
+ Index nnzU = nnzL;
+ m_lu.reserve(n * (nnzL + nnzU + 1));
+
+ // global loop over the rows of the sparse matrix
+ for (Index ii = 0; ii < n; ii++)
+ {
+ // 1 - copy the lower and the upper part of the row i of mat in the working vector u
+
+ Index sizeu = 1; // number of nonzero elements in the upper part of the current row
+ Index sizel = 0; // number of nonzero elements in the lower part of the current row
+ ju(ii) = convert_index<StorageIndex>(ii);
+ u(ii) = 0;
+ jr(ii) = convert_index<StorageIndex>(ii);
+ RealScalar rownorm = 0;
+
+ typename FactorType::InnerIterator j_it(mat, ii); // Iterate through the current row ii
+ for (; j_it; ++j_it)
+ {
+ Index k = j_it.index();
+ if (k < ii)
+ {
+ // copy the lower part
+ ju(sizel) = convert_index<StorageIndex>(k);
+ u(sizel) = j_it.value();
+ jr(k) = convert_index<StorageIndex>(sizel);
+ ++sizel;
+ }
+ else if (k == ii)
+ {
+ u(ii) = j_it.value();
+ }
+ else
+ {
+ // copy the upper part
+ Index jpos = ii + sizeu;
+ ju(jpos) = convert_index<StorageIndex>(k);
+ u(jpos) = j_it.value();
+ jr(k) = convert_index<StorageIndex>(jpos);
+ ++sizeu;
+ }
+ rownorm += numext::abs2(j_it.value());
+ }
+
+ // 2 - detect possible zero row
+ if(rownorm==0)
+ {
+ m_info = NumericalIssue;
+ return;
+ }
+ // Take the 2-norm of the current row as a relative tolerance
+ rownorm = sqrt(rownorm);
+
+ // 3 - eliminate the previous nonzero rows
+ Index jj = 0;
+ Index len = 0;
+ while (jj < sizel)
+ {
+ // In order to eliminate in the correct order,
+ // we must select first the smallest column index among ju(jj:sizel)
+ Index k;
+ Index minrow = ju.segment(jj,sizel-jj).minCoeff(&k); // k is relative to the segment
+ k += jj;
+ if (minrow != ju(jj))
+ {
+ // swap the two locations
+ Index j = ju(jj);
+ swap(ju(jj), ju(k));
+ jr(minrow) = convert_index<StorageIndex>(jj);
+ jr(j) = convert_index<StorageIndex>(k);
+ swap(u(jj), u(k));
+ }
+ // Reset this location
+ jr(minrow) = -1;
+
+ // Start elimination
+ typename FactorType::InnerIterator ki_it(m_lu, minrow);
+ while (ki_it && ki_it.index() < minrow) ++ki_it;
+ eigen_internal_assert(ki_it && ki_it.col()==minrow);
+ Scalar fact = u(jj) / ki_it.value();
+
+ // drop too small elements
+ if(abs(fact) <= m_droptol)
+ {
+ jj++;
+ continue;
+ }
+
+ // linear combination of the current row ii and the row minrow
+ ++ki_it;
+ for (; ki_it; ++ki_it)
+ {
+ Scalar prod = fact * ki_it.value();
+ Index j = ki_it.index();
+ Index jpos = jr(j);
+ if (jpos == -1) // fill-in element
+ {
+ Index newpos;
+ if (j >= ii) // dealing with the upper part
+ {
+ newpos = ii + sizeu;
+ sizeu++;
+ eigen_internal_assert(sizeu<=n);
+ }
+ else // dealing with the lower part
+ {
+ newpos = sizel;
+ sizel++;
+ eigen_internal_assert(sizel<=ii);
+ }
+ ju(newpos) = convert_index<StorageIndex>(j);
+ u(newpos) = -prod;
+ jr(j) = convert_index<StorageIndex>(newpos);
+ }
+ else
+ u(jpos) -= prod;
+ }
+ // store the pivot element
+ u(len) = fact;
+ ju(len) = convert_index<StorageIndex>(minrow);
+ ++len;
+
+ jj++;
+ } // end of the elimination on the row ii
+
+ // reset the upper part of the pointer jr to zero
+ for(Index k = 0; k <sizeu; k++) jr(ju(ii+k)) = -1;
+
+ // 4 - partially sort and insert the elements in the m_lu matrix
+
+ // sort the L-part of the row
+ sizel = len;
+ len = (std::min)(sizel, nnzL);
+ typename Vector::SegmentReturnType ul(u.segment(0, sizel));
+ typename VectorI::SegmentReturnType jul(ju.segment(0, sizel));
+ internal::QuickSplit(ul, jul, len);
+
+ // store the largest m_fill elements of the L part
+ m_lu.startVec(ii);
+ for(Index k = 0; k < len; k++)
+ m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+
+ // store the diagonal element
+ // apply a shifting rule to avoid zero pivots (we are doing an incomplete factorization)
+ if (u(ii) == Scalar(0))
+ u(ii) = sqrt(m_droptol) * rownorm;
+ m_lu.insertBackByOuterInnerUnordered(ii, ii) = u(ii);
+
+ // sort the U-part of the row
+ // apply the dropping rule first
+ len = 0;
+ for(Index k = 1; k < sizeu; k++)
+ {
+ if(abs(u(ii+k)) > m_droptol * rownorm )
+ {
+ ++len;
+ u(ii + len) = u(ii + k);
+ ju(ii + len) = ju(ii + k);
+ }
+ }
+ sizeu = len + 1; // +1 to take into account the diagonal element
+ len = (std::min)(sizeu, nnzU);
+ typename Vector::SegmentReturnType uu(u.segment(ii+1, sizeu-1));
+ typename VectorI::SegmentReturnType juu(ju.segment(ii+1, sizeu-1));
+ internal::QuickSplit(uu, juu, len);
+
+ // store the largest elements of the U part
+ for(Index k = ii + 1; k < ii + len; k++)
+ m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+ }
+ m_lu.finalize();
+ m_lu.makeCompressed();
+
+ m_factorizationIsOk = true;
+ m_info = Success;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_INCOMPLETE_LUT_H
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
new file mode 100644
index 0000000..28a0c51
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
@@ -0,0 +1,444 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_ITERATIVE_SOLVER_BASE_H
+#define EIGEN_ITERATIVE_SOLVER_BASE_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename MatrixType>
+struct is_ref_compatible_impl
+{
+private:
+ template <typename T0>
+ struct any_conversion
+ {
+ template <typename T> any_conversion(const volatile T&);
+ template <typename T> any_conversion(T&);
+ };
+ struct yes {int a[1];};
+ struct no {int a[2];};
+
+ template<typename T>
+ static yes test(const Ref<const T>&, int);
+ template<typename T>
+ static no test(any_conversion<T>, ...);
+
+public:
+ static MatrixType ms_from;
+ enum { value = sizeof(test<MatrixType>(ms_from, 0))==sizeof(yes) };
+};
+
+template<typename MatrixType>
+struct is_ref_compatible
+{
+ enum { value = is_ref_compatible_impl<typename remove_all<MatrixType>::type>::value };
+};
+
+template<typename MatrixType, bool MatrixFree = !internal::is_ref_compatible<MatrixType>::value>
+class generic_matrix_wrapper;
+
+// We have an explicit matrix at hand, compatible with Ref<>
+template<typename MatrixType>
+class generic_matrix_wrapper<MatrixType,false>
+{
+public:
+ typedef Ref<const MatrixType> ActualMatrixType;
+ template<int UpLo> struct ConstSelfAdjointViewReturnType {
+ typedef typename ActualMatrixType::template ConstSelfAdjointViewReturnType<UpLo>::Type Type;
+ };
+
+ enum {
+ MatrixFree = false
+ };
+
+ generic_matrix_wrapper()
+ : m_dummy(0,0), m_matrix(m_dummy)
+ {}
+
+ template<typename InputType>
+ generic_matrix_wrapper(const InputType &mat)
+ : m_matrix(mat)
+ {}
+
+ const ActualMatrixType& matrix() const
+ {
+ return m_matrix;
+ }
+
+ template<typename MatrixDerived>
+ void grab(const EigenBase<MatrixDerived> &mat)
+ {
+ m_matrix.~Ref<const MatrixType>();
+ ::new (&m_matrix) Ref<const MatrixType>(mat.derived());
+ }
+
+ void grab(const Ref<const MatrixType> &mat)
+ {
+ if(&(mat.derived()) != &m_matrix)
+ {
+ m_matrix.~Ref<const MatrixType>();
+ ::new (&m_matrix) Ref<const MatrixType>(mat);
+ }
+ }
+
+protected:
+ MatrixType m_dummy; // used to default initialize the Ref<> object
+ ActualMatrixType m_matrix;
+};
+
+// MatrixType is not compatible with Ref<> -> matrix-free wrapper
+template<typename MatrixType>
+class generic_matrix_wrapper<MatrixType,true>
+{
+public:
+ typedef MatrixType ActualMatrixType;
+ template<int UpLo> struct ConstSelfAdjointViewReturnType
+ {
+ typedef ActualMatrixType Type;
+ };
+
+ enum {
+ MatrixFree = true
+ };
+
+ generic_matrix_wrapper()
+ : mp_matrix(0)
+ {}
+
+ generic_matrix_wrapper(const MatrixType &mat)
+ : mp_matrix(&mat)
+ {}
+
+ const ActualMatrixType& matrix() const
+ {
+ return *mp_matrix;
+ }
+
+ void grab(const MatrixType &mat)
+ {
+ mp_matrix = &mat;
+ }
+
+protected:
+ const ActualMatrixType *mp_matrix;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief Base class for linear iterative solvers
+ *
+ * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+ */
+template< typename Derived>
+class IterativeSolverBase : public SparseSolverBase<Derived>
+{
+protected:
+ typedef SparseSolverBase<Derived> Base;
+ using Base::m_isInitialized;
+
+public:
+ typedef typename internal::traits<Derived>::MatrixType MatrixType;
+ typedef typename internal::traits<Derived>::Preconditioner Preconditioner;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef typename MatrixType::RealScalar RealScalar;
+
+ enum {
+ ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+ MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+ };
+
+public:
+
+ using Base::derived;
+
+ /** Default constructor. */
+ IterativeSolverBase()
+ {
+ init();
+ }
+
+ /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+ *
+ * This constructor is a shortcut for the default constructor followed
+ * by a call to compute().
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ explicit IterativeSolverBase(const EigenBase<MatrixDerived>& A)
+ : m_matrixWrapper(A.derived())
+ {
+ init();
+ compute(matrix());
+ }
+
+ ~IterativeSolverBase() {}
+
+ /** Initializes the iterative solver for the sparsity pattern of the matrix \a A for further solving \c Ax=b problems.
+ *
+ * Currently, this function mostly calls analyzePattern on the preconditioner. In the future
+ * we might, for instance, implement column reordering for faster matrix vector products.
+ */
+ template<typename MatrixDerived>
+ Derived& analyzePattern(const EigenBase<MatrixDerived>& A)
+ {
+ grab(A.derived());
+ m_preconditioner.analyzePattern(matrix());
+ m_isInitialized = true;
+ m_analysisIsOk = true;
+ m_info = m_preconditioner.info();
+ return derived();
+ }
+
+ /** Initializes the iterative solver with the numerical values of the matrix \a A for further solving \c Ax=b problems.
+ *
+ * Currently, this function mostly calls factorize on the preconditioner.
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ Derived& factorize(const EigenBase<MatrixDerived>& A)
+ {
+ eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+ grab(A.derived());
+ m_preconditioner.factorize(matrix());
+ m_factorizationIsOk = true;
+ m_info = m_preconditioner.info();
+ return derived();
+ }
+
+ /** Initializes the iterative solver with the matrix \a A for further solving \c Ax=b problems.
+ *
+ * Currently, this function mostly initializes/computes the preconditioner. In the future
+ * we might, for instance, implement column reordering for faster matrix vector products.
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ Derived& compute(const EigenBase<MatrixDerived>& A)
+ {
+ grab(A.derived());
+ m_preconditioner.compute(matrix());
+ m_isInitialized = true;
+ m_analysisIsOk = true;
+ m_factorizationIsOk = true;
+ m_info = m_preconditioner.info();
+ return derived();
+ }
+
+ /** \internal */
+ EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return matrix().rows(); }
+
+ /** \internal */
+ EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return matrix().cols(); }
+
+ /** \returns the tolerance threshold used by the stopping criteria.
+ * \sa setTolerance()
+ */
+ RealScalar tolerance() const { return m_tolerance; }
+
+ /** Sets the tolerance threshold used by the stopping criteria.
+ *
+ * This value is used as an upper bound to the relative residual error: |Ax-b|/|b|.
+ * The default value is the machine precision given by NumTraits<Scalar>::epsilon()
+ */
+ Derived& setTolerance(const RealScalar& tolerance)
+ {
+ m_tolerance = tolerance;
+ return derived();
+ }
+
+ /** \returns a read-write reference to the preconditioner for custom configuration. */
+ Preconditioner& preconditioner() { return m_preconditioner; }
+
+ /** \returns a read-only reference to the preconditioner. */
+ const Preconditioner& preconditioner() const { return m_preconditioner; }
+
+ /** \returns the max number of iterations.
+ * It is either the value set by setMaxIterations or, by default,
+ * twice the number of columns of the matrix.
+ */
+ Index maxIterations() const
+ {
+ return (m_maxIterations<0) ? 2*matrix().cols() : m_maxIterations;
+ }
+
+ /** Sets the max number of iterations.
+ * Default is twice the number of columns of the matrix.
+ */
+ Derived& setMaxIterations(Index maxIters)
+ {
+ m_maxIterations = maxIters;
+ return derived();
+ }
+
+ /** \returns the number of iterations performed during the last solve */
+ Index iterations() const
+ {
+ eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+ return m_iterations;
+ }
+
+ /** \returns the tolerance error reached during the last solve.
+ * It is a close approximation of the true relative residual error |Ax-b|/|b|.
+ */
+ RealScalar error() const
+ {
+ eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+ return m_error;
+ }
+
+ /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A
+ * and \a x0 as an initial solution.
+ *
+ * \sa solve(), compute()
+ */
+ template<typename Rhs,typename Guess>
+ inline const SolveWithGuess<Derived, Rhs, Guess>
+ solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const
+ {
+ eigen_assert(m_isInitialized && "Solver is not initialized.");
+ eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+ return SolveWithGuess<Derived, Rhs, Guess>(derived(), b.derived(), x0);
+ }
+
+ /** \returns Success if the iterations converged, and NoConvergence otherwise. */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized.");
+ return m_info;
+ }
+
+ /** \internal */
+ template<typename Rhs, typename DestDerived>
+ void _solve_with_guess_impl(const Rhs& b, SparseMatrixBase<DestDerived> &aDest) const
+ {
+ eigen_assert(rows()==b.rows());
+
+ Index rhsCols = b.cols();
+ Index size = b.rows();
+ DestDerived& dest(aDest.derived());
+ typedef typename DestDerived::Scalar DestScalar;
+ Eigen::Matrix<DestScalar,Dynamic,1> tb(size);
+ Eigen::Matrix<DestScalar,Dynamic,1> tx(cols());
+ // We do not directly fill dest because sparse expressions have to be free of aliasing issue.
+ // For non square least-square problems, b and dest might not have the same size whereas they might alias each-other.
+ typename DestDerived::PlainObject tmp(cols(),rhsCols);
+ ComputationInfo global_info = Success;
+ for(Index k=0; k<rhsCols; ++k)
+ {
+ tb = b.col(k);
+ tx = dest.col(k);
+ derived()._solve_vector_with_guess_impl(tb,tx);
+ tmp.col(k) = tx.sparseView(0);
+
+ // The call to _solve_vector_with_guess_impl updates m_info, so if it failed for a previous column
+ // we need to restore it to the worst value.
+ if(m_info==NumericalIssue)
+ global_info = NumericalIssue;
+ else if(m_info==NoConvergence)
+ global_info = NoConvergence;
+ }
+ m_info = global_info;
+ dest.swap(tmp);
+ }
+
+ template<typename Rhs, typename DestDerived>
+ typename internal::enable_if<Rhs::ColsAtCompileTime!=1 && DestDerived::ColsAtCompileTime!=1>::type
+ _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &aDest) const
+ {
+ eigen_assert(rows()==b.rows());
+
+ Index rhsCols = b.cols();
+ DestDerived& dest(aDest.derived());
+ ComputationInfo global_info = Success;
+ for(Index k=0; k<rhsCols; ++k)
+ {
+ typename DestDerived::ColXpr xk(dest,k);
+ typename Rhs::ConstColXpr bk(b,k);
+ derived()._solve_vector_with_guess_impl(bk,xk);
+
+ // The call to _solve_vector_with_guess updates m_info, so if it failed for a previous column
+ // we need to restore it to the worst value.
+ if(m_info==NumericalIssue)
+ global_info = NumericalIssue;
+ else if(m_info==NoConvergence)
+ global_info = NoConvergence;
+ }
+ m_info = global_info;
+ }
+
+ template<typename Rhs, typename DestDerived>
+ typename internal::enable_if<Rhs::ColsAtCompileTime==1 || DestDerived::ColsAtCompileTime==1>::type
+ _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &dest) const
+ {
+ derived()._solve_vector_with_guess_impl(b,dest.derived());
+ }
+
+ /** \internal default initial guess = 0 */
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const Rhs& b, Dest& x) const
+ {
+ x.setZero();
+ derived()._solve_with_guess_impl(b,x);
+ }
+
+protected:
+ void init()
+ {
+ m_isInitialized = false;
+ m_analysisIsOk = false;
+ m_factorizationIsOk = false;
+ m_maxIterations = -1;
+ m_tolerance = NumTraits<Scalar>::epsilon();
+ }
+
+ typedef internal::generic_matrix_wrapper<MatrixType> MatrixWrapper;
+ typedef typename MatrixWrapper::ActualMatrixType ActualMatrixType;
+
+ const ActualMatrixType& matrix() const
+ {
+ return m_matrixWrapper.matrix();
+ }
+
+ template<typename InputType>
+ void grab(const InputType &A)
+ {
+ m_matrixWrapper.grab(A);
+ }
+
+ MatrixWrapper m_matrixWrapper;
+ Preconditioner m_preconditioner;
+
+ Index m_maxIterations;
+ RealScalar m_tolerance;
+
+ mutable RealScalar m_error;
+ mutable Index m_iterations;
+ mutable ComputationInfo m_info;
+ mutable bool m_analysisIsOk, m_factorizationIsOk;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_ITERATIVE_SOLVER_BASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
new file mode 100644
index 0000000..203fd0e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_LEAST_SQUARE_CONJUGATE_GRADIENT_H
+#define EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal Low-level conjugate gradient algorithm for least-square problems
+ * \param mat The matrix A
+ * \param rhs The right hand side vector b
+ * \param x On input and initial solution, on output the computed solution.
+ * \param precond A preconditioner being able to efficiently solve for an
+ * approximation of A'Ax=b (regardless of b)
+ * \param iters On input the max number of iteration, on output the number of performed iterations.
+ * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+ */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE
+void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
+ const Preconditioner& precond, Index& iters,
+ typename Dest::RealScalar& tol_error)
+{
+ using std::sqrt;
+ using std::abs;
+ typedef typename Dest::RealScalar RealScalar;
+ typedef typename Dest::Scalar Scalar;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+
+ RealScalar tol = tol_error;
+ Index maxIters = iters;
+
+ Index m = mat.rows(), n = mat.cols();
+
+ VectorType residual = rhs - mat * x;
+ VectorType normal_residual = mat.adjoint() * residual;
+
+ RealScalar rhsNorm2 = (mat.adjoint()*rhs).squaredNorm();
+ if(rhsNorm2 == 0)
+ {
+ x.setZero();
+ iters = 0;
+ tol_error = 0;
+ return;
+ }
+ RealScalar threshold = tol*tol*rhsNorm2;
+ RealScalar residualNorm2 = normal_residual.squaredNorm();
+ if (residualNorm2 < threshold)
+ {
+ iters = 0;
+ tol_error = sqrt(residualNorm2 / rhsNorm2);
+ return;
+ }
+
+ VectorType p(n);
+ p = precond.solve(normal_residual); // initial search direction
+
+ VectorType z(n), tmp(m);
+ RealScalar absNew = numext::real(normal_residual.dot(p)); // the square of the absolute value of r scaled by invM
+ Index i = 0;
+ while(i < maxIters)
+ {
+ tmp.noalias() = mat * p;
+
+ Scalar alpha = absNew / tmp.squaredNorm(); // the amount we travel on dir
+ x += alpha * p; // update solution
+ residual -= alpha * tmp; // update residual
+ normal_residual = mat.adjoint() * residual; // update residual of the normal equation
+
+ residualNorm2 = normal_residual.squaredNorm();
+ if(residualNorm2 < threshold)
+ break;
+
+ z = precond.solve(normal_residual); // approximately solve for "A'A z = normal_residual"
+
+ RealScalar absOld = absNew;
+ absNew = numext::real(normal_residual.dot(z)); // update the absolute value of r
+ RealScalar beta = absNew / absOld; // calculate the Gram-Schmidt value used to create the new search direction
+ p = z + beta * p; // update search direction
+ i++;
+ }
+ tol_error = sqrt(residualNorm2 / rhsNorm2);
+ iters = i;
+}
+
+}
+
+template< typename _MatrixType,
+ typename _Preconditioner = LeastSquareDiagonalPreconditioner<typename _MatrixType::Scalar> >
+class LeastSquaresConjugateGradient;
+
+namespace internal {
+
+template< typename _MatrixType, typename _Preconditioner>
+struct traits<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+ * \brief A conjugate gradient solver for sparse (or dense) least-square problems
+ *
+ * This class allows to solve for A x = b linear problems using an iterative conjugate gradient algorithm.
+ * The matrix A can be non symmetric and rectangular, but the matrix A' A should be positive-definite to guaranty stability.
+ * Otherwise, the SparseLU or SparseQR classes might be preferable.
+ * The matrix A and the vectors x and b can be either dense or sparse.
+ *
+ * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
+ * \tparam _Preconditioner the type of the preconditioner. Default is LeastSquareDiagonalPreconditioner
+ *
+ * \implsparsesolverconcept
+ *
+ * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+ * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+ * and NumTraits<Scalar>::epsilon() for the tolerance.
+ *
+ * This class can be used as the direct solver classes. Here is a typical usage example:
+ \code
+ int m=1000000, n = 10000;
+ VectorXd x(n), b(m);
+ SparseMatrix<double> A(m,n);
+ // fill A and b
+ LeastSquaresConjugateGradient<SparseMatrix<double> > lscg;
+ lscg.compute(A);
+ x = lscg.solve(b);
+ std::cout << "#iterations: " << lscg.iterations() << std::endl;
+ std::cout << "estimated error: " << lscg.error() << std::endl;
+ // update b, and solve again
+ x = lscg.solve(b);
+ \endcode
+ *
+ * By default the iterations start with x=0 as an initial guess of the solution.
+ * One can control the start using the solveWithGuess() method.
+ *
+ * \sa class ConjugateGradient, SparseLU, SparseQR
+ */
+template< typename _MatrixType, typename _Preconditioner>
+class LeastSquaresConjugateGradient : public IterativeSolverBase<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
+{
+ typedef IterativeSolverBase<LeastSquaresConjugateGradient> Base;
+ using Base::matrix;
+ using Base::m_error;
+ using Base::m_iterations;
+ using Base::m_info;
+ using Base::m_isInitialized;
+public:
+ typedef _MatrixType MatrixType;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef _Preconditioner Preconditioner;
+
+public:
+
+ /** Default constructor. */
+ LeastSquaresConjugateGradient() : Base() {}
+
+ /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+ *
+ * This constructor is a shortcut for the default constructor followed
+ * by a call to compute().
+ *
+ * \warning this class stores a reference to the matrix A as well as some
+ * precomputed values that depend on it. Therefore, if \a A is changed
+ * this class becomes invalid. Call compute() to update it with the new
+ * matrix A, or modify a copy of A.
+ */
+ template<typename MatrixDerived>
+ explicit LeastSquaresConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+ ~LeastSquaresConjugateGradient() {}
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+ {
+ m_iterations = Base::maxIterations();
+ m_error = Base::m_tolerance;
+
+ internal::least_square_conjugate_gradient(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
+ m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
+ }
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
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
new file mode 100644
index 0000000..7b89657
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
@@ -0,0 +1,117 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SOLVEWITHGUESS_H
+#define EIGEN_SOLVEWITHGUESS_H
+
+namespace Eigen {
+
+template<typename Decomposition, typename RhsType, typename GuessType> class SolveWithGuess;
+
+/** \class SolveWithGuess
+ * \ingroup IterativeLinearSolvers_Module
+ *
+ * \brief Pseudo expression representing a solving operation
+ *
+ * \tparam Decomposition the type of the matrix or decomposion object
+ * \tparam Rhstype the type of the right-hand side
+ *
+ * This class represents an expression of A.solve(B)
+ * and most of the time this is the only way it is used.
+ *
+ */
+namespace internal {
+
+
+template<typename Decomposition, typename RhsType, typename GuessType>
+struct traits<SolveWithGuess<Decomposition, RhsType, GuessType> >
+ : traits<Solve<Decomposition,RhsType> >
+{};
+
+}
+
+
+template<typename Decomposition, typename RhsType, typename GuessType>
+class SolveWithGuess : public internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type
+{
+public:
+ typedef typename internal::traits<SolveWithGuess>::Scalar Scalar;
+ typedef typename internal::traits<SolveWithGuess>::PlainObject PlainObject;
+ typedef typename internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type Base;
+ typedef typename internal::ref_selector<SolveWithGuess>::type Nested;
+
+ SolveWithGuess(const Decomposition &dec, const RhsType &rhs, const GuessType &guess)
+ : m_dec(dec), m_rhs(rhs), m_guess(guess)
+ {}
+
+ EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+ Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
+ EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+ Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
+
+ EIGEN_DEVICE_FUNC const Decomposition& dec() const { return m_dec; }
+ EIGEN_DEVICE_FUNC const RhsType& rhs() const { return m_rhs; }
+ EIGEN_DEVICE_FUNC const GuessType& guess() const { return m_guess; }
+
+protected:
+ const Decomposition &m_dec;
+ const RhsType &m_rhs;
+ const GuessType &m_guess;
+
+private:
+ Scalar coeff(Index row, Index col) const;
+ Scalar coeff(Index i) const;
+};
+
+namespace internal {
+
+// Evaluator of SolveWithGuess -> eval into a temporary
+template<typename Decomposition, typename RhsType, typename GuessType>
+struct evaluator<SolveWithGuess<Decomposition,RhsType, GuessType> >
+ : public evaluator<typename SolveWithGuess<Decomposition,RhsType,GuessType>::PlainObject>
+{
+ typedef SolveWithGuess<Decomposition,RhsType,GuessType> SolveType;
+ typedef typename SolveType::PlainObject PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ evaluator(const SolveType& solve)
+ : m_result(solve.rows(), solve.cols())
+ {
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ m_result = solve.guess();
+ solve.dec()._solve_with_guess_impl(solve.rhs(), m_result);
+ }
+
+protected:
+ PlainObject m_result;
+};
+
+// Specialization for "dst = dec.solveWithGuess(rhs)"
+// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse specialization must exist somewhere
+template<typename DstXprType, typename DecType, typename RhsType, typename GuessType, typename Scalar>
+struct Assignment<DstXprType, SolveWithGuess<DecType,RhsType,GuessType>, internal::assign_op<Scalar,Scalar>, Dense2Dense>
+{
+ typedef SolveWithGuess<DecType,RhsType,GuessType> SrcXprType;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
+ {
+ Index dstRows = src.rows();
+ Index dstCols = src.cols();
+ if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+ dst.resize(dstRows, dstCols);
+
+ dst = src.guess();
+ src.dec()._solve_with_guess_impl(src.rhs(), dst/*, src.guess()*/);
+ }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SOLVEWITHGUESS_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
new file mode 100644
index 0000000..7ca3f33
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
@@ -0,0 +1,435 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+NOTE: this routine has been adapted from the CSparse library:
+
+Copyright (c) 2006, Timothy A. Davis.
+http://www.suitesparse.com
+
+The author of CSparse, Timothy A. Davis., has executed a license with Google LLC
+to permit distribution of this code and derivative works as part of Eigen under
+the Mozilla Public License v. 2.0, as stated at the top of this file.
+*/
+
+#ifndef EIGEN_SPARSE_AMD_H
+#define EIGEN_SPARSE_AMD_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename T> inline T amd_flip(const T& i) { return -i-2; }
+template<typename T> inline T amd_unflip(const T& i) { return i<0 ? amd_flip(i) : i; }
+template<typename T0, typename T1> inline bool amd_marked(const T0* w, const T1& j) { return w[j]<0; }
+template<typename T0, typename T1> inline void amd_mark(const T0* w, const T1& j) { return w[j] = amd_flip(w[j]); }
+
+/* clear w */
+template<typename StorageIndex>
+static StorageIndex cs_wclear (StorageIndex mark, StorageIndex lemax, StorageIndex *w, StorageIndex n)
+{
+ StorageIndex k;
+ if(mark < 2 || (mark + lemax < 0))
+ {
+ for(k = 0; k < n; k++)
+ if(w[k] != 0)
+ w[k] = 1;
+ mark = 2;
+ }
+ return (mark); /* at this point, w[0..n-1] < mark holds */
+}
+
+/* depth-first search and postorder of a tree rooted at node j */
+template<typename StorageIndex>
+StorageIndex cs_tdfs(StorageIndex j, StorageIndex k, StorageIndex *head, const StorageIndex *next, StorageIndex *post, StorageIndex *stack)
+{
+ StorageIndex i, p, top = 0;
+ if(!head || !next || !post || !stack) return (-1); /* check inputs */
+ stack[0] = j; /* place j on the stack */
+ while (top >= 0) /* while (stack is not empty) */
+ {
+ p = stack[top]; /* p = top of stack */
+ i = head[p]; /* i = youngest child of p */
+ if(i == -1)
+ {
+ top--; /* p has no unordered children left */
+ post[k++] = p; /* node p is the kth postordered node */
+ }
+ else
+ {
+ head[p] = next[i]; /* remove i from children of p */
+ stack[++top] = i; /* start dfs on child node i */
+ }
+ }
+ return k;
+}
+
+
+/** \internal
+ * \ingroup OrderingMethods_Module
+ * Approximate minimum degree ordering algorithm.
+ *
+ * \param[in] C the input selfadjoint matrix stored in compressed column major format.
+ * \param[out] perm the permutation P reducing the fill-in of the input matrix \a C
+ *
+ * Note that the input matrix \a C must be complete, that is both the upper and lower parts have to be stored, as well as the diagonal entries.
+ * On exit the values of C are destroyed */
+template<typename Scalar, typename StorageIndex>
+void minimum_degree_ordering(SparseMatrix<Scalar,ColMajor,StorageIndex>& C, PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm)
+{
+ using std::sqrt;
+
+ StorageIndex d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
+ k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
+ ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t, h;
+
+ StorageIndex n = StorageIndex(C.cols());
+ dense = std::max<StorageIndex> (16, StorageIndex(10 * sqrt(double(n)))); /* find dense threshold */
+ dense = (std::min)(n-2, dense);
+
+ StorageIndex cnz = StorageIndex(C.nonZeros());
+ perm.resize(n+1);
+ t = cnz + cnz/5 + 2*n; /* add elbow room to C */
+ C.resizeNonZeros(t);
+
+ // get workspace
+ ei_declare_aligned_stack_constructed_variable(StorageIndex,W,8*(n+1),0);
+ StorageIndex* len = W;
+ StorageIndex* nv = W + (n+1);
+ StorageIndex* next = W + 2*(n+1);
+ StorageIndex* head = W + 3*(n+1);
+ StorageIndex* elen = W + 4*(n+1);
+ StorageIndex* degree = W + 5*(n+1);
+ StorageIndex* w = W + 6*(n+1);
+ StorageIndex* hhead = W + 7*(n+1);
+ StorageIndex* last = perm.indices().data(); /* use P as workspace for last */
+
+ /* --- Initialize quotient graph ---------------------------------------- */
+ StorageIndex* Cp = C.outerIndexPtr();
+ StorageIndex* Ci = C.innerIndexPtr();
+ for(k = 0; k < n; k++)
+ len[k] = Cp[k+1] - Cp[k];
+ len[n] = 0;
+ nzmax = t;
+
+ for(i = 0; i <= n; i++)
+ {
+ head[i] = -1; // degree list i is empty
+ last[i] = -1;
+ next[i] = -1;
+ hhead[i] = -1; // hash list i is empty
+ nv[i] = 1; // node i is just one node
+ w[i] = 1; // node i is alive
+ elen[i] = 0; // Ek of node i is empty
+ degree[i] = len[i]; // degree of node i
+ }
+ mark = internal::cs_wclear<StorageIndex>(0, 0, w, n); /* clear w */
+
+ /* --- Initialize degree lists ------------------------------------------ */
+ for(i = 0; i < n; i++)
+ {
+ bool has_diag = false;
+ for(p = Cp[i]; p<Cp[i+1]; ++p)
+ if(Ci[p]==i)
+ {
+ has_diag = true;
+ break;
+ }
+
+ d = degree[i];
+ if(d == 1 && has_diag) /* node i is empty */
+ {
+ elen[i] = -2; /* element i is dead */
+ nel++;
+ Cp[i] = -1; /* i is a root of assembly tree */
+ w[i] = 0;
+ }
+ else if(d > dense || !has_diag) /* node i is dense or has no structural diagonal element */
+ {
+ nv[i] = 0; /* absorb i into element n */
+ elen[i] = -1; /* node i is dead */
+ nel++;
+ Cp[i] = amd_flip (n);
+ nv[n]++;
+ }
+ else
+ {
+ if(head[d] != -1) last[head[d]] = i;
+ next[i] = head[d]; /* put node i in degree list d */
+ head[d] = i;
+ }
+ }
+
+ elen[n] = -2; /* n is a dead element */
+ Cp[n] = -1; /* n is a root of assembly tree */
+ w[n] = 0; /* n is a dead element */
+
+ while (nel < n) /* while (selecting pivots) do */
+ {
+ /* --- Select node of minimum approximate degree -------------------- */
+ for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {}
+ if(next[k] != -1) last[next[k]] = -1;
+ head[mindeg] = next[k]; /* remove k from degree list */
+ elenk = elen[k]; /* elenk = |Ek| */
+ nvk = nv[k]; /* # of nodes k represents */
+ nel += nvk; /* nv[k] nodes of A eliminated */
+
+ /* --- Garbage collection ------------------------------------------- */
+ if(elenk > 0 && cnz + mindeg >= nzmax)
+ {
+ for(j = 0; j < n; j++)
+ {
+ if((p = Cp[j]) >= 0) /* j is a live node or element */
+ {
+ Cp[j] = Ci[p]; /* save first entry of object */
+ Ci[p] = amd_flip (j); /* first entry is now amd_flip(j) */
+ }
+ }
+ for(q = 0, p = 0; p < cnz; ) /* scan all of memory */
+ {
+ if((j = amd_flip (Ci[p++])) >= 0) /* found object j */
+ {
+ Ci[q] = Cp[j]; /* restore first entry of object */
+ Cp[j] = q++; /* new pointer to object j */
+ for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++];
+ }
+ }
+ cnz = q; /* Ci[cnz...nzmax-1] now free */
+ }
+
+ /* --- Construct new element ---------------------------------------- */
+ dk = 0;
+ nv[k] = -nvk; /* flag k as in Lk */
+ p = Cp[k];
+ pk1 = (elenk == 0) ? p : cnz; /* do in place if elen[k] == 0 */
+ pk2 = pk1;
+ for(k1 = 1; k1 <= elenk + 1; k1++)
+ {
+ if(k1 > elenk)
+ {
+ e = k; /* search the nodes in k */
+ pj = p; /* list of nodes starts at Ci[pj]*/
+ ln = len[k] - elenk; /* length of list of nodes in k */
+ }
+ else
+ {
+ e = Ci[p++]; /* search the nodes in e */
+ pj = Cp[e];
+ ln = len[e]; /* length of list of nodes in e */
+ }
+ for(k2 = 1; k2 <= ln; k2++)
+ {
+ i = Ci[pj++];
+ if((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */
+ dk += nvi; /* degree[Lk] += size of node i */
+ nv[i] = -nvi; /* negate nv[i] to denote i in Lk*/
+ Ci[pk2++] = i; /* place i in Lk */
+ if(next[i] != -1) last[next[i]] = last[i];
+ if(last[i] != -1) /* remove i from degree list */
+ {
+ next[last[i]] = next[i];
+ }
+ else
+ {
+ head[degree[i]] = next[i];
+ }
+ }
+ if(e != k)
+ {
+ Cp[e] = amd_flip (k); /* absorb e into k */
+ w[e] = 0; /* e is now a dead element */
+ }
+ }
+ if(elenk != 0) cnz = pk2; /* Ci[cnz...nzmax] is free */
+ degree[k] = dk; /* external degree of k - |Lk\i| */
+ Cp[k] = pk1; /* element k is in Ci[pk1..pk2-1] */
+ len[k] = pk2 - pk1;
+ elen[k] = -2; /* k is now an element */
+
+ /* --- Find set differences ----------------------------------------- */
+ mark = internal::cs_wclear<StorageIndex>(mark, lemax, w, n); /* clear w if necessary */
+ for(pk = pk1; pk < pk2; pk++) /* scan 1: find |Le\Lk| */
+ {
+ i = Ci[pk];
+ if((eln = elen[i]) <= 0) continue;/* skip if elen[i] empty */
+ nvi = -nv[i]; /* nv[i] was negated */
+ wnvi = mark - nvi;
+ for(p = Cp[i]; p <= Cp[i] + eln - 1; p++) /* scan Ei */
+ {
+ e = Ci[p];
+ if(w[e] >= mark)
+ {
+ w[e] -= nvi; /* decrement |Le\Lk| */
+ }
+ else if(w[e] != 0) /* ensure e is a live element */
+ {
+ w[e] = degree[e] + wnvi; /* 1st time e seen in scan 1 */
+ }
+ }
+ }
+
+ /* --- Degree update ------------------------------------------------ */
+ for(pk = pk1; pk < pk2; pk++) /* scan2: degree update */
+ {
+ i = Ci[pk]; /* consider node i in Lk */
+ p1 = Cp[i];
+ p2 = p1 + elen[i] - 1;
+ pn = p1;
+ for(h = 0, d = 0, p = p1; p <= p2; p++) /* scan Ei */
+ {
+ e = Ci[p];
+ if(w[e] != 0) /* e is an unabsorbed element */
+ {
+ dext = w[e] - mark; /* dext = |Le\Lk| */
+ if(dext > 0)
+ {
+ d += dext; /* sum up the set differences */
+ Ci[pn++] = e; /* keep e in Ei */
+ h += e; /* compute the hash of node i */
+ }
+ else
+ {
+ Cp[e] = amd_flip (k); /* aggressive absorb. e->k */
+ w[e] = 0; /* e is a dead element */
+ }
+ }
+ }
+ elen[i] = pn - p1 + 1; /* elen[i] = |Ei| */
+ p3 = pn;
+ p4 = p1 + len[i];
+ for(p = p2 + 1; p < p4; p++) /* prune edges in Ai */
+ {
+ j = Ci[p];
+ if((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */
+ d += nvj; /* degree(i) += |j| */
+ Ci[pn++] = j; /* place j in node list of i */
+ h += j; /* compute hash for node i */
+ }
+ if(d == 0) /* check for mass elimination */
+ {
+ Cp[i] = amd_flip (k); /* absorb i into k */
+ nvi = -nv[i];
+ dk -= nvi; /* |Lk| -= |i| */
+ nvk += nvi; /* |k| += nv[i] */
+ nel += nvi;
+ nv[i] = 0;
+ elen[i] = -1; /* node i is dead */
+ }
+ else
+ {
+ degree[i] = std::min<StorageIndex> (degree[i], d); /* update degree(i) */
+ Ci[pn] = Ci[p3]; /* move first node to end */
+ Ci[p3] = Ci[p1]; /* move 1st el. to end of Ei */
+ Ci[p1] = k; /* add k as 1st element in of Ei */
+ len[i] = pn - p1 + 1; /* new len of adj. list of node i */
+ h %= n; /* finalize hash of i */
+ next[i] = hhead[h]; /* place i in hash bucket */
+ hhead[h] = i;
+ last[i] = h; /* save hash of i in last[i] */
+ }
+ } /* scan2 is done */
+ degree[k] = dk; /* finalize |Lk| */
+ lemax = std::max<StorageIndex>(lemax, dk);
+ mark = internal::cs_wclear<StorageIndex>(mark+lemax, lemax, w, n); /* clear w */
+
+ /* --- Supernode detection ------------------------------------------ */
+ for(pk = pk1; pk < pk2; pk++)
+ {
+ i = Ci[pk];
+ if(nv[i] >= 0) continue; /* skip if i is dead */
+ h = last[i]; /* scan hash bucket of node i */
+ i = hhead[h];
+ hhead[h] = -1; /* hash bucket will be empty */
+ for(; i != -1 && next[i] != -1; i = next[i], mark++)
+ {
+ ln = len[i];
+ eln = elen[i];
+ for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark;
+ jlast = i;
+ for(j = next[i]; j != -1; ) /* compare i with all j */
+ {
+ ok = (len[j] == ln) && (elen[j] == eln);
+ for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++)
+ {
+ if(w[Ci[p]] != mark) ok = 0; /* compare i and j*/
+ }
+ if(ok) /* i and j are identical */
+ {
+ Cp[j] = amd_flip (i); /* absorb j into i */
+ nv[i] += nv[j];
+ nv[j] = 0;
+ elen[j] = -1; /* node j is dead */
+ j = next[j]; /* delete j from hash bucket */
+ next[jlast] = j;
+ }
+ else
+ {
+ jlast = j; /* j and i are different */
+ j = next[j];
+ }
+ }
+ }
+ }
+
+ /* --- Finalize new element------------------------------------------ */
+ for(p = pk1, pk = pk1; pk < pk2; pk++) /* finalize Lk */
+ {
+ i = Ci[pk];
+ if((nvi = -nv[i]) <= 0) continue;/* skip if i is dead */
+ nv[i] = nvi; /* restore nv[i] */
+ d = degree[i] + dk - nvi; /* compute external degree(i) */
+ d = std::min<StorageIndex> (d, n - nel - nvi);
+ if(head[d] != -1) last[head[d]] = i;
+ next[i] = head[d]; /* put i back in degree list */
+ last[i] = -1;
+ head[d] = i;
+ mindeg = std::min<StorageIndex> (mindeg, d); /* find new minimum degree */
+ degree[i] = d;
+ Ci[p++] = i; /* place i in Lk */
+ }
+ nv[k] = nvk; /* # nodes absorbed into k */
+ if((len[k] = p-pk1) == 0) /* length of adj list of element k*/
+ {
+ Cp[k] = -1; /* k is a root of the tree */
+ w[k] = 0; /* k is now a dead element */
+ }
+ if(elenk != 0) cnz = p; /* free unused space in Lk */
+ }
+
+ /* --- Postordering ----------------------------------------------------- */
+ for(i = 0; i < n; i++) Cp[i] = amd_flip (Cp[i]);/* fix assembly tree */
+ for(j = 0; j <= n; j++) head[j] = -1;
+ for(j = n; j >= 0; j--) /* place unordered nodes in lists */
+ {
+ if(nv[j] > 0) continue; /* skip if j is an element */
+ next[j] = head[Cp[j]]; /* place j in list of its parent */
+ head[Cp[j]] = j;
+ }
+ for(e = n; e >= 0; e--) /* place elements in lists */
+ {
+ if(nv[e] <= 0) continue; /* skip unless e is an element */
+ if(Cp[e] != -1)
+ {
+ next[e] = head[Cp[e]]; /* place e in list of its parent */
+ head[Cp[e]] = e;
+ }
+ }
+ for(k = 0, i = 0; i <= n; i++) /* postorder the assembly tree */
+ {
+ if(Cp[i] == -1) k = internal::cs_tdfs<StorageIndex>(i, k, head, next, perm.indices().data(), w);
+ }
+
+ perm.indices().conservativeResize(n);
+}
+
+} // namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_AMD_H
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
new file mode 100644
index 0000000..8e339a7
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
@@ -0,0 +1,1863 @@
+// // This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+// This file is modified from the colamd/symamd library. The copyright is below
+
+// The authors of the code itself are Stefan I. Larimore and Timothy A.
+// Davis (davis@cise.ufl.edu), University of Florida. The algorithm was
+// developed in collaboration with John Gilbert, Xerox PARC, and Esmond
+// Ng, Oak Ridge National Laboratory.
+//
+// Date:
+//
+// September 8, 2003. Version 2.3.
+//
+// Acknowledgements:
+//
+// This work was supported by the National Science Foundation, under
+// grants DMS-9504974 and DMS-9803599.
+//
+// Notice:
+//
+// Copyright (c) 1998-2003 by the University of Florida.
+// All Rights Reserved.
+//
+// THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+// EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+//
+// Permission is hereby granted to use, copy, modify, and/or distribute
+// this program, provided that the Copyright, this License, and the
+// Availability of the original version is retained on all copies and made
+// accessible to the end-user of any code or package that includes COLAMD
+// or any modified version of COLAMD.
+//
+// Availability:
+//
+// The colamd/symamd library is available at
+//
+// http://www.suitesparse.com
+
+
+#ifndef EIGEN_COLAMD_H
+#define EIGEN_COLAMD_H
+
+namespace internal {
+
+namespace Colamd {
+
+/* Ensure that debugging is turned off: */
+#ifndef COLAMD_NDEBUG
+#define COLAMD_NDEBUG
+#endif /* NDEBUG */
+
+
+/* ========================================================================== */
+/* === Knob and statistics definitions ====================================== */
+/* ========================================================================== */
+
+/* size of the knobs [ ] array. Only knobs [0..1] are currently used. */
+const int NKnobs = 20;
+
+/* number of output statistics. Only stats [0..6] are currently used. */
+const int NStats = 20;
+
+/* Indices into knobs and stats array. */
+enum KnobsStatsIndex {
+ /* knobs [0] and stats [0]: dense row knob and output statistic. */
+ DenseRow = 0,
+
+ /* knobs [1] and stats [1]: dense column knob and output statistic. */
+ DenseCol = 1,
+
+ /* stats [2]: memory defragmentation count output statistic */
+ DefragCount = 2,
+
+ /* stats [3]: colamd status: zero OK, > 0 warning or notice, < 0 error */
+ Status = 3,
+
+ /* stats [4..6]: error info, or info on jumbled columns */
+ Info1 = 4,
+ Info2 = 5,
+ Info3 = 6
+};
+
+/* error codes returned in stats [3]: */
+enum Status {
+ Ok = 0,
+ OkButJumbled = 1,
+ ErrorANotPresent = -1,
+ ErrorPNotPresent = -2,
+ ErrorNrowNegative = -3,
+ ErrorNcolNegative = -4,
+ ErrorNnzNegative = -5,
+ ErrorP0Nonzero = -6,
+ ErrorATooSmall = -7,
+ ErrorColLengthNegative = -8,
+ ErrorRowIndexOutOfBounds = -9,
+ ErrorOutOfMemory = -10,
+ ErrorInternalError = -999
+};
+/* ========================================================================== */
+/* === Definitions ========================================================== */
+/* ========================================================================== */
+
+template <typename IndexType>
+IndexType ones_complement(const IndexType r) {
+ return (-(r)-1);
+}
+
+/* -------------------------------------------------------------------------- */
+const int Empty = -1;
+
+/* Row and column status */
+enum RowColumnStatus {
+ Alive = 0,
+ Dead = -1
+};
+
+/* Column status */
+enum ColumnStatus {
+ DeadPrincipal = -1,
+ DeadNonPrincipal = -2
+};
+
+/* ========================================================================== */
+/* === Colamd reporting mechanism =========================================== */
+/* ========================================================================== */
+
+// == Row and Column structures ==
+template <typename IndexType>
+struct ColStructure
+{
+ IndexType start ; /* index for A of first row in this column, or Dead */
+ /* if column is dead */
+ IndexType length ; /* number of rows in this column */
+ union
+ {
+ IndexType thickness ; /* number of original columns represented by this */
+ /* col, if the column is alive */
+ IndexType parent ; /* parent in parent tree super-column structure, if */
+ /* the column is dead */
+ } shared1 ;
+ union
+ {
+ IndexType score ; /* the score used to maintain heap, if col is alive */
+ IndexType order ; /* pivot ordering of this column, if col is dead */
+ } shared2 ;
+ union
+ {
+ IndexType headhash ; /* head of a hash bucket, if col is at the head of */
+ /* a degree list */
+ IndexType hash ; /* hash value, if col is not in a degree list */
+ IndexType prev ; /* previous column in degree list, if col is in a */
+ /* degree list (but not at the head of a degree list) */
+ } shared3 ;
+ union
+ {
+ IndexType degree_next ; /* next column, if col is in a degree list */
+ IndexType hash_next ; /* next column, if col is in a hash list */
+ } shared4 ;
+
+ inline bool is_dead() const { return start < Alive; }
+
+ inline bool is_alive() const { return start >= Alive; }
+
+ inline bool is_dead_principal() const { return start == DeadPrincipal; }
+
+ inline void kill_principal() { start = DeadPrincipal; }
+
+ inline void kill_non_principal() { start = DeadNonPrincipal; }
+
+};
+
+template <typename IndexType>
+struct RowStructure
+{
+ IndexType start ; /* index for A of first col in this row */
+ IndexType length ; /* number of principal columns in this row */
+ union
+ {
+ IndexType degree ; /* number of principal & non-principal columns in row */
+ IndexType p ; /* used as a row pointer in init_rows_cols () */
+ } shared1 ;
+ union
+ {
+ IndexType mark ; /* for computing set differences and marking dead rows*/
+ IndexType first_column ;/* first column in row (used in garbage collection) */
+ } shared2 ;
+
+ inline bool is_dead() const { return shared2.mark < Alive; }
+
+ inline bool is_alive() const { return shared2.mark >= Alive; }
+
+ inline void kill() { shared2.mark = Dead; }
+
+};
+
+/* ========================================================================== */
+/* === Colamd recommended memory size ======================================= */
+/* ========================================================================== */
+
+/*
+ The recommended length Alen of the array A passed to colamd is given by
+ the COLAMD_RECOMMENDED (nnz, n_row, n_col) macro. It returns -1 if any
+ argument is negative. 2*nnz space is required for the row and column
+ indices of the matrix. colamd_c (n_col) + colamd_r (n_row) space is
+ required for the Col and Row arrays, respectively, which are internal to
+ colamd. An additional n_col space is the minimal amount of "elbow room",
+ and nnz/5 more space is recommended for run time efficiency.
+
+ This macro is not needed when using symamd.
+
+ Explicit typecast to IndexType added Sept. 23, 2002, COLAMD version 2.2, to avoid
+ gcc -pedantic warning messages.
+*/
+template <typename IndexType>
+inline IndexType colamd_c(IndexType n_col)
+{ return IndexType( ((n_col) + 1) * sizeof (ColStructure<IndexType>) / sizeof (IndexType) ) ; }
+
+template <typename IndexType>
+inline IndexType colamd_r(IndexType n_row)
+{ return IndexType(((n_row) + 1) * sizeof (RowStructure<IndexType>) / sizeof (IndexType)); }
+
+// Prototypes of non-user callable routines
+template <typename IndexType>
+static IndexType init_rows_cols (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> col [], IndexType A [], IndexType p [], IndexType stats[NStats] );
+
+template <typename IndexType>
+static void init_scoring (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], double knobs[NKnobs], IndexType *p_n_row2, IndexType *p_n_col2, IndexType *p_max_deg);
+
+template <typename IndexType>
+static IndexType find_ordering (IndexType n_row, IndexType n_col, IndexType Alen, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType n_col2, IndexType max_deg, IndexType pfree);
+
+template <typename IndexType>
+static void order_children (IndexType n_col, ColStructure<IndexType> Col [], IndexType p []);
+
+template <typename IndexType>
+static void detect_super_cols (ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType row_start, IndexType row_length ) ;
+
+template <typename IndexType>
+static IndexType garbage_collection (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType *pfree) ;
+
+template <typename IndexType>
+static inline IndexType clear_mark (IndexType n_row, RowStructure<IndexType> Row [] ) ;
+
+/* === No debugging ========================================================= */
+
+#define COLAMD_DEBUG0(params) ;
+#define COLAMD_DEBUG1(params) ;
+#define COLAMD_DEBUG2(params) ;
+#define COLAMD_DEBUG3(params) ;
+#define COLAMD_DEBUG4(params) ;
+
+#define COLAMD_ASSERT(expression) ((void) 0)
+
+
+/**
+ * \brief Returns the recommended value of Alen
+ *
+ * Returns recommended value of Alen for use by colamd.
+ * Returns -1 if any input argument is negative.
+ * The use of this routine or macro is optional.
+ * Note that the macro uses its arguments more than once,
+ * so be careful for side effects, if you pass expressions as arguments to COLAMD_RECOMMENDED.
+ *
+ * \param nnz nonzeros in A
+ * \param n_row number of rows in A
+ * \param n_col number of columns in A
+ * \return recommended value of Alen for use by colamd
+ */
+template <typename IndexType>
+inline IndexType recommended ( IndexType nnz, IndexType n_row, IndexType n_col)
+{
+ if ((nnz) < 0 || (n_row) < 0 || (n_col) < 0)
+ return (-1);
+ else
+ return (2 * (nnz) + colamd_c (n_col) + colamd_r (n_row) + (n_col) + ((nnz) / 5));
+}
+
+/**
+ * \brief set default parameters The use of this routine is optional.
+ *
+ * Colamd: rows with more than (knobs [DenseRow] * n_col)
+ * entries are removed prior to ordering. Columns with more than
+ * (knobs [DenseCol] * n_row) entries are removed prior to
+ * ordering, and placed last in the output column ordering.
+ *
+ * DenseRow and DenseCol are defined as 0 and 1,
+ * respectively, in colamd.h. Default values of these two knobs
+ * are both 0.5. Currently, only knobs [0] and knobs [1] are
+ * used, but future versions may use more knobs. If so, they will
+ * be properly set to their defaults by the future version of
+ * colamd_set_defaults, so that the code that calls colamd will
+ * not need to change, assuming that you either use
+ * colamd_set_defaults, or pass a (double *) NULL pointer as the
+ * knobs array to colamd or symamd.
+ *
+ * \param knobs parameter settings for colamd
+ */
+
+static inline void set_defaults(double knobs[NKnobs])
+{
+ /* === Local variables ================================================== */
+
+ int i ;
+
+ if (!knobs)
+ {
+ return ; /* no knobs to initialize */
+ }
+ for (i = 0 ; i < NKnobs ; i++)
+ {
+ knobs [i] = 0 ;
+ }
+ knobs [Colamd::DenseRow] = 0.5 ; /* ignore rows over 50% dense */
+ knobs [Colamd::DenseCol] = 0.5 ; /* ignore columns over 50% dense */
+}
+
+/**
+ * \brief Computes a column ordering using the column approximate minimum degree ordering
+ *
+ * Computes a column ordering (Q) of A such that P(AQ)=LU or
+ * (AQ)'AQ=LL' have less fill-in and require fewer floating point
+ * operations than factorizing the unpermuted matrix A or A'A,
+ * respectively.
+ *
+ *
+ * \param n_row number of rows in A
+ * \param n_col number of columns in 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
+ * \param stats colamd output statistics and error codes
+ */
+template <typename IndexType>
+static bool compute_ordering(IndexType n_row, IndexType n_col, IndexType Alen, IndexType *A, IndexType *p, double knobs[NKnobs], IndexType stats[NStats])
+{
+ /* === Local variables ================================================== */
+
+ IndexType i ; /* loop index */
+ IndexType nnz ; /* nonzeros in A */
+ IndexType Row_size ; /* size of Row [], in integers */
+ IndexType Col_size ; /* size of Col [], in integers */
+ IndexType need ; /* minimum required length of A */
+ Colamd::RowStructure<IndexType> *Row ; /* pointer into A of Row [0..n_row] array */
+ Colamd::ColStructure<IndexType> *Col ; /* pointer into A of Col [0..n_col] array */
+ IndexType n_col2 ; /* number of non-dense, non-empty columns */
+ IndexType n_row2 ; /* number of non-dense, non-empty rows */
+ IndexType ngarbage ; /* number of garbage collections performed */
+ IndexType max_deg ; /* maximum row degree */
+ double default_knobs [NKnobs] ; /* default knobs array */
+
+
+ /* === Check the input arguments ======================================== */
+
+ if (!stats)
+ {
+ COLAMD_DEBUG0 (("colamd: stats not present\n")) ;
+ return (false) ;
+ }
+ for (i = 0 ; i < NStats ; i++)
+ {
+ stats [i] = 0 ;
+ }
+ stats [Colamd::Status] = Colamd::Ok ;
+ stats [Colamd::Info1] = -1 ;
+ stats [Colamd::Info2] = -1 ;
+
+ if (!A) /* A is not present */
+ {
+ stats [Colamd::Status] = Colamd::ErrorANotPresent ;
+ COLAMD_DEBUG0 (("colamd: A not present\n")) ;
+ return (false) ;
+ }
+
+ if (!p) /* p is not present */
+ {
+ stats [Colamd::Status] = Colamd::ErrorPNotPresent ;
+ COLAMD_DEBUG0 (("colamd: p not present\n")) ;
+ return (false) ;
+ }
+
+ if (n_row < 0) /* n_row must be >= 0 */
+ {
+ stats [Colamd::Status] = Colamd::ErrorNrowNegative ;
+ stats [Colamd::Info1] = n_row ;
+ COLAMD_DEBUG0 (("colamd: nrow negative %d\n", n_row)) ;
+ return (false) ;
+ }
+
+ if (n_col < 0) /* n_col must be >= 0 */
+ {
+ stats [Colamd::Status] = Colamd::ErrorNcolNegative ;
+ stats [Colamd::Info1] = n_col ;
+ COLAMD_DEBUG0 (("colamd: ncol negative %d\n", n_col)) ;
+ return (false) ;
+ }
+
+ nnz = p [n_col] ;
+ if (nnz < 0) /* nnz must be >= 0 */
+ {
+ stats [Colamd::Status] = Colamd::ErrorNnzNegative ;
+ stats [Colamd::Info1] = nnz ;
+ COLAMD_DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ;
+ return (false) ;
+ }
+
+ if (p [0] != 0)
+ {
+ stats [Colamd::Status] = Colamd::ErrorP0Nonzero ;
+ stats [Colamd::Info1] = p [0] ;
+ COLAMD_DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ;
+ return (false) ;
+ }
+
+ /* === If no knobs, set default knobs =================================== */
+
+ if (!knobs)
+ {
+ set_defaults (default_knobs) ;
+ knobs = default_knobs ;
+ }
+
+ /* === Allocate the Row and Col arrays from array A ===================== */
+
+ Col_size = colamd_c (n_col) ;
+ Row_size = colamd_r (n_row) ;
+ need = 2*nnz + n_col + Col_size + Row_size ;
+
+ if (need > Alen)
+ {
+ /* not enough space in array A to perform the ordering */
+ stats [Colamd::Status] = Colamd::ErrorATooSmall ;
+ stats [Colamd::Info1] = need ;
+ stats [Colamd::Info2] = Alen ;
+ COLAMD_DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen));
+ return (false) ;
+ }
+
+ Alen -= Col_size + Row_size ;
+ Col = (ColStructure<IndexType> *) &A [Alen] ;
+ Row = (RowStructure<IndexType> *) &A [Alen + Col_size] ;
+
+ /* === Construct the row and column data structures ===================== */
+
+ if (!Colamd::init_rows_cols (n_row, n_col, Row, Col, A, p, stats))
+ {
+ /* input matrix is invalid */
+ COLAMD_DEBUG0 (("colamd: Matrix invalid\n")) ;
+ return (false) ;
+ }
+
+ /* === Initialize scores, kill dense rows/columns ======================= */
+
+ Colamd::init_scoring (n_row, n_col, Row, Col, A, p, knobs,
+ &n_row2, &n_col2, &max_deg) ;
+
+ /* === Order the supercolumns =========================================== */
+
+ ngarbage = Colamd::find_ordering (n_row, n_col, Alen, Row, Col, A, p,
+ n_col2, max_deg, 2*nnz) ;
+
+ /* === Order the non-principal columns ================================== */
+
+ Colamd::order_children (n_col, Col, p) ;
+
+ /* === Return statistics in stats ======================================= */
+
+ stats [Colamd::DenseRow] = n_row - n_row2 ;
+ stats [Colamd::DenseCol] = n_col - n_col2 ;
+ stats [Colamd::DefragCount] = ngarbage ;
+ COLAMD_DEBUG0 (("colamd: done.\n")) ;
+ return (true) ;
+}
+
+/* ========================================================================== */
+/* === NON-USER-CALLABLE ROUTINES: ========================================== */
+/* ========================================================================== */
+
+/* There are no user-callable routines beyond this point in the file */
+
+/* ========================================================================== */
+/* === init_rows_cols ======================================================= */
+/* ========================================================================== */
+
+/*
+ Takes the column form of the matrix in A and creates the row form of the
+ matrix. Also, row and column attributes are stored in the Col and Row
+ structs. If the columns are un-sorted or contain duplicate row indices,
+ this routine will also sort and remove duplicate row indices from the
+ column form of the matrix. Returns false if the matrix is invalid,
+ true otherwise. Not user-callable.
+*/
+template <typename IndexType>
+static IndexType init_rows_cols /* returns true if OK, or false otherwise */
+ (
+ /* === Parameters ======================================================= */
+
+ IndexType n_row, /* number of rows of A */
+ IndexType n_col, /* number of columns of A */
+ RowStructure<IndexType> Row [], /* of size n_row+1 */
+ ColStructure<IndexType> Col [], /* of size n_col+1 */
+ IndexType A [], /* row indices of A, of size Alen */
+ IndexType p [], /* pointers to columns in A, of size n_col+1 */
+ IndexType stats [NStats] /* colamd statistics */
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType col ; /* a column index */
+ IndexType row ; /* a row index */
+ IndexType *cp ; /* a column pointer */
+ IndexType *cp_end ; /* a pointer to the end of a column */
+ IndexType *rp ; /* a row pointer */
+ IndexType *rp_end ; /* a pointer to the end of a row */
+ IndexType last_row ; /* previous row */
+
+ /* === Initialize columns, and check column pointers ==================== */
+
+ for (col = 0 ; col < n_col ; col++)
+ {
+ Col [col].start = p [col] ;
+ Col [col].length = p [col+1] - p [col] ;
+
+ if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
+ {
+ /* column pointers must be non-decreasing */
+ stats [Colamd::Status] = Colamd::ErrorColLengthNegative ;
+ stats [Colamd::Info1] = col ;
+ stats [Colamd::Info2] = Col [col].length ;
+ COLAMD_DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ;
+ return (false) ;
+ }
+
+ Col [col].shared1.thickness = 1 ;
+ Col [col].shared2.score = 0 ;
+ Col [col].shared3.prev = Empty ;
+ Col [col].shared4.degree_next = Empty ;
+ }
+
+ /* p [0..n_col] no longer needed, used as "head" in subsequent routines */
+
+ /* === Scan columns, compute row degrees, and check row indices ========= */
+
+ stats [Info3] = 0 ; /* number of duplicate or unsorted row indices*/
+
+ for (row = 0 ; row < n_row ; row++)
+ {
+ Row [row].length = 0 ;
+ Row [row].shared2.mark = -1 ;
+ }
+
+ for (col = 0 ; col < n_col ; col++)
+ {
+ last_row = -1 ;
+
+ cp = &A [p [col]] ;
+ cp_end = &A [p [col+1]] ;
+
+ while (cp < cp_end)
+ {
+ row = *cp++ ;
+
+ /* make sure row indices within range */
+ if (row < 0 || row >= n_row)
+ {
+ stats [Colamd::Status] = Colamd::ErrorRowIndexOutOfBounds ;
+ stats [Colamd::Info1] = col ;
+ stats [Colamd::Info2] = row ;
+ stats [Colamd::Info3] = n_row ;
+ COLAMD_DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ;
+ return (false) ;
+ }
+
+ if (row <= last_row || Row [row].shared2.mark == col)
+ {
+ /* row index are unsorted or repeated (or both), thus col */
+ /* is jumbled. This is a notice, not an error condition. */
+ stats [Colamd::Status] = Colamd::OkButJumbled ;
+ stats [Colamd::Info1] = col ;
+ stats [Colamd::Info2] = row ;
+ (stats [Colamd::Info3]) ++ ;
+ COLAMD_DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col));
+ }
+
+ if (Row [row].shared2.mark != col)
+ {
+ Row [row].length++ ;
+ }
+ else
+ {
+ /* this is a repeated entry in the column, */
+ /* it will be removed */
+ Col [col].length-- ;
+ }
+
+ /* mark the row as having been seen in this column */
+ Row [row].shared2.mark = col ;
+
+ last_row = row ;
+ }
+ }
+
+ /* === Compute row pointers ============================================= */
+
+ /* row form of the matrix starts directly after the column */
+ /* form of matrix in A */
+ Row [0].start = p [n_col] ;
+ Row [0].shared1.p = Row [0].start ;
+ Row [0].shared2.mark = -1 ;
+ for (row = 1 ; row < n_row ; row++)
+ {
+ Row [row].start = Row [row-1].start + Row [row-1].length ;
+ Row [row].shared1.p = Row [row].start ;
+ Row [row].shared2.mark = -1 ;
+ }
+
+ /* === Create row form ================================================== */
+
+ if (stats [Status] == OkButJumbled)
+ {
+ /* if cols jumbled, watch for repeated row indices */
+ for (col = 0 ; col < n_col ; col++)
+ {
+ cp = &A [p [col]] ;
+ cp_end = &A [p [col+1]] ;
+ while (cp < cp_end)
+ {
+ row = *cp++ ;
+ if (Row [row].shared2.mark != col)
+ {
+ A [(Row [row].shared1.p)++] = col ;
+ Row [row].shared2.mark = col ;
+ }
+ }
+ }
+ }
+ else
+ {
+ /* if cols not jumbled, we don't need the mark (this is faster) */
+ for (col = 0 ; col < n_col ; col++)
+ {
+ cp = &A [p [col]] ;
+ cp_end = &A [p [col+1]] ;
+ while (cp < cp_end)
+ {
+ A [(Row [*cp++].shared1.p)++] = col ;
+ }
+ }
+ }
+
+ /* === Clear the row marks and set row degrees ========================== */
+
+ for (row = 0 ; row < n_row ; row++)
+ {
+ Row [row].shared2.mark = 0 ;
+ Row [row].shared1.degree = Row [row].length ;
+ }
+
+ /* === See if we need to re-create columns ============================== */
+
+ if (stats [Status] == OkButJumbled)
+ {
+ COLAMD_DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ;
+
+
+ /* === Compute col pointers ========================================= */
+
+ /* col form of the matrix starts at A [0]. */
+ /* Note, we may have a gap between the col form and the row */
+ /* form if there were duplicate entries, if so, it will be */
+ /* removed upon the first garbage collection */
+ Col [0].start = 0 ;
+ p [0] = Col [0].start ;
+ for (col = 1 ; col < n_col ; col++)
+ {
+ /* note that the lengths here are for pruned columns, i.e. */
+ /* no duplicate row indices will exist for these columns */
+ Col [col].start = Col [col-1].start + Col [col-1].length ;
+ p [col] = Col [col].start ;
+ }
+
+ /* === Re-create col form =========================================== */
+
+ for (row = 0 ; row < n_row ; row++)
+ {
+ rp = &A [Row [row].start] ;
+ rp_end = rp + Row [row].length ;
+ while (rp < rp_end)
+ {
+ A [(p [*rp++])++] = row ;
+ }
+ }
+ }
+
+ /* === Done. Matrix is not (or no longer) jumbled ====================== */
+
+ return (true) ;
+}
+
+
+/* ========================================================================== */
+/* === init_scoring ========================================================= */
+/* ========================================================================== */
+
+/*
+ Kills dense or empty columns and rows, calculates an initial score for
+ each column, and places all columns in the degree lists. Not user-callable.
+*/
+template <typename IndexType>
+static void init_scoring
+ (
+ /* === Parameters ======================================================= */
+
+ IndexType n_row, /* number of rows of A */
+ IndexType n_col, /* number of columns of A */
+ RowStructure<IndexType> Row [], /* of size n_row+1 */
+ ColStructure<IndexType> Col [], /* of size n_col+1 */
+ IndexType A [], /* column form and row form of A */
+ IndexType head [], /* of size n_col+1 */
+ double knobs [NKnobs],/* parameters */
+ IndexType *p_n_row2, /* number of non-dense, non-empty rows */
+ IndexType *p_n_col2, /* number of non-dense, non-empty columns */
+ IndexType *p_max_deg /* maximum row degree */
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType c ; /* a column index */
+ IndexType r, row ; /* a row index */
+ IndexType *cp ; /* a column pointer */
+ IndexType deg ; /* degree of a row or column */
+ IndexType *cp_end ; /* a pointer to the end of a column */
+ IndexType *new_cp ; /* new column pointer */
+ IndexType col_length ; /* length of pruned column */
+ IndexType score ; /* current column score */
+ IndexType n_col2 ; /* number of non-dense, non-empty columns */
+ IndexType n_row2 ; /* number of non-dense, non-empty rows */
+ IndexType dense_row_count ; /* remove rows with more entries than this */
+ IndexType dense_col_count ; /* remove cols with more entries than this */
+ IndexType min_score ; /* smallest column score */
+ IndexType max_deg ; /* maximum row degree */
+ IndexType next_col ; /* Used to add to degree list.*/
+
+
+ /* === Extract knobs ==================================================== */
+
+ dense_row_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseRow] * n_col), n_col)) ;
+ dense_col_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseCol] * n_row), n_row)) ;
+ COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
+ max_deg = 0 ;
+ n_col2 = n_col ;
+ n_row2 = n_row ;
+
+ /* === Kill empty columns =============================================== */
+
+ /* Put the empty columns at the end in their natural order, so that LU */
+ /* factorization can proceed as far as possible. */
+ for (c = n_col-1 ; c >= 0 ; c--)
+ {
+ deg = Col [c].length ;
+ if (deg == 0)
+ {
+ /* this is a empty column, kill and order it last */
+ Col [c].shared2.order = --n_col2 ;
+ Col[c].kill_principal() ;
+ }
+ }
+ COLAMD_DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ;
+
+ /* === Kill dense columns =============================================== */
+
+ /* Put the dense columns at the end, in their natural order */
+ for (c = n_col-1 ; c >= 0 ; c--)
+ {
+ /* skip any dead columns */
+ if (Col[c].is_dead())
+ {
+ continue ;
+ }
+ deg = Col [c].length ;
+ if (deg > dense_col_count)
+ {
+ /* this is a dense column, kill and order it last */
+ Col [c].shared2.order = --n_col2 ;
+ /* decrement the row degrees */
+ cp = &A [Col [c].start] ;
+ cp_end = cp + Col [c].length ;
+ while (cp < cp_end)
+ {
+ Row [*cp++].shared1.degree-- ;
+ }
+ Col[c].kill_principal() ;
+ }
+ }
+ COLAMD_DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ;
+
+ /* === Kill dense and empty rows ======================================== */
+
+ for (r = 0 ; r < n_row ; r++)
+ {
+ deg = Row [r].shared1.degree ;
+ COLAMD_ASSERT (deg >= 0 && deg <= n_col) ;
+ if (deg > dense_row_count || deg == 0)
+ {
+ /* kill a dense or empty row */
+ Row[r].kill() ;
+ --n_row2 ;
+ }
+ else
+ {
+ /* keep track of max degree of remaining rows */
+ max_deg = numext::maxi(max_deg, deg) ;
+ }
+ }
+ COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
+
+ /* === Compute initial column scores ==================================== */
+
+ /* At this point the row degrees are accurate. They reflect the number */
+ /* of "live" (non-dense) columns in each row. No empty rows exist. */
+ /* Some "live" columns may contain only dead rows, however. These are */
+ /* pruned in the code below. */
+
+ /* now find the initial matlab score for each column */
+ for (c = n_col-1 ; c >= 0 ; c--)
+ {
+ /* skip dead column */
+ if (Col[c].is_dead())
+ {
+ continue ;
+ }
+ score = 0 ;
+ cp = &A [Col [c].start] ;
+ new_cp = cp ;
+ cp_end = cp + Col [c].length ;
+ while (cp < cp_end)
+ {
+ /* get a row */
+ row = *cp++ ;
+ /* skip if dead */
+ if (Row[row].is_dead())
+ {
+ continue ;
+ }
+ /* compact the column */
+ *new_cp++ = row ;
+ /* add row's external degree */
+ score += Row [row].shared1.degree - 1 ;
+ /* guard against integer overflow */
+ score = numext::mini(score, n_col) ;
+ }
+ /* determine pruned column length */
+ col_length = (IndexType) (new_cp - &A [Col [c].start]) ;
+ if (col_length == 0)
+ {
+ /* a newly-made null column (all rows in this col are "dense" */
+ /* and have already been killed) */
+ COLAMD_DEBUG2 (("Newly null killed: %d\n", c)) ;
+ Col [c].shared2.order = --n_col2 ;
+ Col[c].kill_principal() ;
+ }
+ else
+ {
+ /* set column length and set score */
+ COLAMD_ASSERT (score >= 0) ;
+ COLAMD_ASSERT (score <= n_col) ;
+ Col [c].length = col_length ;
+ Col [c].shared2.score = score ;
+ }
+ }
+ COLAMD_DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n",
+ n_col-n_col2)) ;
+
+ /* At this point, all empty rows and columns are dead. All live columns */
+ /* are "clean" (containing no dead rows) and simplicial (no supercolumns */
+ /* yet). Rows may contain dead columns, but all live rows contain at */
+ /* least one live column. */
+
+ /* === Initialize degree lists ========================================== */
+
+
+ /* clear the hash buckets */
+ for (c = 0 ; c <= n_col ; c++)
+ {
+ head [c] = Empty ;
+ }
+ min_score = n_col ;
+ /* place in reverse order, so low column indices are at the front */
+ /* of the lists. This is to encourage natural tie-breaking */
+ for (c = n_col-1 ; c >= 0 ; c--)
+ {
+ /* only add principal columns to degree lists */
+ if (Col[c].is_alive())
+ {
+ COLAMD_DEBUG4 (("place %d score %d minscore %d ncol %d\n",
+ c, Col [c].shared2.score, min_score, n_col)) ;
+
+ /* === Add columns score to DList =============================== */
+
+ score = Col [c].shared2.score ;
+
+ COLAMD_ASSERT (min_score >= 0) ;
+ COLAMD_ASSERT (min_score <= n_col) ;
+ COLAMD_ASSERT (score >= 0) ;
+ COLAMD_ASSERT (score <= n_col) ;
+ COLAMD_ASSERT (head [score] >= Empty) ;
+
+ /* now add this column to dList at proper score location */
+ next_col = head [score] ;
+ Col [c].shared3.prev = Empty ;
+ Col [c].shared4.degree_next = next_col ;
+
+ /* if there already was a column with the same score, set its */
+ /* previous pointer to this new column */
+ if (next_col != Empty)
+ {
+ Col [next_col].shared3.prev = c ;
+ }
+ head [score] = c ;
+
+ /* see if this score is less than current min */
+ min_score = numext::mini(min_score, score) ;
+
+
+ }
+ }
+
+
+ /* === Return number of remaining columns, and max row degree =========== */
+
+ *p_n_col2 = n_col2 ;
+ *p_n_row2 = n_row2 ;
+ *p_max_deg = max_deg ;
+}
+
+
+/* ========================================================================== */
+/* === find_ordering ======================================================== */
+/* ========================================================================== */
+
+/*
+ Order the principal columns of the supercolumn form of the matrix
+ (no supercolumns on input). Uses a minimum approximate column minimum
+ degree ordering method. Not user-callable.
+*/
+template <typename IndexType>
+static IndexType find_ordering /* return the number of garbage collections */
+ (
+ /* === Parameters ======================================================= */
+
+ IndexType n_row, /* number of rows of A */
+ IndexType n_col, /* number of columns of A */
+ IndexType Alen, /* size of A, 2*nnz + n_col or larger */
+ RowStructure<IndexType> Row [], /* of size n_row+1 */
+ ColStructure<IndexType> Col [], /* of size n_col+1 */
+ IndexType A [], /* column form and row form of A */
+ IndexType head [], /* of size n_col+1 */
+ IndexType n_col2, /* Remaining columns to order */
+ IndexType max_deg, /* Maximum row degree */
+ IndexType pfree /* index of first free slot (2*nnz on entry) */
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType k ; /* current pivot ordering step */
+ IndexType pivot_col ; /* current pivot column */
+ IndexType *cp ; /* a column pointer */
+ IndexType *rp ; /* a row pointer */
+ IndexType pivot_row ; /* current pivot row */
+ IndexType *new_cp ; /* modified column pointer */
+ IndexType *new_rp ; /* modified row pointer */
+ IndexType pivot_row_start ; /* pointer to start of pivot row */
+ IndexType pivot_row_degree ; /* number of columns in pivot row */
+ IndexType pivot_row_length ; /* number of supercolumns in pivot row */
+ IndexType pivot_col_score ; /* score of pivot column */
+ IndexType needed_memory ; /* free space needed for pivot row */
+ IndexType *cp_end ; /* pointer to the end of a column */
+ IndexType *rp_end ; /* pointer to the end of a row */
+ IndexType row ; /* a row index */
+ IndexType col ; /* a column index */
+ IndexType max_score ; /* maximum possible score */
+ IndexType cur_score ; /* score of current column */
+ unsigned int hash ; /* hash value for supernode detection */
+ IndexType head_column ; /* head of hash bucket */
+ IndexType first_col ; /* first column in hash bucket */
+ IndexType tag_mark ; /* marker value for mark array */
+ IndexType row_mark ; /* Row [row].shared2.mark */
+ IndexType set_difference ; /* set difference size of row with pivot row */
+ IndexType min_score ; /* smallest column score */
+ IndexType col_thickness ; /* "thickness" (no. of columns in a supercol) */
+ IndexType max_mark ; /* maximum value of tag_mark */
+ IndexType pivot_col_thickness ; /* number of columns represented by pivot col */
+ IndexType prev_col ; /* Used by Dlist operations. */
+ IndexType next_col ; /* Used by Dlist operations. */
+ IndexType ngarbage ; /* number of garbage collections performed */
+
+
+ /* === Initialization and clear mark ==================================== */
+
+ max_mark = INT_MAX - n_col ; /* INT_MAX defined in <limits.h> */
+ tag_mark = Colamd::clear_mark (n_row, Row) ;
+ min_score = 0 ;
+ ngarbage = 0 ;
+ COLAMD_DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ;
+
+ /* === Order the columns ================================================ */
+
+ for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */)
+ {
+
+ /* === Select pivot column, and order it ============================ */
+
+ /* make sure degree list isn't empty */
+ COLAMD_ASSERT (min_score >= 0) ;
+ COLAMD_ASSERT (min_score <= n_col) ;
+ COLAMD_ASSERT (head [min_score] >= Empty) ;
+
+ /* get pivot column from head of minimum degree list */
+ while (min_score < n_col && head [min_score] == Empty)
+ {
+ min_score++ ;
+ }
+ pivot_col = head [min_score] ;
+ COLAMD_ASSERT (pivot_col >= 0 && pivot_col <= n_col) ;
+ next_col = Col [pivot_col].shared4.degree_next ;
+ head [min_score] = next_col ;
+ if (next_col != Empty)
+ {
+ Col [next_col].shared3.prev = Empty ;
+ }
+
+ COLAMD_ASSERT (Col[pivot_col].is_alive()) ;
+ COLAMD_DEBUG3 (("Pivot col: %d\n", pivot_col)) ;
+
+ /* remember score for defrag check */
+ pivot_col_score = Col [pivot_col].shared2.score ;
+
+ /* the pivot column is the kth column in the pivot order */
+ Col [pivot_col].shared2.order = k ;
+
+ /* increment order count by column thickness */
+ pivot_col_thickness = Col [pivot_col].shared1.thickness ;
+ k += pivot_col_thickness ;
+ COLAMD_ASSERT (pivot_col_thickness > 0) ;
+
+ /* === Garbage_collection, if necessary ============================= */
+
+ needed_memory = numext::mini(pivot_col_score, n_col - k) ;
+ if (pfree + needed_memory >= Alen)
+ {
+ pfree = Colamd::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
+ ngarbage++ ;
+ /* after garbage collection we will have enough */
+ COLAMD_ASSERT (pfree + needed_memory < Alen) ;
+ /* garbage collection has wiped out the Row[].shared2.mark array */
+ tag_mark = Colamd::clear_mark (n_row, Row) ;
+
+ }
+
+ /* === Compute pivot row pattern ==================================== */
+
+ /* get starting location for this new merged row */
+ pivot_row_start = pfree ;
+
+ /* initialize new row counts to zero */
+ pivot_row_degree = 0 ;
+
+ /* tag pivot column as having been visited so it isn't included */
+ /* in merged pivot row */
+ Col [pivot_col].shared1.thickness = -pivot_col_thickness ;
+
+ /* pivot row is the union of all rows in the pivot column pattern */
+ cp = &A [Col [pivot_col].start] ;
+ cp_end = cp + Col [pivot_col].length ;
+ while (cp < cp_end)
+ {
+ /* get a row */
+ row = *cp++ ;
+ COLAMD_DEBUG4 (("Pivot col pattern %d %d\n", Row[row].is_alive(), row)) ;
+ /* skip if row is dead */
+ if (Row[row].is_dead())
+ {
+ continue ;
+ }
+ rp = &A [Row [row].start] ;
+ rp_end = rp + Row [row].length ;
+ while (rp < rp_end)
+ {
+ /* get a column */
+ col = *rp++ ;
+ /* add the column, if alive and untagged */
+ col_thickness = Col [col].shared1.thickness ;
+ if (col_thickness > 0 && Col[col].is_alive())
+ {
+ /* tag column in pivot row */
+ Col [col].shared1.thickness = -col_thickness ;
+ COLAMD_ASSERT (pfree < Alen) ;
+ /* place column in pivot row */
+ A [pfree++] = col ;
+ pivot_row_degree += col_thickness ;
+ }
+ }
+ }
+
+ /* clear tag on pivot column */
+ Col [pivot_col].shared1.thickness = pivot_col_thickness ;
+ max_deg = numext::maxi(max_deg, pivot_row_degree) ;
+
+
+ /* === Kill all rows used to construct pivot row ==================== */
+
+ /* also kill pivot row, temporarily */
+ cp = &A [Col [pivot_col].start] ;
+ cp_end = cp + Col [pivot_col].length ;
+ while (cp < cp_end)
+ {
+ /* may be killing an already dead row */
+ row = *cp++ ;
+ COLAMD_DEBUG3 (("Kill row in pivot col: %d\n", row)) ;
+ Row[row].kill() ;
+ }
+
+ /* === Select a row index to use as the new pivot row =============== */
+
+ pivot_row_length = pfree - pivot_row_start ;
+ if (pivot_row_length > 0)
+ {
+ /* pick the "pivot" row arbitrarily (first row in col) */
+ pivot_row = A [Col [pivot_col].start] ;
+ COLAMD_DEBUG3 (("Pivotal row is %d\n", pivot_row)) ;
+ }
+ else
+ {
+ /* there is no pivot row, since it is of zero length */
+ pivot_row = Empty ;
+ COLAMD_ASSERT (pivot_row_length == 0) ;
+ }
+ COLAMD_ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ;
+
+ /* === Approximate degree computation =============================== */
+
+ /* Here begins the computation of the approximate degree. The column */
+ /* score is the sum of the pivot row "length", plus the size of the */
+ /* set differences of each row in the column minus the pattern of the */
+ /* pivot row itself. The column ("thickness") itself is also */
+ /* excluded from the column score (we thus use an approximate */
+ /* external degree). */
+
+ /* The time taken by the following code (compute set differences, and */
+ /* add them up) is proportional to the size of the data structure */
+ /* being scanned - that is, the sum of the sizes of each column in */
+ /* the pivot row. Thus, the amortized time to compute a column score */
+ /* is proportional to the size of that column (where size, in this */
+ /* context, is the column "length", or the number of row indices */
+ /* in that column). The number of row indices in a column is */
+ /* monotonically non-decreasing, from the length of the original */
+ /* column on input to colamd. */
+
+ /* === Compute set differences ====================================== */
+
+ COLAMD_DEBUG3 (("** Computing set differences phase. **\n")) ;
+
+ /* pivot row is currently dead - it will be revived later. */
+
+ COLAMD_DEBUG3 (("Pivot row: ")) ;
+ /* for each column in pivot row */
+ rp = &A [pivot_row_start] ;
+ rp_end = rp + pivot_row_length ;
+ while (rp < rp_end)
+ {
+ col = *rp++ ;
+ COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
+ COLAMD_DEBUG3 (("Col: %d\n", col)) ;
+
+ /* clear tags used to construct pivot row pattern */
+ col_thickness = -Col [col].shared1.thickness ;
+ COLAMD_ASSERT (col_thickness > 0) ;
+ Col [col].shared1.thickness = col_thickness ;
+
+ /* === Remove column from degree list =========================== */
+
+ cur_score = Col [col].shared2.score ;
+ prev_col = Col [col].shared3.prev ;
+ next_col = Col [col].shared4.degree_next ;
+ COLAMD_ASSERT (cur_score >= 0) ;
+ COLAMD_ASSERT (cur_score <= n_col) ;
+ COLAMD_ASSERT (cur_score >= Empty) ;
+ if (prev_col == Empty)
+ {
+ head [cur_score] = next_col ;
+ }
+ else
+ {
+ Col [prev_col].shared4.degree_next = next_col ;
+ }
+ if (next_col != Empty)
+ {
+ Col [next_col].shared3.prev = prev_col ;
+ }
+
+ /* === Scan the column ========================================== */
+
+ cp = &A [Col [col].start] ;
+ cp_end = cp + Col [col].length ;
+ while (cp < cp_end)
+ {
+ /* get a row */
+ row = *cp++ ;
+ /* skip if dead */
+ if (Row[row].is_dead())
+ {
+ continue ;
+ }
+ row_mark = Row [row].shared2.mark ;
+ COLAMD_ASSERT (row != pivot_row) ;
+ set_difference = row_mark - tag_mark ;
+ /* check if the row has been seen yet */
+ if (set_difference < 0)
+ {
+ COLAMD_ASSERT (Row [row].shared1.degree <= max_deg) ;
+ set_difference = Row [row].shared1.degree ;
+ }
+ /* subtract column thickness from this row's set difference */
+ set_difference -= col_thickness ;
+ COLAMD_ASSERT (set_difference >= 0) ;
+ /* absorb this row if the set difference becomes zero */
+ if (set_difference == 0)
+ {
+ COLAMD_DEBUG3 (("aggressive absorption. Row: %d\n", row)) ;
+ Row[row].kill() ;
+ }
+ else
+ {
+ /* save the new mark */
+ Row [row].shared2.mark = set_difference + tag_mark ;
+ }
+ }
+ }
+
+
+ /* === Add up set differences for each column ======================= */
+
+ COLAMD_DEBUG3 (("** Adding set differences phase. **\n")) ;
+
+ /* for each column in pivot row */
+ rp = &A [pivot_row_start] ;
+ rp_end = rp + pivot_row_length ;
+ while (rp < rp_end)
+ {
+ /* get a column */
+ col = *rp++ ;
+ COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
+ hash = 0 ;
+ cur_score = 0 ;
+ cp = &A [Col [col].start] ;
+ /* compact the column */
+ new_cp = cp ;
+ cp_end = cp + Col [col].length ;
+
+ COLAMD_DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ;
+
+ while (cp < cp_end)
+ {
+ /* get a row */
+ row = *cp++ ;
+ COLAMD_ASSERT(row >= 0 && row < n_row) ;
+ /* skip if dead */
+ if (Row [row].is_dead())
+ {
+ continue ;
+ }
+ row_mark = Row [row].shared2.mark ;
+ COLAMD_ASSERT (row_mark > tag_mark) ;
+ /* compact the column */
+ *new_cp++ = row ;
+ /* compute hash function */
+ hash += row ;
+ /* add set difference */
+ cur_score += row_mark - tag_mark ;
+ /* integer overflow... */
+ cur_score = numext::mini(cur_score, n_col) ;
+ }
+
+ /* recompute the column's length */
+ Col [col].length = (IndexType) (new_cp - &A [Col [col].start]) ;
+
+ /* === Further mass elimination ================================= */
+
+ if (Col [col].length == 0)
+ {
+ COLAMD_DEBUG4 (("further mass elimination. Col: %d\n", col)) ;
+ /* nothing left but the pivot row in this column */
+ Col[col].kill_principal() ;
+ pivot_row_degree -= Col [col].shared1.thickness ;
+ COLAMD_ASSERT (pivot_row_degree >= 0) ;
+ /* order it */
+ Col [col].shared2.order = k ;
+ /* increment order count by column thickness */
+ k += Col [col].shared1.thickness ;
+ }
+ else
+ {
+ /* === Prepare for supercolumn detection ==================== */
+
+ COLAMD_DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ;
+
+ /* save score so far */
+ Col [col].shared2.score = cur_score ;
+
+ /* add column to hash table, for supercolumn detection */
+ hash %= n_col + 1 ;
+
+ COLAMD_DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ;
+ COLAMD_ASSERT (hash <= n_col) ;
+
+ head_column = head [hash] ;
+ if (head_column > Empty)
+ {
+ /* degree list "hash" is non-empty, use prev (shared3) of */
+ /* first column in degree list as head of hash bucket */
+ first_col = Col [head_column].shared3.headhash ;
+ Col [head_column].shared3.headhash = col ;
+ }
+ else
+ {
+ /* degree list "hash" is empty, use head as hash bucket */
+ first_col = - (head_column + 2) ;
+ head [hash] = - (col + 2) ;
+ }
+ Col [col].shared4.hash_next = first_col ;
+
+ /* save hash function in Col [col].shared3.hash */
+ Col [col].shared3.hash = (IndexType) hash ;
+ COLAMD_ASSERT (Col[col].is_alive()) ;
+ }
+ }
+
+ /* The approximate external column degree is now computed. */
+
+ /* === Supercolumn detection ======================================== */
+
+ COLAMD_DEBUG3 (("** Supercolumn detection phase. **\n")) ;
+
+ Colamd::detect_super_cols (Col, A, head, pivot_row_start, pivot_row_length) ;
+
+ /* === Kill the pivotal column ====================================== */
+
+ Col[pivot_col].kill_principal() ;
+
+ /* === Clear mark =================================================== */
+
+ tag_mark += (max_deg + 1) ;
+ if (tag_mark >= max_mark)
+ {
+ COLAMD_DEBUG2 (("clearing tag_mark\n")) ;
+ tag_mark = Colamd::clear_mark (n_row, Row) ;
+ }
+
+ /* === Finalize the new pivot row, and column scores ================ */
+
+ COLAMD_DEBUG3 (("** Finalize scores phase. **\n")) ;
+
+ /* for each column in pivot row */
+ rp = &A [pivot_row_start] ;
+ /* compact the pivot row */
+ new_rp = rp ;
+ rp_end = rp + pivot_row_length ;
+ while (rp < rp_end)
+ {
+ col = *rp++ ;
+ /* skip dead columns */
+ if (Col[col].is_dead())
+ {
+ continue ;
+ }
+ *new_rp++ = col ;
+ /* add new pivot row to column */
+ A [Col [col].start + (Col [col].length++)] = pivot_row ;
+
+ /* retrieve score so far and add on pivot row's degree. */
+ /* (we wait until here for this in case the pivot */
+ /* row's degree was reduced due to mass elimination). */
+ cur_score = Col [col].shared2.score + pivot_row_degree ;
+
+ /* calculate the max possible score as the number of */
+ /* external columns minus the 'k' value minus the */
+ /* columns thickness */
+ max_score = n_col - k - Col [col].shared1.thickness ;
+
+ /* make the score the external degree of the union-of-rows */
+ cur_score -= Col [col].shared1.thickness ;
+
+ /* make sure score is less or equal than the max score */
+ cur_score = numext::mini(cur_score, max_score) ;
+ COLAMD_ASSERT (cur_score >= 0) ;
+
+ /* store updated score */
+ Col [col].shared2.score = cur_score ;
+
+ /* === Place column back in degree list ========================= */
+
+ COLAMD_ASSERT (min_score >= 0) ;
+ COLAMD_ASSERT (min_score <= n_col) ;
+ COLAMD_ASSERT (cur_score >= 0) ;
+ COLAMD_ASSERT (cur_score <= n_col) ;
+ COLAMD_ASSERT (head [cur_score] >= Empty) ;
+ next_col = head [cur_score] ;
+ Col [col].shared4.degree_next = next_col ;
+ Col [col].shared3.prev = Empty ;
+ if (next_col != Empty)
+ {
+ Col [next_col].shared3.prev = col ;
+ }
+ head [cur_score] = col ;
+
+ /* see if this score is less than current min */
+ min_score = numext::mini(min_score, cur_score) ;
+
+ }
+
+ /* === Resurrect the new pivot row ================================== */
+
+ if (pivot_row_degree > 0)
+ {
+ /* update pivot row length to reflect any cols that were killed */
+ /* during super-col detection and mass elimination */
+ Row [pivot_row].start = pivot_row_start ;
+ Row [pivot_row].length = (IndexType) (new_rp - &A[pivot_row_start]) ;
+ Row [pivot_row].shared1.degree = pivot_row_degree ;
+ Row [pivot_row].shared2.mark = 0 ;
+ /* pivot row is no longer dead */
+ }
+ }
+
+ /* === All principal columns have now been ordered ====================== */
+
+ return (ngarbage) ;
+}
+
+
+/* ========================================================================== */
+/* === order_children ======================================================= */
+/* ========================================================================== */
+
+/*
+ The find_ordering routine has ordered all of the principal columns (the
+ representatives of the supercolumns). The non-principal columns have not
+ yet been ordered. This routine orders those columns by walking up the
+ parent tree (a column is a child of the column which absorbed it). The
+ final permutation vector is then placed in p [0 ... n_col-1], with p [0]
+ being the first column, and p [n_col-1] being the last. It doesn't look
+ like it at first glance, but be assured that this routine takes time linear
+ in the number of columns. Although not immediately obvious, the time
+ taken by this routine is O (n_col), that is, linear in the number of
+ columns. Not user-callable.
+*/
+template <typename IndexType>
+static inline void order_children
+(
+ /* === Parameters ======================================================= */
+
+ IndexType n_col, /* number of columns of A */
+ ColStructure<IndexType> Col [], /* of size n_col+1 */
+ IndexType p [] /* p [0 ... n_col-1] is the column permutation*/
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType i ; /* loop counter for all columns */
+ IndexType c ; /* column index */
+ IndexType parent ; /* index of column's parent */
+ IndexType order ; /* column's order */
+
+ /* === Order each non-principal column ================================== */
+
+ for (i = 0 ; i < n_col ; i++)
+ {
+ /* find an un-ordered non-principal column */
+ COLAMD_ASSERT (col_is_dead(Col, i)) ;
+ if (!Col[i].is_dead_principal() && Col [i].shared2.order == Empty)
+ {
+ parent = i ;
+ /* once found, find its principal parent */
+ do
+ {
+ parent = Col [parent].shared1.parent ;
+ } while (!Col[parent].is_dead_principal()) ;
+
+ /* now, order all un-ordered non-principal columns along path */
+ /* to this parent. collapse tree at the same time */
+ c = i ;
+ /* get order of parent */
+ order = Col [parent].shared2.order ;
+
+ do
+ {
+ COLAMD_ASSERT (Col [c].shared2.order == Empty) ;
+
+ /* order this column */
+ Col [c].shared2.order = order++ ;
+ /* collaps tree */
+ Col [c].shared1.parent = parent ;
+
+ /* get immediate parent of this column */
+ c = Col [c].shared1.parent ;
+
+ /* continue until we hit an ordered column. There are */
+ /* guaranteed not to be anymore unordered columns */
+ /* above an ordered column */
+ } while (Col [c].shared2.order == Empty) ;
+
+ /* re-order the super_col parent to largest order for this group */
+ Col [parent].shared2.order = order ;
+ }
+ }
+
+ /* === Generate the permutation ========================================= */
+
+ for (c = 0 ; c < n_col ; c++)
+ {
+ p [Col [c].shared2.order] = c ;
+ }
+}
+
+
+/* ========================================================================== */
+/* === detect_super_cols ==================================================== */
+/* ========================================================================== */
+
+/*
+ Detects supercolumns by finding matches between columns in the hash buckets.
+ Check amongst columns in the set A [row_start ... row_start + row_length-1].
+ The columns under consideration are currently *not* in the degree lists,
+ and have already been placed in the hash buckets.
+
+ The hash bucket for columns whose hash function is equal to h is stored
+ as follows:
+
+ if head [h] is >= 0, then head [h] contains a degree list, so:
+
+ head [h] is the first column in degree bucket h.
+ Col [head [h]].headhash gives the first column in hash bucket h.
+
+ otherwise, the degree list is empty, and:
+
+ -(head [h] + 2) is the first column in hash bucket h.
+
+ For a column c in a hash bucket, Col [c].shared3.prev is NOT a "previous
+ column" pointer. Col [c].shared3.hash is used instead as the hash number
+ for that column. The value of Col [c].shared4.hash_next is the next column
+ in the same hash bucket.
+
+ Assuming no, or "few" hash collisions, the time taken by this routine is
+ linear in the sum of the sizes (lengths) of each column whose score has
+ just been computed in the approximate degree computation.
+ Not user-callable.
+*/
+template <typename IndexType>
+static void detect_super_cols
+(
+ /* === Parameters ======================================================= */
+
+ ColStructure<IndexType> Col [], /* of size n_col+1 */
+ IndexType A [], /* row indices of A */
+ IndexType head [], /* head of degree lists and hash buckets */
+ IndexType row_start, /* pointer to set of columns to check */
+ IndexType row_length /* number of columns to check */
+)
+{
+ /* === Local variables ================================================== */
+
+ IndexType hash ; /* hash value for a column */
+ IndexType *rp ; /* pointer to a row */
+ IndexType c ; /* a column index */
+ IndexType super_c ; /* column index of the column to absorb into */
+ IndexType *cp1 ; /* column pointer for column super_c */
+ IndexType *cp2 ; /* column pointer for column c */
+ IndexType length ; /* length of column super_c */
+ IndexType prev_c ; /* column preceding c in hash bucket */
+ IndexType i ; /* loop counter */
+ IndexType *rp_end ; /* pointer to the end of the row */
+ IndexType col ; /* a column index in the row to check */
+ IndexType head_column ; /* first column in hash bucket or degree list */
+ IndexType first_col ; /* first column in hash bucket */
+
+ /* === Consider each column in the row ================================== */
+
+ rp = &A [row_start] ;
+ rp_end = rp + row_length ;
+ while (rp < rp_end)
+ {
+ col = *rp++ ;
+ if (Col[col].is_dead())
+ {
+ continue ;
+ }
+
+ /* get hash number for this column */
+ hash = Col [col].shared3.hash ;
+ COLAMD_ASSERT (hash <= n_col) ;
+
+ /* === Get the first column in this hash bucket ===================== */
+
+ head_column = head [hash] ;
+ if (head_column > Empty)
+ {
+ first_col = Col [head_column].shared3.headhash ;
+ }
+ else
+ {
+ first_col = - (head_column + 2) ;
+ }
+
+ /* === Consider each column in the hash bucket ====================== */
+
+ for (super_c = first_col ; super_c != Empty ;
+ super_c = Col [super_c].shared4.hash_next)
+ {
+ COLAMD_ASSERT (Col [super_c].is_alive()) ;
+ COLAMD_ASSERT (Col [super_c].shared3.hash == hash) ;
+ length = Col [super_c].length ;
+
+ /* prev_c is the column preceding column c in the hash bucket */
+ prev_c = super_c ;
+
+ /* === Compare super_c with all columns after it ================ */
+
+ for (c = Col [super_c].shared4.hash_next ;
+ c != Empty ; c = Col [c].shared4.hash_next)
+ {
+ COLAMD_ASSERT (c != super_c) ;
+ COLAMD_ASSERT (Col[c].is_alive()) ;
+ COLAMD_ASSERT (Col [c].shared3.hash == hash) ;
+
+ /* not identical if lengths or scores are different */
+ if (Col [c].length != length ||
+ Col [c].shared2.score != Col [super_c].shared2.score)
+ {
+ prev_c = c ;
+ continue ;
+ }
+
+ /* compare the two columns */
+ cp1 = &A [Col [super_c].start] ;
+ cp2 = &A [Col [c].start] ;
+
+ for (i = 0 ; i < length ; i++)
+ {
+ /* the columns are "clean" (no dead rows) */
+ COLAMD_ASSERT ( cp1->is_alive() );
+ COLAMD_ASSERT ( cp2->is_alive() );
+ /* row indices will same order for both supercols, */
+ /* no gather scatter necessary */
+ if (*cp1++ != *cp2++)
+ {
+ break ;
+ }
+ }
+
+ /* the two columns are different if the for-loop "broke" */
+ if (i != length)
+ {
+ prev_c = c ;
+ continue ;
+ }
+
+ /* === Got it! two columns are identical =================== */
+
+ COLAMD_ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ;
+
+ Col [super_c].shared1.thickness += Col [c].shared1.thickness ;
+ Col [c].shared1.parent = super_c ;
+ Col[c].kill_non_principal() ;
+ /* order c later, in order_children() */
+ Col [c].shared2.order = Empty ;
+ /* remove c from hash bucket */
+ Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ;
+ }
+ }
+
+ /* === Empty this hash bucket ======================================= */
+
+ if (head_column > Empty)
+ {
+ /* corresponding degree list "hash" is not empty */
+ Col [head_column].shared3.headhash = Empty ;
+ }
+ else
+ {
+ /* corresponding degree list "hash" is empty */
+ head [hash] = Empty ;
+ }
+ }
+}
+
+
+/* ========================================================================== */
+/* === garbage_collection =================================================== */
+/* ========================================================================== */
+
+/*
+ Defragments and compacts columns and rows in the workspace A. Used when
+ all available memory has been used while performing row merging. Returns
+ the index of the first free position in A, after garbage collection. The
+ time taken by this routine is linear is the size of the array A, which is
+ itself linear in the number of nonzeros in the input matrix.
+ Not user-callable.
+*/
+template <typename IndexType>
+static IndexType garbage_collection /* returns the new value of pfree */
+ (
+ /* === Parameters ======================================================= */
+
+ IndexType n_row, /* number of rows */
+ IndexType n_col, /* number of columns */
+ RowStructure<IndexType> Row [], /* row info */
+ ColStructure<IndexType> Col [], /* column info */
+ IndexType A [], /* A [0 ... Alen-1] holds the matrix */
+ IndexType *pfree /* &A [0] ... pfree is in use */
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType *psrc ; /* source pointer */
+ IndexType *pdest ; /* destination pointer */
+ IndexType j ; /* counter */
+ IndexType r ; /* a row index */
+ IndexType c ; /* a column index */
+ IndexType length ; /* length of a row or column */
+
+ /* === Defragment the columns =========================================== */
+
+ pdest = &A[0] ;
+ for (c = 0 ; c < n_col ; c++)
+ {
+ if (Col[c].is_alive())
+ {
+ psrc = &A [Col [c].start] ;
+
+ /* move and compact the column */
+ COLAMD_ASSERT (pdest <= psrc) ;
+ Col [c].start = (IndexType) (pdest - &A [0]) ;
+ length = Col [c].length ;
+ for (j = 0 ; j < length ; j++)
+ {
+ r = *psrc++ ;
+ if (Row[r].is_alive())
+ {
+ *pdest++ = r ;
+ }
+ }
+ Col [c].length = (IndexType) (pdest - &A [Col [c].start]) ;
+ }
+ }
+
+ /* === Prepare to defragment the rows =================================== */
+
+ for (r = 0 ; r < n_row ; r++)
+ {
+ if (Row[r].is_alive())
+ {
+ if (Row [r].length == 0)
+ {
+ /* this row is of zero length. cannot compact it, so kill it */
+ COLAMD_DEBUG3 (("Defrag row kill\n")) ;
+ Row[r].kill() ;
+ }
+ else
+ {
+ /* save first column index in Row [r].shared2.first_column */
+ psrc = &A [Row [r].start] ;
+ Row [r].shared2.first_column = *psrc ;
+ COLAMD_ASSERT (Row[r].is_alive()) ;
+ /* flag the start of the row with the one's complement of row */
+ *psrc = ones_complement(r) ;
+
+ }
+ }
+ }
+
+ /* === Defragment the rows ============================================== */
+
+ psrc = pdest ;
+ while (psrc < pfree)
+ {
+ /* find a negative number ... the start of a row */
+ if (*psrc++ < 0)
+ {
+ psrc-- ;
+ /* get the row index */
+ r = ones_complement(*psrc) ;
+ COLAMD_ASSERT (r >= 0 && r < n_row) ;
+ /* restore first column index */
+ *psrc = Row [r].shared2.first_column ;
+ COLAMD_ASSERT (Row[r].is_alive()) ;
+
+ /* move and compact the row */
+ COLAMD_ASSERT (pdest <= psrc) ;
+ Row [r].start = (IndexType) (pdest - &A [0]) ;
+ length = Row [r].length ;
+ for (j = 0 ; j < length ; j++)
+ {
+ c = *psrc++ ;
+ if (Col[c].is_alive())
+ {
+ *pdest++ = c ;
+ }
+ }
+ Row [r].length = (IndexType) (pdest - &A [Row [r].start]) ;
+
+ }
+ }
+ /* ensure we found all the rows */
+ COLAMD_ASSERT (debug_rows == 0) ;
+
+ /* === Return the new value of pfree ==================================== */
+
+ return ((IndexType) (pdest - &A [0])) ;
+}
+
+
+/* ========================================================================== */
+/* === clear_mark =========================================================== */
+/* ========================================================================== */
+
+/*
+ Clears the Row [].shared2.mark array, and returns the new tag_mark.
+ Return value is the new tag_mark. Not user-callable.
+*/
+template <typename IndexType>
+static inline IndexType clear_mark /* return the new value for tag_mark */
+ (
+ /* === Parameters ======================================================= */
+
+ IndexType n_row, /* number of rows in A */
+ RowStructure<IndexType> Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */
+ )
+{
+ /* === Local variables ================================================== */
+
+ IndexType r ;
+
+ for (r = 0 ; r < n_row ; r++)
+ {
+ if (Row[r].is_alive())
+ {
+ Row [r].shared2.mark = 0 ;
+ }
+ }
+ return (1) ;
+}
+
+} // namespace Colamd
+
+} // namespace internal
+#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
new file mode 100644
index 0000000..c578970
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
@@ -0,0 +1,153 @@
+
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_ORDERING_H
+#define EIGEN_ORDERING_H
+
+namespace Eigen {
+
+#include "Eigen_Colamd.h"
+
+namespace internal {
+
+/** \internal
+ * \ingroup OrderingMethods_Module
+ * \param[in] A the input non-symmetric matrix
+ * \param[out] symmat the symmetric pattern A^T+A from the input matrix \a A.
+ * FIXME: The values should not be considered here
+ */
+template<typename MatrixType>
+void ordering_helper_at_plus_a(const MatrixType& A, MatrixType& symmat)
+{
+ MatrixType C;
+ C = A.transpose(); // NOTE: Could be costly
+ for (int i = 0; i < C.rows(); i++)
+ {
+ for (typename MatrixType::InnerIterator it(C, i); it; ++it)
+ it.valueRef() = typename MatrixType::Scalar(0);
+ }
+ symmat = C + A;
+}
+
+}
+
+/** \ingroup OrderingMethods_Module
+ * \class AMDOrdering
+ *
+ * Functor computing the \em approximate \em minimum \em degree ordering
+ * If the matrix is not structurally symmetric, an ordering of A^T+A is computed
+ * \tparam StorageIndex The type of indices of the matrix
+ * \sa COLAMDOrdering
+ */
+template <typename StorageIndex>
+class AMDOrdering
+{
+ public:
+ typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+ /** Compute the permutation vector from a sparse matrix
+ * This routine is much faster if the input matrix is column-major
+ */
+ template <typename MatrixType>
+ void operator()(const MatrixType& mat, PermutationType& perm)
+ {
+ // Compute the symmetric pattern
+ SparseMatrix<typename MatrixType::Scalar, ColMajor, StorageIndex> symm;
+ internal::ordering_helper_at_plus_a(mat,symm);
+
+ // Call the AMD routine
+ //m_mat.prune(keep_diag());
+ internal::minimum_degree_ordering(symm, perm);
+ }
+
+ /** Compute the permutation with a selfadjoint matrix */
+ template <typename SrcType, unsigned int SrcUpLo>
+ void operator()(const SparseSelfAdjointView<SrcType, SrcUpLo>& mat, PermutationType& perm)
+ {
+ SparseMatrix<typename SrcType::Scalar, ColMajor, StorageIndex> C; C = mat;
+
+ // Call the AMD routine
+ // m_mat.prune(keep_diag()); //Remove the diagonal elements
+ internal::minimum_degree_ordering(C, perm);
+ }
+};
+
+/** \ingroup OrderingMethods_Module
+ * \class NaturalOrdering
+ *
+ * Functor computing the natural ordering (identity)
+ *
+ * \note Returns an empty permutation matrix
+ * \tparam StorageIndex The type of indices of the matrix
+ */
+template <typename StorageIndex>
+class NaturalOrdering
+{
+ public:
+ typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+ /** Compute the permutation vector from a column-major sparse matrix */
+ template <typename MatrixType>
+ void operator()(const MatrixType& /*mat*/, PermutationType& perm)
+ {
+ perm.resize(0);
+ }
+
+};
+
+/** \ingroup OrderingMethods_Module
+ * \class COLAMDOrdering
+ *
+ * \tparam StorageIndex The type of indices of the matrix
+ *
+ * Functor computing the \em column \em approximate \em minimum \em degree ordering
+ * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
+ */
+template<typename StorageIndex>
+class COLAMDOrdering
+{
+ public:
+ typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+ typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+
+ /** Compute the permutation vector \a perm form the sparse matrix \a mat
+ * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+ */
+ template <typename MatrixType>
+ void operator() (const MatrixType& mat, PermutationType& perm)
+ {
+ eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
+
+ StorageIndex m = StorageIndex(mat.rows());
+ StorageIndex n = StorageIndex(mat.cols());
+ StorageIndex nnz = StorageIndex(mat.nonZeros());
+ // Get the recommended value of Alen to be used by colamd
+ StorageIndex Alen = internal::Colamd::recommended(nnz, m, n);
+ // Set the default parameters
+ double knobs [internal::Colamd::NKnobs];
+ StorageIndex stats [internal::Colamd::NStats];
+ internal::Colamd::set_defaults(knobs);
+
+ IndexVector p(n+1), A(Alen);
+ for(StorageIndex i=0; i <= n; i++) p(i) = mat.outerIndexPtr()[i];
+ for(StorageIndex i=0; i < nnz; i++) A(i) = mat.innerIndexPtr()[i];
+ // Call Colamd routine to compute the ordering
+ StorageIndex info = internal::Colamd::compute_ordering(m, n, Alen, A.data(), p.data(), knobs, stats);
+ EIGEN_UNUSED_VARIABLE(info);
+ eigen_assert( info && "COLAMD failed " );
+
+ perm.resize(n);
+ for (StorageIndex i = 0; i < n; i++) perm.indices()(p(i)) = i;
+ }
+};
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
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
new file mode 100644
index 0000000..9f93e32
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
@@ -0,0 +1,697 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SIMPLICIAL_CHOLESKY_H
+#define EIGEN_SIMPLICIAL_CHOLESKY_H
+
+namespace Eigen {
+
+enum SimplicialCholeskyMode {
+ SimplicialCholeskyLLT,
+ SimplicialCholeskyLDLT
+};
+
+namespace internal {
+ template<typename CholMatrixType, typename InputMatrixType>
+ struct simplicial_cholesky_grab_input {
+ typedef CholMatrixType const * ConstCholMatrixPtr;
+ static void run(const InputMatrixType& input, ConstCholMatrixPtr &pmat, CholMatrixType &tmp)
+ {
+ tmp = input;
+ pmat = &tmp;
+ }
+ };
+
+ template<typename MatrixType>
+ struct simplicial_cholesky_grab_input<MatrixType,MatrixType> {
+ typedef MatrixType const * ConstMatrixPtr;
+ static void run(const MatrixType& input, ConstMatrixPtr &pmat, MatrixType &/*tmp*/)
+ {
+ pmat = &input;
+ }
+ };
+} // end namespace internal
+
+/** \ingroup SparseCholesky_Module
+ * \brief A base class for direct sparse Cholesky factorizations
+ *
+ * This is a base class for LL^T and LDL^T Cholesky factorizations of sparse matrices that are
+ * selfadjoint and positive definite. These factorizations allow for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam Derived the type of the derived class, that is the actual factorization type.
+ *
+ */
+template<typename Derived>
+class SimplicialCholeskyBase : public SparseSolverBase<Derived>
+{
+ typedef SparseSolverBase<Derived> Base;
+ using Base::m_isInitialized;
+
+ public:
+ typedef typename internal::traits<Derived>::MatrixType MatrixType;
+ typedef typename internal::traits<Derived>::OrderingType OrderingType;
+ enum { UpLo = internal::traits<Derived>::UpLo };
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+ typedef CholMatrixType const * ConstCholMatrixPtr;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+
+ enum {
+ ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+ MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+ };
+
+ public:
+
+ using Base::derived;
+
+ /** Default constructor */
+ SimplicialCholeskyBase()
+ : m_info(Success),
+ m_factorizationIsOk(false),
+ m_analysisIsOk(false),
+ m_shiftOffset(0),
+ m_shiftScale(1)
+ {}
+
+ explicit SimplicialCholeskyBase(const MatrixType& matrix)
+ : m_info(Success),
+ m_factorizationIsOk(false),
+ m_analysisIsOk(false),
+ m_shiftOffset(0),
+ m_shiftScale(1)
+ {
+ derived().compute(matrix);
+ }
+
+ ~SimplicialCholeskyBase()
+ {
+ }
+
+ Derived& derived() { return *static_cast<Derived*>(this); }
+ const Derived& derived() const { return *static_cast<const Derived*>(this); }
+
+ inline Index cols() const { return m_matrix.cols(); }
+ inline Index rows() const { return m_matrix.rows(); }
+
+ /** \brief Reports whether previous computation was successful.
+ *
+ * \returns \c Success if computation was successful,
+ * \c NumericalIssue if the matrix.appears to be negative.
+ */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+ return m_info;
+ }
+
+ /** \returns the permutation P
+ * \sa permutationPinv() */
+ const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationP() const
+ { return m_P; }
+
+ /** \returns the inverse P^-1 of the permutation P
+ * \sa permutationP() */
+ const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationPinv() const
+ { return m_Pinv; }
+
+ /** Sets the shift parameters that will be used to adjust the diagonal coefficients during the numerical factorization.
+ *
+ * During the numerical factorization, the diagonal coefficients are transformed by the following linear model:\n
+ * \c d_ii = \a offset + \a scale * \c d_ii
+ *
+ * The default is the identity transformation with \a offset=0, and \a scale=1.
+ *
+ * \returns a reference to \c *this.
+ */
+ Derived& setShift(const RealScalar& offset, const RealScalar& scale = 1)
+ {
+ m_shiftOffset = offset;
+ m_shiftScale = scale;
+ return derived();
+ }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+ /** \internal */
+ template<typename Stream>
+ void dumpMemory(Stream& s)
+ {
+ int total = 0;
+ s << " L: " << ((total+=(m_matrix.cols()+1) * sizeof(int) + m_matrix.nonZeros()*(sizeof(int)+sizeof(Scalar))) >> 20) << "Mb" << "\n";
+ s << " diag: " << ((total+=m_diag.size() * sizeof(Scalar)) >> 20) << "Mb" << "\n";
+ s << " tree: " << ((total+=m_parent.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+ s << " nonzeros: " << ((total+=m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+ s << " perm: " << ((total+=m_P.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+ s << " perm^-1: " << ((total+=m_Pinv.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+ s << " TOTAL: " << (total>> 20) << "Mb" << "\n";
+ }
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
+ {
+ eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+ eigen_assert(m_matrix.rows()==b.rows());
+
+ if(m_info!=Success)
+ return;
+
+ if(m_P.size()>0)
+ dest = m_P * b;
+ else
+ dest = b;
+
+ if(m_matrix.nonZeros()>0) // otherwise L==I
+ derived().matrixL().solveInPlace(dest);
+
+ if(m_diag.size()>0)
+ dest = m_diag.asDiagonal().inverse() * dest;
+
+ if (m_matrix.nonZeros()>0) // otherwise U==I
+ derived().matrixU().solveInPlace(dest);
+
+ if(m_P.size()>0)
+ dest = m_Pinv * dest;
+ }
+
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+ {
+ internal::solve_sparse_through_dense_panels(derived(), b, dest);
+ }
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+ protected:
+
+ /** Computes the sparse Cholesky decomposition of \a matrix */
+ template<bool DoLDLT>
+ void compute(const MatrixType& matrix)
+ {
+ eigen_assert(matrix.rows()==matrix.cols());
+ Index size = matrix.cols();
+ CholMatrixType tmp(size,size);
+ ConstCholMatrixPtr pmat;
+ ordering(matrix, pmat, tmp);
+ analyzePattern_preordered(*pmat, DoLDLT);
+ factorize_preordered<DoLDLT>(*pmat);
+ }
+
+ template<bool DoLDLT>
+ void factorize(const MatrixType& a)
+ {
+ eigen_assert(a.rows()==a.cols());
+ Index size = a.cols();
+ CholMatrixType tmp(size,size);
+ ConstCholMatrixPtr pmat;
+
+ if(m_P.size() == 0 && (int(UpLo) & int(Upper)) == Upper)
+ {
+ // If there is no ordering, try to directly use the input matrix without any copy
+ internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, tmp);
+ }
+ else
+ {
+ tmp.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
+ pmat = &tmp;
+ }
+
+ factorize_preordered<DoLDLT>(*pmat);
+ }
+
+ template<bool DoLDLT>
+ void factorize_preordered(const CholMatrixType& a);
+
+ void analyzePattern(const MatrixType& a, bool doLDLT)
+ {
+ eigen_assert(a.rows()==a.cols());
+ Index size = a.cols();
+ CholMatrixType tmp(size,size);
+ ConstCholMatrixPtr pmat;
+ ordering(a, pmat, tmp);
+ analyzePattern_preordered(*pmat,doLDLT);
+ }
+ void analyzePattern_preordered(const CholMatrixType& a, bool doLDLT);
+
+ void ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap);
+
+ /** keeps off-diagonal entries; drops diagonal entries */
+ struct keep_diag {
+ inline bool operator() (const Index& row, const Index& col, const Scalar&) const
+ {
+ return row!=col;
+ }
+ };
+
+ mutable ComputationInfo m_info;
+ bool m_factorizationIsOk;
+ bool m_analysisIsOk;
+
+ CholMatrixType m_matrix;
+ VectorType m_diag; // the diagonal coefficients (LDLT mode)
+ VectorI m_parent; // elimination tree
+ VectorI m_nonZerosPerCol;
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P; // the permutation
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv; // the inverse permutation
+
+ RealScalar m_shiftOffset;
+ RealScalar m_shiftScale;
+};
+
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLDLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialCholesky;
+
+namespace internal {
+
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Ordering OrderingType;
+ enum { UpLo = _UpLo };
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+ typedef TriangularView<const CholMatrixType, Eigen::Lower> MatrixL;
+ typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::Upper> MatrixU;
+ static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
+ static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
+};
+
+template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Ordering OrderingType;
+ enum { UpLo = _UpLo };
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar, ColMajor, StorageIndex> CholMatrixType;
+ typedef TriangularView<const CholMatrixType, Eigen::UnitLower> MatrixL;
+ typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::UnitUpper> MatrixU;
+ static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
+ static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
+};
+
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
+{
+ typedef _MatrixType MatrixType;
+ typedef _Ordering OrderingType;
+ enum { UpLo = _UpLo };
+};
+
+}
+
+/** \ingroup SparseCholesky_Module
+ * \class SimplicialLLT
+ * \brief A direct sparse LLT Cholesky factorizations
+ *
+ * This class provides a LL^T Cholesky factorizations of sparse matrices that are
+ * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+ * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+ * or Upper. Default is Lower.
+ * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
+ */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+ class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+ typedef _MatrixType MatrixType;
+ enum { UpLo = _UpLo };
+ typedef SimplicialCholeskyBase<SimplicialLLT> Base;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,Index> CholMatrixType;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+ typedef internal::traits<SimplicialLLT> Traits;
+ typedef typename Traits::MatrixL MatrixL;
+ typedef typename Traits::MatrixU MatrixU;
+public:
+ /** Default constructor */
+ SimplicialLLT() : Base() {}
+ /** Constructs and performs the LLT factorization of \a matrix */
+ explicit SimplicialLLT(const MatrixType& matrix)
+ : Base(matrix) {}
+
+ /** \returns an expression of the factor L */
+ inline const MatrixL matrixL() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+ return Traits::getL(Base::m_matrix);
+ }
+
+ /** \returns an expression of the factor U (= L^*) */
+ inline const MatrixU matrixU() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+ return Traits::getU(Base::m_matrix);
+ }
+
+ /** Computes the sparse Cholesky decomposition of \a matrix */
+ SimplicialLLT& compute(const MatrixType& matrix)
+ {
+ Base::template compute<false>(matrix);
+ return *this;
+ }
+
+ /** Performs a symbolic decomposition on the sparcity of \a matrix.
+ *
+ * This function is particularly useful when solving for several problems having the same structure.
+ *
+ * \sa factorize()
+ */
+ void analyzePattern(const MatrixType& a)
+ {
+ Base::analyzePattern(a, false);
+ }
+
+ /** Performs a numeric decomposition of \a matrix
+ *
+ * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+ *
+ * \sa analyzePattern()
+ */
+ void factorize(const MatrixType& a)
+ {
+ Base::template factorize<false>(a);
+ }
+
+ /** \returns the determinant of the underlying matrix from the current factorization */
+ Scalar determinant() const
+ {
+ Scalar detL = Base::m_matrix.diagonal().prod();
+ return numext::abs2(detL);
+ }
+};
+
+/** \ingroup SparseCholesky_Module
+ * \class SimplicialLDLT
+ * \brief A direct sparse LDLT Cholesky factorizations without square root.
+ *
+ * This class provides a LDL^T Cholesky factorizations without square root of sparse matrices that are
+ * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+ * X and B can be either dense or sparse.
+ *
+ * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+ * such that the factorized matrix is P A P^-1.
+ *
+ * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+ * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+ * or Upper. Default is Lower.
+ * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
+ */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+ class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+ typedef _MatrixType MatrixType;
+ enum { UpLo = _UpLo };
+ typedef SimplicialCholeskyBase<SimplicialLDLT> Base;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+ typedef internal::traits<SimplicialLDLT> Traits;
+ typedef typename Traits::MatrixL MatrixL;
+ typedef typename Traits::MatrixU MatrixU;
+public:
+ /** Default constructor */
+ SimplicialLDLT() : Base() {}
+
+ /** Constructs and performs the LLT factorization of \a matrix */
+ explicit SimplicialLDLT(const MatrixType& matrix)
+ : Base(matrix) {}
+
+ /** \returns a vector expression of the diagonal D */
+ inline const VectorType vectorD() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+ return Base::m_diag;
+ }
+ /** \returns an expression of the factor L */
+ inline const MatrixL matrixL() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+ return Traits::getL(Base::m_matrix);
+ }
+
+ /** \returns an expression of the factor U (= L^*) */
+ inline const MatrixU matrixU() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+ return Traits::getU(Base::m_matrix);
+ }
+
+ /** Computes the sparse Cholesky decomposition of \a matrix */
+ SimplicialLDLT& compute(const MatrixType& matrix)
+ {
+ Base::template compute<true>(matrix);
+ return *this;
+ }
+
+ /** Performs a symbolic decomposition on the sparcity of \a matrix.
+ *
+ * This function is particularly useful when solving for several problems having the same structure.
+ *
+ * \sa factorize()
+ */
+ void analyzePattern(const MatrixType& a)
+ {
+ Base::analyzePattern(a, true);
+ }
+
+ /** Performs a numeric decomposition of \a matrix
+ *
+ * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+ *
+ * \sa analyzePattern()
+ */
+ void factorize(const MatrixType& a)
+ {
+ Base::template factorize<true>(a);
+ }
+
+ /** \returns the determinant of the underlying matrix from the current factorization */
+ Scalar determinant() const
+ {
+ return Base::m_diag.prod();
+ }
+};
+
+/** \deprecated use SimplicialLDLT or class SimplicialLLT
+ * \ingroup SparseCholesky_Module
+ * \class SimplicialCholesky
+ *
+ * \sa class SimplicialLDLT, class SimplicialLLT
+ */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+ class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+ typedef _MatrixType MatrixType;
+ enum { UpLo = _UpLo };
+ typedef SimplicialCholeskyBase<SimplicialCholesky> Base;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+ typedef Matrix<Scalar,Dynamic,1> VectorType;
+ typedef internal::traits<SimplicialCholesky> Traits;
+ typedef internal::traits<SimplicialLDLT<MatrixType,UpLo> > LDLTTraits;
+ typedef internal::traits<SimplicialLLT<MatrixType,UpLo> > LLTTraits;
+ public:
+ SimplicialCholesky() : Base(), m_LDLT(true) {}
+
+ explicit SimplicialCholesky(const MatrixType& matrix)
+ : Base(), m_LDLT(true)
+ {
+ compute(matrix);
+ }
+
+ SimplicialCholesky& setMode(SimplicialCholeskyMode mode)
+ {
+ switch(mode)
+ {
+ case SimplicialCholeskyLLT:
+ m_LDLT = false;
+ break;
+ case SimplicialCholeskyLDLT:
+ m_LDLT = true;
+ break;
+ default:
+ break;
+ }
+
+ return *this;
+ }
+
+ inline const VectorType vectorD() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+ return Base::m_diag;
+ }
+ inline const CholMatrixType rawMatrix() const {
+ eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+ return Base::m_matrix;
+ }
+
+ /** Computes the sparse Cholesky decomposition of \a matrix */
+ SimplicialCholesky& compute(const MatrixType& matrix)
+ {
+ if(m_LDLT)
+ Base::template compute<true>(matrix);
+ else
+ Base::template compute<false>(matrix);
+ return *this;
+ }
+
+ /** Performs a symbolic decomposition on the sparcity of \a matrix.
+ *
+ * This function is particularly useful when solving for several problems having the same structure.
+ *
+ * \sa factorize()
+ */
+ void analyzePattern(const MatrixType& a)
+ {
+ Base::analyzePattern(a, m_LDLT);
+ }
+
+ /** Performs a numeric decomposition of \a matrix
+ *
+ * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+ *
+ * \sa analyzePattern()
+ */
+ void factorize(const MatrixType& a)
+ {
+ if(m_LDLT)
+ Base::template factorize<true>(a);
+ else
+ Base::template factorize<false>(a);
+ }
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
+ {
+ eigen_assert(Base::m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+ eigen_assert(Base::m_matrix.rows()==b.rows());
+
+ if(Base::m_info!=Success)
+ return;
+
+ if(Base::m_P.size()>0)
+ dest = Base::m_P * b;
+ else
+ dest = b;
+
+ if(Base::m_matrix.nonZeros()>0) // otherwise L==I
+ {
+ if(m_LDLT)
+ LDLTTraits::getL(Base::m_matrix).solveInPlace(dest);
+ else
+ LLTTraits::getL(Base::m_matrix).solveInPlace(dest);
+ }
+
+ if(Base::m_diag.size()>0)
+ dest = Base::m_diag.real().asDiagonal().inverse() * dest;
+
+ if (Base::m_matrix.nonZeros()>0) // otherwise I==I
+ {
+ if(m_LDLT)
+ LDLTTraits::getU(Base::m_matrix).solveInPlace(dest);
+ else
+ LLTTraits::getU(Base::m_matrix).solveInPlace(dest);
+ }
+
+ if(Base::m_P.size()>0)
+ dest = Base::m_Pinv * dest;
+ }
+
+ /** \internal */
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+ {
+ internal::solve_sparse_through_dense_panels(*this, b, dest);
+ }
+
+ Scalar determinant() const
+ {
+ if(m_LDLT)
+ {
+ return Base::m_diag.prod();
+ }
+ else
+ {
+ Scalar detL = Diagonal<const CholMatrixType>(Base::m_matrix).prod();
+ return numext::abs2(detL);
+ }
+ }
+
+ protected:
+ bool m_LDLT;
+};
+
+template<typename Derived>
+void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap)
+{
+ eigen_assert(a.rows()==a.cols());
+ const Index size = a.rows();
+ pmat = ≈
+ // Note that ordering methods compute the inverse permutation
+ if(!internal::is_same<OrderingType,NaturalOrdering<Index> >::value)
+ {
+ {
+ CholMatrixType C;
+ C = a.template selfadjointView<UpLo>();
+
+ OrderingType ordering;
+ ordering(C,m_Pinv);
+ }
+
+ if(m_Pinv.size()>0) m_P = m_Pinv.inverse();
+ else m_P.resize(0);
+
+ ap.resize(size,size);
+ ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
+ }
+ else
+ {
+ m_Pinv.resize(0);
+ m_P.resize(0);
+ if(int(UpLo)==int(Lower) || MatrixType::IsRowMajor)
+ {
+ // we have to transpose the lower part to to the upper one
+ ap.resize(size,size);
+ ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>();
+ }
+ else
+ internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, ap);
+ }
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SIMPLICIAL_CHOLESKY_H
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
new file mode 100644
index 0000000..72e1740
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
@@ -0,0 +1,174 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+NOTE: these functions have been adapted from the LDL library:
+
+LDL Copyright (c) 2005 by Timothy A. Davis. All Rights Reserved.
+
+The author of LDL, Timothy A. Davis., has executed a license with Google LLC
+to permit distribution of this code and derivative works as part of Eigen under
+the Mozilla Public License v. 2.0, as stated at the top of this file.
+ */
+
+#ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
+#define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
+
+namespace Eigen {
+
+template<typename Derived>
+void SimplicialCholeskyBase<Derived>::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);
+
+ ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0);
+
+ 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 */
+ }
+ }
+ }
+ }
+
+ /* 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);
+
+ m_matrix.resizeNonZeros(Lp[size]);
+
+ m_isInitialized = true;
+ m_info = Success;
+ m_analysisIsOk = true;
+ m_factorizationIsOk = false;
+}
+
+
+template<typename Derived>
+template<bool DoLDLT>
+void SimplicialCholeskyBase<Derived>::factorize_preordered(const CholMatrixType& ap)
+{
+ using std::sqrt;
+
+ 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());
+
+ 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);
+
+ bool ok = true;
+ m_diag.resize(DoLDLT ? size : 0);
+
+ for(StorageIndex k = 0; k < size; ++k)
+ {
+ // compute nonzero pattern of kth row of L, in topological order
+ 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
+ for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
+ {
+ StorageIndex i = it.index();
+ if(i <= k)
+ {
+ y[i] += numext::conj(it.value()); /* scatter A(i,k) into Y (sum duplicates) */
+ Index len;
+ for(len = 0; tags[i] != k; i = m_parent[i])
+ {
+ pattern[len++] = i; /* L(k,i) is nonzero */
+ tags[i] = k; /* mark i as visited */
+ }
+ while(len > 0)
+ pattern[--top] = pattern[--len];
+ }
+ }
+
+ /* compute numerical values kth row of L (a sparse triangular solve) */
+
+ RealScalar d = numext::real(y[k]) * m_shiftScale + m_shiftOffset; // get D(k,k), apply the shift function, and clear Y(k)
+ y[k] = Scalar(0);
+ for(; top < size; ++top)
+ {
+ Index i = pattern[top]; /* pattern[top:n-1] is pattern of L(:,k) */
+ Scalar yi = y[i]; /* get and clear Y(i) */
+ y[i] = Scalar(0);
+
+ /* the nonzero entry L(k,i) */
+ Scalar l_ki;
+ if(DoLDLT)
+ l_ki = yi / numext::real(m_diag[i]);
+ else
+ yi = l_ki = yi / Lx[Lp[i]];
+
+ Index p2 = Lp[i] + m_nonZerosPerCol[i];
+ Index p;
+ for(p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p)
+ y[Li[p]] -= numext::conj(Lx[p]) * yi;
+ d -= numext::real(l_ki * numext::conj(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 */
+ }
+ if(DoLDLT)
+ {
+ m_diag[k] = d;
+ if(d == RealScalar(0))
+ {
+ ok = false; /* failure, D(k,k) is zero */
+ break;
+ }
+ }
+ else
+ {
+ Index p = Lp[k] + m_nonZerosPerCol[k]++;
+ Li[p] = k ; /* store L(k,k) = sqrt (d) in column k */
+ if(d <= RealScalar(0)) {
+ ok = false; /* failure, matrix is not positive definite */
+ break;
+ }
+ Lx[p] = sqrt(d) ;
+ }
+ }
+
+ m_info = ok ? Success : NumericalIssue;
+ m_factorizationIsOk = true;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
new file mode 100644
index 0000000..2cb7747
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
@@ -0,0 +1,378 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_AMBIVECTOR_H
+#define EIGEN_AMBIVECTOR_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal
+ * Hybrid sparse/dense vector class designed for intensive read-write operations.
+ *
+ * See BasicSparseLLT and SparseProduct for usage examples.
+ */
+template<typename _Scalar, typename _StorageIndex>
+class AmbiVector
+{
+ public:
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+
+ explicit AmbiVector(Index size)
+ : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1)
+ {
+ resize(size);
+ }
+
+ void init(double estimatedDensity);
+ void init(int mode);
+
+ Index nonZeros() const;
+
+ /** Specifies a sub-vector to work on */
+ void setBounds(Index start, Index end) { m_start = convert_index(start); m_end = convert_index(end); }
+
+ void setZero();
+
+ void restart();
+ Scalar& coeffRef(Index i);
+ Scalar& coeff(Index i);
+
+ class Iterator;
+
+ ~AmbiVector() { delete[] m_buffer; }
+
+ void resize(Index size)
+ {
+ if (m_allocatedSize < size)
+ reallocate(size);
+ m_size = convert_index(size);
+ }
+
+ StorageIndex size() const { return m_size; }
+
+ protected:
+ StorageIndex convert_index(Index idx)
+ {
+ return internal::convert_index<StorageIndex>(idx);
+ }
+
+ void reallocate(Index size)
+ {
+ // if the size of the matrix is not too large, let's allocate a bit more than needed such
+ // that we can handle dense vector even in sparse mode.
+ delete[] m_buffer;
+ if (size<1000)
+ {
+ Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar);
+ m_allocatedElements = convert_index((allocSize*sizeof(Scalar))/sizeof(ListEl));
+ m_buffer = new Scalar[allocSize];
+ }
+ else
+ {
+ m_allocatedElements = convert_index((size*sizeof(Scalar))/sizeof(ListEl));
+ m_buffer = new Scalar[size];
+ }
+ m_size = convert_index(size);
+ m_start = 0;
+ m_end = m_size;
+ }
+
+ void reallocateSparse()
+ {
+ Index copyElements = m_allocatedElements;
+ m_allocatedElements = (std::min)(StorageIndex(m_allocatedElements*1.5),m_size);
+ Index allocSize = m_allocatedElements * sizeof(ListEl);
+ allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
+ Scalar* newBuffer = new Scalar[allocSize];
+ std::memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl));
+ delete[] m_buffer;
+ m_buffer = newBuffer;
+ }
+
+ protected:
+ // element type of the linked list
+ struct ListEl
+ {
+ StorageIndex next;
+ StorageIndex index;
+ Scalar value;
+ };
+
+ // used to store data in both mode
+ Scalar* m_buffer;
+ Scalar m_zero;
+ StorageIndex m_size;
+ StorageIndex m_start;
+ StorageIndex m_end;
+ StorageIndex m_allocatedSize;
+ StorageIndex m_allocatedElements;
+ StorageIndex m_mode;
+
+ // linked list mode
+ StorageIndex m_llStart;
+ StorageIndex m_llCurrent;
+ StorageIndex m_llSize;
+};
+
+/** \returns the number of non zeros in the current sub vector */
+template<typename _Scalar,typename _StorageIndex>
+Index AmbiVector<_Scalar,_StorageIndex>::nonZeros() const
+{
+ if (m_mode==IsSparse)
+ return m_llSize;
+ else
+ return m_end - m_start;
+}
+
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::init(double estimatedDensity)
+{
+ if (estimatedDensity>0.1)
+ init(IsDense);
+ else
+ init(IsSparse);
+}
+
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::init(int mode)
+{
+ m_mode = mode;
+ // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings
+ // if (m_mode==IsSparse)
+ {
+ m_llSize = 0;
+ m_llStart = -1;
+ }
+}
+
+/** Must be called whenever we might perform a write access
+ * with an index smaller than the previous one.
+ *
+ * Don't worry, this function is extremely cheap.
+ */
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::restart()
+{
+ m_llCurrent = m_llStart;
+}
+
+/** Set all coefficients of current subvector to zero */
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::setZero()
+{
+ if (m_mode==IsDense)
+ {
+ for (Index i=m_start; i<m_end; ++i)
+ m_buffer[i] = Scalar(0);
+ }
+ else
+ {
+ eigen_assert(m_mode==IsSparse);
+ m_llSize = 0;
+ m_llStart = -1;
+ }
+}
+
+template<typename _Scalar,typename _StorageIndex>
+_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeffRef(Index i)
+{
+ if (m_mode==IsDense)
+ return m_buffer[i];
+ else
+ {
+ ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+ // TODO factorize the following code to reduce code generation
+ eigen_assert(m_mode==IsSparse);
+ if (m_llSize==0)
+ {
+ // this is the first element
+ m_llStart = 0;
+ m_llCurrent = 0;
+ ++m_llSize;
+ llElements[0].value = Scalar(0);
+ llElements[0].index = convert_index(i);
+ llElements[0].next = -1;
+ return llElements[0].value;
+ }
+ else if (i<llElements[m_llStart].index)
+ {
+ // this is going to be the new first element of the list
+ ListEl& el = llElements[m_llSize];
+ el.value = Scalar(0);
+ el.index = convert_index(i);
+ el.next = m_llStart;
+ m_llStart = m_llSize;
+ ++m_llSize;
+ m_llCurrent = m_llStart;
+ return el.value;
+ }
+ else
+ {
+ StorageIndex nextel = llElements[m_llCurrent].next;
+ eigen_assert(i>=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index");
+ while (nextel >= 0 && llElements[nextel].index<=i)
+ {
+ m_llCurrent = nextel;
+ nextel = llElements[nextel].next;
+ }
+
+ if (llElements[m_llCurrent].index==i)
+ {
+ // the coefficient already exists and we found it !
+ return llElements[m_llCurrent].value;
+ }
+ else
+ {
+ if (m_llSize>=m_allocatedElements)
+ {
+ reallocateSparse();
+ llElements = reinterpret_cast<ListEl*>(m_buffer);
+ }
+ eigen_internal_assert(m_llSize<m_allocatedElements && "internal error: overflow in sparse mode");
+ // let's insert a new coefficient
+ ListEl& el = llElements[m_llSize];
+ el.value = Scalar(0);
+ el.index = convert_index(i);
+ el.next = llElements[m_llCurrent].next;
+ llElements[m_llCurrent].next = m_llSize;
+ ++m_llSize;
+ return el.value;
+ }
+ }
+ }
+}
+
+template<typename _Scalar,typename _StorageIndex>
+_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeff(Index i)
+{
+ if (m_mode==IsDense)
+ return m_buffer[i];
+ else
+ {
+ ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+ eigen_assert(m_mode==IsSparse);
+ if ((m_llSize==0) || (i<llElements[m_llStart].index))
+ {
+ return m_zero;
+ }
+ else
+ {
+ Index elid = m_llStart;
+ while (elid >= 0 && llElements[elid].index<i)
+ elid = llElements[elid].next;
+
+ if (llElements[elid].index==i)
+ return llElements[m_llCurrent].value;
+ else
+ return m_zero;
+ }
+ }
+}
+
+/** Iterator over the nonzero coefficients */
+template<typename _Scalar,typename _StorageIndex>
+class AmbiVector<_Scalar,_StorageIndex>::Iterator
+{
+ public:
+ typedef _Scalar Scalar;
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+
+ /** Default constructor
+ * \param vec the vector on which we iterate
+ * \param epsilon the minimal value used to prune zero coefficients.
+ * In practice, all coefficients having a magnitude smaller than \a epsilon
+ * are skipped.
+ */
+ explicit Iterator(const AmbiVector& vec, const RealScalar& epsilon = 0)
+ : m_vector(vec)
+ {
+ using std::abs;
+ m_epsilon = epsilon;
+ m_isDense = m_vector.m_mode==IsDense;
+ if (m_isDense)
+ {
+ m_currentEl = 0; // this is to avoid a compilation warning
+ m_cachedValue = 0; // this is to avoid a compilation warning
+ m_cachedIndex = m_vector.m_start-1;
+ ++(*this);
+ }
+ else
+ {
+ ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+ m_currentEl = m_vector.m_llStart;
+ while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon)
+ m_currentEl = llElements[m_currentEl].next;
+ if (m_currentEl<0)
+ {
+ m_cachedValue = 0; // this is to avoid a compilation warning
+ m_cachedIndex = -1;
+ }
+ else
+ {
+ m_cachedIndex = llElements[m_currentEl].index;
+ m_cachedValue = llElements[m_currentEl].value;
+ }
+ }
+ }
+
+ StorageIndex index() const { return m_cachedIndex; }
+ Scalar value() const { return m_cachedValue; }
+
+ operator bool() const { return m_cachedIndex>=0; }
+
+ Iterator& operator++()
+ {
+ using std::abs;
+ if (m_isDense)
+ {
+ do {
+ ++m_cachedIndex;
+ } while (m_cachedIndex<m_vector.m_end && abs(m_vector.m_buffer[m_cachedIndex])<=m_epsilon);
+ if (m_cachedIndex<m_vector.m_end)
+ m_cachedValue = m_vector.m_buffer[m_cachedIndex];
+ else
+ m_cachedIndex=-1;
+ }
+ else
+ {
+ ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+ do {
+ m_currentEl = llElements[m_currentEl].next;
+ } while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon);
+ if (m_currentEl<0)
+ {
+ m_cachedIndex = -1;
+ }
+ else
+ {
+ m_cachedIndex = llElements[m_currentEl].index;
+ m_cachedValue = llElements[m_currentEl].value;
+ }
+ }
+ return *this;
+ }
+
+ protected:
+ const AmbiVector& m_vector; // the target vector
+ StorageIndex m_currentEl; // the current element in sparse/linked-list mode
+ RealScalar m_epsilon; // epsilon used to prune zero coefficients
+ StorageIndex m_cachedIndex; // current coordinate
+ Scalar m_cachedValue; // current value
+ bool m_isDense; // mode of the vector
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_AMBIVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
new file mode 100644
index 0000000..acd986f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
@@ -0,0 +1,274 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_COMPRESSED_STORAGE_H
+#define EIGEN_COMPRESSED_STORAGE_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal
+ * Stores a sparse set of values as a list of values and a list of indices.
+ *
+ */
+template<typename _Scalar,typename _StorageIndex>
+class CompressedStorage
+{
+ public:
+
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+
+ protected:
+
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+
+ public:
+
+ CompressedStorage()
+ : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+ {}
+
+ explicit CompressedStorage(Index size)
+ : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+ {
+ resize(size);
+ }
+
+ CompressedStorage(const CompressedStorage& other)
+ : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+ {
+ *this = other;
+ }
+
+ CompressedStorage& operator=(const CompressedStorage& other)
+ {
+ resize(other.size());
+ if(other.size()>0)
+ {
+ internal::smart_copy(other.m_values, other.m_values + m_size, m_values);
+ internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
+ }
+ return *this;
+ }
+
+ void swap(CompressedStorage& other)
+ {
+ std::swap(m_values, other.m_values);
+ std::swap(m_indices, other.m_indices);
+ std::swap(m_size, other.m_size);
+ std::swap(m_allocatedSize, other.m_allocatedSize);
+ }
+
+ ~CompressedStorage()
+ {
+ delete[] m_values;
+ delete[] m_indices;
+ }
+
+ void reserve(Index size)
+ {
+ Index newAllocatedSize = m_size + size;
+ if (newAllocatedSize > m_allocatedSize)
+ reallocate(newAllocatedSize);
+ }
+
+ void squeeze()
+ {
+ if (m_allocatedSize>m_size)
+ reallocate(m_size);
+ }
+
+ void resize(Index size, double reserveSizeFactor = 0)
+ {
+ if (m_allocatedSize<size)
+ {
+ Index realloc_size = (std::min<Index>)(NumTraits<StorageIndex>::highest(), size + Index(reserveSizeFactor*double(size)));
+ if(realloc_size<size)
+ internal::throw_std_bad_alloc();
+ reallocate(realloc_size);
+ }
+ m_size = size;
+ }
+
+ void append(const Scalar& v, Index i)
+ {
+ Index id = m_size;
+ resize(m_size+1, 1);
+ m_values[id] = v;
+ m_indices[id] = internal::convert_index<StorageIndex>(i);
+ }
+
+ inline Index size() const { return m_size; }
+ inline Index allocatedSize() const { return m_allocatedSize; }
+ inline void clear() { m_size = 0; }
+
+ const Scalar* valuePtr() const { return m_values; }
+ Scalar* valuePtr() { return m_values; }
+ const StorageIndex* indexPtr() const { return m_indices; }
+ StorageIndex* indexPtr() { return m_indices; }
+
+ inline Scalar& value(Index i) { eigen_internal_assert(m_values!=0); return m_values[i]; }
+ inline const Scalar& value(Index i) const { eigen_internal_assert(m_values!=0); return m_values[i]; }
+
+ inline StorageIndex& index(Index i) { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
+ inline const StorageIndex& index(Index i) const { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
+
+ /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */
+ inline Index searchLowerIndex(Index key) const
+ {
+ return searchLowerIndex(0, m_size, key);
+ }
+
+ /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */
+ inline Index searchLowerIndex(Index start, Index end, Index key) const
+ {
+ while(end>start)
+ {
+ Index mid = (end+start)>>1;
+ if (m_indices[mid]<key)
+ start = mid+1;
+ else
+ end = mid;
+ }
+ return start;
+ }
+
+ /** \returns the stored value at index \a key
+ * If the value does not exist, then the value \a defaultValue is returned without any insertion. */
+ inline Scalar at(Index key, const Scalar& defaultValue = Scalar(0)) const
+ {
+ if (m_size==0)
+ return defaultValue;
+ else if (key==m_indices[m_size-1])
+ return m_values[m_size-1];
+ // ^^ optimization: let's first check if it is the last coefficient
+ // (very common in high level algorithms)
+ const Index id = searchLowerIndex(0,m_size-1,key);
+ return ((id<m_size) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+ }
+
+ /** Like at(), but the search is performed in the range [start,end) */
+ inline Scalar atInRange(Index start, Index end, Index key, const Scalar &defaultValue = Scalar(0)) const
+ {
+ if (start>=end)
+ return defaultValue;
+ else if (end>start && key==m_indices[end-1])
+ return m_values[end-1];
+ // ^^ optimization: let's first check if it is the last coefficient
+ // (very common in high level algorithms)
+ const Index id = searchLowerIndex(start,end-1,key);
+ return ((id<end) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+ }
+
+ /** \returns a reference to the value at index \a key
+ * If the value does not exist, then the value \a defaultValue is inserted
+ * such that the keys are sorted. */
+ inline Scalar& atWithInsertion(Index key, const Scalar& defaultValue = Scalar(0))
+ {
+ Index id = searchLowerIndex(0,m_size,key);
+ if (id>=m_size || m_indices[id]!=key)
+ {
+ if (m_allocatedSize<m_size+1)
+ {
+ m_allocatedSize = 2*(m_size+1);
+ internal::scoped_array<Scalar> newValues(m_allocatedSize);
+ internal::scoped_array<StorageIndex> newIndices(m_allocatedSize);
+
+ // copy first chunk
+ internal::smart_copy(m_values, m_values +id, newValues.ptr());
+ internal::smart_copy(m_indices, m_indices+id, newIndices.ptr());
+
+ // copy the rest
+ if(m_size>id)
+ {
+ internal::smart_copy(m_values +id, m_values +m_size, newValues.ptr() +id+1);
+ internal::smart_copy(m_indices+id, m_indices+m_size, newIndices.ptr()+id+1);
+ }
+ std::swap(m_values,newValues.ptr());
+ std::swap(m_indices,newIndices.ptr());
+ }
+ else if(m_size>id)
+ {
+ internal::smart_memmove(m_values +id, m_values +m_size, m_values +id+1);
+ internal::smart_memmove(m_indices+id, m_indices+m_size, m_indices+id+1);
+ }
+ m_size++;
+ m_indices[id] = internal::convert_index<StorageIndex>(key);
+ m_values[id] = defaultValue;
+ }
+ return m_values[id];
+ }
+
+ void moveChunk(Index from, Index to, Index chunkSize)
+ {
+ eigen_internal_assert(to+chunkSize <= m_size);
+ if(to>from && from+chunkSize>to)
+ {
+ // move backward
+ internal::smart_memmove(m_values+from, m_values+from+chunkSize, m_values+to);
+ internal::smart_memmove(m_indices+from, m_indices+from+chunkSize, m_indices+to);
+ }
+ else
+ {
+ internal::smart_copy(m_values+from, m_values+from+chunkSize, m_values+to);
+ internal::smart_copy(m_indices+from, m_indices+from+chunkSize, m_indices+to);
+ }
+ }
+
+ void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+ {
+ Index k = 0;
+ Index n = size();
+ for (Index i=0; i<n; ++i)
+ {
+ if (!internal::isMuchSmallerThan(value(i), reference, epsilon))
+ {
+ value(k) = value(i);
+ index(k) = index(i);
+ ++k;
+ }
+ }
+ resize(k,0);
+ }
+
+ protected:
+
+ inline void reallocate(Index size)
+ {
+ #ifdef EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+ EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+ #endif
+ eigen_internal_assert(size!=m_allocatedSize);
+ internal::scoped_array<Scalar> newValues(size);
+ internal::scoped_array<StorageIndex> newIndices(size);
+ Index copySize = (std::min)(size, m_size);
+ if (copySize>0) {
+ internal::smart_copy(m_values, m_values+copySize, newValues.ptr());
+ internal::smart_copy(m_indices, m_indices+copySize, newIndices.ptr());
+ }
+ std::swap(m_values,newValues.ptr());
+ std::swap(m_indices,newIndices.ptr());
+ m_allocatedSize = size;
+ }
+
+ protected:
+ Scalar* m_values;
+ StorageIndex* m_indices;
+ Index m_size;
+ Index m_allocatedSize;
+
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_COMPRESSED_STORAGE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
new file mode 100644
index 0000000..9486502
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
@@ -0,0 +1,352 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_CONSERVATIVESPARSESPARSEPRODUCT_H
+#define EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType>
+static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, bool sortedInsertion = false)
+{
+ typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+ typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+ typedef typename remove_all<ResultType>::type::Scalar ResScalar;
+
+ // make sure to call innerSize/outerSize since we fake the storage order.
+ Index rows = lhs.innerSize();
+ Index cols = rhs.outerSize();
+ eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+ ei_declare_aligned_stack_constructed_variable(bool, mask, rows, 0);
+ ei_declare_aligned_stack_constructed_variable(ResScalar, values, rows, 0);
+ ei_declare_aligned_stack_constructed_variable(Index, indices, rows, 0);
+
+ std::memset(mask,0,sizeof(bool)*rows);
+
+ evaluator<Lhs> lhsEval(lhs);
+ evaluator<Rhs> rhsEval(rhs);
+
+ // estimate the number of non zero entries
+ // given a rhs column containing Y non zeros, we assume that the respective Y columns
+ // of the lhs differs in average of one non zeros, thus the number of non zeros for
+ // the product of a rhs column with the lhs is X+Y where X is the average number of non zero
+ // per column of the lhs.
+ // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs)
+ Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate();
+
+ res.setZero();
+ res.reserve(Index(estimated_nnz_prod));
+ // we compute each column of the result, one after the other
+ for (Index j=0; j<cols; ++j)
+ {
+
+ res.startVec(j);
+ Index nnz = 0;
+ for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+ {
+ RhsScalar y = rhsIt.value();
+ Index k = rhsIt.index();
+ for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
+ {
+ Index i = lhsIt.index();
+ LhsScalar x = lhsIt.value();
+ if(!mask[i])
+ {
+ mask[i] = true;
+ values[i] = x * y;
+ indices[nnz] = i;
+ ++nnz;
+ }
+ else
+ values[i] += x * y;
+ }
+ }
+ if(!sortedInsertion)
+ {
+ // unordered insertion
+ for(Index k=0; k<nnz; ++k)
+ {
+ Index i = indices[k];
+ res.insertBackByOuterInnerUnordered(j,i) = values[i];
+ mask[i] = false;
+ }
+ }
+ else
+ {
+ // alternative ordered insertion code:
+ const Index t200 = rows/11; // 11 == (log2(200)*1.39)
+ const Index t = (rows*100)/139;
+
+ // FIXME reserve nnz non zeros
+ // FIXME implement faster sorting algorithms for very small nnz
+ // if the result is sparse enough => use a quick sort
+ // otherwise => loop through the entire vector
+ // In order to avoid to perform an expensive log2 when the
+ // result is clearly very sparse we use a linear bound up to 200.
+ if((nnz<200 && nnz<t200) || nnz * numext::log2(int(nnz)) < t)
+ {
+ if(nnz>1) std::sort(indices,indices+nnz);
+ for(Index k=0; k<nnz; ++k)
+ {
+ Index i = indices[k];
+ res.insertBackByOuterInner(j,i) = values[i];
+ mask[i] = false;
+ }
+ }
+ else
+ {
+ // dense path
+ for(Index i=0; i<rows; ++i)
+ {
+ if(mask[i])
+ {
+ mask[i] = false;
+ res.insertBackByOuterInner(j,i) = values[i];
+ }
+ }
+ }
+ }
+ }
+ res.finalize();
+}
+
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType,
+ int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+ int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+ int ResStorageOrder = (traits<ResultType>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+struct conservative_sparse_sparse_product_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
+{
+ typedef typename remove_all<Lhs>::type LhsCleaned;
+ typedef typename LhsCleaned::Scalar Scalar;
+
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrixAux;
+ typedef typename sparse_eval<ColMajorMatrixAux,ResultType::RowsAtCompileTime,ResultType::ColsAtCompileTime,ColMajorMatrixAux::Flags>::type ColMajorMatrix;
+
+ // If the result is tall and thin (in the extreme case a column vector)
+ // then it is faster to sort the coefficients inplace instead of transposing twice.
+ // FIXME, the following heuristic is probably not very good.
+ if(lhs.rows()>rhs.cols())
+ {
+ ColMajorMatrix resCol(lhs.rows(),rhs.cols());
+ // perform sorted insertion
+ internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol, true);
+ res = resCol.markAsRValue();
+ }
+ else
+ {
+ ColMajorMatrixAux resCol(lhs.rows(),rhs.cols());
+ // resort to transpose to sort the entries
+ internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrixAux>(lhs, rhs, resCol, false);
+ RowMajorMatrix resRow(resCol);
+ res = resRow.markAsRValue();
+ }
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRhs;
+ typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+ RowMajorRhs rhsRow = rhs;
+ RowMajorRes resRow(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<RowMajorRhs,Lhs,RowMajorRes>(rhsRow, lhs, resRow);
+ res = resRow;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorLhs;
+ typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+ RowMajorLhs lhsRow = lhs;
+ RowMajorRes resRow(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorLhs,RowMajorRes>(rhs, lhsRow, resRow);
+ res = resRow;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+ RowMajorMatrix resRow(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+ res = resRow;
+ }
+};
+
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
+{
+ typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar;
+
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
+ ColMajorMatrix resCol(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol);
+ res = resCol;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+ ColMajorLhs lhsCol = lhs;
+ ColMajorRes resCol(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<ColMajorLhs,Rhs,ColMajorRes>(lhsCol, rhs, resCol);
+ res = resCol;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+ ColMajorRhs rhsCol = rhs;
+ ColMajorRes resCol(lhs.rows(), rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorRhs,ColMajorRes>(lhs, rhsCol, resCol);
+ res = resCol;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
+ RowMajorMatrix resRow(lhs.rows(),rhs.cols());
+ internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+ // sort the non zeros:
+ ColMajorMatrix resCol(resRow);
+ res = resCol;
+ }
+};
+
+} // end namespace internal
+
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+{
+ typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+ typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+ Index cols = rhs.outerSize();
+ eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+ evaluator<Lhs> lhsEval(lhs);
+ evaluator<Rhs> rhsEval(rhs);
+
+ for (Index j=0; j<cols; ++j)
+ {
+ for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+ {
+ RhsScalar y = rhsIt.value();
+ Index k = rhsIt.index();
+ for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
+ {
+ Index i = lhsIt.index();
+ LhsScalar x = lhsIt.value();
+ res.coeffRef(i,j) += x * y;
+ }
+ }
+ }
+}
+
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType,
+ int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+ int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+struct sparse_sparse_to_dense_product_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ internal::sparse_sparse_to_dense_product_impl<Lhs,Rhs,ResultType>(lhs, rhs, res);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+ ColMajorLhs lhsCol(lhs);
+ internal::sparse_sparse_to_dense_product_impl<ColMajorLhs,Rhs,ResultType>(lhsCol, rhs, res);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+ ColMajorRhs rhsCol(rhs);
+ internal::sparse_sparse_to_dense_product_impl<Lhs,ColMajorRhs,ResultType>(lhs, rhsCol, res);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor>
+{
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+ {
+ Transpose<ResultType> trRes(res);
+ internal::sparse_sparse_to_dense_product_impl<Rhs,Lhs,Transpose<ResultType> >(rhs, lhs, trRes);
+ }
+};
+
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
new file mode 100644
index 0000000..67718c8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
@@ -0,0 +1,67 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_MAPPED_SPARSEMATRIX_H
+#define EIGEN_MAPPED_SPARSEMATRIX_H
+
+namespace Eigen {
+
+/** \deprecated Use Map<SparseMatrix<> >
+ * \class MappedSparseMatrix
+ *
+ * \brief Sparse matrix
+ *
+ * \param _Scalar the scalar type, i.e. the type of the coefficients
+ *
+ * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+ *
+ */
+namespace internal {
+template<typename _Scalar, int _Flags, typename _StorageIndex>
+struct traits<MappedSparseMatrix<_Scalar, _Flags, _StorageIndex> > : traits<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
+{};
+} // end namespace internal
+
+template<typename _Scalar, int _Flags, typename _StorageIndex>
+class MappedSparseMatrix
+ : public Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
+{
+ typedef Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> > Base;
+
+ public:
+
+ typedef typename Base::StorageIndex StorageIndex;
+ typedef typename Base::Scalar Scalar;
+
+ inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0)
+ : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr)
+ {}
+
+ /** Empty destructor */
+ inline ~MappedSparseMatrix() {}
+};
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct evaluator<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> >
+ : evaluator<SparseCompressedBase<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> > >
+{
+ typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType;
+ typedef evaluator<SparseCompressedBase<XprType> > Base;
+
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MAPPED_SPARSEMATRIX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
new file mode 100644
index 0000000..905485c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
@@ -0,0 +1,270 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEASSIGN_H
+#define EIGEN_SPARSEASSIGN_H
+
+namespace Eigen {
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator=(const EigenBase<OtherDerived> &other)
+{
+ internal::call_assignment_no_alias(derived(), other.derived());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
+{
+ // TODO use the evaluator mechanism
+ other.evalTo(derived());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+inline Derived& SparseMatrixBase<Derived>::operator=(const SparseMatrixBase<OtherDerived>& other)
+{
+ // by default sparse evaluation do not alias, so we can safely bypass the generic call_assignment routine
+ internal::Assignment<Derived,OtherDerived,internal::assign_op<Scalar,typename OtherDerived::Scalar> >
+ ::run(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+ return derived();
+}
+
+template<typename Derived>
+inline Derived& SparseMatrixBase<Derived>::operator=(const Derived& other)
+{
+ internal::call_assignment_no_alias(derived(), other.derived());
+ return derived();
+}
+
+namespace internal {
+
+template<>
+struct storage_kind_to_evaluator_kind<Sparse> {
+ typedef IteratorBased Kind;
+};
+
+template<>
+struct storage_kind_to_shape<Sparse> {
+ typedef SparseShape Shape;
+};
+
+struct Sparse2Sparse {};
+struct Sparse2Dense {};
+
+template<> struct AssignmentKind<SparseShape, SparseShape> { typedef Sparse2Sparse Kind; };
+template<> struct AssignmentKind<SparseShape, SparseTriangularShape> { typedef Sparse2Sparse Kind; };
+template<> struct AssignmentKind<DenseShape, SparseShape> { typedef Sparse2Dense Kind; };
+template<> struct AssignmentKind<DenseShape, SparseTriangularShape> { typedef Sparse2Dense Kind; };
+
+
+template<typename DstXprType, typename SrcXprType>
+void assign_sparse_to_sparse(DstXprType &dst, const SrcXprType &src)
+{
+ typedef typename DstXprType::Scalar Scalar;
+ typedef internal::evaluator<DstXprType> DstEvaluatorType;
+ typedef internal::evaluator<SrcXprType> SrcEvaluatorType;
+
+ SrcEvaluatorType srcEvaluator(src);
+
+ const bool transpose = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit);
+ const Index outerEvaluationSize = (SrcEvaluatorType::Flags&RowMajorBit) ? src.rows() : src.cols();
+ if ((!transpose) && src.isRValue())
+ {
+ // eval without temporary
+ dst.resize(src.rows(), src.cols());
+ dst.setZero();
+ dst.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
+ for (Index j=0; j<outerEvaluationSize; ++j)
+ {
+ dst.startVec(j);
+ for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
+ {
+ Scalar v = it.value();
+ dst.insertBackByOuterInner(j,it.index()) = v;
+ }
+ }
+ dst.finalize();
+ }
+ else
+ {
+ // eval through a temporary
+ eigen_assert(( ((internal::traits<DstXprType>::SupportedAccessPatterns & OuterRandomAccessPattern)==OuterRandomAccessPattern) ||
+ (!((DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit)))) &&
+ "the transpose operation is supposed to be handled in SparseMatrix::operator=");
+
+ enum { Flip = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit) };
+
+
+ DstXprType temp(src.rows(), src.cols());
+
+ temp.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
+ for (Index j=0; j<outerEvaluationSize; ++j)
+ {
+ temp.startVec(j);
+ for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
+ {
+ Scalar v = it.value();
+ temp.insertBackByOuterInner(Flip?it.index():j,Flip?j:it.index()) = v;
+ }
+ }
+ temp.finalize();
+
+ dst = temp.markAsRValue();
+ }
+}
+
+// Generic Sparse to Sparse assignment
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Sparse>
+{
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+ {
+ assign_sparse_to_sparse(dst.derived(), src.derived());
+ }
+};
+
+// Generic Sparse to Dense assignment
+template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Dense, Weak>
+{
+ static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+ {
+ if(internal::is_same<Functor,internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> >::value)
+ dst.setZero();
+
+ internal::evaluator<SrcXprType> srcEval(src);
+ resize_if_allowed(dst, src, func);
+ internal::evaluator<DstXprType> dstEval(dst);
+
+ const Index outerEvaluationSize = (internal::evaluator<SrcXprType>::Flags&RowMajorBit) ? src.rows() : src.cols();
+ for (Index j=0; j<outerEvaluationSize; ++j)
+ for (typename internal::evaluator<SrcXprType>::InnerIterator i(srcEval,j); i; ++i)
+ func.assignCoeff(dstEval.coeffRef(i.row(),i.col()), i.value());
+ }
+};
+
+// Specialization for dense ?= dense +/- sparse and dense ?= sparse +/- dense
+template<typename DstXprType, typename Func1, typename Func2>
+struct assignment_from_dense_op_sparse
+{
+ template<typename SrcXprType, typename InitialFunc>
+ static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/)
+ {
+ #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
+ EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
+ #endif
+
+ call_assignment_no_alias(dst, src.lhs(), Func1());
+ call_assignment_no_alias(dst, src.rhs(), Func2());
+ }
+
+ // Specialization for dense1 = sparse + dense2; -> dense1 = dense2; dense1 += sparse;
+ template<typename Lhs, typename Rhs, typename Scalar>
+ static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
+ run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_sum_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
+ const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
+ {
+ #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
+ EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
+ #endif
+
+ // Apply the dense matrix first, then the sparse one.
+ call_assignment_no_alias(dst, src.rhs(), Func1());
+ call_assignment_no_alias(dst, src.lhs(), Func2());
+ }
+
+ // Specialization for dense1 = sparse - dense2; -> dense1 = -dense2; dense1 += sparse;
+ template<typename Lhs, typename Rhs, typename Scalar>
+ static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
+ run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_difference_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
+ const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
+ {
+ #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
+ EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
+ #endif
+
+ // Apply the dense matrix first, then the sparse one.
+ call_assignment_no_alias(dst, -src.rhs(), Func1());
+ call_assignment_no_alias(dst, src.lhs(), add_assign_op<typename DstXprType::Scalar,typename Lhs::Scalar>());
+ }
+};
+
+#define EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(ASSIGN_OP,BINOP,ASSIGN_OP2) \
+ template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar> \
+ struct Assignment<DstXprType, CwiseBinaryOp<internal::BINOP<Scalar,Scalar>, const Lhs, const Rhs>, internal::ASSIGN_OP<typename DstXprType::Scalar,Scalar>, \
+ Sparse2Dense, \
+ typename internal::enable_if< internal::is_same<typename internal::evaluator_traits<Lhs>::Shape,DenseShape>::value \
+ || internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type> \
+ : assignment_from_dense_op_sparse<DstXprType, internal::ASSIGN_OP<typename DstXprType::Scalar,typename Lhs::Scalar>, internal::ASSIGN_OP2<typename DstXprType::Scalar,typename Rhs::Scalar> > \
+ {}
+
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_sum_op,add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_sum_op,add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_sum_op,sub_assign_op);
+
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op, scalar_difference_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_difference_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_difference_op,add_assign_op);
+
+
+// Specialization for "dst = dec.solve(rhs)"
+// NOTE we need to specialize it for Sparse2Sparse to avoid ambiguous specialization error
+template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<DstXprType, Solve<DecType,RhsType>, internal::assign_op<Scalar,Scalar>, Sparse2Sparse>
+{
+ typedef Solve<DecType,RhsType> SrcXprType;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
+ {
+ Index dstRows = src.rows();
+ Index dstCols = src.cols();
+ if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+ dst.resize(dstRows, dstCols);
+
+ src.dec()._solve_impl(src.rhs(), dst);
+ }
+};
+
+struct Diagonal2Sparse {};
+
+template<> struct AssignmentKind<SparseShape,DiagonalShape> { typedef Diagonal2Sparse Kind; };
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Sparse>
+{
+ typedef typename DstXprType::StorageIndex StorageIndex;
+ typedef typename DstXprType::Scalar Scalar;
+
+ template<int Options, typename AssignFunc>
+ static void run(SparseMatrix<Scalar,Options,StorageIndex> &dst, const SrcXprType &src, const AssignFunc &func)
+ { dst.assignDiagonal(src.diagonal(), func); }
+
+ template<typename DstDerived>
+ static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+ { dst.derived().diagonal() = src.diagonal(); }
+
+ template<typename DstDerived>
+ static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+ { dst.derived().diagonal() += src.diagonal(); }
+
+ template<typename DstDerived>
+ static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+ { dst.derived().diagonal() -= src.diagonal(); }
+};
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEASSIGN_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
new file mode 100644
index 0000000..5b4f6cc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
@@ -0,0 +1,571 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_BLOCK_H
+#define EIGEN_SPARSE_BLOCK_H
+
+namespace Eigen {
+
+// Subset of columns or rows
+template<typename XprType, int BlockRows, int BlockCols>
+class BlockImpl<XprType,BlockRows,BlockCols,true,Sparse>
+ : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,true> >
+{
+ typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
+ typedef Block<XprType, BlockRows, BlockCols, true> BlockType;
+public:
+ enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+protected:
+ enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+ typedef SparseMatrixBase<BlockType> Base;
+ using Base::convert_index;
+public:
+ EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+
+ inline BlockImpl(XprType& xpr, Index i)
+ : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
+ {}
+
+ inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+ : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
+ {}
+
+ EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+ EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+ Index nonZeros() const
+ {
+ typedef internal::evaluator<XprType> EvaluatorType;
+ EvaluatorType matEval(m_matrix);
+ Index nnz = 0;
+ Index end = m_outerStart + m_outerSize.value();
+ for(Index j=m_outerStart; j<end; ++j)
+ for(typename EvaluatorType::InnerIterator it(matEval, j); it; ++it)
+ ++nnz;
+ return nnz;
+ }
+
+ inline const Scalar coeff(Index row, Index col) const
+ {
+ return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+ }
+
+ inline const Scalar coeff(Index index) const
+ {
+ return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart);
+ }
+
+ inline const XprType& nestedExpression() const { return m_matrix; }
+ inline XprType& nestedExpression() { return m_matrix; }
+ Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+ Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+ Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+ Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+ protected:
+
+ typename internal::ref_selector<XprType>::non_const_type m_matrix;
+ Index m_outerStart;
+ const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+ protected:
+ // Disable assignment with clear error message.
+ // Note that simply removing operator= yields compilation errors with ICC+MSVC
+ template<typename T>
+ BlockImpl& operator=(const T&)
+ {
+ EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+ return *this;
+ }
+};
+
+
+/***************************************************************************
+* specialization for SparseMatrix
+***************************************************************************/
+
+namespace internal {
+
+template<typename SparseMatrixType, int BlockRows, int BlockCols>
+class sparse_matrix_block_impl
+ : public SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> >
+{
+ typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
+ typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
+ typedef SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> > Base;
+ using Base::convert_index;
+public:
+ enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+ EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+protected:
+ typedef typename Base::IndexVector IndexVector;
+ enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+public:
+
+ inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index i)
+ : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
+ {}
+
+ inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+ : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
+ {}
+
+ template<typename OtherDerived>
+ inline BlockType& operator=(const SparseMatrixBase<OtherDerived>& other)
+ {
+ typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _NestedMatrixType;
+ _NestedMatrixType& matrix = m_matrix;
+ // This assignment is slow if this vector set is not empty
+ // and/or it is not at the end of the nonzeros of the underlying matrix.
+
+ // 1 - eval to a temporary to avoid transposition and/or aliasing issues
+ Ref<const SparseMatrix<Scalar, IsRowMajor ? RowMajor : ColMajor, StorageIndex> > tmp(other.derived());
+ eigen_internal_assert(tmp.outerSize()==m_outerSize.value());
+
+ // 2 - let's check whether there is enough allocated memory
+ Index nnz = tmp.nonZeros();
+ Index start = m_outerStart==0 ? 0 : m_matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block
+ Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending position of the current block
+ Index block_size = end - start; // available room in the current block
+ Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end;
+
+ Index free_size = m_matrix.isCompressed()
+ ? Index(matrix.data().allocatedSize()) + block_size
+ : block_size;
+
+ Index tmp_start = tmp.outerIndexPtr()[0];
+
+ bool update_trailing_pointers = false;
+ if(nnz>free_size)
+ {
+ // realloc manually to reduce copies
+ typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz);
+
+ internal::smart_copy(m_matrix.valuePtr(), m_matrix.valuePtr() + start, newdata.valuePtr());
+ internal::smart_copy(m_matrix.innerIndexPtr(), m_matrix.innerIndexPtr() + start, newdata.indexPtr());
+
+ internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, newdata.valuePtr() + start);
+ internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz, newdata.indexPtr() + start);
+
+ internal::smart_copy(matrix.valuePtr()+end, matrix.valuePtr()+end + tail_size, newdata.valuePtr()+start+nnz);
+ internal::smart_copy(matrix.innerIndexPtr()+end, matrix.innerIndexPtr()+end + tail_size, newdata.indexPtr()+start+nnz);
+
+ newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz);
+
+ matrix.data().swap(newdata);
+
+ update_trailing_pointers = true;
+ }
+ else
+ {
+ if(m_matrix.isCompressed() && nnz!=block_size)
+ {
+ // no need to realloc, simply copy the tail at its respective position and insert tmp
+ matrix.data().resize(start + nnz + tail_size);
+
+ internal::smart_memmove(matrix.valuePtr()+end, matrix.valuePtr() + end+tail_size, matrix.valuePtr() + start+nnz);
+ internal::smart_memmove(matrix.innerIndexPtr()+end, matrix.innerIndexPtr() + end+tail_size, matrix.innerIndexPtr() + start+nnz);
+
+ update_trailing_pointers = true;
+ }
+
+ internal::smart_copy(tmp.valuePtr() + tmp_start, tmp.valuePtr() + tmp_start + nnz, matrix.valuePtr() + start);
+ internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz, matrix.innerIndexPtr() + start);
+ }
+
+ // update outer index pointers and innerNonZeros
+ if(IsVectorAtCompileTime)
+ {
+ if(!m_matrix.isCompressed())
+ matrix.innerNonZeroPtr()[m_outerStart] = StorageIndex(nnz);
+ matrix.outerIndexPtr()[m_outerStart] = StorageIndex(start);
+ }
+ else
+ {
+ StorageIndex p = StorageIndex(start);
+ for(Index k=0; k<m_outerSize.value(); ++k)
+ {
+ StorageIndex nnz_k = internal::convert_index<StorageIndex>(tmp.innerVector(k).nonZeros());
+ if(!m_matrix.isCompressed())
+ matrix.innerNonZeroPtr()[m_outerStart+k] = nnz_k;
+ matrix.outerIndexPtr()[m_outerStart+k] = p;
+ p += nnz_k;
+ }
+ }
+
+ if(update_trailing_pointers)
+ {
+ StorageIndex offset = internal::convert_index<StorageIndex>(nnz - block_size);
+ for(Index k = m_outerStart + m_outerSize.value(); k<=matrix.outerSize(); ++k)
+ {
+ matrix.outerIndexPtr()[k] += offset;
+ }
+ }
+
+ return derived();
+ }
+
+ inline BlockType& operator=(const BlockType& other)
+ {
+ return operator=<BlockType>(other);
+ }
+
+ inline const Scalar* valuePtr() const
+ { return m_matrix.valuePtr(); }
+ inline Scalar* valuePtr()
+ { return m_matrix.valuePtr(); }
+
+ inline const StorageIndex* innerIndexPtr() const
+ { return m_matrix.innerIndexPtr(); }
+ inline StorageIndex* innerIndexPtr()
+ { return m_matrix.innerIndexPtr(); }
+
+ inline const StorageIndex* outerIndexPtr() const
+ { return m_matrix.outerIndexPtr() + m_outerStart; }
+ inline StorageIndex* outerIndexPtr()
+ { return m_matrix.outerIndexPtr() + m_outerStart; }
+
+ inline const StorageIndex* innerNonZeroPtr() const
+ { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
+ inline StorageIndex* innerNonZeroPtr()
+ { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
+
+ bool isCompressed() const { return m_matrix.innerNonZeroPtr()==0; }
+
+ inline Scalar& coeffRef(Index row, Index col)
+ {
+ return m_matrix.coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+ }
+
+ inline const Scalar coeff(Index row, Index col) const
+ {
+ return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart));
+ }
+
+ inline const Scalar coeff(Index index) const
+ {
+ return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart);
+ }
+
+ const Scalar& lastCoeff() const
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(sparse_matrix_block_impl);
+ eigen_assert(Base::nonZeros()>0);
+ if(m_matrix.isCompressed())
+ return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1];
+ else
+ return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1];
+ }
+
+ EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+ EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+ inline const SparseMatrixType& nestedExpression() const { return m_matrix; }
+ inline SparseMatrixType& nestedExpression() { return m_matrix; }
+ Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+ Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+ Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+ Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+ protected:
+
+ typename internal::ref_selector<SparseMatrixType>::non_const_type m_matrix;
+ Index m_outerStart;
+ const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+};
+
+} // namespace internal
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+class BlockImpl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
+ : public internal::sparse_matrix_block_impl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
+{
+public:
+ typedef _StorageIndex StorageIndex;
+ typedef SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
+ typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
+ inline BlockImpl(SparseMatrixType& xpr, Index i)
+ : Base(xpr, i)
+ {}
+
+ inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+ : Base(xpr, startRow, startCol, blockRows, blockCols)
+ {}
+
+ using Base::operator=;
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+class BlockImpl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
+ : public internal::sparse_matrix_block_impl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
+{
+public:
+ typedef _StorageIndex StorageIndex;
+ typedef const SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
+ typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
+ inline BlockImpl(SparseMatrixType& xpr, Index i)
+ : Base(xpr, i)
+ {}
+
+ inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+ : Base(xpr, startRow, startCol, blockRows, blockCols)
+ {}
+
+ using Base::operator=;
+private:
+ template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr, Index i);
+ template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr);
+};
+
+//----------
+
+/** Generic implementation of sparse Block expression.
+ * Real-only.
+ */
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl<XprType,BlockRows,BlockCols,InnerPanel,Sparse>
+ : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,InnerPanel> >, internal::no_assignment_operator
+{
+ typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+ typedef SparseMatrixBase<BlockType> Base;
+ using Base::convert_index;
+public:
+ enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+ EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+
+ typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
+
+ /** Column or Row constructor
+ */
+ inline BlockImpl(XprType& xpr, Index i)
+ : m_matrix(xpr),
+ m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? convert_index(i) : 0),
+ m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? convert_index(i) : 0),
+ m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
+ m_blockCols(BlockCols==1 ? 1 : xpr.cols())
+ {}
+
+ /** Dynamic-size constructor
+ */
+ inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+ : m_matrix(xpr), m_startRow(convert_index(startRow)), m_startCol(convert_index(startCol)), m_blockRows(convert_index(blockRows)), m_blockCols(convert_index(blockCols))
+ {}
+
+ inline Index rows() const { return m_blockRows.value(); }
+ inline Index cols() const { return m_blockCols.value(); }
+
+ inline Scalar& coeffRef(Index row, Index col)
+ {
+ return m_matrix.coeffRef(row + m_startRow.value(), col + m_startCol.value());
+ }
+
+ inline const Scalar coeff(Index row, Index col) const
+ {
+ return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
+ }
+
+ inline Scalar& coeffRef(Index index)
+ {
+ return m_matrix.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+ m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+ }
+
+ inline const Scalar coeff(Index index) const
+ {
+ return m_matrix.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+ m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+ }
+
+ inline const XprType& nestedExpression() const { return m_matrix; }
+ inline XprType& nestedExpression() { return m_matrix; }
+ Index startRow() const { return m_startRow.value(); }
+ Index startCol() const { return m_startCol.value(); }
+ Index blockRows() const { return m_blockRows.value(); }
+ Index blockCols() const { return m_blockCols.value(); }
+
+ protected:
+// friend class internal::GenericSparseBlockInnerIteratorImpl<XprType,BlockRows,BlockCols,InnerPanel>;
+ friend struct internal::unary_evaluator<Block<XprType,BlockRows,BlockCols,InnerPanel>, internal::IteratorBased, Scalar >;
+
+ Index nonZeros() const { return Dynamic; }
+
+ typename internal::ref_selector<XprType>::non_const_type m_matrix;
+ const internal::variable_if_dynamic<Index, XprType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
+ const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
+ const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
+ const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
+
+ protected:
+ // Disable assignment with clear error message.
+ // Note that simply removing operator= yields compilation errors with ICC+MSVC
+ template<typename T>
+ BlockImpl& operator=(const T&)
+ {
+ EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+ return *this;
+ }
+
+};
+
+namespace internal {
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+struct unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased >
+ : public evaluator_base<Block<ArgType,BlockRows,BlockCols,InnerPanel> >
+{
+ class InnerVectorInnerIterator;
+ class OuterVectorInnerIterator;
+ public:
+ typedef Block<ArgType,BlockRows,BlockCols,InnerPanel> XprType;
+ typedef typename XprType::StorageIndex StorageIndex;
+ typedef typename XprType::Scalar Scalar;
+
+ enum {
+ IsRowMajor = XprType::IsRowMajor,
+
+ OuterVector = (BlockCols==1 && ArgType::IsRowMajor)
+ | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&".
+ // revert to || as soon as not needed anymore.
+ (BlockRows==1 && !ArgType::IsRowMajor),
+
+ CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+ Flags = XprType::Flags
+ };
+
+ typedef typename internal::conditional<OuterVector,OuterVectorInnerIterator,InnerVectorInnerIterator>::type InnerIterator;
+
+ explicit unary_evaluator(const XprType& op)
+ : m_argImpl(op.nestedExpression()), m_block(op)
+ {}
+
+ inline Index nonZerosEstimate() const {
+ const Index nnz = m_block.nonZeros();
+ if(nnz < 0) {
+ // Scale the non-zero estimate for the underlying expression linearly with block size.
+ // Return zero if the underlying block is empty.
+ const Index nested_sz = m_block.nestedExpression().size();
+ return nested_sz == 0 ? 0 : m_argImpl.nonZerosEstimate() * m_block.size() / nested_sz;
+ }
+ return nnz;
+ }
+
+ protected:
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+ evaluator<ArgType> m_argImpl;
+ const XprType &m_block;
+};
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::InnerVectorInnerIterator
+ : public EvalIterator
+{
+ // NOTE MSVC fails to compile if we don't explicitely "import" IsRowMajor from unary_evaluator
+ // because the base class EvalIterator has a private IsRowMajor enum too. (bug #1786)
+ // NOTE We cannot call it IsRowMajor because it would shadow unary_evaluator::IsRowMajor
+ enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
+ const XprType& m_block;
+ Index m_end;
+public:
+
+ EIGEN_STRONG_INLINE InnerVectorInnerIterator(const unary_evaluator& aEval, Index outer)
+ : EvalIterator(aEval.m_argImpl, outer + (XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol())),
+ m_block(aEval.m_block),
+ m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows())
+ {
+ while( (EvalIterator::operator bool()) && (EvalIterator::index() < (XprIsRowMajor ? m_block.startCol() : m_block.startRow())) )
+ EvalIterator::operator++();
+ }
+
+ inline StorageIndex index() const { return EvalIterator::index() - convert_index<StorageIndex>(XprIsRowMajor ? m_block.startCol() : m_block.startRow()); }
+ inline Index outer() const { return EvalIterator::outer() - (XprIsRowMajor ? m_block.startRow() : m_block.startCol()); }
+ inline Index row() const { return EvalIterator::row() - m_block.startRow(); }
+ inline Index col() const { return EvalIterator::col() - m_block.startCol(); }
+
+ inline operator bool() const { return EvalIterator::operator bool() && EvalIterator::index() < m_end; }
+};
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::OuterVectorInnerIterator
+{
+ // NOTE see above
+ enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
+ const unary_evaluator& m_eval;
+ Index m_outerPos;
+ const Index m_innerIndex;
+ Index m_end;
+ EvalIterator m_it;
+public:
+
+ EIGEN_STRONG_INLINE OuterVectorInnerIterator(const unary_evaluator& aEval, Index outer)
+ : m_eval(aEval),
+ m_outerPos( (XprIsRowMajor ? aEval.m_block.startCol() : aEval.m_block.startRow()) ),
+ m_innerIndex(XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol()),
+ m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows()),
+ m_it(m_eval.m_argImpl, m_outerPos)
+ {
+ EIGEN_UNUSED_VARIABLE(outer);
+ eigen_assert(outer==0);
+
+ while(m_it && m_it.index() < m_innerIndex) ++m_it;
+ if((!m_it) || (m_it.index()!=m_innerIndex))
+ ++(*this);
+ }
+
+ inline StorageIndex index() const { return convert_index<StorageIndex>(m_outerPos - (XprIsRowMajor ? m_eval.m_block.startCol() : m_eval.m_block.startRow())); }
+ inline Index outer() const { return 0; }
+ inline Index row() const { return XprIsRowMajor ? 0 : index(); }
+ inline Index col() const { return XprIsRowMajor ? index() : 0; }
+
+ inline Scalar value() const { return m_it.value(); }
+ inline Scalar& valueRef() { return m_it.valueRef(); }
+
+ inline OuterVectorInnerIterator& operator++()
+ {
+ // search next non-zero entry
+ while(++m_outerPos<m_end)
+ {
+ // Restart iterator at the next inner-vector:
+ m_it.~EvalIterator();
+ ::new (&m_it) EvalIterator(m_eval.m_argImpl, m_outerPos);
+ // search for the key m_innerIndex in the current outer-vector
+ while(m_it && m_it.index() < m_innerIndex) ++m_it;
+ if(m_it && m_it.index()==m_innerIndex) break;
+ }
+ return *this;
+ }
+
+ inline operator bool() const { return m_outerPos < m_end; }
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
+ : evaluator<SparseCompressedBase<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
+{
+ typedef Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+ typedef evaluator<SparseCompressedBase<XprType> > Base;
+ explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
+ : evaluator<SparseCompressedBase<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
+{
+ typedef Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+ typedef evaluator<SparseCompressedBase<XprType> > Base;
+ explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+};
+
+} // end namespace internal
+
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_BLOCK_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
new file mode 100644
index 0000000..ebe02d1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
@@ -0,0 +1,206 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+
+/*
+
+ * NOTE: This file is the modified version of sp_coletree.c file in SuperLU
+
+ * -- SuperLU routine (version 3.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * August 1, 2008
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSE_COLETREE_H
+#define SPARSE_COLETREE_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** Find the root of the tree/set containing the vertex i : Use Path halving */
+template<typename Index, typename IndexVector>
+Index etree_find (Index i, IndexVector& pp)
+{
+ Index p = pp(i); // Parent
+ Index gp = pp(p); // Grand parent
+ while (gp != p)
+ {
+ pp(i) = gp; // Parent pointer on find path is changed to former grand parent
+ i = gp;
+ p = pp(i);
+ gp = pp(p);
+ }
+ return p;
+}
+
+/** Compute the column elimination tree of a sparse matrix
+ * \param mat The matrix in column-major format.
+ * \param parent The elimination tree
+ * \param firstRowElt The column index of the first element in each row
+ * \param perm The permutation to apply to the column of \b mat
+ */
+template <typename MatrixType, typename IndexVector>
+int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowElt, typename MatrixType::StorageIndex *perm=0)
+{
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ StorageIndex nc = convert_index<StorageIndex>(mat.cols()); // Number of columns
+ StorageIndex m = convert_index<StorageIndex>(mat.rows());
+ StorageIndex diagSize = (std::min)(nc,m);
+ IndexVector root(nc); // root of subtree of etree
+ root.setZero();
+ IndexVector pp(nc); // disjoint sets
+ pp.setZero(); // Initialize disjoint sets
+ parent.resize(mat.cols());
+ //Compute first nonzero column in each row
+ firstRowElt.resize(m);
+ firstRowElt.setConstant(nc);
+ firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1);
+ bool found_diag;
+ for (StorageIndex col = 0; col < nc; col++)
+ {
+ StorageIndex pcol = col;
+ if(perm) pcol = perm[col];
+ for (typename MatrixType::InnerIterator it(mat, pcol); it; ++it)
+ {
+ Index row = it.row();
+ firstRowElt(row) = (std::min)(firstRowElt(row), col);
+ }
+ }
+ /* Compute etree by Liu's algorithm for symmetric matrices,
+ except use (firstRowElt[r],c) in place of an edge (r,c) of A.
+ Thus each row clique in A'*A is replaced by a star
+ centered at its first vertex, which has the same fill. */
+ StorageIndex rset, cset, rroot;
+ for (StorageIndex col = 0; col < nc; col++)
+ {
+ found_diag = col>=m;
+ pp(col) = col;
+ cset = col;
+ root(cset) = col;
+ parent(col) = nc;
+ /* The diagonal element is treated here even if it does not exist in the matrix
+ * hence the loop is executed once more */
+ StorageIndex pcol = col;
+ if(perm) pcol = perm[col];
+ for (typename MatrixType::InnerIterator it(mat, pcol); it||!found_diag; ++it)
+ { // A sequence of interleaved find and union is performed
+ Index i = col;
+ if(it) i = it.index();
+ if (i == col) found_diag = true;
+
+ StorageIndex row = firstRowElt(i);
+ if (row >= col) continue;
+ rset = internal::etree_find(row, pp); // Find the name of the set containing row
+ rroot = root(rset);
+ if (rroot != col)
+ {
+ parent(rroot) = col;
+ pp(cset) = rset;
+ cset = rset;
+ root(cset) = col;
+ }
+ }
+ }
+ return 0;
+}
+
+/**
+ * Depth-first search from vertex n. No recursion.
+ * This routine was contributed by Cédric Doucet, CEDRAT Group, Meylan, France.
+*/
+template <typename IndexVector>
+void nr_etdfs (typename IndexVector::Scalar n, IndexVector& parent, IndexVector& first_kid, IndexVector& next_kid, IndexVector& post, typename IndexVector::Scalar postnum)
+{
+ typedef typename IndexVector::Scalar StorageIndex;
+ StorageIndex current = n, first, next;
+ while (postnum != n)
+ {
+ // No kid for the current node
+ first = first_kid(current);
+
+ // no kid for the current node
+ if (first == -1)
+ {
+ // Numbering this node because it has no kid
+ post(current) = postnum++;
+
+ // looking for the next kid
+ next = next_kid(current);
+ while (next == -1)
+ {
+ // No more kids : back to the parent node
+ current = parent(current);
+ // numbering the parent node
+ post(current) = postnum++;
+
+ // Get the next kid
+ next = next_kid(current);
+ }
+ // stopping criterion
+ if (postnum == n+1) return;
+
+ // Updating current node
+ current = next;
+ }
+ else
+ {
+ current = first;
+ }
+ }
+}
+
+
+/**
+ * \brief Post order a tree
+ * \param n the number of nodes
+ * \param parent Input tree
+ * \param post postordered tree
+ */
+template <typename IndexVector>
+void treePostorder(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& post)
+{
+ typedef typename IndexVector::Scalar StorageIndex;
+ IndexVector first_kid, next_kid; // Linked list of children
+ StorageIndex postnum;
+ // Allocate storage for working arrays and results
+ first_kid.resize(n+1);
+ next_kid.setZero(n+1);
+ post.setZero(n+1);
+
+ // Set up structure describing children
+ first_kid.setConstant(-1);
+ for (StorageIndex v = n-1; v >= 0; v--)
+ {
+ StorageIndex dad = parent(v);
+ next_kid(v) = first_kid(dad);
+ first_kid(dad) = v;
+ }
+
+ // Depth-first search from dummy root vertex #n
+ postnum = 0;
+ internal::nr_etdfs(n, parent, first_kid, next_kid, post, postnum);
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // SPARSE_COLETREE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
new file mode 100644
index 0000000..6a2c7a8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
@@ -0,0 +1,370 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_COMPRESSED_BASE_H
+#define EIGEN_SPARSE_COMPRESSED_BASE_H
+
+namespace Eigen {
+
+template<typename Derived> class SparseCompressedBase;
+
+namespace internal {
+
+template<typename Derived>
+struct traits<SparseCompressedBase<Derived> > : traits<Derived>
+{};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+ * \class SparseCompressedBase
+ * \brief Common base class for sparse [compressed]-{row|column}-storage format.
+ *
+ * This class defines the common interface for all derived classes implementing the compressed sparse storage format, such as:
+ * - SparseMatrix
+ * - Ref<SparseMatrixType,Options>
+ * - Map<SparseMatrixType>
+ *
+ */
+template<typename Derived>
+class SparseCompressedBase
+ : public SparseMatrixBase<Derived>
+{
+ public:
+ typedef SparseMatrixBase<Derived> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(SparseCompressedBase)
+ using Base::operator=;
+ using Base::IsRowMajor;
+
+ class InnerIterator;
+ class ReverseInnerIterator;
+
+ protected:
+ typedef typename Base::IndexVector IndexVector;
+ Eigen::Map<IndexVector> innerNonZeros() { return Eigen::Map<IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
+ const Eigen::Map<const IndexVector> innerNonZeros() const { return Eigen::Map<const IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
+
+ public:
+
+ /** \returns the number of non zero coefficients */
+ inline Index nonZeros() const
+ {
+ if(Derived::IsVectorAtCompileTime && outerIndexPtr()==0)
+ return derived().nonZeros();
+ else if(isCompressed())
+ return outerIndexPtr()[derived().outerSize()]-outerIndexPtr()[0];
+ else if(derived().outerSize()==0)
+ return 0;
+ else
+ return innerNonZeros().sum();
+ }
+
+ /** \returns a const pointer to the array of values.
+ * This function is aimed at interoperability with other libraries.
+ * \sa innerIndexPtr(), outerIndexPtr() */
+ inline const Scalar* valuePtr() const { return derived().valuePtr(); }
+ /** \returns a non-const pointer to the array of values.
+ * This function is aimed at interoperability with other libraries.
+ * \sa innerIndexPtr(), outerIndexPtr() */
+ inline Scalar* valuePtr() { return derived().valuePtr(); }
+
+ /** \returns a const pointer to the array of inner indices.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), outerIndexPtr() */
+ inline const StorageIndex* innerIndexPtr() const { return derived().innerIndexPtr(); }
+ /** \returns a non-const pointer to the array of inner indices.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), outerIndexPtr() */
+ inline StorageIndex* innerIndexPtr() { return derived().innerIndexPtr(); }
+
+ /** \returns a const pointer to the array of the starting positions of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 for SparseVector
+ * \sa valuePtr(), innerIndexPtr() */
+ inline const StorageIndex* outerIndexPtr() const { return derived().outerIndexPtr(); }
+ /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 for SparseVector
+ * \sa valuePtr(), innerIndexPtr() */
+ inline StorageIndex* outerIndexPtr() { return derived().outerIndexPtr(); }
+
+ /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 in compressed mode */
+ inline const StorageIndex* innerNonZeroPtr() const { return derived().innerNonZeroPtr(); }
+ /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 in compressed mode */
+ inline StorageIndex* innerNonZeroPtr() { return derived().innerNonZeroPtr(); }
+
+ /** \returns whether \c *this is in compressed form. */
+ inline bool isCompressed() const { return innerNonZeroPtr()==0; }
+
+ /** \returns a read-only view of the stored coefficients as a 1D array expression.
+ *
+ * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+ *
+ * \sa valuePtr(), isCompressed() */
+ const Map<const Array<Scalar,Dynamic,1> > coeffs() const { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+
+ /** \returns a read-write view of the stored coefficients as a 1D array expression
+ *
+ * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+ *
+ * Here is an example:
+ * \include SparseMatrix_coeffs.cpp
+ * and the output is:
+ * \include SparseMatrix_coeffs.out
+ *
+ * \sa valuePtr(), isCompressed() */
+ Map<Array<Scalar,Dynamic,1> > coeffs() { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+
+ protected:
+ /** Default constructor. Do nothing. */
+ SparseCompressedBase() {}
+
+ /** \internal return the index of the coeff at (row,col) or just before if it does not exist.
+ * This is an analogue of std::lower_bound.
+ */
+ internal::LowerBoundIndex lower_bound(Index row, Index col) const
+ {
+ eigen_internal_assert(row>=0 && row<this->rows() && col>=0 && col<this->cols());
+
+ const Index outer = Derived::IsRowMajor ? row : col;
+ const Index inner = Derived::IsRowMajor ? col : row;
+
+ Index start = this->outerIndexPtr()[outer];
+ Index end = this->isCompressed() ? this->outerIndexPtr()[outer+1] : this->outerIndexPtr()[outer] + this->innerNonZeroPtr()[outer];
+ eigen_assert(end>=start && "you are using a non finalized sparse matrix or written coefficient does not exist");
+ internal::LowerBoundIndex p;
+ p.value = std::lower_bound(this->innerIndexPtr()+start, this->innerIndexPtr()+end,inner) - this->innerIndexPtr();
+ p.found = (p.value<end) && (this->innerIndexPtr()[p.value]==inner);
+ return p;
+ }
+
+ friend struct internal::evaluator<SparseCompressedBase<Derived> >;
+
+ private:
+ template<typename OtherDerived> explicit SparseCompressedBase(const SparseCompressedBase<OtherDerived>&);
+};
+
+template<typename Derived>
+class SparseCompressedBase<Derived>::InnerIterator
+{
+ public:
+ InnerIterator()
+ : m_values(0), m_indices(0), m_outer(0), m_id(0), m_end(0)
+ {}
+
+ InnerIterator(const InnerIterator& other)
+ : m_values(other.m_values), m_indices(other.m_indices), m_outer(other.m_outer), m_id(other.m_id), m_end(other.m_end)
+ {}
+
+ InnerIterator& operator=(const InnerIterator& other)
+ {
+ m_values = other.m_values;
+ m_indices = other.m_indices;
+ const_cast<OuterType&>(m_outer).setValue(other.m_outer.value());
+ m_id = other.m_id;
+ m_end = other.m_end;
+ return *this;
+ }
+
+ InnerIterator(const SparseCompressedBase& mat, Index outer)
+ : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
+ {
+ if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
+ {
+ m_id = 0;
+ m_end = mat.nonZeros();
+ }
+ else
+ {
+ m_id = mat.outerIndexPtr()[outer];
+ if(mat.isCompressed())
+ m_end = mat.outerIndexPtr()[outer+1];
+ else
+ m_end = m_id + mat.innerNonZeroPtr()[outer];
+ }
+ }
+
+ explicit InnerIterator(const SparseCompressedBase& mat)
+ : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_id(0), m_end(mat.nonZeros())
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+ }
+
+ explicit InnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
+ : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_id(0), m_end(data.size())
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+ }
+
+ inline InnerIterator& operator++() { m_id++; return *this; }
+ inline InnerIterator& operator+=(Index i) { m_id += i ; return *this; }
+
+ inline InnerIterator operator+(Index i)
+ {
+ InnerIterator result = *this;
+ result += i;
+ return result;
+ }
+
+ inline const Scalar& value() const { return m_values[m_id]; }
+ inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id]); }
+
+ inline StorageIndex index() const { return m_indices[m_id]; }
+ inline Index outer() const { return m_outer.value(); }
+ inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+ inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+
+ inline operator bool() const { return (m_id < m_end); }
+
+ protected:
+ const Scalar* m_values;
+ const StorageIndex* m_indices;
+ typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
+ const OuterType m_outer;
+ Index m_id;
+ Index m_end;
+ private:
+ // If you get here, then you're not using the right InnerIterator type, e.g.:
+ // SparseMatrix<double,RowMajor> A;
+ // SparseMatrix<double>::InnerIterator it(A,0);
+ template<typename T> InnerIterator(const SparseMatrixBase<T>&, Index outer);
+};
+
+template<typename Derived>
+class SparseCompressedBase<Derived>::ReverseInnerIterator
+{
+ public:
+ ReverseInnerIterator(const SparseCompressedBase& mat, Index outer)
+ : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
+ {
+ if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
+ {
+ m_start = 0;
+ m_id = mat.nonZeros();
+ }
+ else
+ {
+ m_start = mat.outerIndexPtr()[outer];
+ if(mat.isCompressed())
+ m_id = mat.outerIndexPtr()[outer+1];
+ else
+ m_id = m_start + mat.innerNonZeroPtr()[outer];
+ }
+ }
+
+ explicit ReverseInnerIterator(const SparseCompressedBase& mat)
+ : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_start(0), m_id(mat.nonZeros())
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+ }
+
+ explicit ReverseInnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
+ : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_start(0), m_id(data.size())
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+ }
+
+ inline ReverseInnerIterator& operator--() { --m_id; return *this; }
+ inline ReverseInnerIterator& operator-=(Index i) { m_id -= i; return *this; }
+
+ inline ReverseInnerIterator operator-(Index i)
+ {
+ ReverseInnerIterator result = *this;
+ result -= i;
+ return result;
+ }
+
+ inline const Scalar& value() const { return m_values[m_id-1]; }
+ inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id-1]); }
+
+ inline StorageIndex index() const { return m_indices[m_id-1]; }
+ inline Index outer() const { return m_outer.value(); }
+ inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+ inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+
+ inline operator bool() const { return (m_id > m_start); }
+
+ protected:
+ const Scalar* m_values;
+ const StorageIndex* m_indices;
+ typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
+ const OuterType m_outer;
+ Index m_start;
+ Index m_id;
+};
+
+namespace internal {
+
+template<typename Derived>
+struct evaluator<SparseCompressedBase<Derived> >
+ : evaluator_base<Derived>
+{
+ typedef typename Derived::Scalar Scalar;
+ typedef typename Derived::InnerIterator InnerIterator;
+
+ enum {
+ CoeffReadCost = NumTraits<Scalar>::ReadCost,
+ Flags = Derived::Flags
+ };
+
+ evaluator() : m_matrix(0), m_zero(0)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+ explicit evaluator(const Derived &mat) : m_matrix(&mat), m_zero(0)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_matrix->nonZeros();
+ }
+
+ operator Derived&() { return m_matrix->const_cast_derived(); }
+ operator const Derived&() const { return *m_matrix; }
+
+ typedef typename DenseCoeffsBase<Derived,ReadOnlyAccessors>::CoeffReturnType CoeffReturnType;
+ const Scalar& coeff(Index row, Index col) const
+ {
+ Index p = find(row,col);
+
+ if(p==Dynamic)
+ return m_zero;
+ else
+ return m_matrix->const_cast_derived().valuePtr()[p];
+ }
+
+ Scalar& coeffRef(Index row, Index col)
+ {
+ Index p = find(row,col);
+ eigen_assert(p!=Dynamic && "written coefficient does not exist");
+ return m_matrix->const_cast_derived().valuePtr()[p];
+ }
+
+protected:
+
+ Index find(Index row, Index col) const
+ {
+ internal::LowerBoundIndex p = m_matrix->lower_bound(row,col);
+ return p.found ? p.value : Dynamic;
+ }
+
+ const Derived *m_matrix;
+ const Scalar m_zero;
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_COMPRESSED_BASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
new file mode 100644
index 0000000..9b0d3f9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
@@ -0,0 +1,722 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_CWISE_BINARY_OP_H
+#define EIGEN_SPARSE_CWISE_BINARY_OP_H
+
+namespace Eigen {
+
+// Here we have to handle 3 cases:
+// 1 - sparse op dense
+// 2 - dense op sparse
+// 3 - sparse op sparse
+// We also need to implement a 4th iterator for:
+// 4 - dense op dense
+// Finally, we also need to distinguish between the product and other operations :
+// configuration returned mode
+// 1 - sparse op dense product sparse
+// generic dense
+// 2 - dense op sparse product sparse
+// generic dense
+// 3 - sparse op sparse product sparse
+// generic sparse
+// 4 - dense op dense product dense
+// generic dense
+//
+// TODO to ease compiler job, we could specialize product/quotient with a scalar
+// and fallback to cwise-unary evaluator using bind1st_op and bind2nd_op.
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
+ : public SparseMatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+ public:
+ typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> Derived;
+ typedef SparseMatrixBase<Derived> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
+ CwiseBinaryOpImpl()
+ {
+ EIGEN_STATIC_ASSERT((
+ (!internal::is_same<typename internal::traits<Lhs>::StorageKind,
+ typename internal::traits<Rhs>::StorageKind>::value)
+ || ((internal::evaluator<Lhs>::Flags&RowMajorBit) == (internal::evaluator<Rhs>::Flags&RowMajorBit))),
+ THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
+ }
+};
+
+namespace internal {
+
+
+// Generic "sparse OP sparse"
+template<typename XprType> struct binary_sparse_evaluator;
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IteratorBased>
+ : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ typedef typename evaluator<Rhs>::InnerIterator RhsIterator;
+ typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+ typedef typename traits<XprType>::Scalar Scalar;
+ typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+ class InnerIterator
+ {
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+ : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
+ {
+ this->operator++();
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index()))
+ {
+ m_id = m_lhsIter.index();
+ m_value = m_functor(m_lhsIter.value(), m_rhsIter.value());
+ ++m_lhsIter;
+ ++m_rhsIter;
+ }
+ else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index())))
+ {
+ m_id = m_lhsIter.index();
+ m_value = m_functor(m_lhsIter.value(), Scalar(0));
+ ++m_lhsIter;
+ }
+ else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index())))
+ {
+ m_id = m_rhsIter.index();
+ m_value = m_functor(Scalar(0), m_rhsIter.value());
+ ++m_rhsIter;
+ }
+ else
+ {
+ m_value = Scalar(0); // this is to avoid a compilation warning
+ m_id = -1;
+ }
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const { return m_value; }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+ EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return Lhs::IsRowMajor ? m_lhsIter.row() : index(); }
+ EIGEN_STRONG_INLINE Index col() const { return Lhs::IsRowMajor ? index() : m_lhsIter.col(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_id>=0; }
+
+ protected:
+ LhsIterator m_lhsIter;
+ RhsIterator m_rhsIter;
+ const BinaryOp& m_functor;
+ Scalar m_value;
+ StorageIndex m_id;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit binary_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate();
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<Lhs> m_lhsImpl;
+ evaluator<Rhs> m_rhsImpl;
+};
+
+// dense op sparse
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IndexBased, IteratorBased>
+ : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+ typedef typename evaluator<Rhs>::InnerIterator RhsIterator;
+ typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+ typedef typename traits<XprType>::Scalar Scalar;
+ typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+ class InnerIterator
+ {
+ enum { IsRowMajor = (int(Rhs::Flags)&RowMajorBit)==RowMajorBit };
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+ : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.rhs().innerSize())
+ {
+ this->operator++();
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ ++m_id;
+ if(m_id<m_innerSize)
+ {
+ Scalar lhsVal = m_lhsEval.coeff(IsRowMajor?m_rhsIter.outer():m_id,
+ IsRowMajor?m_id:m_rhsIter.outer());
+ if(m_rhsIter && m_rhsIter.index()==m_id)
+ {
+ m_value = m_functor(lhsVal, m_rhsIter.value());
+ ++m_rhsIter;
+ }
+ else
+ m_value = m_functor(lhsVal, Scalar(0));
+ }
+
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+ EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_rhsIter.outer() : m_id; }
+ EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_rhsIter.outer(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+
+ protected:
+ const evaluator<Lhs> &m_lhsEval;
+ RhsIterator m_rhsIter;
+ const BinaryOp& m_functor;
+ Scalar m_value;
+ StorageIndex m_id;
+ StorageIndex m_innerSize;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit binary_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs()),
+ m_expr(xpr)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_expr.size();
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<Lhs> m_lhsImpl;
+ evaluator<Rhs> m_rhsImpl;
+ const XprType &m_expr;
+};
+
+// sparse op dense
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IndexBased>
+ : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+ typedef typename traits<XprType>::Scalar Scalar;
+ typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+ class InnerIterator
+ {
+ enum { IsRowMajor = (int(Lhs::Flags)&RowMajorBit)==RowMajorBit };
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+ : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.lhs().innerSize())
+ {
+ this->operator++();
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ ++m_id;
+ if(m_id<m_innerSize)
+ {
+ Scalar rhsVal = m_rhsEval.coeff(IsRowMajor?m_lhsIter.outer():m_id,
+ IsRowMajor?m_id:m_lhsIter.outer());
+ if(m_lhsIter && m_lhsIter.index()==m_id)
+ {
+ m_value = m_functor(m_lhsIter.value(), rhsVal);
+ ++m_lhsIter;
+ }
+ else
+ m_value = m_functor(Scalar(0),rhsVal);
+ }
+
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+ EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_lhsIter.outer() : m_id; }
+ EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_lhsIter.outer(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+
+ protected:
+ LhsIterator m_lhsIter;
+ const evaluator<Rhs> &m_rhsEval;
+ const BinaryOp& m_functor;
+ Scalar m_value;
+ StorageIndex m_id;
+ StorageIndex m_innerSize;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit binary_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs()),
+ m_expr(xpr)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_expr.size();
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<Lhs> m_lhsImpl;
+ evaluator<Rhs> m_rhsImpl;
+ const XprType &m_expr;
+};
+
+template<typename T,
+ typename LhsKind = typename evaluator_traits<typename T::Lhs>::Kind,
+ typename RhsKind = typename evaluator_traits<typename T::Rhs>::Kind,
+ typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
+ typename RhsScalar = typename traits<typename T::Rhs>::Scalar> struct sparse_conjunction_evaluator;
+
+// "sparse .* sparse"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IteratorBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "dense .* sparse"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IndexBased, IteratorBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "sparse .* dense"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse ./ dense"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse && sparse"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IteratorBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "dense && sparse"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IndexBased, IteratorBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "sparse && dense"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IndexBased>
+ : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+ typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+ typedef sparse_conjunction_evaluator<XprType> Base;
+ explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse ^ sparse"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IteratorBased>
+ : evaluator_base<XprType>
+{
+protected:
+ typedef typename XprType::Functor BinaryOp;
+ typedef typename XprType::Lhs LhsArg;
+ typedef typename XprType::Rhs RhsArg;
+ typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+ typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
+ typedef typename XprType::StorageIndex StorageIndex;
+ typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+ class InnerIterator
+ {
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+ : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
+ {
+ while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+ {
+ if (m_lhsIter.index() < m_rhsIter.index())
+ ++m_lhsIter;
+ else
+ ++m_rhsIter;
+ }
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ ++m_lhsIter;
+ ++m_rhsIter;
+ while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+ {
+ if (m_lhsIter.index() < m_rhsIter.index())
+ ++m_lhsIter;
+ else
+ ++m_rhsIter;
+ }
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_lhsIter.value(), m_rhsIter.value()); }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
+ EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); }
+ EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return (m_lhsIter && m_rhsIter); }
+
+ protected:
+ LhsIterator m_lhsIter;
+ RhsIterator m_rhsIter;
+ const BinaryOp& m_functor;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit sparse_conjunction_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return (std::min)(m_lhsImpl.nonZerosEstimate(), m_rhsImpl.nonZerosEstimate());
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<LhsArg> m_lhsImpl;
+ evaluator<RhsArg> m_rhsImpl;
+};
+
+// "dense ^ sparse"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IndexBased, IteratorBased>
+ : evaluator_base<XprType>
+{
+protected:
+ typedef typename XprType::Functor BinaryOp;
+ typedef typename XprType::Lhs LhsArg;
+ typedef typename XprType::Rhs RhsArg;
+ typedef evaluator<LhsArg> LhsEvaluator;
+ typedef typename evaluator<RhsArg>::InnerIterator RhsIterator;
+ typedef typename XprType::StorageIndex StorageIndex;
+ typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+ class InnerIterator
+ {
+ enum { IsRowMajor = (int(RhsArg::Flags)&RowMajorBit)==RowMajorBit };
+
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+ : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_outer(outer)
+ {}
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ ++m_rhsIter;
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const
+ { return m_functor(m_lhsEval.coeff(IsRowMajor?m_outer:m_rhsIter.index(),IsRowMajor?m_rhsIter.index():m_outer), m_rhsIter.value()); }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_rhsIter.index(); }
+ EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return m_rhsIter.row(); }
+ EIGEN_STRONG_INLINE Index col() const { return m_rhsIter.col(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_rhsIter; }
+
+ protected:
+ const LhsEvaluator &m_lhsEval;
+ RhsIterator m_rhsIter;
+ const BinaryOp& m_functor;
+ const Index m_outer;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit sparse_conjunction_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_rhsImpl.nonZerosEstimate();
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<LhsArg> m_lhsImpl;
+ evaluator<RhsArg> m_rhsImpl;
+};
+
+// "sparse ^ dense"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IndexBased>
+ : evaluator_base<XprType>
+{
+protected:
+ typedef typename XprType::Functor BinaryOp;
+ typedef typename XprType::Lhs LhsArg;
+ typedef typename XprType::Rhs RhsArg;
+ typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+ typedef evaluator<RhsArg> RhsEvaluator;
+ typedef typename XprType::StorageIndex StorageIndex;
+ typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+ class InnerIterator
+ {
+ enum { IsRowMajor = (int(LhsArg::Flags)&RowMajorBit)==RowMajorBit };
+
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+ : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_outer(outer)
+ {}
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ ++m_lhsIter;
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const
+ { return m_functor(m_lhsIter.value(),
+ m_rhsEval.coeff(IsRowMajor?m_outer:m_lhsIter.index(),IsRowMajor?m_lhsIter.index():m_outer)); }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
+ EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+ EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); }
+ EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_lhsIter; }
+
+ protected:
+ LhsIterator m_lhsIter;
+ const evaluator<RhsArg> &m_rhsEval;
+ const BinaryOp& m_functor;
+ const Index m_outer;
+ };
+
+
+ enum {
+ CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit sparse_conjunction_evaluator(const XprType& xpr)
+ : m_functor(xpr.functor()),
+ m_lhsImpl(xpr.lhs()),
+ m_rhsImpl(xpr.rhs())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_lhsImpl.nonZerosEstimate();
+ }
+
+protected:
+ const BinaryOp m_functor;
+ evaluator<LhsArg> m_lhsImpl;
+ evaluator<RhsArg> m_rhsImpl;
+};
+
+}
+
+/***************************************************************************
+* Implementation of SparseMatrixBase and SparseCwise functions/operators
+***************************************************************************/
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const EigenBase<OtherDerived> &other)
+{
+ call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
+{
+ call_assignment(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator-=(const SparseMatrixBase<OtherDerived> &other)
+{
+ return derived() = derived() - other.derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& other)
+{
+ return derived() = derived() + other.derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const DiagonalBase<OtherDerived>& other)
+{
+ call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const DiagonalBase<OtherDerived>& other)
+{
+ call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+ return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::template CwiseProductDenseReturnType<OtherDerived>::Type
+SparseMatrixBase<Derived>::cwiseProduct(const MatrixBase<OtherDerived> &other) const
+{
+ return typename CwiseProductDenseReturnType<OtherDerived>::Type(derived(), other.derived());
+}
+
+template<typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
+operator+(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
+{
+ return CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+}
+
+template<typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
+operator+(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
+{
+ return CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+}
+
+template<typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
+operator-(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
+{
+ return CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+}
+
+template<typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
+operator-(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
+{
+ return CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_CWISE_BINARY_OP_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
new file mode 100644
index 0000000..32dac0f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
@@ -0,0 +1,150 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_CWISE_UNARY_OP_H
+#define EIGEN_SPARSE_CWISE_UNARY_OP_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename UnaryOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>
+ : public evaluator_base<CwiseUnaryOp<UnaryOp,ArgType> >
+{
+ public:
+ typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
+
+ class InnerIterator;
+
+ enum {
+ CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_argImpl.nonZerosEstimate();
+ }
+
+ protected:
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+ const UnaryOp m_functor;
+ evaluator<ArgType> m_argImpl;
+};
+
+template<typename UnaryOp, typename ArgType>
+class unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::InnerIterator
+ : public unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator
+{
+ protected:
+ typedef typename XprType::Scalar Scalar;
+ typedef typename unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator Base;
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+ : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
+ {}
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ { Base::operator++(); return *this; }
+
+ EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+
+ protected:
+ const UnaryOp m_functor;
+ private:
+ Scalar& valueRef();
+};
+
+template<typename ViewOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>
+ : public evaluator_base<CwiseUnaryView<ViewOp,ArgType> >
+{
+ public:
+ typedef CwiseUnaryView<ViewOp, ArgType> XprType;
+
+ class InnerIterator;
+
+ enum {
+ CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<ViewOp>::Cost),
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<ViewOp>::Cost);
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ protected:
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+ const ViewOp m_functor;
+ evaluator<ArgType> m_argImpl;
+};
+
+template<typename ViewOp, typename ArgType>
+class unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::InnerIterator
+ : public unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator
+{
+ protected:
+ typedef typename XprType::Scalar Scalar;
+ typedef typename unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator Base;
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+ : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
+ {}
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ { Base::operator++(); return *this; }
+
+ EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+ EIGEN_STRONG_INLINE Scalar& valueRef() { return m_functor(Base::valueRef()); }
+
+ protected:
+ const ViewOp m_functor;
+};
+
+} // end namespace internal
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator*=(const Scalar& other)
+{
+ typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
+ internal::evaluator<Derived> thisEval(derived());
+ for (Index j=0; j<outerSize(); ++j)
+ for (EvalIterator i(thisEval,j); i; ++i)
+ i.valueRef() *= other;
+ return derived();
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator/=(const Scalar& other)
+{
+ typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
+ internal::evaluator<Derived> thisEval(derived());
+ for (Index j=0; j<outerSize(); ++j)
+ for (EvalIterator i(thisEval,j); i; ++i)
+ i.valueRef() /= other;
+ return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_CWISE_UNARY_OP_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
new file mode 100644
index 0000000..f005a18
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
@@ -0,0 +1,342 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEDENSEPRODUCT_H
+#define EIGEN_SPARSEDENSEPRODUCT_H
+
+namespace Eigen {
+
+namespace internal {
+
+template <> struct product_promote_storage_type<Sparse,Dense, OuterProduct> { typedef Sparse ret; };
+template <> struct product_promote_storage_type<Dense,Sparse, OuterProduct> { typedef Sparse ret; };
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,
+ typename AlphaType,
+ int LhsStorageOrder = ((SparseLhsType::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor,
+ bool ColPerCol = ((DenseRhsType::Flags&RowMajorBit)==0) || DenseRhsType::ColsAtCompileTime==1>
+struct sparse_time_dense_product_impl;
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, true>
+{
+ typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+ typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+ typedef typename internal::remove_all<DenseResType>::type Res;
+ typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
+ typedef evaluator<Lhs> LhsEval;
+ static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+ {
+ LhsEval lhsEval(lhs);
+
+ Index n = lhs.outerSize();
+#ifdef EIGEN_HAS_OPENMP
+ Eigen::initParallel();
+ Index threads = Eigen::nbThreads();
+#endif
+
+ for(Index c=0; c<rhs.cols(); ++c)
+ {
+#ifdef EIGEN_HAS_OPENMP
+ // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
+ // It basically represents the minimal amount of work to be done to be worth it.
+ if(threads>1 && lhsEval.nonZerosEstimate() > 20000)
+ {
+ #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
+ for(Index i=0; i<n; ++i)
+ processRow(lhsEval,rhs,res,alpha,i,c);
+ }
+ else
+#endif
+ {
+ for(Index i=0; i<n; ++i)
+ processRow(lhsEval,rhs,res,alpha,i,c);
+ }
+ }
+ }
+
+ static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha, Index i, Index col)
+ {
+ typename Res::Scalar tmp(0);
+ for(LhsInnerIterator it(lhsEval,i); it ;++it)
+ tmp += it.value() * rhs.coeff(it.index(),col);
+ res.coeffRef(i,col) += alpha * tmp;
+ }
+
+};
+
+// FIXME: what is the purpose of the following specialization? Is it for the BlockedSparse format?
+// -> let's disable it for now as it is conflicting with generic scalar*matrix and matrix*scalar operators
+// template<typename T1, typename T2/*, int _Options, typename _StrideType*/>
+// struct ScalarBinaryOpTraits<T1, Ref<T2/*, _Options, _StrideType*/> >
+// {
+// enum {
+// Defined = 1
+// };
+// typedef typename CwiseUnaryOp<scalar_multiple2_op<T1, typename T2::Scalar>, T2>::PlainObject ReturnType;
+// };
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType, ColMajor, true>
+{
+ typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+ typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+ typedef typename internal::remove_all<DenseResType>::type Res;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename LhsEval::InnerIterator LhsInnerIterator;
+ static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+ {
+ LhsEval lhsEval(lhs);
+ for(Index c=0; c<rhs.cols(); ++c)
+ {
+ for(Index j=0; j<lhs.outerSize(); ++j)
+ {
+// typename Res::Scalar rhs_j = alpha * rhs.coeff(j,c);
+ typename ScalarBinaryOpTraits<AlphaType, typename Rhs::Scalar>::ReturnType rhs_j(alpha * rhs.coeff(j,c));
+ for(LhsInnerIterator it(lhsEval,j); it ;++it)
+ res.coeffRef(it.index(),c) += it.value() * rhs_j;
+ }
+ }
+ }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, false>
+{
+ typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+ typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+ typedef typename internal::remove_all<DenseResType>::type Res;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename LhsEval::InnerIterator LhsInnerIterator;
+ static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+ {
+ Index n = lhs.rows();
+ LhsEval lhsEval(lhs);
+
+#ifdef EIGEN_HAS_OPENMP
+ Eigen::initParallel();
+ Index threads = Eigen::nbThreads();
+ // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
+ // It basically represents the minimal amount of work to be done to be worth it.
+ if(threads>1 && lhsEval.nonZerosEstimate()*rhs.cols() > 20000)
+ {
+ #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
+ for(Index i=0; i<n; ++i)
+ processRow(lhsEval,rhs,res,alpha,i);
+ }
+ else
+#endif
+ {
+ for(Index i=0; i<n; ++i)
+ processRow(lhsEval, rhs, res, alpha, i);
+ }
+ }
+
+ static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, Res& res, const typename Res::Scalar& alpha, Index i)
+ {
+ typename Res::RowXpr res_i(res.row(i));
+ for(LhsInnerIterator it(lhsEval,i); it ;++it)
+ res_i += (alpha*it.value()) * rhs.row(it.index());
+ }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, ColMajor, false>
+{
+ typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+ typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+ typedef typename internal::remove_all<DenseResType>::type Res;
+ typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
+ static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+ {
+ evaluator<Lhs> lhsEval(lhs);
+ for(Index j=0; j<lhs.outerSize(); ++j)
+ {
+ typename Rhs::ConstRowXpr rhs_j(rhs.row(j));
+ for(LhsInnerIterator it(lhsEval,j); it ;++it)
+ res.row(it.index()) += (alpha*it.value()) * rhs_j;
+ }
+ }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,typename AlphaType>
+inline void sparse_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+{
+ sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType>::run(lhs, rhs, res, alpha);
+}
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
+ : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,SparseShape,DenseShape,ProductType> >
+{
+ typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+
+ template<typename Dest>
+ static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+ {
+ typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? 1 : Rhs::ColsAtCompileTime>::type LhsNested;
+ typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==0) ? 1 : Dynamic>::type RhsNested;
+ LhsNested lhsNested(lhs);
+ RhsNested rhsNested(rhs);
+ internal::sparse_time_dense_product(lhsNested, rhsNested, dst, alpha);
+ }
+};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, DenseShape, ProductType>
+ : generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
+{};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
+ : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,SparseShape,ProductType> >
+{
+ typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+
+ template<typename Dst>
+ static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+ {
+ typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? Dynamic : 1>::type LhsNested;
+ typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==RowMajorBit) ? 1 : Lhs::RowsAtCompileTime>::type RhsNested;
+ LhsNested lhsNested(lhs);
+ RhsNested rhsNested(rhs);
+
+ // transpose everything
+ Transpose<Dst> dstT(dst);
+ internal::sparse_time_dense_product(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
+ }
+};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, DenseShape, SparseTriangularShape, ProductType>
+ : generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
+{};
+
+template<typename LhsT, typename RhsT, bool NeedToTranspose>
+struct sparse_dense_outer_product_evaluator
+{
+protected:
+ typedef typename conditional<NeedToTranspose,RhsT,LhsT>::type Lhs1;
+ typedef typename conditional<NeedToTranspose,LhsT,RhsT>::type ActualRhs;
+ typedef Product<LhsT,RhsT,DefaultProduct> ProdXprType;
+
+ // if the actual left-hand side is a dense vector,
+ // then build a sparse-view so that we can seamlessly iterate over it.
+ typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
+ Lhs1, SparseView<Lhs1> >::type ActualLhs;
+ typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
+ Lhs1 const&, SparseView<Lhs1> >::type LhsArg;
+
+ typedef evaluator<ActualLhs> LhsEval;
+ typedef evaluator<ActualRhs> RhsEval;
+ typedef typename evaluator<ActualLhs>::InnerIterator LhsIterator;
+ typedef typename ProdXprType::Scalar Scalar;
+
+public:
+ enum {
+ Flags = NeedToTranspose ? RowMajorBit : 0,
+ CoeffReadCost = HugeCost
+ };
+
+ class InnerIterator : public LhsIterator
+ {
+ public:
+ InnerIterator(const sparse_dense_outer_product_evaluator &xprEval, Index outer)
+ : LhsIterator(xprEval.m_lhsXprImpl, 0),
+ m_outer(outer),
+ m_empty(false),
+ m_factor(get(xprEval.m_rhsXprImpl, outer, typename internal::traits<ActualRhs>::StorageKind() ))
+ {}
+
+ EIGEN_STRONG_INLINE Index outer() const { return m_outer; }
+ EIGEN_STRONG_INLINE Index row() const { return NeedToTranspose ? m_outer : LhsIterator::index(); }
+ EIGEN_STRONG_INLINE Index col() const { return NeedToTranspose ? LhsIterator::index() : m_outer; }
+
+ EIGEN_STRONG_INLINE Scalar value() const { return LhsIterator::value() * m_factor; }
+ EIGEN_STRONG_INLINE operator bool() const { return LhsIterator::operator bool() && (!m_empty); }
+
+ protected:
+ Scalar get(const RhsEval &rhs, Index outer, Dense = Dense()) const
+ {
+ return rhs.coeff(outer);
+ }
+
+ Scalar get(const RhsEval &rhs, Index outer, Sparse = Sparse())
+ {
+ typename RhsEval::InnerIterator it(rhs, outer);
+ if (it && it.index()==0 && it.value()!=Scalar(0))
+ return it.value();
+ m_empty = true;
+ return Scalar(0);
+ }
+
+ Index m_outer;
+ bool m_empty;
+ Scalar m_factor;
+ };
+
+ sparse_dense_outer_product_evaluator(const Lhs1 &lhs, const ActualRhs &rhs)
+ : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ // transpose case
+ sparse_dense_outer_product_evaluator(const ActualRhs &rhs, const Lhs1 &lhs)
+ : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+protected:
+ const LhsArg m_lhs;
+ evaluator<ActualLhs> m_lhsXprImpl;
+ evaluator<ActualRhs> m_rhsXprImpl;
+};
+
+// sparse * dense outer product
+template<typename Lhs, typename Rhs>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, SparseShape, DenseShape>
+ : sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor>
+{
+ typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor> Base;
+
+ typedef Product<Lhs, Rhs> XprType;
+ typedef typename XprType::PlainObject PlainObject;
+
+ explicit product_evaluator(const XprType& xpr)
+ : Base(xpr.lhs(), xpr.rhs())
+ {}
+
+};
+
+template<typename Lhs, typename Rhs>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, DenseShape, SparseShape>
+ : sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor>
+{
+ typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor> Base;
+
+ typedef Product<Lhs, Rhs> XprType;
+ typedef typename XprType::PlainObject PlainObject;
+
+ explicit product_evaluator(const XprType& xpr)
+ : Base(xpr.lhs(), xpr.rhs())
+ {}
+
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEDENSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
new file mode 100644
index 0000000..941c03b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
@@ -0,0 +1,138 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_DIAGONAL_PRODUCT_H
+#define EIGEN_SPARSE_DIAGONAL_PRODUCT_H
+
+namespace Eigen {
+
+// The product of a diagonal matrix with a sparse matrix can be easily
+// implemented using expression template.
+// We have two consider very different cases:
+// 1 - diag * row-major sparse
+// => each inner vector <=> scalar * sparse vector product
+// => so we can reuse CwiseUnaryOp::InnerIterator
+// 2 - diag * col-major sparse
+// => each inner vector <=> densevector * sparse vector cwise product
+// => again, we can reuse specialization of CwiseBinaryOp::InnerIterator
+// for that particular case
+// The two other cases are symmetric.
+
+namespace internal {
+
+enum {
+ SDP_AsScalarProduct,
+ SDP_AsCwiseProduct
+};
+
+template<typename SparseXprType, typename DiagonalCoeffType, int SDP_Tag>
+struct sparse_diagonal_product_evaluator;
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, DiagonalShape, SparseShape>
+ : public sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct>
+{
+ typedef Product<Lhs, Rhs, DefaultProduct> XprType;
+ enum { CoeffReadCost = HugeCost, Flags = Rhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
+
+ typedef sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct> Base;
+ explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {}
+};
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, SparseShape, DiagonalShape>
+ : public sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct>
+{
+ typedef Product<Lhs, Rhs, DefaultProduct> XprType;
+ enum { CoeffReadCost = HugeCost, Flags = Lhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
+
+ typedef sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct> Base;
+ explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal().transpose()) {}
+};
+
+template<typename SparseXprType, typename DiagonalCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagonalCoeffType, SDP_AsScalarProduct>
+{
+protected:
+ typedef typename evaluator<SparseXprType>::InnerIterator SparseXprInnerIterator;
+ typedef typename SparseXprType::Scalar Scalar;
+
+public:
+ class InnerIterator : public SparseXprInnerIterator
+ {
+ public:
+ InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
+ : SparseXprInnerIterator(xprEval.m_sparseXprImpl, outer),
+ m_coeff(xprEval.m_diagCoeffImpl.coeff(outer))
+ {}
+
+ EIGEN_STRONG_INLINE Scalar value() const { return m_coeff * SparseXprInnerIterator::value(); }
+ protected:
+ typename DiagonalCoeffType::Scalar m_coeff;
+ };
+
+ sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagonalCoeffType &diagCoeff)
+ : m_sparseXprImpl(sparseXpr), m_diagCoeffImpl(diagCoeff)
+ {}
+
+ Index nonZerosEstimate() const { return m_sparseXprImpl.nonZerosEstimate(); }
+
+protected:
+ evaluator<SparseXprType> m_sparseXprImpl;
+ evaluator<DiagonalCoeffType> m_diagCoeffImpl;
+};
+
+
+template<typename SparseXprType, typename DiagCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagCoeffType, SDP_AsCwiseProduct>
+{
+ typedef typename SparseXprType::Scalar Scalar;
+ typedef typename SparseXprType::StorageIndex StorageIndex;
+
+ typedef typename nested_eval<DiagCoeffType,SparseXprType::IsRowMajor ? SparseXprType::RowsAtCompileTime
+ : SparseXprType::ColsAtCompileTime>::type DiagCoeffNested;
+
+ class InnerIterator
+ {
+ typedef typename evaluator<SparseXprType>::InnerIterator SparseXprIter;
+ public:
+ InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
+ : m_sparseIter(xprEval.m_sparseXprEval, outer), m_diagCoeffNested(xprEval.m_diagCoeffNested)
+ {}
+
+ inline Scalar value() const { return m_sparseIter.value() * m_diagCoeffNested.coeff(index()); }
+ inline StorageIndex index() const { return m_sparseIter.index(); }
+ inline Index outer() const { return m_sparseIter.outer(); }
+ inline Index col() const { return SparseXprType::IsRowMajor ? m_sparseIter.index() : m_sparseIter.outer(); }
+ inline Index row() const { return SparseXprType::IsRowMajor ? m_sparseIter.outer() : m_sparseIter.index(); }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++() { ++m_sparseIter; return *this; }
+ inline operator bool() const { return m_sparseIter; }
+
+ protected:
+ SparseXprIter m_sparseIter;
+ DiagCoeffNested m_diagCoeffNested;
+ };
+
+ sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagCoeffType &diagCoeff)
+ : m_sparseXprEval(sparseXpr), m_diagCoeffNested(diagCoeff)
+ {}
+
+ Index nonZerosEstimate() const { return m_sparseXprEval.nonZerosEstimate(); }
+
+protected:
+ evaluator<SparseXprType> m_sparseXprEval;
+ DiagCoeffNested m_diagCoeffNested;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_DIAGONAL_PRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
new file mode 100644
index 0000000..38bc4aa
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
@@ -0,0 +1,98 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_DOT_H
+#define EIGEN_SPARSE_DOT_H
+
+namespace Eigen {
+
+template<typename Derived>
+template<typename OtherDerived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
+{
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+ EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+ EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+ YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+ eigen_assert(size() == other.size());
+ eigen_assert(other.size()>0 && "you are using a non initialized vector");
+
+ internal::evaluator<Derived> thisEval(derived());
+ typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
+ Scalar res(0);
+ while (i)
+ {
+ res += numext::conj(i.value()) * other.coeff(i.index());
+ ++i;
+ }
+ return res;
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const SparseMatrixBase<OtherDerived>& other) const
+{
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+ EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+ EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+ YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+ eigen_assert(size() == other.size());
+
+ internal::evaluator<Derived> thisEval(derived());
+ typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
+
+ internal::evaluator<OtherDerived> otherEval(other.derived());
+ typename internal::evaluator<OtherDerived>::InnerIterator j(otherEval, 0);
+
+ Scalar res(0);
+ while (i && j)
+ {
+ if (i.index()==j.index())
+ {
+ res += numext::conj(i.value()) * j.value();
+ ++i; ++j;
+ }
+ else if (i.index()<j.index())
+ ++i;
+ else
+ ++j;
+ }
+ return res;
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::squaredNorm() const
+{
+ return numext::real((*this).cwiseAbs2().sum());
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::norm() const
+{
+ using std::sqrt;
+ return sqrt(squaredNorm());
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::blueNorm() const
+{
+ return internal::blueNorm_impl(*this);
+}
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_DOT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
new file mode 100644
index 0000000..7d47eb9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
@@ -0,0 +1,29 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_FUZZY_H
+#define EIGEN_SPARSE_FUZZY_H
+
+namespace Eigen {
+
+template<typename Derived>
+template<typename OtherDerived>
+bool SparseMatrixBase<Derived>::isApprox(const SparseMatrixBase<OtherDerived>& other, const RealScalar &prec) const
+{
+ const typename internal::nested_eval<Derived,2,PlainObject>::type actualA(derived());
+ typename internal::conditional<bool(IsRowMajor)==bool(OtherDerived::IsRowMajor),
+ const typename internal::nested_eval<OtherDerived,2,PlainObject>::type,
+ const PlainObject>::type actualB(other.derived());
+
+ return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_FUZZY_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
new file mode 100644
index 0000000..f99be33
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
@@ -0,0 +1,305 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_MAP_H
+#define EIGEN_SPARSE_MAP_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+ typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ typedef traits<PlainObjectType> TraitsBase;
+ enum {
+ Flags = TraitsBase::Flags & (~NestByRefBit)
+ };
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+ typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ typedef traits<PlainObjectType> TraitsBase;
+ enum {
+ Flags = TraitsBase::Flags & (~ (NestByRefBit | LvalueBit))
+ };
+};
+
+} // end namespace internal
+
+template<typename Derived,
+ int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors
+> class SparseMapBase;
+
+/** \ingroup SparseCore_Module
+ * class SparseMapBase
+ * \brief Common base class for Map and Ref instance of sparse matrix and vector.
+ */
+template<typename Derived>
+class SparseMapBase<Derived,ReadOnlyAccessors>
+ : public SparseCompressedBase<Derived>
+{
+ public:
+ typedef SparseCompressedBase<Derived> Base;
+ typedef typename Base::Scalar Scalar;
+ typedef typename Base::StorageIndex StorageIndex;
+ enum { IsRowMajor = Base::IsRowMajor };
+ using Base::operator=;
+ protected:
+
+ typedef typename internal::conditional<
+ bool(internal::is_lvalue<Derived>::value),
+ Scalar *, const Scalar *>::type ScalarPointer;
+ typedef typename internal::conditional<
+ bool(internal::is_lvalue<Derived>::value),
+ StorageIndex *, const StorageIndex *>::type IndexPointer;
+
+ Index m_outerSize;
+ Index m_innerSize;
+ Array<StorageIndex,2,1> m_zero_nnz;
+ IndexPointer m_outerIndex;
+ IndexPointer m_innerIndices;
+ ScalarPointer m_values;
+ IndexPointer m_innerNonZeros;
+
+ public:
+
+ /** \copydoc SparseMatrixBase::rows() */
+ inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+ /** \copydoc SparseMatrixBase::cols() */
+ inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+ /** \copydoc SparseMatrixBase::innerSize() */
+ inline Index innerSize() const { return m_innerSize; }
+ /** \copydoc SparseMatrixBase::outerSize() */
+ inline Index outerSize() const { return m_outerSize; }
+ /** \copydoc SparseCompressedBase::nonZeros */
+ inline Index nonZeros() const { return m_zero_nnz[1]; }
+
+ /** \copydoc SparseCompressedBase::isCompressed */
+ bool isCompressed() const { return m_innerNonZeros==0; }
+
+ //----------------------------------------
+ // direct access interface
+ /** \copydoc SparseMatrix::valuePtr */
+ inline const Scalar* valuePtr() const { return m_values; }
+ /** \copydoc SparseMatrix::innerIndexPtr */
+ inline const StorageIndex* innerIndexPtr() const { return m_innerIndices; }
+ /** \copydoc SparseMatrix::outerIndexPtr */
+ inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+ /** \copydoc SparseMatrix::innerNonZeroPtr */
+ inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+ //----------------------------------------
+
+ /** \copydoc SparseMatrix::coeff */
+ inline Scalar coeff(Index row, Index col) const
+ {
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ Index start = m_outerIndex[outer];
+ Index end = isCompressed() ? m_outerIndex[outer+1] : start + m_innerNonZeros[outer];
+ if (start==end)
+ return Scalar(0);
+ else if (end>0 && inner==m_innerIndices[end-1])
+ return m_values[end-1];
+ // ^^ optimization: let's first check if it is the last coefficient
+ // (very common in high level algorithms)
+
+ const StorageIndex* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end-1],inner);
+ const Index id = r-&m_innerIndices[0];
+ return ((*r==inner) && (id<end)) ? m_values[id] : Scalar(0);
+ }
+
+ inline SparseMapBase(Index rows, Index cols, Index nnz, IndexPointer outerIndexPtr, IndexPointer innerIndexPtr,
+ ScalarPointer valuePtr, IndexPointer innerNonZerosPtr = 0)
+ : m_outerSize(IsRowMajor?rows:cols), m_innerSize(IsRowMajor?cols:rows), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(outerIndexPtr),
+ m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(innerNonZerosPtr)
+ {}
+
+ // for vectors
+ inline SparseMapBase(Index size, Index nnz, IndexPointer innerIndexPtr, ScalarPointer valuePtr)
+ : m_outerSize(1), m_innerSize(size), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(m_zero_nnz.data()),
+ m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(0)
+ {}
+
+ /** Empty destructor */
+ inline ~SparseMapBase() {}
+
+ protected:
+ inline SparseMapBase() {}
+};
+
+/** \ingroup SparseCore_Module
+ * class SparseMapBase
+ * \brief Common base class for writable Map and Ref instance of sparse matrix and vector.
+ */
+template<typename Derived>
+class SparseMapBase<Derived,WriteAccessors>
+ : public SparseMapBase<Derived,ReadOnlyAccessors>
+{
+ typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
+
+ public:
+ typedef SparseMapBase<Derived, ReadOnlyAccessors> Base;
+ typedef typename Base::Scalar Scalar;
+ typedef typename Base::StorageIndex StorageIndex;
+ enum { IsRowMajor = Base::IsRowMajor };
+
+ using Base::operator=;
+
+ public:
+
+ //----------------------------------------
+ // direct access interface
+ using Base::valuePtr;
+ using Base::innerIndexPtr;
+ using Base::outerIndexPtr;
+ using Base::innerNonZeroPtr;
+ /** \copydoc SparseMatrix::valuePtr */
+ inline Scalar* valuePtr() { return Base::m_values; }
+ /** \copydoc SparseMatrix::innerIndexPtr */
+ inline StorageIndex* innerIndexPtr() { return Base::m_innerIndices; }
+ /** \copydoc SparseMatrix::outerIndexPtr */
+ inline StorageIndex* outerIndexPtr() { return Base::m_outerIndex; }
+ /** \copydoc SparseMatrix::innerNonZeroPtr */
+ inline StorageIndex* innerNonZeroPtr() { return Base::m_innerNonZeros; }
+ //----------------------------------------
+
+ /** \copydoc SparseMatrix::coeffRef */
+ inline Scalar& coeffRef(Index row, Index col)
+ {
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ Index start = Base::m_outerIndex[outer];
+ Index end = Base::isCompressed() ? Base::m_outerIndex[outer+1] : start + Base::m_innerNonZeros[outer];
+ eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+ eigen_assert(end>start && "coeffRef cannot be called on a zero coefficient");
+ StorageIndex* r = std::lower_bound(&Base::m_innerIndices[start],&Base::m_innerIndices[end],inner);
+ const Index id = r - &Base::m_innerIndices[0];
+ eigen_assert((*r==inner) && (id<end) && "coeffRef cannot be called on a zero coefficient");
+ return const_cast<Scalar*>(Base::m_values)[id];
+ }
+
+ inline SparseMapBase(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr,
+ Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+ : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+ {}
+
+ // for vectors
+ inline SparseMapBase(Index size, Index nnz, StorageIndex* innerIndexPtr, Scalar* valuePtr)
+ : Base(size, nnz, innerIndexPtr, valuePtr)
+ {}
+
+ /** Empty destructor */
+ inline ~SparseMapBase() {}
+
+ protected:
+ inline SparseMapBase() {}
+};
+
+/** \ingroup SparseCore_Module
+ *
+ * \brief Specialization of class Map for SparseMatrix-like storage.
+ *
+ * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
+ *
+ * \sa class Map, class SparseMatrix, class Ref<SparseMatrixType,Options>
+ */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+ : public SparseMapBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+#else
+template<typename SparseMatrixType>
+class Map<SparseMatrixType>
+ : public SparseMapBase<Derived,WriteAccessors>
+#endif
+{
+ public:
+ typedef SparseMapBase<Map> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+ enum { IsRowMajor = Base::IsRowMajor };
+
+ public:
+
+ /** Constructs a read-write Map to a sparse matrix of size \a rows x \a cols, containing \a nnz non-zero coefficients,
+ * stored as a sparse format as defined by the pointers \a outerIndexPtr, \a innerIndexPtr, and \a valuePtr.
+ * If the optional parameter \a innerNonZerosPtr is the null pointer, then a standard compressed format is assumed.
+ *
+ * This constructor is available only if \c SparseMatrixType is non-const.
+ *
+ * More details on the expected storage schemes are given in the \ref TutorialSparse "manual pages".
+ */
+ inline Map(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr,
+ StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+ : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+ {}
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+ /** Empty destructor */
+ inline ~Map() {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+ : public SparseMapBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+ public:
+ typedef SparseMapBase<Map> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+ enum { IsRowMajor = Base::IsRowMajor };
+
+ public:
+#endif
+ /** This is the const version of the above constructor.
+ *
+ * This constructor is available only if \c SparseMatrixType is const, e.g.:
+ * \code Map<const SparseMatrix<double> > \endcode
+ */
+ inline Map(Index rows, Index cols, Index nnz, const StorageIndex* outerIndexPtr,
+ const StorageIndex* innerIndexPtr, const Scalar* valuePtr, const StorageIndex* innerNonZerosPtr = 0)
+ : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+ {}
+
+ /** Empty destructor */
+ inline ~Map() {}
+};
+
+namespace internal {
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_MAP_H
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
new file mode 100644
index 0000000..616b4a0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
@@ -0,0 +1,1518 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEMATRIX_H
+#define EIGEN_SPARSEMATRIX_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+ *
+ * \class SparseMatrix
+ *
+ * \brief A versatible sparse matrix representation
+ *
+ * This class implements a more versatile variants of the common \em compressed row/column storage format.
+ * Each colmun's (resp. row) non zeros are stored as a pair of value with associated row (resp. colmiun) index.
+ * All the non zeros are stored in a single large buffer. Unlike the \em compressed format, there might be extra
+ * space in between the nonzeros of two successive colmuns (resp. rows) such that insertion of new non-zero
+ * can be done with limited memory reallocation and copies.
+ *
+ * A call to the function makeCompressed() turns the matrix into the standard \em compressed format
+ * compatible with many library.
+ *
+ * More details on this storage sceheme are given in the \ref TutorialSparse "manual pages".
+ *
+ * \tparam _Scalar the scalar type, i.e. the type of the coefficients
+ * \tparam _Options Union of bit flags controlling the storage scheme. Currently the only possibility
+ * is ColMajor or RowMajor. The default is 0 which means column-major.
+ * \tparam _StorageIndex the type of the indices. It has to be a \b signed type (e.g., short, int, std::ptrdiff_t). Default is \c int.
+ *
+ * \warning In %Eigen 3.2, the undocumented type \c SparseMatrix::Index was improperly defined as the storage index type (e.g., int),
+ * whereas it is now (starting from %Eigen 3.3) deprecated and always defined as Eigen::Index.
+ * Codes making use of \c SparseMatrix::Index, might thus likely have to be changed to use \c SparseMatrix::StorageIndex instead.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN.
+ */
+
+namespace internal {
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct traits<SparseMatrix<_Scalar, _Options, _StorageIndex> >
+{
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+ typedef Sparse StorageKind;
+ typedef MatrixXpr XprKind;
+ enum {
+ RowsAtCompileTime = Dynamic,
+ ColsAtCompileTime = Dynamic,
+ MaxRowsAtCompileTime = Dynamic,
+ MaxColsAtCompileTime = Dynamic,
+ Flags = _Options | NestByRefBit | LvalueBit | CompressedAccessBit,
+ SupportedAccessPatterns = InnerRandomAccessPattern
+ };
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
+struct traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+{
+ typedef SparseMatrix<_Scalar, _Options, _StorageIndex> MatrixType;
+ typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
+ typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+
+ typedef _Scalar Scalar;
+ typedef Dense StorageKind;
+ typedef _StorageIndex StorageIndex;
+ typedef MatrixXpr XprKind;
+
+ enum {
+ RowsAtCompileTime = Dynamic,
+ ColsAtCompileTime = 1,
+ MaxRowsAtCompileTime = Dynamic,
+ MaxColsAtCompileTime = 1,
+ Flags = LvalueBit
+ };
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
+struct traits<Diagonal<const SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+ : public traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+{
+ enum {
+ Flags = 0
+ };
+};
+
+} // end namespace internal
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+class SparseMatrix
+ : public SparseCompressedBase<SparseMatrix<_Scalar, _Options, _StorageIndex> >
+{
+ typedef SparseCompressedBase<SparseMatrix> Base;
+ using Base::convert_index;
+ friend class SparseVector<_Scalar,0,_StorageIndex>;
+ template<typename, typename, typename, typename, typename>
+ friend struct internal::Assignment;
+ public:
+ using Base::isCompressed;
+ using Base::nonZeros;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix)
+ using Base::operator+=;
+ using Base::operator-=;
+
+ typedef MappedSparseMatrix<Scalar,Flags> Map;
+ typedef Diagonal<SparseMatrix> DiagonalReturnType;
+ typedef Diagonal<const SparseMatrix> ConstDiagonalReturnType;
+ typedef typename Base::InnerIterator InnerIterator;
+ typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
+
+
+ using Base::IsRowMajor;
+ typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
+ enum {
+ Options = _Options
+ };
+
+ typedef typename Base::IndexVector IndexVector;
+ typedef typename Base::ScalarVector ScalarVector;
+ protected:
+ typedef SparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
+
+ Index m_outerSize;
+ Index m_innerSize;
+ StorageIndex* m_outerIndex;
+ StorageIndex* m_innerNonZeros; // optional, if null then the data is compressed
+ Storage m_data;
+
+ public:
+
+ /** \returns the number of rows of the matrix */
+ inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+ /** \returns the number of columns of the matrix */
+ inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+
+ /** \returns the number of rows (resp. columns) of the matrix if the storage order column major (resp. row major) */
+ inline Index innerSize() const { return m_innerSize; }
+ /** \returns the number of columns (resp. rows) of the matrix if the storage order column major (resp. row major) */
+ inline Index outerSize() const { return m_outerSize; }
+
+ /** \returns a const pointer to the array of values.
+ * This function is aimed at interoperability with other libraries.
+ * \sa innerIndexPtr(), outerIndexPtr() */
+ inline const Scalar* valuePtr() const { return m_data.valuePtr(); }
+ /** \returns a non-const pointer to the array of values.
+ * This function is aimed at interoperability with other libraries.
+ * \sa innerIndexPtr(), outerIndexPtr() */
+ inline Scalar* valuePtr() { return m_data.valuePtr(); }
+
+ /** \returns a const pointer to the array of inner indices.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), outerIndexPtr() */
+ inline const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+ /** \returns a non-const pointer to the array of inner indices.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), outerIndexPtr() */
+ inline StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+
+ /** \returns a const pointer to the array of the starting positions of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), innerIndexPtr() */
+ inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+ /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \sa valuePtr(), innerIndexPtr() */
+ inline StorageIndex* outerIndexPtr() { return m_outerIndex; }
+
+ /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 in compressed mode */
+ inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+ /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+ * This function is aimed at interoperability with other libraries.
+ * \warning it returns the null pointer 0 in compressed mode */
+ inline StorageIndex* innerNonZeroPtr() { return m_innerNonZeros; }
+
+ /** \internal */
+ inline Storage& data() { return m_data; }
+ /** \internal */
+ inline const Storage& data() const { return m_data; }
+
+ /** \returns the value of the matrix at position \a i, \a j
+ * This function returns Scalar(0) if the element is an explicit \em zero */
+ inline Scalar coeff(Index row, Index col) const
+ {
+ eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+ Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
+ return m_data.atInRange(m_outerIndex[outer], end, StorageIndex(inner));
+ }
+
+ /** \returns a non-const reference to the value of the matrix at position \a i, \a j
+ *
+ * If the element does not exist then it is inserted via the insert(Index,Index) function
+ * which itself turns the matrix into a non compressed form if that was not the case.
+ *
+ * This is a O(log(nnz_j)) operation (binary search) plus the cost of insert(Index,Index)
+ * function if the element does not already exist.
+ */
+ inline Scalar& coeffRef(Index row, Index col)
+ {
+ eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ Index start = m_outerIndex[outer];
+ Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
+ eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+ if(end<=start)
+ return insert(row,col);
+ const Index p = m_data.searchLowerIndex(start,end-1,StorageIndex(inner));
+ if((p<end) && (m_data.index(p)==inner))
+ return m_data.value(p);
+ else
+ return insert(row,col);
+ }
+
+ /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col.
+ * The non zero coefficient must \b not already exist.
+ *
+ * If the matrix \c *this is in compressed mode, then \c *this is turned into uncompressed
+ * mode while reserving room for 2 x this->innerSize() non zeros if reserve(Index) has not been called earlier.
+ * In this case, the insertion procedure is optimized for a \e sequential insertion mode where elements are assumed to be
+ * inserted by increasing outer-indices.
+ *
+ * If that's not the case, then it is strongly recommended to either use a triplet-list to assemble the matrix, or to first
+ * call reserve(const SizesType &) to reserve the appropriate number of non-zero elements per inner vector.
+ *
+ * Assuming memory has been appropriately reserved, this function performs a sorted insertion in O(1)
+ * if the elements of each inner vector are inserted in increasing inner index order, and in O(nnz_j) for a random insertion.
+ *
+ */
+ Scalar& insert(Index row, Index col);
+
+ public:
+
+ /** Removes all non zeros but keep allocated memory
+ *
+ * This function does not free the currently allocated memory. To release as much as memory as possible,
+ * call \code mat.data().squeeze(); \endcode after resizing it.
+ *
+ * \sa resize(Index,Index), data()
+ */
+ inline void setZero()
+ {
+ m_data.clear();
+ memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
+ if(m_innerNonZeros)
+ memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
+ }
+
+ /** Preallocates \a reserveSize non zeros.
+ *
+ * Precondition: the matrix must be in compressed mode. */
+ inline void reserve(Index reserveSize)
+ {
+ eigen_assert(isCompressed() && "This function does not make sense in non compressed mode.");
+ m_data.reserve(reserveSize);
+ }
+
+ #ifdef EIGEN_PARSED_BY_DOXYGEN
+ /** Preallocates \a reserveSize[\c j] non zeros for each column (resp. row) \c j.
+ *
+ * This function turns the matrix in non-compressed mode.
+ *
+ * The type \c SizesType must expose the following interface:
+ \code
+ typedef value_type;
+ const value_type& operator[](i) const;
+ \endcode
+ * for \c i in the [0,this->outerSize()[ range.
+ * Typical choices include std::vector<int>, Eigen::VectorXi, Eigen::VectorXi::Constant, etc.
+ */
+ template<class SizesType>
+ inline void reserve(const SizesType& reserveSizes);
+ #else
+ template<class SizesType>
+ inline void reserve(const SizesType& reserveSizes, const typename SizesType::value_type& enableif =
+ #if (!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1500) // MSVC 2005 fails to compile with this typename
+ typename
+ #endif
+ SizesType::value_type())
+ {
+ EIGEN_UNUSED_VARIABLE(enableif);
+ reserveInnerVectors(reserveSizes);
+ }
+ #endif // EIGEN_PARSED_BY_DOXYGEN
+ protected:
+ template<class SizesType>
+ inline void reserveInnerVectors(const SizesType& reserveSizes)
+ {
+ if(isCompressed())
+ {
+ Index totalReserveSize = 0;
+ // turn the matrix into non-compressed mode
+ m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+ if (!m_innerNonZeros) internal::throw_std_bad_alloc();
+
+ // temporarily use m_innerSizes to hold the new starting points.
+ StorageIndex* newOuterIndex = m_innerNonZeros;
+
+ StorageIndex count = 0;
+ for(Index j=0; j<m_outerSize; ++j)
+ {
+ newOuterIndex[j] = count;
+ count += reserveSizes[j] + (m_outerIndex[j+1]-m_outerIndex[j]);
+ totalReserveSize += reserveSizes[j];
+ }
+ m_data.reserve(totalReserveSize);
+ StorageIndex previousOuterIndex = m_outerIndex[m_outerSize];
+ for(Index j=m_outerSize-1; j>=0; --j)
+ {
+ StorageIndex innerNNZ = previousOuterIndex - m_outerIndex[j];
+ for(Index i=innerNNZ-1; i>=0; --i)
+ {
+ m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
+ m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
+ }
+ previousOuterIndex = m_outerIndex[j];
+ m_outerIndex[j] = newOuterIndex[j];
+ m_innerNonZeros[j] = innerNNZ;
+ }
+ if(m_outerSize>0)
+ m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1];
+
+ m_data.resize(m_outerIndex[m_outerSize]);
+ }
+ else
+ {
+ StorageIndex* newOuterIndex = static_cast<StorageIndex*>(std::malloc((m_outerSize+1)*sizeof(StorageIndex)));
+ if (!newOuterIndex) internal::throw_std_bad_alloc();
+
+ StorageIndex count = 0;
+ for(Index j=0; j<m_outerSize; ++j)
+ {
+ newOuterIndex[j] = count;
+ StorageIndex alreadyReserved = (m_outerIndex[j+1]-m_outerIndex[j]) - m_innerNonZeros[j];
+ StorageIndex toReserve = std::max<StorageIndex>(reserveSizes[j], alreadyReserved);
+ count += toReserve + m_innerNonZeros[j];
+ }
+ newOuterIndex[m_outerSize] = count;
+
+ m_data.resize(count);
+ for(Index j=m_outerSize-1; j>=0; --j)
+ {
+ Index offset = newOuterIndex[j] - m_outerIndex[j];
+ if(offset>0)
+ {
+ StorageIndex innerNNZ = m_innerNonZeros[j];
+ for(Index i=innerNNZ-1; i>=0; --i)
+ {
+ m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
+ m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
+ }
+ }
+ }
+
+ std::swap(m_outerIndex, newOuterIndex);
+ std::free(newOuterIndex);
+ }
+
+ }
+ public:
+
+ //--- low level purely coherent filling ---
+
+ /** \internal
+ * \returns a reference to the non zero coefficient at position \a row, \a col assuming that:
+ * - the nonzero does not already exist
+ * - the new coefficient is the last one according to the storage order
+ *
+ * Before filling a given inner vector you must call the statVec(Index) function.
+ *
+ * After an insertion session, you should call the finalize() function.
+ *
+ * \sa insert, insertBackByOuterInner, startVec */
+ inline Scalar& insertBack(Index row, Index col)
+ {
+ return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row);
+ }
+
+ /** \internal
+ * \sa insertBack, startVec */
+ inline Scalar& insertBackByOuterInner(Index outer, Index inner)
+ {
+ eigen_assert(Index(m_outerIndex[outer+1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)");
+ eigen_assert( (m_outerIndex[outer+1]-m_outerIndex[outer]==0 || m_data.index(m_data.size()-1)<inner) && "Invalid ordered insertion (invalid inner index)");
+ Index p = m_outerIndex[outer+1];
+ ++m_outerIndex[outer+1];
+ m_data.append(Scalar(0), inner);
+ return m_data.value(p);
+ }
+
+ /** \internal
+ * \warning use it only if you know what you are doing */
+ inline Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
+ {
+ Index p = m_outerIndex[outer+1];
+ ++m_outerIndex[outer+1];
+ m_data.append(Scalar(0), inner);
+ return m_data.value(p);
+ }
+
+ /** \internal
+ * \sa insertBack, insertBackByOuterInner */
+ inline void startVec(Index outer)
+ {
+ eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially");
+ eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially");
+ m_outerIndex[outer+1] = m_outerIndex[outer];
+ }
+
+ /** \internal
+ * Must be called after inserting a set of non zero entries using the low level compressed API.
+ */
+ inline void finalize()
+ {
+ if(isCompressed())
+ {
+ StorageIndex size = internal::convert_index<StorageIndex>(m_data.size());
+ Index i = m_outerSize;
+ // find the last filled column
+ while (i>=0 && m_outerIndex[i]==0)
+ --i;
+ ++i;
+ while (i<=m_outerSize)
+ {
+ m_outerIndex[i] = size;
+ ++i;
+ }
+ }
+ }
+
+ //---
+
+ template<typename InputIterators>
+ void setFromTriplets(const InputIterators& begin, const InputIterators& end);
+
+ template<typename InputIterators,typename DupFunctor>
+ void setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+ void sumupDuplicates() { collapseDuplicates(internal::scalar_sum_op<Scalar,Scalar>()); }
+
+ template<typename DupFunctor>
+ void collapseDuplicates(DupFunctor dup_func = DupFunctor());
+
+ //---
+
+ /** \internal
+ * same as insert(Index,Index) except that the indices are given relative to the storage order */
+ Scalar& insertByOuterInner(Index j, Index i)
+ {
+ return insert(IsRowMajor ? j : i, IsRowMajor ? i : j);
+ }
+
+ /** Turns the matrix into the \em compressed format.
+ */
+ void makeCompressed()
+ {
+ if(isCompressed())
+ return;
+
+ eigen_internal_assert(m_outerIndex!=0 && m_outerSize>0);
+
+ Index oldStart = m_outerIndex[1];
+ m_outerIndex[1] = m_innerNonZeros[0];
+ for(Index j=1; j<m_outerSize; ++j)
+ {
+ Index nextOldStart = m_outerIndex[j+1];
+ Index offset = oldStart - m_outerIndex[j];
+ if(offset>0)
+ {
+ for(Index k=0; k<m_innerNonZeros[j]; ++k)
+ {
+ m_data.index(m_outerIndex[j]+k) = m_data.index(oldStart+k);
+ m_data.value(m_outerIndex[j]+k) = m_data.value(oldStart+k);
+ }
+ }
+ m_outerIndex[j+1] = m_outerIndex[j] + m_innerNonZeros[j];
+ oldStart = nextOldStart;
+ }
+ std::free(m_innerNonZeros);
+ m_innerNonZeros = 0;
+ m_data.resize(m_outerIndex[m_outerSize]);
+ m_data.squeeze();
+ }
+
+ /** Turns the matrix into the uncompressed mode */
+ void uncompress()
+ {
+ if(m_innerNonZeros != 0)
+ return;
+ m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+ for (Index i = 0; i < m_outerSize; i++)
+ {
+ m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i];
+ }
+ }
+
+ /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */
+ void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+ {
+ prune(default_prunning_func(reference,epsilon));
+ }
+
+ /** Turns the matrix into compressed format, and suppresses all nonzeros which do not satisfy the predicate \a keep.
+ * The functor type \a KeepFunc must implement the following function:
+ * \code
+ * bool operator() (const Index& row, const Index& col, const Scalar& value) const;
+ * \endcode
+ * \sa prune(Scalar,RealScalar)
+ */
+ template<typename KeepFunc>
+ void prune(const KeepFunc& keep = KeepFunc())
+ {
+ // TODO optimize the uncompressed mode to avoid moving and allocating the data twice
+ makeCompressed();
+
+ StorageIndex k = 0;
+ for(Index j=0; j<m_outerSize; ++j)
+ {
+ Index previousStart = m_outerIndex[j];
+ m_outerIndex[j] = k;
+ Index end = m_outerIndex[j+1];
+ for(Index i=previousStart; i<end; ++i)
+ {
+ if(keep(IsRowMajor?j:m_data.index(i), IsRowMajor?m_data.index(i):j, m_data.value(i)))
+ {
+ m_data.value(k) = m_data.value(i);
+ m_data.index(k) = m_data.index(i);
+ ++k;
+ }
+ }
+ }
+ m_outerIndex[m_outerSize] = k;
+ m_data.resize(k,0);
+ }
+
+ /** Resizes the matrix to a \a rows x \a cols matrix leaving old values untouched.
+ *
+ * If the sizes of the matrix are decreased, then the matrix is turned to \b uncompressed-mode
+ * and the storage of the out of bounds coefficients is kept and reserved.
+ * Call makeCompressed() to pack the entries and squeeze extra memory.
+ *
+ * \sa reserve(), setZero(), makeCompressed()
+ */
+ void conservativeResize(Index rows, Index cols)
+ {
+ // No change
+ if (this->rows() == rows && this->cols() == cols) return;
+
+ // If one dimension is null, then there is nothing to be preserved
+ if(rows==0 || cols==0) return resize(rows,cols);
+
+ Index innerChange = IsRowMajor ? cols - this->cols() : rows - this->rows();
+ Index outerChange = IsRowMajor ? rows - this->rows() : cols - this->cols();
+ StorageIndex newInnerSize = convert_index(IsRowMajor ? cols : rows);
+
+ // Deals with inner non zeros
+ if (m_innerNonZeros)
+ {
+ // Resize m_innerNonZeros
+ StorageIndex *newInnerNonZeros = static_cast<StorageIndex*>(std::realloc(m_innerNonZeros, (m_outerSize + outerChange) * sizeof(StorageIndex)));
+ if (!newInnerNonZeros) internal::throw_std_bad_alloc();
+ m_innerNonZeros = newInnerNonZeros;
+
+ for(Index i=m_outerSize; i<m_outerSize+outerChange; i++)
+ m_innerNonZeros[i] = 0;
+ }
+ else if (innerChange < 0)
+ {
+ // Inner size decreased: allocate a new m_innerNonZeros
+ m_innerNonZeros = static_cast<StorageIndex*>(std::malloc((m_outerSize + outerChange) * sizeof(StorageIndex)));
+ if (!m_innerNonZeros) internal::throw_std_bad_alloc();
+ for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
+ m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i];
+ for(Index i = m_outerSize; i < m_outerSize + outerChange; i++)
+ m_innerNonZeros[i] = 0;
+ }
+
+ // Change the m_innerNonZeros in case of a decrease of inner size
+ if (m_innerNonZeros && innerChange < 0)
+ {
+ for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
+ {
+ StorageIndex &n = m_innerNonZeros[i];
+ StorageIndex start = m_outerIndex[i];
+ while (n > 0 && m_data.index(start+n-1) >= newInnerSize) --n;
+ }
+ }
+
+ m_innerSize = newInnerSize;
+
+ // Re-allocate outer index structure if necessary
+ if (outerChange == 0)
+ return;
+
+ StorageIndex *newOuterIndex = static_cast<StorageIndex*>(std::realloc(m_outerIndex, (m_outerSize + outerChange + 1) * sizeof(StorageIndex)));
+ if (!newOuterIndex) internal::throw_std_bad_alloc();
+ m_outerIndex = newOuterIndex;
+ if (outerChange > 0)
+ {
+ StorageIndex lastIdx = m_outerSize == 0 ? 0 : m_outerIndex[m_outerSize];
+ for(Index i=m_outerSize; i<m_outerSize+outerChange+1; i++)
+ m_outerIndex[i] = lastIdx;
+ }
+ m_outerSize += outerChange;
+ }
+
+ /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero.
+ *
+ * This function does not free the currently allocated memory. To release as much as memory as possible,
+ * call \code mat.data().squeeze(); \endcode after resizing it.
+ *
+ * \sa reserve(), setZero()
+ */
+ void resize(Index rows, Index cols)
+ {
+ const Index outerSize = IsRowMajor ? rows : cols;
+ m_innerSize = IsRowMajor ? cols : rows;
+ m_data.clear();
+ if (m_outerSize != outerSize || m_outerSize==0)
+ {
+ std::free(m_outerIndex);
+ m_outerIndex = static_cast<StorageIndex*>(std::malloc((outerSize + 1) * sizeof(StorageIndex)));
+ if (!m_outerIndex) internal::throw_std_bad_alloc();
+
+ m_outerSize = outerSize;
+ }
+ if(m_innerNonZeros)
+ {
+ std::free(m_innerNonZeros);
+ m_innerNonZeros = 0;
+ }
+ memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
+ }
+
+ /** \internal
+ * Resize the nonzero vector to \a size */
+ void resizeNonZeros(Index size)
+ {
+ m_data.resize(size);
+ }
+
+ /** \returns a const expression of the diagonal coefficients. */
+ const ConstDiagonalReturnType diagonal() const { return ConstDiagonalReturnType(*this); }
+
+ /** \returns a read-write expression of the diagonal coefficients.
+ * \warning If the diagonal entries are written, then all diagonal
+ * entries \b must already exist, otherwise an assertion will be raised.
+ */
+ DiagonalReturnType diagonal() { return DiagonalReturnType(*this); }
+
+ /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */
+ inline SparseMatrix()
+ : m_outerSize(-1), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ resize(0, 0);
+ }
+
+ /** Constructs a \a rows \c x \a cols empty matrix */
+ inline SparseMatrix(Index rows, Index cols)
+ : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ resize(rows, cols);
+ }
+
+ /** Constructs a sparse matrix from the sparse expression \a other */
+ template<typename OtherDerived>
+ inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
+ : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+ YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+ check_template_parameters();
+ const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
+ if (needToTranspose)
+ *this = other.derived();
+ else
+ {
+ #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ #endif
+ internal::call_assignment_no_alias(*this, other.derived());
+ }
+ }
+
+ /** Constructs a sparse matrix from the sparse selfadjoint view \a other */
+ template<typename OtherDerived, unsigned int UpLo>
+ inline SparseMatrix(const SparseSelfAdjointView<OtherDerived, UpLo>& other)
+ : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ Base::operator=(other);
+ }
+
+ /** Copy constructor (it performs a deep copy) */
+ inline SparseMatrix(const SparseMatrix& other)
+ : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ *this = other.derived();
+ }
+
+ /** \brief Copy constructor with in-place evaluation */
+ template<typename OtherDerived>
+ SparseMatrix(const ReturnByValue<OtherDerived>& other)
+ : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ initAssignment(other);
+ other.evalTo(*this);
+ }
+
+ /** \brief Copy constructor with in-place evaluation */
+ template<typename OtherDerived>
+ explicit SparseMatrix(const DiagonalBase<OtherDerived>& other)
+ : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+ {
+ check_template_parameters();
+ *this = other.derived();
+ }
+
+ /** Swaps the content of two sparse matrices of the same type.
+ * This is a fast operation that simply swaps the underlying pointers and parameters. */
+ inline void swap(SparseMatrix& other)
+ {
+ //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
+ std::swap(m_outerIndex, other.m_outerIndex);
+ std::swap(m_innerSize, other.m_innerSize);
+ std::swap(m_outerSize, other.m_outerSize);
+ std::swap(m_innerNonZeros, other.m_innerNonZeros);
+ m_data.swap(other.m_data);
+ }
+
+ /** Sets *this to the identity matrix.
+ * This function also turns the matrix into compressed mode, and drop any reserved memory. */
+ inline void setIdentity()
+ {
+ eigen_assert(rows() == cols() && "ONLY FOR SQUARED MATRICES");
+ this->m_data.resize(rows());
+ Eigen::Map<IndexVector>(this->m_data.indexPtr(), rows()).setLinSpaced(0, StorageIndex(rows()-1));
+ Eigen::Map<ScalarVector>(this->m_data.valuePtr(), rows()).setOnes();
+ Eigen::Map<IndexVector>(this->m_outerIndex, rows()+1).setLinSpaced(0, StorageIndex(rows()));
+ std::free(m_innerNonZeros);
+ m_innerNonZeros = 0;
+ }
+ inline SparseMatrix& operator=(const SparseMatrix& other)
+ {
+ if (other.isRValue())
+ {
+ swap(other.const_cast_derived());
+ }
+ else if(this!=&other)
+ {
+ #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ #endif
+ initAssignment(other);
+ if(other.isCompressed())
+ {
+ internal::smart_copy(other.m_outerIndex, other.m_outerIndex + m_outerSize + 1, m_outerIndex);
+ m_data = other.m_data;
+ }
+ else
+ {
+ Base::operator=(other);
+ }
+ }
+ return *this;
+ }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+ template<typename OtherDerived>
+ inline SparseMatrix& operator=(const EigenBase<OtherDerived>& other)
+ { return Base::operator=(other.derived()); }
+
+ template<typename Lhs, typename Rhs>
+ inline SparseMatrix& operator=(const Product<Lhs,Rhs,AliasFreeProduct>& other);
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+ template<typename OtherDerived>
+ EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other);
+
+ friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m)
+ {
+ EIGEN_DBG_SPARSE(
+ s << "Nonzero entries:\n";
+ if(m.isCompressed())
+ {
+ for (Index i=0; i<m.nonZeros(); ++i)
+ s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+ }
+ else
+ {
+ for (Index i=0; i<m.outerSize(); ++i)
+ {
+ Index p = m.m_outerIndex[i];
+ Index pe = m.m_outerIndex[i]+m.m_innerNonZeros[i];
+ Index k=p;
+ for (; k<pe; ++k) {
+ s << "(" << m.m_data.value(k) << "," << m.m_data.index(k) << ") ";
+ }
+ for (; k<m.m_outerIndex[i+1]; ++k) {
+ s << "(_,_) ";
+ }
+ }
+ }
+ s << std::endl;
+ s << std::endl;
+ s << "Outer pointers:\n";
+ for (Index i=0; i<m.outerSize(); ++i) {
+ s << m.m_outerIndex[i] << " ";
+ }
+ s << " $" << std::endl;
+ if(!m.isCompressed())
+ {
+ s << "Inner non zeros:\n";
+ for (Index i=0; i<m.outerSize(); ++i) {
+ s << m.m_innerNonZeros[i] << " ";
+ }
+ s << " $" << std::endl;
+ }
+ s << std::endl;
+ );
+ s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
+ return s;
+ }
+
+ /** Destructor */
+ inline ~SparseMatrix()
+ {
+ std::free(m_outerIndex);
+ std::free(m_innerNonZeros);
+ }
+
+ /** Overloaded for performance */
+ Scalar sum() const;
+
+# ifdef EIGEN_SPARSEMATRIX_PLUGIN
+# include EIGEN_SPARSEMATRIX_PLUGIN
+# endif
+
+protected:
+
+ template<typename Other>
+ void initAssignment(const Other& other)
+ {
+ resize(other.rows(), other.cols());
+ if(m_innerNonZeros)
+ {
+ std::free(m_innerNonZeros);
+ m_innerNonZeros = 0;
+ }
+ }
+
+ /** \internal
+ * \sa insert(Index,Index) */
+ EIGEN_DONT_INLINE Scalar& insertCompressed(Index row, Index col);
+
+ /** \internal
+ * A vector object that is equal to 0 everywhere but v at the position i */
+ class SingletonVector
+ {
+ StorageIndex m_index;
+ StorageIndex m_value;
+ public:
+ typedef StorageIndex value_type;
+ SingletonVector(Index i, Index v)
+ : m_index(convert_index(i)), m_value(convert_index(v))
+ {}
+
+ StorageIndex operator[](Index i) const { return i==m_index ? m_value : 0; }
+ };
+
+ /** \internal
+ * \sa insert(Index,Index) */
+ EIGEN_DONT_INLINE Scalar& insertUncompressed(Index row, Index col);
+
+public:
+ /** \internal
+ * \sa insert(Index,Index) */
+ EIGEN_STRONG_INLINE Scalar& insertBackUncompressed(Index row, Index col)
+ {
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ eigen_assert(!isCompressed());
+ eigen_assert(m_innerNonZeros[outer]<=(m_outerIndex[outer+1] - m_outerIndex[outer]));
+
+ Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++;
+ m_data.index(p) = convert_index(inner);
+ return (m_data.value(p) = Scalar(0));
+ }
+protected:
+ struct IndexPosPair {
+ IndexPosPair(Index a_i, Index a_p) : i(a_i), p(a_p) {}
+ Index i;
+ Index p;
+ };
+
+ /** \internal assign \a diagXpr to the diagonal of \c *this
+ * There are different strategies:
+ * 1 - if *this is overwritten (Func==assign_op) or *this is empty, then we can work treat *this as a dense vector expression.
+ * 2 - otherwise, for each diagonal coeff,
+ * 2.a - if it already exists, then we update it,
+ * 2.b - otherwise, if *this is uncompressed and that the current inner-vector has empty room for at least 1 element, then we perform an in-place insertion.
+ * 2.c - otherwise, we'll have to reallocate and copy everything, so instead of doing so for each new element, it is recorded in a std::vector.
+ * 3 - at the end, if some entries failed to be inserted in-place, then we alloc a new buffer, copy each chunk at the right position, and insert the new elements.
+ *
+ * TODO: some piece of code could be isolated and reused for a general in-place update strategy.
+ * TODO: if we start to defer the insertion of some elements (i.e., case 2.c executed once),
+ * then it *might* be better to disable case 2.b since they will have to be copied anyway.
+ */
+ template<typename DiagXpr, typename Func>
+ void assignDiagonal(const DiagXpr diagXpr, const Func& assignFunc)
+ {
+ Index n = diagXpr.size();
+
+ const bool overwrite = internal::is_same<Func, internal::assign_op<Scalar,Scalar> >::value;
+ if(overwrite)
+ {
+ if((this->rows()!=n) || (this->cols()!=n))
+ this->resize(n, n);
+ }
+
+ if(m_data.size()==0 || overwrite)
+ {
+ typedef Array<StorageIndex,Dynamic,1> ArrayXI;
+ this->makeCompressed();
+ this->resizeNonZeros(n);
+ Eigen::Map<ArrayXI>(this->innerIndexPtr(), n).setLinSpaced(0,StorageIndex(n)-1);
+ Eigen::Map<ArrayXI>(this->outerIndexPtr(), n+1).setLinSpaced(0,StorageIndex(n));
+ Eigen::Map<Array<Scalar,Dynamic,1> > values = this->coeffs();
+ values.setZero();
+ internal::call_assignment_no_alias(values, diagXpr, assignFunc);
+ }
+ else
+ {
+ bool isComp = isCompressed();
+ internal::evaluator<DiagXpr> diaEval(diagXpr);
+ std::vector<IndexPosPair> newEntries;
+
+ // 1 - try in-place update and record insertion failures
+ for(Index i = 0; i<n; ++i)
+ {
+ internal::LowerBoundIndex lb = this->lower_bound(i,i);
+ Index p = lb.value;
+ if(lb.found)
+ {
+ // the coeff already exists
+ assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
+ }
+ else if((!isComp) && m_innerNonZeros[i] < (m_outerIndex[i+1]-m_outerIndex[i]))
+ {
+ // non compressed mode with local room for inserting one element
+ m_data.moveChunk(p, p+1, m_outerIndex[i]+m_innerNonZeros[i]-p);
+ m_innerNonZeros[i]++;
+ m_data.value(p) = Scalar(0);
+ m_data.index(p) = StorageIndex(i);
+ assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
+ }
+ else
+ {
+ // defer insertion
+ newEntries.push_back(IndexPosPair(i,p));
+ }
+ }
+ // 2 - insert deferred entries
+ Index n_entries = Index(newEntries.size());
+ if(n_entries>0)
+ {
+ Storage newData(m_data.size()+n_entries);
+ Index prev_p = 0;
+ Index prev_i = 0;
+ for(Index k=0; k<n_entries;++k)
+ {
+ Index i = newEntries[k].i;
+ Index p = newEntries[k].p;
+ internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+p, newData.valuePtr()+prev_p+k);
+ internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+p, newData.indexPtr()+prev_p+k);
+ for(Index j=prev_i;j<i;++j)
+ m_outerIndex[j+1] += k;
+ if(!isComp)
+ m_innerNonZeros[i]++;
+ prev_p = p;
+ prev_i = i;
+ newData.value(p+k) = Scalar(0);
+ newData.index(p+k) = StorageIndex(i);
+ assignFunc.assignCoeff(newData.value(p+k), diaEval.coeff(i));
+ }
+ {
+ internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+m_data.size(), newData.valuePtr()+prev_p+n_entries);
+ internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+m_data.size(), newData.indexPtr()+prev_p+n_entries);
+ for(Index j=prev_i+1;j<=m_outerSize;++j)
+ m_outerIndex[j] += n_entries;
+ }
+ m_data.swap(newData);
+ }
+ }
+ }
+
+private:
+ static void check_template_parameters()
+ {
+ EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
+ EIGEN_STATIC_ASSERT((Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
+ }
+
+ struct default_prunning_func {
+ default_prunning_func(const Scalar& ref, const RealScalar& eps) : reference(ref), epsilon(eps) {}
+ inline bool operator() (const Index&, const Index&, const Scalar& value) const
+ {
+ return !internal::isMuchSmallerThan(value, reference, epsilon);
+ }
+ Scalar reference;
+ RealScalar epsilon;
+ };
+};
+
+namespace internal {
+
+template<typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void set_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat, DupFunctor dup_func)
+{
+ enum { IsRowMajor = SparseMatrixType::IsRowMajor };
+ typedef typename SparseMatrixType::Scalar Scalar;
+ typedef typename SparseMatrixType::StorageIndex StorageIndex;
+ SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor,StorageIndex> trMat(mat.rows(),mat.cols());
+
+ if(begin!=end)
+ {
+ // pass 1: count the nnz per inner-vector
+ typename SparseMatrixType::IndexVector wi(trMat.outerSize());
+ wi.setZero();
+ for(InputIterator it(begin); it!=end; ++it)
+ {
+ eigen_assert(it->row()>=0 && it->row()<mat.rows() && it->col()>=0 && it->col()<mat.cols());
+ wi(IsRowMajor ? it->col() : it->row())++;
+ }
+
+ // pass 2: insert all the elements into trMat
+ trMat.reserve(wi);
+ for(InputIterator it(begin); it!=end; ++it)
+ trMat.insertBackUncompressed(it->row(),it->col()) = it->value();
+
+ // pass 3:
+ trMat.collapseDuplicates(dup_func);
+ }
+
+ // pass 4: transposed copy -> implicit sorting
+ mat = trMat;
+}
+
+}
+
+
+/** Fill the matrix \c *this with the list of \em triplets defined by the iterator range \a begin - \a end.
+ *
+ * A \em triplet is a tuple (i,j,value) defining a non-zero element.
+ * The input list of triplets does not have to be sorted, and can contains duplicated elements.
+ * In any case, the result is a \b sorted and \b compressed sparse matrix where the duplicates have been summed up.
+ * This is a \em O(n) operation, with \em n the number of triplet elements.
+ * The initial contents of \c *this is destroyed.
+ * The matrix \c *this must be properly resized beforehand using the SparseMatrix(Index,Index) constructor,
+ * or the resize(Index,Index) method. The sizes are not extracted from the triplet list.
+ *
+ * The \a InputIterators value_type must provide the following interface:
+ * \code
+ * Scalar value() const; // the value
+ * Scalar row() const; // the row index i
+ * Scalar col() const; // the column index j
+ * \endcode
+ * See for instance the Eigen::Triplet template class.
+ *
+ * Here is a typical usage example:
+ * \code
+ typedef Triplet<double> T;
+ std::vector<T> tripletList;
+ tripletList.reserve(estimation_of_entries);
+ for(...)
+ {
+ // ...
+ tripletList.push_back(T(i,j,v_ij));
+ }
+ SparseMatrixType m(rows,cols);
+ m.setFromTriplets(tripletList.begin(), tripletList.end());
+ // m is ready to go!
+ * \endcode
+ *
+ * \warning The list of triplets is read multiple times (at least twice). Therefore, it is not recommended to define
+ * an abstract iterator over a complex data-structure that would be expensive to evaluate. The triplets should rather
+ * be explicitly stored into a std::vector for instance.
+ */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename InputIterators>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end)
+{
+ internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex> >(begin, end, *this, internal::scalar_sum_op<Scalar,Scalar>());
+}
+
+/** The same as setFromTriplets but when duplicates are met the functor \a dup_func is applied:
+ * \code
+ * value = dup_func(OldValue, NewValue)
+ * \endcode
+ * Here is a C++11 example keeping the latest entry only:
+ * \code
+ * mat.setFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+ * \endcode
+ */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename InputIterators,typename DupFunctor>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func)
+{
+ internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex>, DupFunctor>(begin, end, *this, dup_func);
+}
+
+/** \internal */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename DupFunctor>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::collapseDuplicates(DupFunctor dup_func)
+{
+ eigen_assert(!isCompressed());
+ // TODO, in practice we should be able to use m_innerNonZeros for that task
+ IndexVector wi(innerSize());
+ wi.fill(-1);
+ StorageIndex count = 0;
+ // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers
+ for(Index j=0; j<outerSize(); ++j)
+ {
+ StorageIndex start = count;
+ Index oldEnd = m_outerIndex[j]+m_innerNonZeros[j];
+ for(Index k=m_outerIndex[j]; k<oldEnd; ++k)
+ {
+ Index i = m_data.index(k);
+ if(wi(i)>=start)
+ {
+ // we already meet this entry => accumulate it
+ m_data.value(wi(i)) = dup_func(m_data.value(wi(i)), m_data.value(k));
+ }
+ else
+ {
+ m_data.value(count) = m_data.value(k);
+ m_data.index(count) = m_data.index(k);
+ wi(i) = count;
+ ++count;
+ }
+ }
+ m_outerIndex[j] = start;
+ }
+ m_outerIndex[m_outerSize] = count;
+
+ // turn the matrix into compressed form
+ std::free(m_innerNonZeros);
+ m_innerNonZeros = 0;
+ m_data.resize(m_outerIndex[m_outerSize]);
+}
+
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename OtherDerived>
+EIGEN_DONT_INLINE SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const SparseMatrixBase<OtherDerived>& other)
+{
+ EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+ YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+ #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ #endif
+
+ const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
+ if (needToTranspose)
+ {
+ #ifdef EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+ EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+ #endif
+ // two passes algorithm:
+ // 1 - compute the number of coeffs per dest inner vector
+ // 2 - do the actual copy/eval
+ // Since each coeff of the rhs has to be evaluated twice, let's evaluate it if needed
+ typedef typename internal::nested_eval<OtherDerived,2,typename internal::plain_matrix_type<OtherDerived>::type >::type OtherCopy;
+ typedef typename internal::remove_all<OtherCopy>::type _OtherCopy;
+ typedef internal::evaluator<_OtherCopy> OtherCopyEval;
+ OtherCopy otherCopy(other.derived());
+ OtherCopyEval otherCopyEval(otherCopy);
+
+ SparseMatrix dest(other.rows(),other.cols());
+ Eigen::Map<IndexVector> (dest.m_outerIndex,dest.outerSize()).setZero();
+
+ // pass 1
+ // FIXME the above copy could be merged with that pass
+ for (Index j=0; j<otherCopy.outerSize(); ++j)
+ for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
+ ++dest.m_outerIndex[it.index()];
+
+ // prefix sum
+ StorageIndex count = 0;
+ IndexVector positions(dest.outerSize());
+ for (Index j=0; j<dest.outerSize(); ++j)
+ {
+ StorageIndex tmp = dest.m_outerIndex[j];
+ dest.m_outerIndex[j] = count;
+ positions[j] = count;
+ count += tmp;
+ }
+ dest.m_outerIndex[dest.outerSize()] = count;
+ // alloc
+ dest.m_data.resize(count);
+ // pass 2
+ for (StorageIndex j=0; j<otherCopy.outerSize(); ++j)
+ {
+ for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
+ {
+ Index pos = positions[it.index()]++;
+ dest.m_data.index(pos) = j;
+ dest.m_data.value(pos) = it.value();
+ }
+ }
+ this->swap(dest);
+ return *this;
+ }
+ else
+ {
+ if(other.isRValue())
+ {
+ initAssignment(other.derived());
+ }
+ // there is no special optimization
+ return Base::operator=(other.derived());
+ }
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insert(Index row, Index col)
+{
+ eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ if(isCompressed())
+ {
+ if(nonZeros()==0)
+ {
+ // reserve space if not already done
+ if(m_data.allocatedSize()==0)
+ m_data.reserve(2*m_innerSize);
+
+ // turn the matrix into non-compressed mode
+ m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+ if(!m_innerNonZeros) internal::throw_std_bad_alloc();
+
+ memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
+
+ // pack all inner-vectors to the end of the pre-allocated space
+ // and allocate the entire free-space to the first inner-vector
+ StorageIndex end = convert_index(m_data.allocatedSize());
+ for(Index j=1; j<=m_outerSize; ++j)
+ m_outerIndex[j] = end;
+ }
+ else
+ {
+ // turn the matrix into non-compressed mode
+ m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+ if(!m_innerNonZeros) internal::throw_std_bad_alloc();
+ for(Index j=0; j<m_outerSize; ++j)
+ m_innerNonZeros[j] = m_outerIndex[j+1]-m_outerIndex[j];
+ }
+ }
+
+ // check whether we can do a fast "push back" insertion
+ Index data_end = m_data.allocatedSize();
+
+ // First case: we are filling a new inner vector which is packed at the end.
+ // We assume that all remaining inner-vectors are also empty and packed to the end.
+ if(m_outerIndex[outer]==data_end)
+ {
+ eigen_internal_assert(m_innerNonZeros[outer]==0);
+
+ // pack previous empty inner-vectors to end of the used-space
+ // and allocate the entire free-space to the current inner-vector.
+ StorageIndex p = convert_index(m_data.size());
+ Index j = outer;
+ while(j>=0 && m_innerNonZeros[j]==0)
+ m_outerIndex[j--] = p;
+
+ // push back the new element
+ ++m_innerNonZeros[outer];
+ m_data.append(Scalar(0), inner);
+
+ // check for reallocation
+ if(data_end != m_data.allocatedSize())
+ {
+ // m_data has been reallocated
+ // -> move remaining inner-vectors back to the end of the free-space
+ // so that the entire free-space is allocated to the current inner-vector.
+ eigen_internal_assert(data_end < m_data.allocatedSize());
+ StorageIndex new_end = convert_index(m_data.allocatedSize());
+ for(Index k=outer+1; k<=m_outerSize; ++k)
+ if(m_outerIndex[k]==data_end)
+ m_outerIndex[k] = new_end;
+ }
+ return m_data.value(p);
+ }
+
+ // Second case: the next inner-vector is packed to the end
+ // and the current inner-vector end match the used-space.
+ if(m_outerIndex[outer+1]==data_end && m_outerIndex[outer]+m_innerNonZeros[outer]==m_data.size())
+ {
+ eigen_internal_assert(outer+1==m_outerSize || m_innerNonZeros[outer+1]==0);
+
+ // add space for the new element
+ ++m_innerNonZeros[outer];
+ m_data.resize(m_data.size()+1);
+
+ // check for reallocation
+ if(data_end != m_data.allocatedSize())
+ {
+ // m_data has been reallocated
+ // -> move remaining inner-vectors back to the end of the free-space
+ // so that the entire free-space is allocated to the current inner-vector.
+ eigen_internal_assert(data_end < m_data.allocatedSize());
+ StorageIndex new_end = convert_index(m_data.allocatedSize());
+ for(Index k=outer+1; k<=m_outerSize; ++k)
+ if(m_outerIndex[k]==data_end)
+ m_outerIndex[k] = new_end;
+ }
+
+ // and insert it at the right position (sorted insertion)
+ Index startId = m_outerIndex[outer];
+ Index p = m_outerIndex[outer]+m_innerNonZeros[outer]-1;
+ while ( (p > startId) && (m_data.index(p-1) > inner) )
+ {
+ m_data.index(p) = m_data.index(p-1);
+ m_data.value(p) = m_data.value(p-1);
+ --p;
+ }
+
+ m_data.index(p) = convert_index(inner);
+ return (m_data.value(p) = Scalar(0));
+ }
+
+ if(m_data.size() != m_data.allocatedSize())
+ {
+ // make sure the matrix is compatible to random un-compressed insertion:
+ m_data.resize(m_data.allocatedSize());
+ this->reserveInnerVectors(Array<StorageIndex,Dynamic,1>::Constant(m_outerSize, 2));
+ }
+
+ return insertUncompressed(row,col);
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertUncompressed(Index row, Index col)
+{
+ eigen_assert(!isCompressed());
+
+ const Index outer = IsRowMajor ? row : col;
+ const StorageIndex inner = convert_index(IsRowMajor ? col : row);
+
+ Index room = m_outerIndex[outer+1] - m_outerIndex[outer];
+ StorageIndex innerNNZ = m_innerNonZeros[outer];
+ if(innerNNZ>=room)
+ {
+ // this inner vector is full, we need to reallocate the whole buffer :(
+ reserve(SingletonVector(outer,std::max<StorageIndex>(2,innerNNZ)));
+ }
+
+ Index startId = m_outerIndex[outer];
+ Index p = startId + m_innerNonZeros[outer];
+ while ( (p > startId) && (m_data.index(p-1) > inner) )
+ {
+ m_data.index(p) = m_data.index(p-1);
+ m_data.value(p) = m_data.value(p-1);
+ --p;
+ }
+ eigen_assert((p<=startId || m_data.index(p-1)!=inner) && "you cannot insert an element that already exists, you must call coeffRef to this end");
+
+ m_innerNonZeros[outer]++;
+
+ m_data.index(p) = inner;
+ return (m_data.value(p) = Scalar(0));
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertCompressed(Index row, Index col)
+{
+ eigen_assert(isCompressed());
+
+ const Index outer = IsRowMajor ? row : col;
+ const Index inner = IsRowMajor ? col : row;
+
+ Index previousOuter = outer;
+ if (m_outerIndex[outer+1]==0)
+ {
+ // we start a new inner vector
+ while (previousOuter>=0 && m_outerIndex[previousOuter]==0)
+ {
+ m_outerIndex[previousOuter] = convert_index(m_data.size());
+ --previousOuter;
+ }
+ m_outerIndex[outer+1] = m_outerIndex[outer];
+ }
+
+ // here we have to handle the tricky case where the outerIndex array
+ // starts with: [ 0 0 0 0 0 1 ...] and we are inserted in, e.g.,
+ // the 2nd inner vector...
+ bool isLastVec = (!(previousOuter==-1 && m_data.size()!=0))
+ && (std::size_t(m_outerIndex[outer+1]) == m_data.size());
+
+ std::size_t startId = m_outerIndex[outer];
+ // FIXME let's make sure sizeof(long int) == sizeof(std::size_t)
+ std::size_t p = m_outerIndex[outer+1];
+ ++m_outerIndex[outer+1];
+
+ double reallocRatio = 1;
+ if (m_data.allocatedSize()<=m_data.size())
+ {
+ // if there is no preallocated memory, let's reserve a minimum of 32 elements
+ if (m_data.size()==0)
+ {
+ m_data.reserve(32);
+ }
+ else
+ {
+ // we need to reallocate the data, to reduce multiple reallocations
+ // we use a smart resize algorithm based on the current filling ratio
+ // in addition, we use double to avoid integers overflows
+ double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1);
+ reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size());
+ // furthermore we bound the realloc ratio to:
+ // 1) reduce multiple minor realloc when the matrix is almost filled
+ // 2) avoid to allocate too much memory when the matrix is almost empty
+ reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.);
+ }
+ }
+ m_data.resize(m_data.size()+1,reallocRatio);
+
+ if (!isLastVec)
+ {
+ if (previousOuter==-1)
+ {
+ // oops wrong guess.
+ // let's correct the outer offsets
+ for (Index k=0; k<=(outer+1); ++k)
+ m_outerIndex[k] = 0;
+ Index k=outer+1;
+ while(m_outerIndex[k]==0)
+ m_outerIndex[k++] = 1;
+ while (k<=m_outerSize && m_outerIndex[k]!=0)
+ m_outerIndex[k++]++;
+ p = 0;
+ --k;
+ k = m_outerIndex[k]-1;
+ while (k>0)
+ {
+ m_data.index(k) = m_data.index(k-1);
+ m_data.value(k) = m_data.value(k-1);
+ k--;
+ }
+ }
+ else
+ {
+ // we are not inserting into the last inner vec
+ // update outer indices:
+ Index j = outer+2;
+ while (j<=m_outerSize && m_outerIndex[j]!=0)
+ m_outerIndex[j++]++;
+ --j;
+ // shift data of last vecs:
+ Index k = m_outerIndex[j]-1;
+ while (k>=Index(p))
+ {
+ m_data.index(k) = m_data.index(k-1);
+ m_data.value(k) = m_data.value(k-1);
+ k--;
+ }
+ }
+ }
+
+ while ( (p > startId) && (m_data.index(p-1) > inner) )
+ {
+ m_data.index(p) = m_data.index(p-1);
+ m_data.value(p) = m_data.value(p-1);
+ --p;
+ }
+
+ m_data.index(p) = inner;
+ return (m_data.value(p) = Scalar(0));
+}
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct evaluator<SparseMatrix<_Scalar,_Options,_StorageIndex> >
+ : evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > >
+{
+ typedef evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > > Base;
+ typedef SparseMatrix<_Scalar,_Options,_StorageIndex> SparseMatrixType;
+ evaluator() : Base() {}
+ explicit evaluator(const SparseMatrixType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEMATRIX_H
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
new file mode 100644
index 0000000..229449f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
@@ -0,0 +1,398 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEMATRIXBASE_H
+#define EIGEN_SPARSEMATRIXBASE_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+ *
+ * \class SparseMatrixBase
+ *
+ * \brief Base class of any sparse matrices or sparse expressions
+ *
+ * \tparam Derived is the derived type, e.g. a sparse matrix type, or an expression, etc.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN.
+ */
+template<typename Derived> class SparseMatrixBase
+ : public EigenBase<Derived>
+{
+ public:
+
+ typedef typename internal::traits<Derived>::Scalar Scalar;
+
+ /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
+ *
+ * It is an alias for the Scalar type */
+ typedef Scalar value_type;
+
+ typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+ typedef typename internal::traits<Derived>::StorageKind StorageKind;
+
+ /** The integer type used to \b store indices within a SparseMatrix.
+ * For a \c SparseMatrix<Scalar,Options,IndexType> it an alias of the third template parameter \c IndexType. */
+ typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+
+ typedef typename internal::add_const_on_value_type_if_arithmetic<
+ typename internal::packet_traits<Scalar>::type
+ >::type PacketReturnType;
+
+ typedef SparseMatrixBase StorageBaseType;
+
+ typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+ typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+
+ template<typename OtherDerived>
+ Derived& operator=(const EigenBase<OtherDerived> &other);
+
+ enum {
+
+ RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+ /**< The number of rows at compile-time. This is just a copy of the value provided
+ * by the \a Derived type. If a value is not known at compile-time,
+ * it is set to the \a Dynamic constant.
+ * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+ ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+ /**< The number of columns at compile-time. This is just a copy of the value provided
+ * by the \a Derived type. If a value is not known at compile-time,
+ * it is set to the \a Dynamic constant.
+ * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+
+ SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
+ internal::traits<Derived>::ColsAtCompileTime>::ret),
+ /**< This is equal to the number of coefficients, i.e. the number of
+ * rows times the number of columns, or to \a Dynamic if this is not
+ * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+ MaxRowsAtCompileTime = RowsAtCompileTime,
+ MaxColsAtCompileTime = ColsAtCompileTime,
+
+ MaxSizeAtCompileTime = (internal::size_at_compile_time<MaxRowsAtCompileTime,
+ MaxColsAtCompileTime>::ret),
+
+ IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1,
+ /**< This is set to true if either the number of rows or the number of
+ * columns is known at compile-time to be equal to 1. Indeed, in that case,
+ * we are dealing with a column-vector (if there is only one column) or with
+ * a row-vector (if there is only one row). */
+
+ NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2,
+ /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
+ * and 2 for matrices.
+ */
+
+ Flags = internal::traits<Derived>::Flags,
+ /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+ * constructed from this one. See the \ref flags "list of flags".
+ */
+
+ IsRowMajor = Flags&RowMajorBit ? 1 : 0,
+
+ InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
+ : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+
+ #ifndef EIGEN_PARSED_BY_DOXYGEN
+ _HasDirectAccess = (int(Flags)&DirectAccessBit) ? 1 : 0 // workaround sunCC
+ #endif
+ };
+
+ /** \internal the return type of MatrixBase::adjoint() */
+ typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
+ CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, Eigen::Transpose<const Derived> >,
+ Transpose<const Derived>
+ >::type AdjointReturnType;
+ typedef Transpose<Derived> TransposeReturnType;
+ typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
+
+ // FIXME storage order do not match evaluator storage order
+ typedef SparseMatrix<Scalar, Flags&RowMajorBit ? RowMajor : ColMajor, StorageIndex> 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<T> then RealScalar is \a T.
+ *
+ * \sa class NumTraits
+ */
+ typedef typename NumTraits<Scalar>::Real RealScalar;
+
+ /** \internal the return type of coeff()
+ */
+ typedef typename internal::conditional<_HasDirectAccess, const Scalar&, Scalar>::type CoeffReturnType;
+
+ /** \internal Represents a matrix with all coefficients equal to one another*/
+ typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,Matrix<Scalar,Dynamic,Dynamic> > ConstantReturnType;
+
+ /** type of the equivalent dense matrix */
+ typedef Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> DenseMatrixType;
+ /** type of the equivalent square matrix */
+ typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
+ EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+
+ inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+ inline Derived& derived() { return *static_cast<Derived*>(this); }
+ inline Derived& const_cast_derived() const
+ { return *static_cast<Derived*>(const_cast<SparseMatrixBase*>(this)); }
+
+ typedef EigenBase<Derived> Base;
+
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+#define EIGEN_DOC_UNARY_ADDONS(METHOD,OP) /** <p>This method does not change the sparsity of \c *this: the OP is applied to explicitly stored coefficients only. \sa SparseCompressedBase::coeffs() </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL /** <p> \warning This method returns a read-only expression for any sparse matrices. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) /** <p> \warning This method returns a read-write expression for COND sparse matrices only. Otherwise, the returned expression is read-only. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
+#else
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
+#endif
+# include "../plugins/CommonCwiseUnaryOps.h"
+# include "../plugins/CommonCwiseBinaryOps.h"
+# include "../plugins/MatrixCwiseUnaryOps.h"
+# include "../plugins/MatrixCwiseBinaryOps.h"
+# include "../plugins/BlockMethods.h"
+# ifdef EIGEN_SPARSEMATRIXBASE_PLUGIN
+# include EIGEN_SPARSEMATRIXBASE_PLUGIN
+# endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_UNARY_ADDONS
+#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
+
+ /** \returns the number of rows. \sa cols() */
+ inline Index rows() const { return derived().rows(); }
+ /** \returns the number of columns. \sa rows() */
+ inline Index cols() const { return derived().cols(); }
+ /** \returns the number of coefficients, which is \a rows()*cols().
+ * \sa rows(), cols(). */
+ inline Index size() const { return rows() * cols(); }
+ /** \returns true if either the number of rows or the number of columns is equal to 1.
+ * In other words, this function returns
+ * \code rows()==1 || cols()==1 \endcode
+ * \sa rows(), cols(), IsVectorAtCompileTime. */
+ inline bool isVector() const { return rows()==1 || cols()==1; }
+ /** \returns the size of the storage major dimension,
+ * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
+ Index outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
+ /** \returns the size of the inner dimension according to the storage order,
+ * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
+ Index innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
+
+ bool isRValue() const { return m_isRValue; }
+ Derived& markAsRValue() { m_isRValue = true; return derived(); }
+
+ SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */ }
+
+
+ template<typename OtherDerived>
+ Derived& operator=(const ReturnByValue<OtherDerived>& other);
+
+ template<typename OtherDerived>
+ inline Derived& operator=(const SparseMatrixBase<OtherDerived>& other);
+
+ inline Derived& operator=(const Derived& other);
+
+ protected:
+
+ template<typename OtherDerived>
+ inline Derived& assign(const OtherDerived& other);
+
+ template<typename OtherDerived>
+ inline void assignGeneric(const OtherDerived& other);
+
+ public:
+
+ friend std::ostream & operator << (std::ostream & s, const SparseMatrixBase& m)
+ {
+ typedef typename Derived::Nested Nested;
+ typedef typename internal::remove_all<Nested>::type NestedCleaned;
+
+ if (Flags&RowMajorBit)
+ {
+ Nested nm(m.derived());
+ internal::evaluator<NestedCleaned> thisEval(nm);
+ for (Index row=0; row<nm.outerSize(); ++row)
+ {
+ Index col = 0;
+ for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, row); it; ++it)
+ {
+ for ( ; col<it.index(); ++col)
+ s << "0 ";
+ s << it.value() << " ";
+ ++col;
+ }
+ for ( ; col<m.cols(); ++col)
+ s << "0 ";
+ s << std::endl;
+ }
+ }
+ else
+ {
+ Nested nm(m.derived());
+ internal::evaluator<NestedCleaned> thisEval(nm);
+ if (m.cols() == 1) {
+ Index row = 0;
+ for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, 0); it; ++it)
+ {
+ for ( ; row<it.index(); ++row)
+ s << "0" << std::endl;
+ s << it.value() << std::endl;
+ ++row;
+ }
+ for ( ; row<m.rows(); ++row)
+ s << "0" << std::endl;
+ }
+ else
+ {
+ SparseMatrix<Scalar, RowMajorBit, StorageIndex> trans = m;
+ s << static_cast<const SparseMatrixBase<SparseMatrix<Scalar, RowMajorBit, StorageIndex> >&>(trans);
+ }
+ }
+ return s;
+ }
+
+ template<typename OtherDerived>
+ Derived& operator+=(const SparseMatrixBase<OtherDerived>& other);
+ template<typename OtherDerived>
+ Derived& operator-=(const SparseMatrixBase<OtherDerived>& other);
+
+ template<typename OtherDerived>
+ Derived& operator+=(const DiagonalBase<OtherDerived>& other);
+ template<typename OtherDerived>
+ Derived& operator-=(const DiagonalBase<OtherDerived>& other);
+
+ template<typename OtherDerived>
+ Derived& operator+=(const EigenBase<OtherDerived> &other);
+ template<typename OtherDerived>
+ Derived& operator-=(const EigenBase<OtherDerived> &other);
+
+ Derived& operator*=(const Scalar& other);
+ Derived& operator/=(const Scalar& other);
+
+ template<typename OtherDerived> struct CwiseProductDenseReturnType {
+ typedef CwiseBinaryOp<internal::scalar_product_op<typename ScalarBinaryOpTraits<
+ typename internal::traits<Derived>::Scalar,
+ typename internal::traits<OtherDerived>::Scalar
+ >::ReturnType>,
+ const Derived,
+ const OtherDerived
+ > Type;
+ };
+
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType<OtherDerived>::Type
+ cwiseProduct(const MatrixBase<OtherDerived> &other) const;
+
+ // sparse * diagonal
+ template<typename OtherDerived>
+ const Product<Derived,OtherDerived>
+ operator*(const DiagonalBase<OtherDerived> &other) const
+ { return Product<Derived,OtherDerived>(derived(), other.derived()); }
+
+ // diagonal * sparse
+ template<typename OtherDerived> friend
+ const Product<OtherDerived,Derived>
+ operator*(const DiagonalBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
+ { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
+
+ // sparse * sparse
+ template<typename OtherDerived>
+ const Product<Derived,OtherDerived,AliasFreeProduct>
+ operator*(const SparseMatrixBase<OtherDerived> &other) const;
+
+ // sparse * dense
+ template<typename OtherDerived>
+ const Product<Derived,OtherDerived>
+ operator*(const MatrixBase<OtherDerived> &other) const
+ { return Product<Derived,OtherDerived>(derived(), other.derived()); }
+
+ // dense * sparse
+ template<typename OtherDerived> friend
+ const Product<OtherDerived,Derived>
+ operator*(const MatrixBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
+ { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
+
+ /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
+ SparseSymmetricPermutationProduct<Derived,Upper|Lower> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
+ {
+ return SparseSymmetricPermutationProduct<Derived,Upper|Lower>(derived(), perm);
+ }
+
+ template<typename OtherDerived>
+ Derived& operator*=(const SparseMatrixBase<OtherDerived>& other);
+
+ template<int Mode>
+ inline const TriangularView<const Derived, Mode> triangularView() const;
+
+ template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SparseSelfAdjointView<Derived, UpLo> Type; };
+ template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SparseSelfAdjointView<const Derived, UpLo> Type; };
+
+ template<unsigned int UpLo> inline
+ typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
+ template<unsigned int UpLo> inline
+ typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
+
+ template<typename OtherDerived> Scalar dot(const MatrixBase<OtherDerived>& other) const;
+ template<typename OtherDerived> Scalar dot(const SparseMatrixBase<OtherDerived>& other) const;
+ RealScalar squaredNorm() const;
+ RealScalar norm() const;
+ RealScalar blueNorm() const;
+
+ TransposeReturnType transpose() { return TransposeReturnType(derived()); }
+ const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); }
+ const AdjointReturnType adjoint() const { return AdjointReturnType(transpose()); }
+
+ DenseMatrixType toDense() const
+ {
+ return DenseMatrixType(derived());
+ }
+
+ template<typename OtherDerived>
+ bool isApprox(const SparseMatrixBase<OtherDerived>& other,
+ const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+ template<typename OtherDerived>
+ bool isApprox(const MatrixBase<OtherDerived>& other,
+ const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
+ { return toDense().isApprox(other,prec); }
+
+ /** \returns the matrix or vector obtained by evaluating this expression.
+ *
+ * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+ * a const reference, in order to avoid a useless copy.
+ */
+ inline const typename internal::eval<Derived>::type eval() const
+ { return typename internal::eval<Derived>::type(derived()); }
+
+ Scalar sum() const;
+
+ inline const SparseView<Derived>
+ pruned(const Scalar& reference = Scalar(0), const RealScalar& epsilon = NumTraits<Scalar>::dummy_precision()) const;
+
+ protected:
+
+ bool m_isRValue;
+
+ static inline StorageIndex convert_index(const Index idx) {
+ return internal::convert_index<StorageIndex>(idx);
+ }
+ private:
+ template<typename Dest> void evalTo(Dest &) const;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEMATRIXBASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
new file mode 100644
index 0000000..ef38357
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
@@ -0,0 +1,178 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_PERMUTATION_H
+#define EIGEN_SPARSE_PERMUTATION_H
+
+// This file implements sparse * permutation products
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename ExpressionType, int Side, bool Transposed>
+struct permutation_matrix_product<ExpressionType, Side, Transposed, SparseShape>
+{
+ typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
+ typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
+
+ typedef typename MatrixTypeCleaned::Scalar Scalar;
+ typedef typename MatrixTypeCleaned::StorageIndex StorageIndex;
+
+ enum {
+ SrcStorageOrder = MatrixTypeCleaned::Flags&RowMajorBit ? RowMajor : ColMajor,
+ MoveOuter = SrcStorageOrder==RowMajor ? Side==OnTheLeft : Side==OnTheRight
+ };
+
+ typedef typename internal::conditional<MoveOuter,
+ SparseMatrix<Scalar,SrcStorageOrder,StorageIndex>,
+ SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> >::type ReturnType;
+
+ template<typename Dest,typename PermutationType>
+ static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
+ {
+ MatrixType mat(xpr);
+ if(MoveOuter)
+ {
+ SparseMatrix<Scalar,SrcStorageOrder,StorageIndex> tmp(mat.rows(), mat.cols());
+ Matrix<StorageIndex,Dynamic,1> sizes(mat.outerSize());
+ for(Index j=0; j<mat.outerSize(); ++j)
+ {
+ Index jp = perm.indices().coeff(j);
+ sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = StorageIndex(mat.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).nonZeros());
+ }
+ tmp.reserve(sizes);
+ for(Index j=0; j<mat.outerSize(); ++j)
+ {
+ Index jp = perm.indices().coeff(j);
+ Index jsrc = ((Side==OnTheRight) ^ Transposed) ? jp : j;
+ Index jdst = ((Side==OnTheLeft) ^ Transposed) ? jp : j;
+ for(typename MatrixTypeCleaned::InnerIterator it(mat,jsrc); it; ++it)
+ tmp.insertByOuterInner(jdst,it.index()) = it.value();
+ }
+ dst = tmp;
+ }
+ else
+ {
+ SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> tmp(mat.rows(), mat.cols());
+ Matrix<StorageIndex,Dynamic,1> sizes(tmp.outerSize());
+ sizes.setZero();
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> perm_cpy;
+ if((Side==OnTheLeft) ^ Transposed)
+ perm_cpy = perm;
+ else
+ perm_cpy = perm.transpose();
+
+ for(Index j=0; j<mat.outerSize(); ++j)
+ for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
+ sizes[perm_cpy.indices().coeff(it.index())]++;
+ tmp.reserve(sizes);
+ for(Index j=0; j<mat.outerSize(); ++j)
+ for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
+ tmp.insertByOuterInner(perm_cpy.indices().coeff(it.index()),j) = it.value();
+ dst = tmp;
+ }
+ }
+};
+
+}
+
+namespace internal {
+
+template <int ProductTag> struct product_promote_storage_type<Sparse, PermutationStorage, ProductTag> { typedef Sparse ret; };
+template <int ProductTag> struct product_promote_storage_type<PermutationStorage, Sparse, ProductTag> { typedef Sparse ret; };
+
+// TODO, the following two overloads are only needed to define the right temporary type through
+// typename traits<permutation_sparse_matrix_product<Rhs,Lhs,OnTheRight,false> >::ReturnType
+// whereas it should be correctly handled by traits<Product<> >::PlainObject
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, PermutationShape, SparseShape>
+ : public evaluator<typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType>
+{
+ typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
+ typedef typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ enum {
+ Flags = Base::Flags | EvalBeforeNestingBit
+ };
+
+ explicit product_evaluator(const XprType& xpr)
+ : m_result(xpr.rows(), xpr.cols())
+ {
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ generic_product_impl<Lhs, Rhs, PermutationShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
+ }
+
+protected:
+ PlainObject m_result;
+};
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, SparseShape, PermutationShape >
+ : public evaluator<typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType>
+{
+ typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
+ typedef typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ enum {
+ Flags = Base::Flags | EvalBeforeNestingBit
+ };
+
+ explicit product_evaluator(const XprType& xpr)
+ : m_result(xpr.rows(), xpr.cols())
+ {
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ generic_product_impl<Lhs, Rhs, SparseShape, PermutationShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
+ }
+
+protected:
+ PlainObject m_result;
+};
+
+} // end namespace internal
+
+/** \returns the matrix with the permutation applied to the columns
+ */
+template<typename SparseDerived, typename PermDerived>
+inline const Product<SparseDerived, PermDerived, AliasFreeProduct>
+operator*(const SparseMatrixBase<SparseDerived>& matrix, const PermutationBase<PermDerived>& perm)
+{ return Product<SparseDerived, PermDerived, AliasFreeProduct>(matrix.derived(), perm.derived()); }
+
+/** \returns the matrix with the permutation applied to the rows
+ */
+template<typename SparseDerived, typename PermDerived>
+inline const Product<PermDerived, SparseDerived, AliasFreeProduct>
+operator*( const PermutationBase<PermDerived>& perm, const SparseMatrixBase<SparseDerived>& matrix)
+{ return Product<PermDerived, SparseDerived, AliasFreeProduct>(perm.derived(), matrix.derived()); }
+
+
+/** \returns the matrix with the inverse permutation applied to the columns.
+ */
+template<typename SparseDerived, typename PermutationType>
+inline const Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>
+operator*(const SparseMatrixBase<SparseDerived>& matrix, const InverseImpl<PermutationType, PermutationStorage>& tperm)
+{
+ return Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>(matrix.derived(), tperm.derived());
+}
+
+/** \returns the matrix with the inverse permutation applied to the rows.
+ */
+template<typename SparseDerived, typename PermutationType>
+inline const Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>
+operator*(const InverseImpl<PermutationType,PermutationStorage>& tperm, const SparseMatrixBase<SparseDerived>& matrix)
+{
+ return Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>(tperm.derived(), matrix.derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
new file mode 100644
index 0000000..af8a774
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
@@ -0,0 +1,181 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEPRODUCT_H
+#define EIGEN_SPARSEPRODUCT_H
+
+namespace Eigen {
+
+/** \returns an expression of the product of two sparse matrices.
+ * By default a conservative product preserving the symbolic non zeros is performed.
+ * The automatic pruning of the small values can be achieved by calling the pruned() function
+ * in which case a totally different product algorithm is employed:
+ * \code
+ * C = (A*B).pruned(); // suppress numerical zeros (exact)
+ * C = (A*B).pruned(ref);
+ * C = (A*B).pruned(ref,epsilon);
+ * \endcode
+ * where \c ref is a meaningful non zero reference value.
+ * */
+template<typename Derived>
+template<typename OtherDerived>
+inline const Product<Derived,OtherDerived,AliasFreeProduct>
+SparseMatrixBase<Derived>::operator*(const SparseMatrixBase<OtherDerived> &other) const
+{
+ return Product<Derived,OtherDerived,AliasFreeProduct>(derived(), other.derived());
+}
+
+namespace internal {
+
+// sparse * sparse
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{
+ template<typename Dest>
+ static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+ {
+ evalTo(dst, lhs, rhs, typename evaluator_traits<Dest>::Shape());
+ }
+
+ // dense += sparse * sparse
+ template<typename Dest,typename ActualLhs>
+ static void addTo(Dest& dst, const ActualLhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
+ {
+ typedef typename nested_eval<ActualLhs,Dynamic>::type LhsNested;
+ typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+ LhsNested lhsNested(lhs);
+ RhsNested rhsNested(rhs);
+ internal::sparse_sparse_to_dense_product_selector<typename remove_all<LhsNested>::type,
+ typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+ }
+
+ // dense -= sparse * sparse
+ template<typename Dest>
+ static void subTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
+ {
+ addTo(dst, -lhs, rhs);
+ }
+
+protected:
+
+ // sparse = sparse * sparse
+ template<typename Dest>
+ static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, SparseShape)
+ {
+ typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+ typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+ LhsNested lhsNested(lhs);
+ RhsNested rhsNested(rhs);
+ internal::conservative_sparse_sparse_product_selector<typename remove_all<LhsNested>::type,
+ typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+ }
+
+ // dense = sparse * sparse
+ template<typename Dest>
+ static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, DenseShape)
+ {
+ dst.setZero();
+ addTo(dst, lhs, rhs);
+ }
+};
+
+// sparse * sparse-triangular
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, SparseTriangularShape, ProductType>
+ : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{};
+
+// sparse-triangular * sparse
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, SparseShape, ProductType>
+ : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{};
+
+// dense = sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+ typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+ {
+ Index dstRows = src.rows();
+ Index dstCols = src.cols();
+ if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+ dst.resize(dstRows, dstCols);
+
+ generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+ }
+};
+
+// dense += sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::add_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+ typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+ {
+ generic_product_impl<Lhs, Rhs>::addTo(dst,src.lhs(),src.rhs());
+ }
+};
+
+// dense -= sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::sub_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+ typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+ {
+ generic_product_impl<Lhs, Rhs>::subTo(dst,src.lhs(),src.rhs());
+ }
+};
+
+template<typename Lhs, typename Rhs, int Options>
+struct unary_evaluator<SparseView<Product<Lhs, Rhs, Options> >, IteratorBased>
+ : public evaluator<typename Product<Lhs, Rhs, DefaultProduct>::PlainObject>
+{
+ typedef SparseView<Product<Lhs, Rhs, Options> > XprType;
+ typedef typename XprType::PlainObject PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ explicit unary_evaluator(const XprType& xpr)
+ : m_result(xpr.rows(), xpr.cols())
+ {
+ using std::abs;
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+ typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+ LhsNested lhsNested(xpr.nestedExpression().lhs());
+ RhsNested rhsNested(xpr.nestedExpression().rhs());
+
+ internal::sparse_sparse_product_with_pruning_selector<typename remove_all<LhsNested>::type,
+ typename remove_all<RhsNested>::type, PlainObject>::run(lhsNested,rhsNested,m_result,
+ abs(xpr.reference())*xpr.epsilon());
+ }
+
+protected:
+ PlainObject m_result;
+};
+
+} // end namespace internal
+
+// sparse matrix = sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename Lhs, typename Rhs>
+SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const Product<Lhs,Rhs,AliasFreeProduct>& src)
+{
+ // std::cout << "in Assignment : " << DstOptions << "\n";
+ SparseMatrix dst(src.rows(),src.cols());
+ internal::generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+ this->swap(dst);
+ return *this;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
new file mode 100644
index 0000000..4587749
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
@@ -0,0 +1,49 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEREDUX_H
+#define EIGEN_SPARSEREDUX_H
+
+namespace Eigen {
+
+template<typename Derived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::sum() const
+{
+ eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+ Scalar res(0);
+ internal::evaluator<Derived> thisEval(derived());
+ for (Index j=0; j<outerSize(); ++j)
+ for (typename internal::evaluator<Derived>::InnerIterator iter(thisEval,j); iter; ++iter)
+ res += iter.value();
+ return res;
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+typename internal::traits<SparseMatrix<_Scalar,_Options,_Index> >::Scalar
+SparseMatrix<_Scalar,_Options,_Index>::sum() const
+{
+ eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+ if(this->isCompressed())
+ return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+ else
+ return Base::sum();
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+typename internal::traits<SparseVector<_Scalar,_Options, _Index> >::Scalar
+SparseVector<_Scalar,_Options,_Index>::sum() const
+{
+ eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+ return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEREDUX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
new file mode 100644
index 0000000..748f87d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
@@ -0,0 +1,397 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_REF_H
+#define EIGEN_SPARSE_REF_H
+
+namespace Eigen {
+
+enum {
+ StandardCompressedFormat = 2 /**< used by Ref<SparseMatrix> to specify whether the input storage must be in standard compressed form */
+};
+
+namespace internal {
+
+template<typename Derived> class SparseRefBase;
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+ : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+ typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ enum {
+ Options = _Options,
+ Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
+ };
+
+ template<typename Derived> struct match {
+ enum {
+ StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
+ MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && StorageOrderMatch
+ };
+ typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+ };
+
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+ : public traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+{
+ enum {
+ Flags = (traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+ };
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+ : public traits<SparseVector<MatScalar,MatOptions,MatIndex> >
+{
+ typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ enum {
+ Options = _Options,
+ Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
+ };
+
+ template<typename Derived> struct match {
+ enum {
+ MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && Derived::IsVectorAtCompileTime
+ };
+ typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+ };
+
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+ : public traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+{
+ enum {
+ Flags = (traits<SparseVector<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+ };
+};
+
+template<typename Derived>
+struct traits<SparseRefBase<Derived> > : public traits<Derived> {};
+
+template<typename Derived> class SparseRefBase
+ : public SparseMapBase<Derived>
+{
+public:
+
+ typedef SparseMapBase<Derived> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(SparseRefBase)
+
+ SparseRefBase()
+ : Base(RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime, 0, 0, 0, 0, 0)
+ {}
+
+protected:
+
+ template<typename Expression>
+ void construct(Expression& expr)
+ {
+ if(expr.outerIndexPtr()==0)
+ ::new (static_cast<Base*>(this)) Base(expr.size(), expr.nonZeros(), expr.innerIndexPtr(), expr.valuePtr());
+ else
+ ::new (static_cast<Base*>(this)) Base(expr.rows(), expr.cols(), expr.nonZeros(), expr.outerIndexPtr(), expr.innerIndexPtr(), expr.valuePtr(), expr.innerNonZeroPtr());
+ }
+};
+
+} // namespace internal
+
+
+/**
+ * \ingroup SparseCore_Module
+ *
+ * \brief A sparse matrix expression referencing an existing sparse expression
+ *
+ * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
+ * \tparam Options specifies whether the a standard compressed format is required \c Options is \c #StandardCompressedFormat, or \c 0.
+ * The default is \c 0.
+ *
+ * \sa class Ref
+ */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType >
+ : public internal::SparseRefBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
+#else
+template<typename SparseMatrixType, int Options>
+class Ref<SparseMatrixType, Options>
+ : public SparseMapBase<Derived,WriteAccessors> // yes, that's weird to use Derived here, but that works!
+#endif
+{
+ typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ typedef internal::traits<Ref> Traits;
+ template<int OtherOptions>
+ inline Ref(const SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
+ template<int OtherOptions>
+ inline Ref(const MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
+ public:
+
+ typedef internal::SparseRefBase<Ref> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+
+ #ifndef EIGEN_PARSED_BY_DOXYGEN
+ template<int OtherOptions>
+ inline Ref(SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
+ {
+ EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+ eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+ Base::construct(expr.derived());
+ }
+
+ template<int OtherOptions>
+ inline Ref(MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
+ {
+ EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+ eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+ Base::construct(expr.derived());
+ }
+
+ template<typename Derived>
+ inline Ref(const SparseCompressedBase<Derived>& expr)
+ #else
+ /** Implicit constructor from any sparse expression (2D matrix or 1D vector) */
+ template<typename Derived>
+ inline Ref(SparseCompressedBase<Derived>& expr)
+ #endif
+ {
+ EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+ EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+ eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+ Base::construct(expr.const_cast_derived());
+ }
+};
+
+// this is the const ref version
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+ : public internal::SparseRefBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+ typedef SparseMatrix<MatScalar,MatOptions,MatIndex> TPlainObjectType;
+ typedef internal::traits<Ref> Traits;
+ public:
+
+ typedef internal::SparseRefBase<Ref> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+ template<typename Derived>
+ inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
+ {
+ construct(expr.derived(), typename Traits::template match<Derived>::type());
+ }
+
+ inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+ // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+ }
+
+ template<typename OtherRef>
+ inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+ construct(other.derived(), typename Traits::template match<OtherRef>::type());
+ }
+
+ ~Ref() {
+ if(m_hasCopy) {
+ TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+ obj->~TPlainObjectType();
+ }
+ }
+
+ protected:
+
+ template<typename Expression>
+ void construct(const Expression& expr,internal::true_type)
+ {
+ if((Options & int(StandardCompressedFormat)) && (!expr.isCompressed()))
+ {
+ TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+ ::new (obj) TPlainObjectType(expr);
+ m_hasCopy = true;
+ Base::construct(*obj);
+ }
+ else
+ {
+ Base::construct(expr);
+ }
+ }
+
+ template<typename Expression>
+ void construct(const Expression& expr, internal::false_type)
+ {
+ TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+ ::new (obj) TPlainObjectType(expr);
+ m_hasCopy = true;
+ Base::construct(*obj);
+ }
+
+ protected:
+ typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+ bool m_hasCopy;
+};
+
+
+
+/**
+ * \ingroup SparseCore_Module
+ *
+ * \brief A sparse vector expression referencing an existing sparse vector expression
+ *
+ * \tparam SparseVectorType the equivalent sparse vector type of the referenced data, it must be a template instance of class SparseVector.
+ *
+ * \sa class Ref
+ */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType >
+ : public internal::SparseRefBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
+#else
+template<typename SparseVectorType>
+class Ref<SparseVectorType>
+ : public SparseMapBase<Derived,WriteAccessors>
+#endif
+{
+ typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
+ typedef internal::traits<Ref> Traits;
+ template<int OtherOptions>
+ inline Ref(const SparseVector<MatScalar,OtherOptions,MatIndex>& expr);
+ public:
+
+ typedef internal::SparseRefBase<Ref> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+ #ifndef EIGEN_PARSED_BY_DOXYGEN
+ template<int OtherOptions>
+ inline Ref(SparseVector<MatScalar,OtherOptions,MatIndex>& expr)
+ {
+ EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseVector<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+ Base::construct(expr.derived());
+ }
+
+ template<typename Derived>
+ inline Ref(const SparseCompressedBase<Derived>& expr)
+ #else
+ /** Implicit constructor from any 1D sparse vector expression */
+ template<typename Derived>
+ inline Ref(SparseCompressedBase<Derived>& expr)
+ #endif
+ {
+ EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+ EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+ Base::construct(expr.const_cast_derived());
+ }
+};
+
+// this is the const ref version
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+ : public internal::SparseRefBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+ typedef SparseVector<MatScalar,MatOptions,MatIndex> TPlainObjectType;
+ typedef internal::traits<Ref> Traits;
+ public:
+
+ typedef internal::SparseRefBase<Ref> Base;
+ EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+ template<typename Derived>
+ inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
+ {
+ construct(expr.derived(), typename Traits::template match<Derived>::type());
+ }
+
+ inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+ // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+ }
+
+ template<typename OtherRef>
+ inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+ construct(other.derived(), typename Traits::template match<OtherRef>::type());
+ }
+
+ ~Ref() {
+ if(m_hasCopy) {
+ TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+ obj->~TPlainObjectType();
+ }
+ }
+
+ protected:
+
+ template<typename Expression>
+ void construct(const Expression& expr,internal::true_type)
+ {
+ Base::construct(expr);
+ }
+
+ template<typename Expression>
+ void construct(const Expression& expr, internal::false_type)
+ {
+ TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+ ::new (obj) TPlainObjectType(expr);
+ m_hasCopy = true;
+ Base::construct(*obj);
+ }
+
+ protected:
+ typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+ bool m_hasCopy;
+};
+
+namespace internal {
+
+// FIXME shall we introduce a general evaluatior_ref that we can specialize for any sparse object once, and thus remove this copy-pasta thing...
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+ : evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+ typedef evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+ typedef Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+ evaluator() : Base() {}
+ explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_REF_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
new file mode 100644
index 0000000..85b00e1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
@@ -0,0 +1,659 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_SELFADJOINTVIEW_H
+#define EIGEN_SPARSE_SELFADJOINTVIEW_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+ * \class SparseSelfAdjointView
+ *
+ * \brief Pseudo expression to manipulate a triangular sparse matrix as a selfadjoint matrix.
+ *
+ * \param MatrixType the type of the dense matrix storing the coefficients
+ * \param Mode can be either \c #Lower or \c #Upper
+ *
+ * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
+ * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
+ * and most of the time this is the only way that it is used.
+ *
+ * \sa SparseMatrixBase::selfadjointView()
+ */
+namespace internal {
+
+template<typename MatrixType, unsigned int Mode>
+struct traits<SparseSelfAdjointView<MatrixType,Mode> > : traits<MatrixType> {
+};
+
+template<int SrcMode,int DstMode,typename MatrixType,int DestOrder>
+void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+
+template<int Mode,typename MatrixType,int DestOrder>
+void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+
+}
+
+template<typename MatrixType, unsigned int _Mode> class SparseSelfAdjointView
+ : public EigenBase<SparseSelfAdjointView<MatrixType,_Mode> >
+{
+ public:
+
+ enum {
+ Mode = _Mode,
+ TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0),
+ RowsAtCompileTime = internal::traits<SparseSelfAdjointView>::RowsAtCompileTime,
+ ColsAtCompileTime = internal::traits<SparseSelfAdjointView>::ColsAtCompileTime
+ };
+
+ typedef EigenBase<SparseSelfAdjointView> Base;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+ typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+
+ explicit inline SparseSelfAdjointView(MatrixType& matrix) : m_matrix(matrix)
+ {
+ eigen_assert(rows()==cols() && "SelfAdjointView is only for squared matrices");
+ }
+
+ inline Index rows() const { return m_matrix.rows(); }
+ inline Index cols() const { return m_matrix.cols(); }
+
+ /** \internal \returns a reference to the nested matrix */
+ const _MatrixTypeNested& matrix() const { return m_matrix; }
+ typename internal::remove_reference<MatrixTypeNested>::type& matrix() { return m_matrix; }
+
+ /** \returns an expression of the matrix product between a sparse self-adjoint matrix \c *this and a sparse matrix \a rhs.
+ *
+ * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
+ * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
+ */
+ template<typename OtherDerived>
+ Product<SparseSelfAdjointView, OtherDerived>
+ operator*(const SparseMatrixBase<OtherDerived>& rhs) const
+ {
+ return Product<SparseSelfAdjointView, OtherDerived>(*this, rhs.derived());
+ }
+
+ /** \returns an expression of the matrix product between a sparse matrix \a lhs and a sparse self-adjoint matrix \a rhs.
+ *
+ * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
+ * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
+ */
+ template<typename OtherDerived> friend
+ Product<OtherDerived, SparseSelfAdjointView>
+ operator*(const SparseMatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
+ {
+ return Product<OtherDerived, SparseSelfAdjointView>(lhs.derived(), rhs);
+ }
+
+ /** Efficient sparse self-adjoint matrix times dense vector/matrix product */
+ template<typename OtherDerived>
+ Product<SparseSelfAdjointView,OtherDerived>
+ operator*(const MatrixBase<OtherDerived>& rhs) const
+ {
+ return Product<SparseSelfAdjointView,OtherDerived>(*this, rhs.derived());
+ }
+
+ /** Efficient dense vector/matrix times sparse self-adjoint matrix product */
+ template<typename OtherDerived> friend
+ Product<OtherDerived,SparseSelfAdjointView>
+ operator*(const MatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
+ {
+ return Product<OtherDerived,SparseSelfAdjointView>(lhs.derived(), rhs);
+ }
+
+ /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
+ * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
+ *
+ * \returns a reference to \c *this
+ *
+ * To perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
+ * call this function with u.adjoint().
+ */
+ template<typename DerivedU>
+ SparseSelfAdjointView& rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
+
+ /** \returns an expression of P H P^-1 */
+ // TODO implement twists in a more evaluator friendly fashion
+ SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
+ {
+ return SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode>(m_matrix, perm);
+ }
+
+ template<typename SrcMatrixType,int SrcMode>
+ SparseSelfAdjointView& operator=(const SparseSymmetricPermutationProduct<SrcMatrixType,SrcMode>& permutedMatrix)
+ {
+ internal::call_assignment_no_alias_no_transpose(*this, permutedMatrix);
+ return *this;
+ }
+
+ SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src)
+ {
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
+ return *this = src.twistedBy(pnull);
+ }
+
+ // Since we override the copy-assignment operator, we need to explicitly re-declare the copy-constructor
+ EIGEN_DEFAULT_COPY_CONSTRUCTOR(SparseSelfAdjointView)
+
+ template<typename SrcMatrixType,unsigned int SrcMode>
+ SparseSelfAdjointView& operator=(const SparseSelfAdjointView<SrcMatrixType,SrcMode>& src)
+ {
+ PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
+ return *this = src.twistedBy(pnull);
+ }
+
+ void resize(Index rows, Index cols)
+ {
+ EIGEN_ONLY_USED_FOR_DEBUG(rows);
+ EIGEN_ONLY_USED_FOR_DEBUG(cols);
+ eigen_assert(rows == this->rows() && cols == this->cols()
+ && "SparseSelfadjointView::resize() does not actually allow to resize.");
+ }
+
+ protected:
+
+ MatrixTypeNested m_matrix;
+ //mutable VectorI m_countPerRow;
+ //mutable VectorI m_countPerCol;
+ private:
+ template<typename Dest> void evalTo(Dest &) const;
+};
+
+/***************************************************************************
+* Implementation of SparseMatrixBase methods
+***************************************************************************/
+
+template<typename Derived>
+template<unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView() const
+{
+ return SparseSelfAdjointView<const Derived, UpLo>(derived());
+}
+
+template<typename Derived>
+template<unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView()
+{
+ return SparseSelfAdjointView<Derived, UpLo>(derived());
+}
+
+/***************************************************************************
+* Implementation of SparseSelfAdjointView methods
+***************************************************************************/
+
+template<typename MatrixType, unsigned int Mode>
+template<typename DerivedU>
+SparseSelfAdjointView<MatrixType,Mode>&
+SparseSelfAdjointView<MatrixType,Mode>::rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha)
+{
+ SparseMatrix<Scalar,(MatrixType::Flags&RowMajorBit)?RowMajor:ColMajor> tmp = u * u.adjoint();
+ if(alpha==Scalar(0))
+ m_matrix = tmp.template triangularView<Mode>();
+ else
+ m_matrix += alpha * tmp.template triangularView<Mode>();
+
+ return *this;
+}
+
+namespace internal {
+
+// TODO currently a selfadjoint expression has the form SelfAdjointView<.,.>
+// in the future selfadjoint-ness should be defined by the expression traits
+// such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
+template<typename MatrixType, unsigned int Mode>
+struct evaluator_traits<SparseSelfAdjointView<MatrixType,Mode> >
+{
+ typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
+ typedef SparseSelfAdjointShape Shape;
+};
+
+struct SparseSelfAdjoint2Sparse {};
+
+template<> struct AssignmentKind<SparseShape,SparseSelfAdjointShape> { typedef SparseSelfAdjoint2Sparse Kind; };
+template<> struct AssignmentKind<SparseSelfAdjointShape,SparseShape> { typedef Sparse2Sparse Kind; };
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, SparseSelfAdjoint2Sparse>
+{
+ typedef typename DstXprType::StorageIndex StorageIndex;
+ typedef internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> AssignOpType;
+
+ template<typename DestScalar,int StorageOrder>
+ static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignOpType&/*func*/)
+ {
+ internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), dst);
+ }
+
+ // FIXME: the handling of += and -= in sparse matrices should be cleanup so that next two overloads could be reduced to:
+ template<typename DestScalar,int StorageOrder,typename AssignFunc>
+ static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignFunc& func)
+ {
+ SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+ run(tmp, src, AssignOpType());
+ call_assignment_no_alias_no_transpose(dst, tmp, func);
+ }
+
+ template<typename DestScalar,int StorageOrder>
+ static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
+ const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
+ {
+ SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+ run(tmp, src, AssignOpType());
+ dst += tmp;
+ }
+
+ template<typename DestScalar,int StorageOrder>
+ static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
+ const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
+ {
+ SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+ run(tmp, src, AssignOpType());
+ dst -= tmp;
+ }
+
+ template<typename DestScalar>
+ static void run(DynamicSparseMatrix<DestScalar,ColMajor,StorageIndex>& dst, const SrcXprType &src, const AssignOpType&/*func*/)
+ {
+ // TODO directly evaluate into dst;
+ SparseMatrix<DestScalar,ColMajor,StorageIndex> tmp(dst.rows(),dst.cols());
+ internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), tmp);
+ dst = tmp;
+ }
+};
+
+} // end namespace internal
+
+/***************************************************************************
+* Implementation of sparse self-adjoint time dense matrix
+***************************************************************************/
+
+namespace internal {
+
+template<int Mode, typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+{
+ EIGEN_ONLY_USED_FOR_DEBUG(alpha);
+
+ typedef typename internal::nested_eval<SparseLhsType,DenseRhsType::MaxColsAtCompileTime>::type SparseLhsTypeNested;
+ typedef typename internal::remove_all<SparseLhsTypeNested>::type SparseLhsTypeNestedCleaned;
+ typedef evaluator<SparseLhsTypeNestedCleaned> LhsEval;
+ typedef typename LhsEval::InnerIterator LhsIterator;
+ typedef typename SparseLhsType::Scalar LhsScalar;
+
+ enum {
+ LhsIsRowMajor = (LhsEval::Flags&RowMajorBit)==RowMajorBit,
+ ProcessFirstHalf =
+ ((Mode&(Upper|Lower))==(Upper|Lower))
+ || ( (Mode&Upper) && !LhsIsRowMajor)
+ || ( (Mode&Lower) && LhsIsRowMajor),
+ ProcessSecondHalf = !ProcessFirstHalf
+ };
+
+ SparseLhsTypeNested lhs_nested(lhs);
+ LhsEval lhsEval(lhs_nested);
+
+ // work on one column at once
+ for (Index k=0; k<rhs.cols(); ++k)
+ {
+ for (Index j=0; j<lhs.outerSize(); ++j)
+ {
+ LhsIterator i(lhsEval,j);
+ // handle diagonal coeff
+ if (ProcessSecondHalf)
+ {
+ while (i && i.index()<j) ++i;
+ if(i && i.index()==j)
+ {
+ res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+ ++i;
+ }
+ }
+
+ // premultiplied rhs for scatters
+ typename ScalarBinaryOpTraits<AlphaType, typename DenseRhsType::Scalar>::ReturnType rhs_j(alpha*rhs(j,k));
+ // accumulator for partial scalar product
+ typename DenseResType::Scalar res_j(0);
+ for(; (ProcessFirstHalf ? i && i.index() < j : i) ; ++i)
+ {
+ LhsScalar lhs_ij = i.value();
+ if(!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij);
+ res_j += lhs_ij * rhs.coeff(i.index(),k);
+ res(i.index(),k) += numext::conj(lhs_ij) * rhs_j;
+ }
+ res.coeffRef(j,k) += alpha * res_j;
+
+ // handle diagonal coeff
+ if (ProcessFirstHalf && i && (i.index()==j))
+ res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+ }
+ }
+}
+
+
+template<typename LhsView, typename Rhs, int ProductType>
+struct generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType>
+: generic_product_impl_base<LhsView, Rhs, generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType> >
+{
+ template<typename Dest>
+ static void scaleAndAddTo(Dest& dst, const LhsView& lhsView, const Rhs& rhs, const typename Dest::Scalar& alpha)
+ {
+ typedef typename LhsView::_MatrixTypeNested Lhs;
+ typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+ typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+ LhsNested lhsNested(lhsView.matrix());
+ RhsNested rhsNested(rhs);
+
+ internal::sparse_selfadjoint_time_dense_product<LhsView::Mode>(lhsNested, rhsNested, dst, alpha);
+ }
+};
+
+template<typename Lhs, typename RhsView, int ProductType>
+struct generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType>
+: generic_product_impl_base<Lhs, RhsView, generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType> >
+{
+ template<typename Dest>
+ static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const RhsView& rhsView, const typename Dest::Scalar& alpha)
+ {
+ typedef typename RhsView::_MatrixTypeNested Rhs;
+ typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+ typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+ LhsNested lhsNested(lhs);
+ RhsNested rhsNested(rhsView.matrix());
+
+ // transpose everything
+ Transpose<Dest> dstT(dst);
+ internal::sparse_selfadjoint_time_dense_product<RhsView::TransposeMode>(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
+ }
+};
+
+// NOTE: these two overloads are needed to evaluate the sparse selfadjoint view into a full sparse matrix
+// TODO: maybe the copy could be handled by generic_product_impl so that these overloads would not be needed anymore
+
+template<typename LhsView, typename Rhs, int ProductTag>
+struct product_evaluator<Product<LhsView, Rhs, DefaultProduct>, ProductTag, SparseSelfAdjointShape, SparseShape>
+ : public evaluator<typename Product<typename Rhs::PlainObject, Rhs, DefaultProduct>::PlainObject>
+{
+ typedef Product<LhsView, Rhs, DefaultProduct> XprType;
+ typedef typename XprType::PlainObject PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ product_evaluator(const XprType& xpr)
+ : m_lhs(xpr.lhs()), m_result(xpr.rows(), xpr.cols())
+ {
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ generic_product_impl<typename Rhs::PlainObject, Rhs, SparseShape, SparseShape, ProductTag>::evalTo(m_result, m_lhs, xpr.rhs());
+ }
+
+protected:
+ typename Rhs::PlainObject m_lhs;
+ PlainObject m_result;
+};
+
+template<typename Lhs, typename RhsView, int ProductTag>
+struct product_evaluator<Product<Lhs, RhsView, DefaultProduct>, ProductTag, SparseShape, SparseSelfAdjointShape>
+ : public evaluator<typename Product<Lhs, typename Lhs::PlainObject, DefaultProduct>::PlainObject>
+{
+ typedef Product<Lhs, RhsView, DefaultProduct> XprType;
+ typedef typename XprType::PlainObject PlainObject;
+ typedef evaluator<PlainObject> Base;
+
+ product_evaluator(const XprType& xpr)
+ : m_rhs(xpr.rhs()), m_result(xpr.rows(), xpr.cols())
+ {
+ ::new (static_cast<Base*>(this)) Base(m_result);
+ generic_product_impl<Lhs, typename Lhs::PlainObject, SparseShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), m_rhs);
+ }
+
+protected:
+ typename Lhs::PlainObject m_rhs;
+ PlainObject m_result;
+};
+
+} // namespace internal
+
+/***************************************************************************
+* Implementation of symmetric copies and permutations
+***************************************************************************/
+namespace internal {
+
+template<int Mode,typename MatrixType,int DestOrder>
+void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
+{
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef SparseMatrix<Scalar,DestOrder,StorageIndex> Dest;
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ typedef evaluator<MatrixType> MatEval;
+ typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
+
+ MatEval matEval(mat);
+ Dest& dest(_dest.derived());
+ enum {
+ StorageOrderMatch = int(Dest::IsRowMajor) == int(MatrixType::IsRowMajor)
+ };
+
+ Index size = mat.rows();
+ VectorI count;
+ count.resize(size);
+ count.setZero();
+ dest.resize(size,size);
+ for(Index j = 0; j<size; ++j)
+ {
+ Index jp = perm ? perm[j] : j;
+ for(MatIterator it(matEval,j); it; ++it)
+ {
+ Index i = it.index();
+ Index r = it.row();
+ Index c = it.col();
+ Index ip = perm ? perm[i] : i;
+ if(Mode==int(Upper|Lower))
+ count[StorageOrderMatch ? jp : ip]++;
+ else if(r==c)
+ count[ip]++;
+ else if(( Mode==Lower && r>c) || ( Mode==Upper && r<c))
+ {
+ count[ip]++;
+ count[jp]++;
+ }
+ }
+ }
+ Index nnz = count.sum();
+
+ // reserve space
+ dest.resizeNonZeros(nnz);
+ dest.outerIndexPtr()[0] = 0;
+ for(Index j=0; j<size; ++j)
+ dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
+ for(Index j=0; j<size; ++j)
+ count[j] = dest.outerIndexPtr()[j];
+
+ // copy data
+ for(StorageIndex j = 0; j<size; ++j)
+ {
+ for(MatIterator it(matEval,j); it; ++it)
+ {
+ StorageIndex i = internal::convert_index<StorageIndex>(it.index());
+ Index r = it.row();
+ Index c = it.col();
+
+ StorageIndex jp = perm ? perm[j] : j;
+ StorageIndex ip = perm ? perm[i] : i;
+
+ if(Mode==int(Upper|Lower))
+ {
+ Index k = count[StorageOrderMatch ? jp : ip]++;
+ dest.innerIndexPtr()[k] = StorageOrderMatch ? ip : jp;
+ dest.valuePtr()[k] = it.value();
+ }
+ else if(r==c)
+ {
+ Index k = count[ip]++;
+ dest.innerIndexPtr()[k] = ip;
+ dest.valuePtr()[k] = it.value();
+ }
+ else if(( (Mode&Lower)==Lower && r>c) || ( (Mode&Upper)==Upper && r<c))
+ {
+ if(!StorageOrderMatch)
+ std::swap(ip,jp);
+ Index k = count[jp]++;
+ dest.innerIndexPtr()[k] = ip;
+ dest.valuePtr()[k] = it.value();
+ k = count[ip]++;
+ dest.innerIndexPtr()[k] = jp;
+ dest.valuePtr()[k] = numext::conj(it.value());
+ }
+ }
+ }
+}
+
+template<int _SrcMode,int _DstMode,typename MatrixType,int DstOrder>
+void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DstOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
+{
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef typename MatrixType::Scalar Scalar;
+ SparseMatrix<Scalar,DstOrder,StorageIndex>& dest(_dest.derived());
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ typedef evaluator<MatrixType> MatEval;
+ typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
+
+ enum {
+ SrcOrder = MatrixType::IsRowMajor ? RowMajor : ColMajor,
+ StorageOrderMatch = int(SrcOrder) == int(DstOrder),
+ DstMode = DstOrder==RowMajor ? (_DstMode==Upper ? Lower : Upper) : _DstMode,
+ SrcMode = SrcOrder==RowMajor ? (_SrcMode==Upper ? Lower : Upper) : _SrcMode
+ };
+
+ MatEval matEval(mat);
+
+ Index size = mat.rows();
+ VectorI count(size);
+ count.setZero();
+ dest.resize(size,size);
+ for(StorageIndex j = 0; j<size; ++j)
+ {
+ StorageIndex jp = perm ? perm[j] : j;
+ for(MatIterator it(matEval,j); it; ++it)
+ {
+ StorageIndex i = it.index();
+ if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
+ continue;
+
+ StorageIndex ip = perm ? perm[i] : i;
+ count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
+ }
+ }
+ dest.outerIndexPtr()[0] = 0;
+ for(Index j=0; j<size; ++j)
+ dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
+ dest.resizeNonZeros(dest.outerIndexPtr()[size]);
+ for(Index j=0; j<size; ++j)
+ count[j] = dest.outerIndexPtr()[j];
+
+ for(StorageIndex j = 0; j<size; ++j)
+ {
+
+ for(MatIterator it(matEval,j); it; ++it)
+ {
+ StorageIndex i = it.index();
+ if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
+ continue;
+
+ StorageIndex jp = perm ? perm[j] : j;
+ StorageIndex ip = perm? perm[i] : i;
+
+ Index k = count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
+ dest.innerIndexPtr()[k] = int(DstMode)==int(Lower) ? (std::max)(ip,jp) : (std::min)(ip,jp);
+
+ if(!StorageOrderMatch) std::swap(ip,jp);
+ if( ((int(DstMode)==int(Lower) && ip<jp) || (int(DstMode)==int(Upper) && ip>jp)))
+ dest.valuePtr()[k] = numext::conj(it.value());
+ else
+ dest.valuePtr()[k] = it.value();
+ }
+ }
+}
+
+}
+
+// TODO implement twists in a more evaluator friendly fashion
+
+namespace internal {
+
+template<typename MatrixType, int Mode>
+struct traits<SparseSymmetricPermutationProduct<MatrixType,Mode> > : traits<MatrixType> {
+};
+
+}
+
+template<typename MatrixType,int Mode>
+class SparseSymmetricPermutationProduct
+ : public EigenBase<SparseSymmetricPermutationProduct<MatrixType,Mode> >
+{
+ public:
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ enum {
+ RowsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::RowsAtCompileTime,
+ ColsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::ColsAtCompileTime
+ };
+ protected:
+ typedef PermutationMatrix<Dynamic,Dynamic,StorageIndex> Perm;
+ public:
+ typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+ typedef typename MatrixType::Nested MatrixTypeNested;
+ typedef typename internal::remove_all<MatrixTypeNested>::type NestedExpression;
+
+ SparseSymmetricPermutationProduct(const MatrixType& mat, const Perm& perm)
+ : m_matrix(mat), m_perm(perm)
+ {}
+
+ inline Index rows() const { return m_matrix.rows(); }
+ inline Index cols() const { return m_matrix.cols(); }
+
+ const NestedExpression& matrix() const { return m_matrix; }
+ const Perm& perm() const { return m_perm; }
+
+ protected:
+ MatrixTypeNested m_matrix;
+ const Perm& m_perm;
+
+};
+
+namespace internal {
+
+template<typename DstXprType, typename MatrixType, int Mode, typename Scalar>
+struct Assignment<DstXprType, SparseSymmetricPermutationProduct<MatrixType,Mode>, internal::assign_op<Scalar,typename MatrixType::Scalar>, Sparse2Sparse>
+{
+ typedef SparseSymmetricPermutationProduct<MatrixType,Mode> SrcXprType;
+ typedef typename DstXprType::StorageIndex DstIndex;
+ template<int Options>
+ static void run(SparseMatrix<Scalar,Options,DstIndex> &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
+ {
+ // internal::permute_symm_to_fullsymm<Mode>(m_matrix,_dest,m_perm.indices().data());
+ SparseMatrix<Scalar,(Options&RowMajor)==RowMajor ? ColMajor : RowMajor, DstIndex> tmp;
+ internal::permute_symm_to_fullsymm<Mode>(src.matrix(),tmp,src.perm().indices().data());
+ dst = tmp;
+ }
+
+ template<typename DestType,unsigned int DestMode>
+ static void run(SparseSelfAdjointView<DestType,DestMode>& dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
+ {
+ internal::permute_symm_to_symm<Mode,DestMode>(src.matrix(),dst.matrix(),src.perm().indices().data());
+ }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
new file mode 100644
index 0000000..b4c9a42
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
@@ -0,0 +1,124 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSESOLVERBASE_H
+#define EIGEN_SPARSESOLVERBASE_H
+
+namespace Eigen {
+
+namespace internal {
+
+ /** \internal
+ * Helper functions to solve with a sparse right-hand-side and result.
+ * The rhs is decomposed into small vertical panels which are solved through dense temporaries.
+ */
+template<typename Decomposition, typename Rhs, typename Dest>
+typename enable_if<Rhs::ColsAtCompileTime!=1 && Dest::ColsAtCompileTime!=1>::type
+solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
+{
+ EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+ typedef typename Dest::Scalar DestScalar;
+ // we process the sparse rhs per block of NbColsAtOnce columns temporarily stored into a dense matrix.
+ static const Index NbColsAtOnce = 4;
+ Index rhsCols = rhs.cols();
+ Index size = rhs.rows();
+ // the temporary matrices do not need more columns than NbColsAtOnce:
+ Index tmpCols = (std::min)(rhsCols, NbColsAtOnce);
+ Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmp(size,tmpCols);
+ Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmpX(size,tmpCols);
+ for(Index k=0; k<rhsCols; k+=NbColsAtOnce)
+ {
+ Index actualCols = std::min<Index>(rhsCols-k, NbColsAtOnce);
+ tmp.leftCols(actualCols) = rhs.middleCols(k,actualCols);
+ tmpX.leftCols(actualCols) = dec.solve(tmp.leftCols(actualCols));
+ dest.middleCols(k,actualCols) = tmpX.leftCols(actualCols).sparseView();
+ }
+}
+
+// Overload for vector as rhs
+template<typename Decomposition, typename Rhs, typename Dest>
+typename enable_if<Rhs::ColsAtCompileTime==1 || Dest::ColsAtCompileTime==1>::type
+solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
+{
+ typedef typename Dest::Scalar DestScalar;
+ Index size = rhs.rows();
+ Eigen::Matrix<DestScalar,Dynamic,1> rhs_dense(rhs);
+ Eigen::Matrix<DestScalar,Dynamic,1> dest_dense(size);
+ dest_dense = dec.solve(rhs_dense);
+ dest = dest_dense.sparseView();
+}
+
+} // end namespace internal
+
+/** \class SparseSolverBase
+ * \ingroup SparseCore_Module
+ * \brief A base class for sparse solvers
+ *
+ * \tparam Derived the actual type of the solver.
+ *
+ */
+template<typename Derived>
+class SparseSolverBase : internal::noncopyable
+{
+ public:
+
+ /** Default constructor */
+ SparseSolverBase()
+ : m_isInitialized(false)
+ {}
+
+ ~SparseSolverBase()
+ {}
+
+ Derived& derived() { return *static_cast<Derived*>(this); }
+ const Derived& derived() const { return *static_cast<const Derived*>(this); }
+
+ /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+ *
+ * \sa compute()
+ */
+ template<typename Rhs>
+ inline const Solve<Derived, Rhs>
+ solve(const MatrixBase<Rhs>& b) const
+ {
+ eigen_assert(m_isInitialized && "Solver is not initialized.");
+ eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+ return Solve<Derived, Rhs>(derived(), b.derived());
+ }
+
+ /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+ *
+ * \sa compute()
+ */
+ template<typename Rhs>
+ inline const Solve<Derived, Rhs>
+ solve(const SparseMatrixBase<Rhs>& b) const
+ {
+ eigen_assert(m_isInitialized && "Solver is not initialized.");
+ eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+ return Solve<Derived, Rhs>(derived(), b.derived());
+ }
+
+ #ifndef EIGEN_PARSED_BY_DOXYGEN
+ /** \internal default implementation of solving with a sparse rhs */
+ template<typename Rhs,typename Dest>
+ void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+ {
+ internal::solve_sparse_through_dense_panels(derived(), b.derived(), dest.derived());
+ }
+ #endif // EIGEN_PARSED_BY_DOXYGEN
+
+ protected:
+
+ mutable bool m_isInitialized;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSESOLVERBASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
new file mode 100644
index 0000000..88820a4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSESPARSEPRODUCTWITHPRUNING_H
+#define EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
+
+namespace Eigen {
+
+namespace internal {
+
+
+// perform a pseudo in-place sparse * sparse product assuming all matrices are col major
+template<typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, const typename ResultType::RealScalar& tolerance)
+{
+ // return sparse_sparse_product_with_pruning_impl2(lhs,rhs,res);
+
+ typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+ typedef typename remove_all<ResultType>::type::Scalar ResScalar;
+ typedef typename remove_all<Lhs>::type::StorageIndex StorageIndex;
+
+ // make sure to call innerSize/outerSize since we fake the storage order.
+ Index rows = lhs.innerSize();
+ Index cols = rhs.outerSize();
+ //Index size = lhs.outerSize();
+ eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+ // allocate a temporary buffer
+ AmbiVector<ResScalar,StorageIndex> tempVector(rows);
+
+ // mimics a resizeByInnerOuter:
+ if(ResultType::IsRowMajor)
+ res.resize(cols, rows);
+ else
+ res.resize(rows, cols);
+
+ evaluator<Lhs> lhsEval(lhs);
+ evaluator<Rhs> rhsEval(rhs);
+
+ // estimate the number of non zero entries
+ // given a rhs column containing Y non zeros, we assume that the respective Y columns
+ // of the lhs differs in average of one non zeros, thus the number of non zeros for
+ // the product of a rhs column with the lhs is X+Y where X is the average number of non zero
+ // per column of the lhs.
+ // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs)
+ Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate();
+
+ res.reserve(estimated_nnz_prod);
+ double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols()));
+ for (Index j=0; j<cols; ++j)
+ {
+ // FIXME:
+ //double ratioColRes = (double(rhs.innerVector(j).nonZeros()) + double(lhs.nonZeros())/double(lhs.cols()))/double(lhs.rows());
+ // let's do a more accurate determination of the nnz ratio for the current column j of res
+ tempVector.init(ratioColRes);
+ tempVector.setZero();
+ for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+ {
+ // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index())
+ tempVector.restart();
+ RhsScalar x = rhsIt.value();
+ for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt)
+ {
+ tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x;
+ }
+ }
+ res.startVec(j);
+ for (typename AmbiVector<ResScalar,StorageIndex>::Iterator it(tempVector,tolerance); it; ++it)
+ res.insertBackByOuterInner(j,it.index()) = it.value();
+ }
+ res.finalize();
+}
+
+template<typename Lhs, typename Rhs, typename ResultType,
+ int LhsStorageOrder = traits<Lhs>::Flags&RowMajorBit,
+ int RhsStorageOrder = traits<Rhs>::Flags&RowMajorBit,
+ int ResStorageOrder = traits<ResultType>::Flags&RowMajorBit>
+struct sparse_sparse_product_with_pruning_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typename remove_all<ResultType>::type _res(res.rows(), res.cols());
+ internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,ResultType>(lhs, rhs, _res, tolerance);
+ res.swap(_res);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ // we need a col-major matrix to hold the result
+ typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> SparseTemporaryType;
+ SparseTemporaryType _res(res.rows(), res.cols());
+ internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,SparseTemporaryType>(lhs, rhs, _res, tolerance);
+ res = _res;
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ // let's transpose the product to get a column x column product
+ typename remove_all<ResultType>::type _res(res.rows(), res.cols());
+ internal::sparse_sparse_product_with_pruning_impl<Rhs,Lhs,ResultType>(rhs, lhs, _res, tolerance);
+ res.swap(_res);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+ typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+ ColMajorMatrixLhs colLhs(lhs);
+ ColMajorMatrixRhs colRhs(rhs);
+ internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,ColMajorMatrixRhs,ResultType>(colLhs, colRhs, res, tolerance);
+
+ // let's transpose the product to get a column x column product
+// typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
+// SparseTemporaryType _res(res.cols(), res.rows());
+// sparse_sparse_product_with_pruning_impl<Rhs,Lhs,SparseTemporaryType>(rhs, lhs, _res);
+// res = _res.transpose();
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixLhs;
+ RowMajorMatrixLhs rowLhs(lhs);
+ sparse_sparse_product_with_pruning_selector<RowMajorMatrixLhs,Rhs,ResultType,RowMajor,RowMajor>(rowLhs,rhs,res,tolerance);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixRhs;
+ RowMajorMatrixRhs rowRhs(rhs);
+ sparse_sparse_product_with_pruning_selector<Lhs,RowMajorMatrixRhs,ResultType,RowMajor,RowMajor,RowMajor>(lhs,rowRhs,res,tolerance);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+ ColMajorMatrixRhs colRhs(rhs);
+ internal::sparse_sparse_product_with_pruning_impl<Lhs,ColMajorMatrixRhs,ResultType>(lhs, colRhs, res, tolerance);
+ }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
+{
+ typedef typename ResultType::RealScalar RealScalar;
+ static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+ {
+ typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+ ColMajorMatrixLhs colLhs(lhs);
+ internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,Rhs,ResultType>(colLhs, rhs, res, tolerance);
+ }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
new file mode 100644
index 0000000..3757d4c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
@@ -0,0 +1,92 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSETRANSPOSE_H
+#define EIGEN_SPARSETRANSPOSE_H
+
+namespace Eigen {
+
+namespace internal {
+ template<typename MatrixType,int CompressedAccess=int(MatrixType::Flags&CompressedAccessBit)>
+ class SparseTransposeImpl
+ : public SparseMatrixBase<Transpose<MatrixType> >
+ {};
+
+ template<typename MatrixType>
+ class SparseTransposeImpl<MatrixType,CompressedAccessBit>
+ : public SparseCompressedBase<Transpose<MatrixType> >
+ {
+ typedef SparseCompressedBase<Transpose<MatrixType> > Base;
+ public:
+ using Base::derived;
+ typedef typename Base::Scalar Scalar;
+ typedef typename Base::StorageIndex StorageIndex;
+
+ inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
+
+ inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); }
+ inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); }
+ inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); }
+ inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); }
+
+ inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); }
+ inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); }
+ inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); }
+ inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); }
+ };
+}
+
+template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>
+ : public internal::SparseTransposeImpl<MatrixType>
+{
+ protected:
+ typedef internal::SparseTransposeImpl<MatrixType> Base;
+};
+
+namespace internal {
+
+template<typename ArgType>
+struct unary_evaluator<Transpose<ArgType>, IteratorBased>
+ : public evaluator_base<Transpose<ArgType> >
+{
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+ public:
+ typedef Transpose<ArgType> XprType;
+
+ inline Index nonZerosEstimate() const {
+ return m_argImpl.nonZerosEstimate();
+ }
+
+ class InnerIterator : public EvalIterator
+ {
+ public:
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+ : EvalIterator(unaryOp.m_argImpl,outer)
+ {}
+
+ Index row() const { return EvalIterator::col(); }
+ Index col() const { return EvalIterator::row(); }
+ };
+
+ enum {
+ CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {}
+
+ protected:
+ evaluator<ArgType> m_argImpl;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSETRANSPOSE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
new file mode 100644
index 0000000..9ac1202
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
@@ -0,0 +1,189 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_SPARSE_TRIANGULARVIEW_H
+#define EIGEN_SPARSE_TRIANGULARVIEW_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+ *
+ * \brief Base class for a triangular part in a \b sparse matrix
+ *
+ * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
+ * It extends class TriangularView with additional methods which are available for sparse expressions only.
+ *
+ * \sa class TriangularView, SparseMatrixBase::triangularView()
+ */
+template<typename MatrixType, unsigned int Mode> class TriangularViewImpl<MatrixType,Mode,Sparse>
+ : public SparseMatrixBase<TriangularView<MatrixType,Mode> >
+{
+ enum { SkipFirst = ((Mode&Lower) && !(MatrixType::Flags&RowMajorBit))
+ || ((Mode&Upper) && (MatrixType::Flags&RowMajorBit)),
+ SkipLast = !SkipFirst,
+ SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
+ HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
+ };
+
+ typedef TriangularView<MatrixType,Mode> TriangularViewType;
+
+ protected:
+ // dummy solve function to make TriangularView happy.
+ void solve() const;
+
+ typedef SparseMatrixBase<TriangularViewType> Base;
+ public:
+
+ EIGEN_SPARSE_PUBLIC_INTERFACE(TriangularViewType)
+
+ typedef typename MatrixType::Nested MatrixTypeNested;
+ typedef typename internal::remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
+ typedef typename internal::remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+
+ template<typename RhsType, typename DstType>
+ EIGEN_DEVICE_FUNC
+ EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
+ if(!(internal::is_same<RhsType,DstType>::value && internal::extract_data(dst) == internal::extract_data(rhs)))
+ dst = rhs;
+ this->solveInPlace(dst);
+ }
+
+ /** Applies the inverse of \c *this to the dense vector or matrix \a other, "in-place" */
+ template<typename OtherDerived> void solveInPlace(MatrixBase<OtherDerived>& other) const;
+
+ /** Applies the inverse of \c *this to the sparse vector or matrix \a other, "in-place" */
+ template<typename OtherDerived> void solveInPlace(SparseMatrixBase<OtherDerived>& other) const;
+
+};
+
+namespace internal {
+
+template<typename ArgType, unsigned int Mode>
+struct unary_evaluator<TriangularView<ArgType,Mode>, IteratorBased>
+ : evaluator_base<TriangularView<ArgType,Mode> >
+{
+ typedef TriangularView<ArgType,Mode> XprType;
+
+protected:
+
+ typedef typename XprType::Scalar Scalar;
+ typedef typename XprType::StorageIndex StorageIndex;
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+ enum { SkipFirst = ((Mode&Lower) && !(ArgType::Flags&RowMajorBit))
+ || ((Mode&Upper) && (ArgType::Flags&RowMajorBit)),
+ SkipLast = !SkipFirst,
+ SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
+ HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
+ };
+
+public:
+
+ enum {
+ CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType &xpr) : m_argImpl(xpr.nestedExpression()), m_arg(xpr.nestedExpression()) {}
+
+ inline Index nonZerosEstimate() const {
+ return m_argImpl.nonZerosEstimate();
+ }
+
+ class InnerIterator : public EvalIterator
+ {
+ typedef EvalIterator Base;
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& xprEval, Index outer)
+ : Base(xprEval.m_argImpl,outer), m_returnOne(false), m_containsDiag(Base::outer()<xprEval.m_arg.innerSize())
+ {
+ if(SkipFirst)
+ {
+ while((*this) && ((HasUnitDiag||SkipDiag) ? this->index()<=outer : this->index()<outer))
+ Base::operator++();
+ if(HasUnitDiag)
+ m_returnOne = m_containsDiag;
+ }
+ else if(HasUnitDiag && ((!Base::operator bool()) || Base::index()>=Base::outer()))
+ {
+ if((!SkipFirst) && Base::operator bool())
+ Base::operator++();
+ m_returnOne = m_containsDiag;
+ }
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ if(HasUnitDiag && m_returnOne)
+ m_returnOne = false;
+ else
+ {
+ Base::operator++();
+ if(HasUnitDiag && (!SkipFirst) && ((!Base::operator bool()) || Base::index()>=Base::outer()))
+ {
+ if((!SkipFirst) && Base::operator bool())
+ Base::operator++();
+ m_returnOne = m_containsDiag;
+ }
+ }
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE operator bool() const
+ {
+ if(HasUnitDiag && m_returnOne)
+ return true;
+ if(SkipFirst) return Base::operator bool();
+ else
+ {
+ if (SkipDiag) return (Base::operator bool() && this->index() < this->outer());
+ else return (Base::operator bool() && this->index() <= this->outer());
+ }
+ }
+
+// inline Index row() const { return (ArgType::Flags&RowMajorBit ? Base::outer() : this->index()); }
+// inline Index col() const { return (ArgType::Flags&RowMajorBit ? this->index() : Base::outer()); }
+ inline StorageIndex index() const
+ {
+ if(HasUnitDiag && m_returnOne) return internal::convert_index<StorageIndex>(Base::outer());
+ else return Base::index();
+ }
+ inline Scalar value() const
+ {
+ if(HasUnitDiag && m_returnOne) return Scalar(1);
+ else return Base::value();
+ }
+
+ protected:
+ bool m_returnOne;
+ bool m_containsDiag;
+ private:
+ Scalar& valueRef();
+ };
+
+protected:
+ evaluator<ArgType> m_argImpl;
+ const ArgType& m_arg;
+};
+
+} // end namespace internal
+
+template<typename Derived>
+template<int Mode>
+inline const TriangularView<const Derived, Mode>
+SparseMatrixBase<Derived>::triangularView() const
+{
+ return TriangularView<const Derived, Mode>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_TRIANGULARVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
new file mode 100644
index 0000000..ceb9368
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
@@ -0,0 +1,186 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEUTIL_H
+#define EIGEN_SPARSEUTIL_H
+
+namespace Eigen {
+
+#ifdef NDEBUG
+#define EIGEN_DBG_SPARSE(X)
+#else
+#define EIGEN_DBG_SPARSE(X) X
+#endif
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename OtherDerived> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase<OtherDerived>& other) \
+{ \
+ return Base::operator Op(other.derived()); \
+} \
+EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \
+{ \
+ return Base::operator Op(other); \
+}
+
+#define EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename Other> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \
+{ \
+ return Base::operator Op(scalar); \
+}
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =)
+
+
+#define EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) \
+ EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)
+
+
+const int CoherentAccessPattern = 0x1;
+const int InnerRandomAccessPattern = 0x2 | CoherentAccessPattern;
+const int OuterRandomAccessPattern = 0x4 | CoherentAccessPattern;
+const int RandomAccessPattern = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern;
+
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int> class SparseMatrix;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int> class DynamicSparseMatrix;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int> class SparseVector;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int> class MappedSparseMatrix;
+
+template<typename MatrixType, unsigned int UpLo> class SparseSelfAdjointView;
+template<typename Lhs, typename Rhs> class SparseDiagonalProduct;
+template<typename MatrixType> class SparseView;
+
+template<typename Lhs, typename Rhs> class SparseSparseProduct;
+template<typename Lhs, typename Rhs> class SparseTimeDenseProduct;
+template<typename Lhs, typename Rhs> class DenseTimeSparseProduct;
+template<typename Lhs, typename Rhs, bool Transpose> class SparseDenseOuterProduct;
+
+template<typename Lhs, typename Rhs> struct SparseSparseProductReturnType;
+template<typename Lhs, typename Rhs,
+ int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct DenseSparseProductReturnType;
+
+template<typename Lhs, typename Rhs,
+ int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct SparseDenseProductReturnType;
+template<typename MatrixType,int UpLo> class SparseSymmetricPermutationProduct;
+
+namespace internal {
+
+template<typename T,int Rows,int Cols,int Flags> struct sparse_eval;
+
+template<typename T> struct eval<T,Sparse>
+ : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime,traits<T>::Flags>
+{};
+
+template<typename T,int Cols,int Flags> struct sparse_eval<T,1,Cols,Flags> {
+ typedef typename traits<T>::Scalar _Scalar;
+ typedef typename traits<T>::StorageIndex _StorageIndex;
+ public:
+ typedef SparseVector<_Scalar, RowMajor, _StorageIndex> type;
+};
+
+template<typename T,int Rows,int Flags> struct sparse_eval<T,Rows,1,Flags> {
+ typedef typename traits<T>::Scalar _Scalar;
+ typedef typename traits<T>::StorageIndex _StorageIndex;
+ public:
+ typedef SparseVector<_Scalar, ColMajor, _StorageIndex> type;
+};
+
+// TODO this seems almost identical to plain_matrix_type<T, Sparse>
+template<typename T,int Rows,int Cols,int Flags> struct sparse_eval {
+ typedef typename traits<T>::Scalar _Scalar;
+ typedef typename traits<T>::StorageIndex _StorageIndex;
+ enum { _Options = ((Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
+ public:
+ typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+};
+
+template<typename T,int Flags> struct sparse_eval<T,1,1,Flags> {
+ typedef typename traits<T>::Scalar _Scalar;
+ public:
+ typedef Matrix<_Scalar, 1, 1> type;
+};
+
+template<typename T> struct plain_matrix_type<T,Sparse>
+{
+ typedef typename traits<T>::Scalar _Scalar;
+ typedef typename traits<T>::StorageIndex _StorageIndex;
+ enum { _Options = ((evaluator<T>::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
+ public:
+ typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+};
+
+template<typename T>
+struct plain_object_eval<T,Sparse>
+ : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime, evaluator<T>::Flags>
+{};
+
+template<typename Decomposition, typename RhsType>
+struct solve_traits<Decomposition,RhsType,Sparse>
+{
+ typedef typename sparse_eval<RhsType, RhsType::RowsAtCompileTime, RhsType::ColsAtCompileTime,traits<RhsType>::Flags>::type PlainObject;
+};
+
+template<typename Derived>
+struct generic_xpr_base<Derived, MatrixXpr, Sparse>
+{
+ typedef SparseMatrixBase<Derived> type;
+};
+
+struct SparseTriangularShape { static std::string debugName() { return "SparseTriangularShape"; } };
+struct SparseSelfAdjointShape { static std::string debugName() { return "SparseSelfAdjointShape"; } };
+
+template<> struct glue_shapes<SparseShape,SelfAdjointShape> { typedef SparseSelfAdjointShape type; };
+template<> struct glue_shapes<SparseShape,TriangularShape > { typedef SparseTriangularShape type; };
+
+// return type of SparseCompressedBase::lower_bound;
+struct LowerBoundIndex {
+ LowerBoundIndex() : value(-1), found(false) {}
+ LowerBoundIndex(Index val, bool ok) : value(val), found(ok) {}
+ Index value;
+ bool found;
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+ *
+ * \class Triplet
+ *
+ * \brief A small structure to hold a non zero as a triplet (i,j,value).
+ *
+ * \sa SparseMatrix::setFromTriplets()
+ */
+template<typename Scalar, typename StorageIndex=typename SparseMatrix<Scalar>::StorageIndex >
+class Triplet
+{
+public:
+ Triplet() : m_row(0), m_col(0), m_value(0) {}
+
+ Triplet(const StorageIndex& i, const StorageIndex& j, const Scalar& v = Scalar(0))
+ : m_row(i), m_col(j), m_value(v)
+ {}
+
+ /** \returns the row index of the element */
+ const StorageIndex& row() const { return m_row; }
+
+ /** \returns the column index of the element */
+ const StorageIndex& col() const { return m_col; }
+
+ /** \returns the value of the element */
+ const Scalar& value() const { return m_value; }
+protected:
+ StorageIndex m_row, m_col;
+ Scalar m_value;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEUTIL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
new file mode 100644
index 0000000..05779be
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
@@ -0,0 +1,478 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEVECTOR_H
+#define EIGEN_SPARSEVECTOR_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+ * \class SparseVector
+ *
+ * \brief a sparse vector class
+ *
+ * \tparam _Scalar the scalar type, i.e. the type of the coefficients
+ *
+ * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+ *
+ * This class can be extended with the help of the plugin mechanism described on the page
+ * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEVECTOR_PLUGIN.
+ */
+
+namespace internal {
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct traits<SparseVector<_Scalar, _Options, _StorageIndex> >
+{
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+ typedef Sparse StorageKind;
+ typedef MatrixXpr XprKind;
+ enum {
+ IsColVector = (_Options & RowMajorBit) ? 0 : 1,
+
+ RowsAtCompileTime = IsColVector ? Dynamic : 1,
+ ColsAtCompileTime = IsColVector ? 1 : Dynamic,
+ MaxRowsAtCompileTime = RowsAtCompileTime,
+ MaxColsAtCompileTime = ColsAtCompileTime,
+ Flags = _Options | NestByRefBit | LvalueBit | (IsColVector ? 0 : RowMajorBit) | CompressedAccessBit,
+ SupportedAccessPatterns = InnerRandomAccessPattern
+ };
+};
+
+// Sparse-Vector-Assignment kinds:
+enum {
+ SVA_RuntimeSwitch,
+ SVA_Inner,
+ SVA_Outer
+};
+
+template< typename Dest, typename Src,
+ int AssignmentKind = !bool(Src::IsVectorAtCompileTime) ? SVA_RuntimeSwitch
+ : Src::InnerSizeAtCompileTime==1 ? SVA_Outer
+ : SVA_Inner>
+struct sparse_vector_assign_selector;
+
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+class SparseVector
+ : public SparseCompressedBase<SparseVector<_Scalar, _Options, _StorageIndex> >
+{
+ typedef SparseCompressedBase<SparseVector> Base;
+ using Base::convert_index;
+ public:
+ EIGEN_SPARSE_PUBLIC_INTERFACE(SparseVector)
+ EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=)
+ EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=)
+
+ typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
+ enum { IsColVector = internal::traits<SparseVector>::IsColVector };
+
+ enum {
+ Options = _Options
+ };
+
+ EIGEN_STRONG_INLINE Index rows() const { return IsColVector ? m_size : 1; }
+ EIGEN_STRONG_INLINE Index cols() const { return IsColVector ? 1 : m_size; }
+ EIGEN_STRONG_INLINE Index innerSize() const { return m_size; }
+ EIGEN_STRONG_INLINE Index outerSize() const { return 1; }
+
+ EIGEN_STRONG_INLINE const Scalar* valuePtr() const { return m_data.valuePtr(); }
+ EIGEN_STRONG_INLINE Scalar* valuePtr() { return m_data.valuePtr(); }
+
+ EIGEN_STRONG_INLINE const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+ EIGEN_STRONG_INLINE StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+
+ inline const StorageIndex* outerIndexPtr() const { return 0; }
+ inline StorageIndex* outerIndexPtr() { return 0; }
+ inline const StorageIndex* innerNonZeroPtr() const { return 0; }
+ inline StorageIndex* innerNonZeroPtr() { return 0; }
+
+ /** \internal */
+ inline Storage& data() { return m_data; }
+ /** \internal */
+ inline const Storage& data() const { return m_data; }
+
+ inline Scalar coeff(Index row, Index col) const
+ {
+ eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+ return coeff(IsColVector ? row : col);
+ }
+ inline Scalar coeff(Index i) const
+ {
+ eigen_assert(i>=0 && i<m_size);
+ return m_data.at(StorageIndex(i));
+ }
+
+ inline Scalar& coeffRef(Index row, Index col)
+ {
+ eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+ return coeffRef(IsColVector ? row : col);
+ }
+
+ /** \returns a reference to the coefficient value at given index \a i
+ * This operation involes a log(rho*size) binary search. If the coefficient does not
+ * exist yet, then a sorted insertion into a sequential buffer is performed.
+ *
+ * This insertion might be very costly if the number of nonzeros above \a i is large.
+ */
+ inline Scalar& coeffRef(Index i)
+ {
+ eigen_assert(i>=0 && i<m_size);
+
+ return m_data.atWithInsertion(StorageIndex(i));
+ }
+
+ public:
+
+ typedef typename Base::InnerIterator InnerIterator;
+ typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
+
+ inline void setZero() { m_data.clear(); }
+
+ /** \returns the number of non zero coefficients */
+ inline Index nonZeros() const { return m_data.size(); }
+
+ inline void startVec(Index outer)
+ {
+ EIGEN_UNUSED_VARIABLE(outer);
+ eigen_assert(outer==0);
+ }
+
+ inline Scalar& insertBackByOuterInner(Index outer, Index inner)
+ {
+ EIGEN_UNUSED_VARIABLE(outer);
+ eigen_assert(outer==0);
+ return insertBack(inner);
+ }
+ inline Scalar& insertBack(Index i)
+ {
+ m_data.append(0, i);
+ return m_data.value(m_data.size()-1);
+ }
+
+ Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
+ {
+ EIGEN_UNUSED_VARIABLE(outer);
+ eigen_assert(outer==0);
+ return insertBackUnordered(inner);
+ }
+ inline Scalar& insertBackUnordered(Index i)
+ {
+ m_data.append(0, i);
+ return m_data.value(m_data.size()-1);
+ }
+
+ inline Scalar& insert(Index row, Index col)
+ {
+ eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+
+ Index inner = IsColVector ? row : col;
+ Index outer = IsColVector ? col : row;
+ EIGEN_ONLY_USED_FOR_DEBUG(outer);
+ eigen_assert(outer==0);
+ return insert(inner);
+ }
+ Scalar& insert(Index i)
+ {
+ eigen_assert(i>=0 && i<m_size);
+
+ Index startId = 0;
+ Index p = Index(m_data.size()) - 1;
+ // TODO smart realloc
+ m_data.resize(p+2,1);
+
+ while ( (p >= startId) && (m_data.index(p) > i) )
+ {
+ m_data.index(p+1) = m_data.index(p);
+ m_data.value(p+1) = m_data.value(p);
+ --p;
+ }
+ m_data.index(p+1) = convert_index(i);
+ m_data.value(p+1) = 0;
+ return m_data.value(p+1);
+ }
+
+ /**
+ */
+ inline void reserve(Index reserveSize) { m_data.reserve(reserveSize); }
+
+
+ inline void finalize() {}
+
+ /** \copydoc SparseMatrix::prune(const Scalar&,const RealScalar&) */
+ void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+ {
+ m_data.prune(reference,epsilon);
+ }
+
+ /** Resizes the sparse vector to \a rows x \a cols
+ *
+ * This method is provided for compatibility with matrices.
+ * For a column vector, \a cols must be equal to 1.
+ * For a row vector, \a rows must be equal to 1.
+ *
+ * \sa resize(Index)
+ */
+ void resize(Index rows, Index cols)
+ {
+ eigen_assert((IsColVector ? cols : rows)==1 && "Outer dimension must equal 1");
+ resize(IsColVector ? rows : cols);
+ }
+
+ /** Resizes the sparse vector to \a newSize
+ * This method deletes all entries, thus leaving an empty sparse vector
+ *
+ * \sa conservativeResize(), setZero() */
+ void resize(Index newSize)
+ {
+ m_size = newSize;
+ m_data.clear();
+ }
+
+ /** Resizes the sparse vector to \a newSize, while leaving old values untouched.
+ *
+ * If the size of the vector is decreased, then the storage of the out-of bounds coefficients is kept and reserved.
+ * Call .data().squeeze() to free extra memory.
+ *
+ * \sa reserve(), setZero()
+ */
+ void conservativeResize(Index newSize)
+ {
+ if (newSize < m_size)
+ {
+ Index i = 0;
+ while (i<m_data.size() && m_data.index(i)<newSize) ++i;
+ m_data.resize(i);
+ }
+ m_size = newSize;
+ }
+
+ void resizeNonZeros(Index size) { m_data.resize(size); }
+
+ inline SparseVector() : m_size(0) { check_template_parameters(); resize(0); }
+
+ explicit inline SparseVector(Index size) : m_size(0) { check_template_parameters(); resize(size); }
+
+ inline SparseVector(Index rows, Index cols) : m_size(0) { check_template_parameters(); resize(rows,cols); }
+
+ template<typename OtherDerived>
+ inline SparseVector(const SparseMatrixBase<OtherDerived>& other)
+ : m_size(0)
+ {
+ #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+ #endif
+ check_template_parameters();
+ *this = other.derived();
+ }
+
+ inline SparseVector(const SparseVector& other)
+ : Base(other), m_size(0)
+ {
+ check_template_parameters();
+ *this = other.derived();
+ }
+
+ /** Swaps the values of \c *this and \a other.
+ * Overloaded for performance: this version performs a \em shallow swap by swapping pointers and attributes only.
+ * \sa SparseMatrixBase::swap()
+ */
+ inline void swap(SparseVector& other)
+ {
+ std::swap(m_size, other.m_size);
+ m_data.swap(other.m_data);
+ }
+
+ template<int OtherOptions>
+ inline void swap(SparseMatrix<Scalar,OtherOptions,StorageIndex>& other)
+ {
+ eigen_assert(other.outerSize()==1);
+ std::swap(m_size, other.m_innerSize);
+ m_data.swap(other.m_data);
+ }
+
+ inline SparseVector& operator=(const SparseVector& other)
+ {
+ if (other.isRValue())
+ {
+ swap(other.const_cast_derived());
+ }
+ else
+ {
+ resize(other.size());
+ m_data = other.m_data;
+ }
+ return *this;
+ }
+
+ template<typename OtherDerived>
+ inline SparseVector& operator=(const SparseMatrixBase<OtherDerived>& other)
+ {
+ SparseVector tmp(other.size());
+ internal::sparse_vector_assign_selector<SparseVector,OtherDerived>::run(tmp,other.derived());
+ this->swap(tmp);
+ return *this;
+ }
+
+ #ifndef EIGEN_PARSED_BY_DOXYGEN
+ template<typename Lhs, typename Rhs>
+ inline SparseVector& operator=(const SparseSparseProduct<Lhs,Rhs>& product)
+ {
+ return Base::operator=(product);
+ }
+ #endif
+
+ friend std::ostream & operator << (std::ostream & s, const SparseVector& m)
+ {
+ for (Index i=0; i<m.nonZeros(); ++i)
+ s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+ s << std::endl;
+ return s;
+ }
+
+ /** Destructor */
+ inline ~SparseVector() {}
+
+ /** Overloaded for performance */
+ Scalar sum() const;
+
+ public:
+
+ /** \internal \deprecated use setZero() and reserve() */
+ EIGEN_DEPRECATED void startFill(Index reserve)
+ {
+ setZero();
+ m_data.reserve(reserve);
+ }
+
+ /** \internal \deprecated use insertBack(Index,Index) */
+ EIGEN_DEPRECATED Scalar& fill(Index r, Index c)
+ {
+ eigen_assert(r==0 || c==0);
+ return fill(IsColVector ? r : c);
+ }
+
+ /** \internal \deprecated use insertBack(Index) */
+ EIGEN_DEPRECATED Scalar& fill(Index i)
+ {
+ m_data.append(0, i);
+ return m_data.value(m_data.size()-1);
+ }
+
+ /** \internal \deprecated use insert(Index,Index) */
+ EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c)
+ {
+ eigen_assert(r==0 || c==0);
+ return fillrand(IsColVector ? r : c);
+ }
+
+ /** \internal \deprecated use insert(Index) */
+ EIGEN_DEPRECATED Scalar& fillrand(Index i)
+ {
+ return insert(i);
+ }
+
+ /** \internal \deprecated use finalize() */
+ EIGEN_DEPRECATED void endFill() {}
+
+ // These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
+ /** \internal \deprecated use data() */
+ EIGEN_DEPRECATED Storage& _data() { return m_data; }
+ /** \internal \deprecated use data() */
+ EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
+
+# ifdef EIGEN_SPARSEVECTOR_PLUGIN
+# include EIGEN_SPARSEVECTOR_PLUGIN
+# endif
+
+protected:
+
+ static void check_template_parameters()
+ {
+ EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
+ EIGEN_STATIC_ASSERT((_Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
+ }
+
+ Storage m_data;
+ Index m_size;
+};
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _Index>
+struct evaluator<SparseVector<_Scalar,_Options,_Index> >
+ : evaluator_base<SparseVector<_Scalar,_Options,_Index> >
+{
+ typedef SparseVector<_Scalar,_Options,_Index> SparseVectorType;
+ typedef evaluator_base<SparseVectorType> Base;
+ typedef typename SparseVectorType::InnerIterator InnerIterator;
+ typedef typename SparseVectorType::ReverseInnerIterator ReverseInnerIterator;
+
+ enum {
+ CoeffReadCost = NumTraits<_Scalar>::ReadCost,
+ Flags = SparseVectorType::Flags
+ };
+
+ evaluator() : Base() {}
+
+ explicit evaluator(const SparseVectorType &mat) : m_matrix(&mat)
+ {
+ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+ }
+
+ inline Index nonZerosEstimate() const {
+ return m_matrix->nonZeros();
+ }
+
+ operator SparseVectorType&() { return m_matrix->const_cast_derived(); }
+ operator const SparseVectorType&() const { return *m_matrix; }
+
+ const SparseVectorType *m_matrix;
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_Inner> {
+ static void run(Dest& dst, const Src& src) {
+ eigen_internal_assert(src.innerSize()==src.size());
+ typedef internal::evaluator<Src> SrcEvaluatorType;
+ SrcEvaluatorType srcEval(src);
+ for(typename SrcEvaluatorType::InnerIterator it(srcEval, 0); it; ++it)
+ dst.insert(it.index()) = it.value();
+ }
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_Outer> {
+ static void run(Dest& dst, const Src& src) {
+ eigen_internal_assert(src.outerSize()==src.size());
+ typedef internal::evaluator<Src> SrcEvaluatorType;
+ SrcEvaluatorType srcEval(src);
+ for(Index i=0; i<src.size(); ++i)
+ {
+ typename SrcEvaluatorType::InnerIterator it(srcEval, i);
+ if(it)
+ dst.insert(i) = it.value();
+ }
+ }
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_RuntimeSwitch> {
+ static void run(Dest& dst, const Src& src) {
+ if(src.outerSize()==1) sparse_vector_assign_selector<Dest,Src,SVA_Inner>::run(dst, src);
+ else sparse_vector_assign_selector<Dest,Src,SVA_Outer>::run(dst, src);
+ }
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
new file mode 100644
index 0000000..92b3d1f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
@@ -0,0 +1,254 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2010 Daniel Lowengrub <lowdanie@gmail.com>
+//
+// 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_SPARSEVIEW_H
+#define EIGEN_SPARSEVIEW_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename MatrixType>
+struct traits<SparseView<MatrixType> > : traits<MatrixType>
+{
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef Sparse StorageKind;
+ enum {
+ Flags = int(traits<MatrixType>::Flags) & (RowMajorBit)
+ };
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+ * \class SparseView
+ *
+ * \brief Expression of a dense or sparse matrix with zero or too small values removed
+ *
+ * \tparam MatrixType the type of the object of which we are removing the small entries
+ *
+ * This class represents an expression of a given dense or sparse matrix with
+ * entries smaller than \c reference * \c epsilon are removed.
+ * It is the return type of MatrixBase::sparseView() and SparseMatrixBase::pruned()
+ * and most of the time this is the only way it is used.
+ *
+ * \sa MatrixBase::sparseView(), SparseMatrixBase::pruned()
+ */
+template<typename MatrixType>
+class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
+{
+ typedef typename MatrixType::Nested MatrixTypeNested;
+ typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+ typedef SparseMatrixBase<SparseView > Base;
+public:
+ EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
+ typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+
+ explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
+ const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
+ : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
+
+ inline Index rows() const { return m_matrix.rows(); }
+ inline Index cols() const { return m_matrix.cols(); }
+
+ inline Index innerSize() const { return m_matrix.innerSize(); }
+ inline Index outerSize() const { return m_matrix.outerSize(); }
+
+ /** \returns the nested expression */
+ const typename internal::remove_all<MatrixTypeNested>::type&
+ nestedExpression() const { return m_matrix; }
+
+ Scalar reference() const { return m_reference; }
+ RealScalar epsilon() const { return m_epsilon; }
+
+protected:
+ MatrixTypeNested m_matrix;
+ Scalar m_reference;
+ RealScalar m_epsilon;
+};
+
+namespace internal {
+
+// TODO find a way to unify the two following variants
+// This is tricky because implementing an inner iterator on top of an IndexBased evaluator is
+// not easy because the evaluators do not expose the sizes of the underlying expression.
+
+template<typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IteratorBased>
+ : public evaluator_base<SparseView<ArgType> >
+{
+ typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+ public:
+ typedef SparseView<ArgType> XprType;
+
+ class InnerIterator : public EvalIterator
+ {
+ protected:
+ typedef typename XprType::Scalar Scalar;
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+ : EvalIterator(sve.m_argImpl,outer), m_view(sve.m_view)
+ {
+ incrementToNonZero();
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ EvalIterator::operator++();
+ incrementToNonZero();
+ return *this;
+ }
+
+ using EvalIterator::value;
+
+ protected:
+ const XprType &m_view;
+
+ private:
+ void incrementToNonZero()
+ {
+ while((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.reference(), m_view.epsilon()))
+ {
+ EvalIterator::operator++();
+ }
+ }
+ };
+
+ enum {
+ CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+
+ protected:
+ evaluator<ArgType> m_argImpl;
+ const XprType &m_view;
+};
+
+template<typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IndexBased>
+ : public evaluator_base<SparseView<ArgType> >
+{
+ public:
+ typedef SparseView<ArgType> XprType;
+ protected:
+ enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit };
+ typedef typename XprType::Scalar Scalar;
+ typedef typename XprType::StorageIndex StorageIndex;
+ public:
+
+ class InnerIterator
+ {
+ public:
+
+ EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+ : m_sve(sve), m_inner(0), m_outer(outer), m_end(sve.m_view.innerSize())
+ {
+ incrementToNonZero();
+ }
+
+ EIGEN_STRONG_INLINE InnerIterator& operator++()
+ {
+ m_inner++;
+ incrementToNonZero();
+ return *this;
+ }
+
+ EIGEN_STRONG_INLINE Scalar value() const
+ {
+ return (IsRowMajor) ? m_sve.m_argImpl.coeff(m_outer, m_inner)
+ : m_sve.m_argImpl.coeff(m_inner, m_outer);
+ }
+
+ EIGEN_STRONG_INLINE StorageIndex index() const { return m_inner; }
+ inline Index row() const { return IsRowMajor ? m_outer : index(); }
+ inline Index col() const { return IsRowMajor ? index() : m_outer; }
+
+ EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+
+ protected:
+ const unary_evaluator &m_sve;
+ Index m_inner;
+ const Index m_outer;
+ const Index m_end;
+
+ private:
+ void incrementToNonZero()
+ {
+ while((bool(*this)) && internal::isMuchSmallerThan(value(), m_sve.m_view.reference(), m_sve.m_view.epsilon()))
+ {
+ m_inner++;
+ }
+ }
+ };
+
+ enum {
+ CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+ Flags = XprType::Flags
+ };
+
+ explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+
+ protected:
+ evaluator<ArgType> m_argImpl;
+ const XprType &m_view;
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+ *
+ * \returns a sparse expression of the dense expression \c *this with values smaller than
+ * \a reference * \a epsilon removed.
+ *
+ * This method is typically used when prototyping to convert a quickly assembled dense Matrix \c D to a SparseMatrix \c S:
+ * \code
+ * MatrixXd D(n,m);
+ * SparseMatrix<double> S;
+ * S = D.sparseView(); // suppress numerical zeros (exact)
+ * S = D.sparseView(reference);
+ * S = D.sparseView(reference,epsilon);
+ * \endcode
+ * where \a reference is a meaningful non zero reference value,
+ * and \a epsilon is a tolerance factor defaulting to NumTraits<Scalar>::dummy_precision().
+ *
+ * \sa SparseMatrixBase::pruned(), class SparseView */
+template<typename Derived>
+const SparseView<Derived> MatrixBase<Derived>::sparseView(const Scalar& reference,
+ const typename NumTraits<Scalar>::Real& epsilon) const
+{
+ return SparseView<Derived>(derived(), reference, epsilon);
+}
+
+/** \returns an expression of \c *this with values smaller than
+ * \a reference * \a epsilon removed.
+ *
+ * This method is typically used in conjunction with the product of two sparse matrices
+ * to automatically prune the smallest values as follows:
+ * \code
+ * C = (A*B).pruned(); // suppress numerical zeros (exact)
+ * C = (A*B).pruned(ref);
+ * C = (A*B).pruned(ref,epsilon);
+ * \endcode
+ * where \c ref is a meaningful non zero reference value.
+ * */
+template<typename Derived>
+const SparseView<Derived>
+SparseMatrixBase<Derived>::pruned(const Scalar& reference,
+ const RealScalar& epsilon) const
+{
+ return SparseView<Derived>(derived(), reference, epsilon);
+}
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
new file mode 100644
index 0000000..f9c56ba
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
@@ -0,0 +1,315 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSETRIANGULARSOLVER_H
+#define EIGEN_SPARSETRIANGULARSOLVER_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int Mode,
+ int UpLo = (Mode & Lower)
+ ? Lower
+ : (Mode & Upper)
+ ? Upper
+ : -1,
+ int StorageOrder = int(traits<Lhs>::Flags) & RowMajorBit>
+struct sparse_solve_triangular_selector;
+
+// forward substitution, row-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,RowMajor>
+{
+ typedef typename Rhs::Scalar Scalar;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ static void run(const Lhs& lhs, Rhs& other)
+ {
+ LhsEval lhsEval(lhs);
+ for(Index col=0 ; col<other.cols() ; ++col)
+ {
+ for(Index i=0; i<lhs.rows(); ++i)
+ {
+ Scalar tmp = other.coeff(i,col);
+ Scalar lastVal(0);
+ Index lastIndex = 0;
+ for(LhsIterator it(lhsEval, i); it; ++it)
+ {
+ lastVal = it.value();
+ lastIndex = it.index();
+ if(lastIndex==i)
+ break;
+ tmp -= lastVal * other.coeff(lastIndex,col);
+ }
+ if (Mode & UnitDiag)
+ other.coeffRef(i,col) = tmp;
+ else
+ {
+ eigen_assert(lastIndex==i);
+ other.coeffRef(i,col) = tmp/lastVal;
+ }
+ }
+ }
+ }
+};
+
+// backward substitution, row-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,RowMajor>
+{
+ typedef typename Rhs::Scalar Scalar;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ static void run(const Lhs& lhs, Rhs& other)
+ {
+ LhsEval lhsEval(lhs);
+ for(Index col=0 ; col<other.cols() ; ++col)
+ {
+ for(Index i=lhs.rows()-1 ; i>=0 ; --i)
+ {
+ Scalar tmp = other.coeff(i,col);
+ Scalar l_ii(0);
+ LhsIterator it(lhsEval, i);
+ while(it && it.index()<i)
+ ++it;
+ if(!(Mode & UnitDiag))
+ {
+ eigen_assert(it && it.index()==i);
+ l_ii = it.value();
+ ++it;
+ }
+ else if (it && it.index() == i)
+ ++it;
+ for(; it; ++it)
+ {
+ tmp -= it.value() * other.coeff(it.index(),col);
+ }
+
+ if (Mode & UnitDiag) other.coeffRef(i,col) = tmp;
+ else other.coeffRef(i,col) = tmp/l_ii;
+ }
+ }
+ }
+};
+
+// forward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,ColMajor>
+{
+ typedef typename Rhs::Scalar Scalar;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ static void run(const Lhs& lhs, Rhs& other)
+ {
+ LhsEval lhsEval(lhs);
+ for(Index col=0 ; col<other.cols() ; ++col)
+ {
+ for(Index i=0; i<lhs.cols(); ++i)
+ {
+ Scalar& tmp = other.coeffRef(i,col);
+ if (tmp!=Scalar(0)) // optimization when other is actually sparse
+ {
+ LhsIterator it(lhsEval, i);
+ while(it && it.index()<i)
+ ++it;
+ if(!(Mode & UnitDiag))
+ {
+ eigen_assert(it && it.index()==i);
+ tmp /= it.value();
+ }
+ if (it && it.index()==i)
+ ++it;
+ for(; it; ++it)
+ other.coeffRef(it.index(), col) -= tmp * it.value();
+ }
+ }
+ }
+ }
+};
+
+// backward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,ColMajor>
+{
+ typedef typename Rhs::Scalar Scalar;
+ typedef evaluator<Lhs> LhsEval;
+ typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+ static void run(const Lhs& lhs, Rhs& other)
+ {
+ LhsEval lhsEval(lhs);
+ for(Index col=0 ; col<other.cols() ; ++col)
+ {
+ for(Index i=lhs.cols()-1; i>=0; --i)
+ {
+ Scalar& tmp = other.coeffRef(i,col);
+ if (tmp!=Scalar(0)) // optimization when other is actually sparse
+ {
+ if(!(Mode & UnitDiag))
+ {
+ // TODO replace this by a binary search. make sure the binary search is safe for partially sorted elements
+ LhsIterator it(lhsEval, i);
+ while(it && it.index()!=i)
+ ++it;
+ eigen_assert(it && it.index()==i);
+ other.coeffRef(i,col) /= it.value();
+ }
+ LhsIterator it(lhsEval, i);
+ for(; it && it.index()<i; ++it)
+ other.coeffRef(it.index(), col) -= tmp * it.value();
+ }
+ }
+ }
+ }
+};
+
+} // end namespace internal
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+template<typename ExpressionType,unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(MatrixBase<OtherDerived>& other) const
+{
+ eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
+ eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+
+ enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
+
+ typedef typename internal::conditional<copy,
+ typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+ OtherCopy otherCopy(other.derived());
+
+ internal::sparse_solve_triangular_selector<ExpressionType, typename internal::remove_reference<OtherCopy>::type, Mode>::run(derived().nestedExpression(), otherCopy);
+
+ if (copy)
+ other = otherCopy;
+}
+#endif
+
+// pure sparse path
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int Mode,
+ int UpLo = (Mode & Lower)
+ ? Lower
+ : (Mode & Upper)
+ ? Upper
+ : -1,
+ int StorageOrder = int(Lhs::Flags) & (RowMajorBit)>
+struct sparse_solve_triangular_sparse_selector;
+
+// forward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode, int UpLo>
+struct sparse_solve_triangular_sparse_selector<Lhs,Rhs,Mode,UpLo,ColMajor>
+{
+ typedef typename Rhs::Scalar Scalar;
+ typedef typename promote_index_type<typename traits<Lhs>::StorageIndex,
+ typename traits<Rhs>::StorageIndex>::type StorageIndex;
+ static void run(const Lhs& lhs, Rhs& other)
+ {
+ const bool IsLower = (UpLo==Lower);
+ AmbiVector<Scalar,StorageIndex> tempVector(other.rows()*2);
+ tempVector.setBounds(0,other.rows());
+
+ Rhs res(other.rows(), other.cols());
+ res.reserve(other.nonZeros());
+
+ for(Index col=0 ; col<other.cols() ; ++col)
+ {
+ // FIXME estimate number of non zeros
+ tempVector.init(.99/*float(other.col(col).nonZeros())/float(other.rows())*/);
+ tempVector.setZero();
+ tempVector.restart();
+ for (typename Rhs::InnerIterator rhsIt(other, col); rhsIt; ++rhsIt)
+ {
+ tempVector.coeffRef(rhsIt.index()) = rhsIt.value();
+ }
+
+ for(Index i=IsLower?0:lhs.cols()-1;
+ IsLower?i<lhs.cols():i>=0;
+ i+=IsLower?1:-1)
+ {
+ tempVector.restart();
+ Scalar& ci = tempVector.coeffRef(i);
+ if (ci!=Scalar(0))
+ {
+ // find
+ typename Lhs::InnerIterator it(lhs, i);
+ if(!(Mode & UnitDiag))
+ {
+ if (IsLower)
+ {
+ eigen_assert(it.index()==i);
+ ci /= it.value();
+ }
+ else
+ ci /= lhs.coeff(i,i);
+ }
+ tempVector.restart();
+ if (IsLower)
+ {
+ if (it.index()==i)
+ ++it;
+ for(; it; ++it)
+ tempVector.coeffRef(it.index()) -= ci * it.value();
+ }
+ else
+ {
+ for(; it && it.index()<i; ++it)
+ tempVector.coeffRef(it.index()) -= ci * it.value();
+ }
+ }
+ }
+
+
+ Index count = 0;
+ // FIXME compute a reference value to filter zeros
+ for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
+ {
+ ++ count;
+// std::cerr << "fill " << it.index() << ", " << col << "\n";
+// std::cout << it.value() << " ";
+ // FIXME use insertBack
+ res.insert(it.index(), col) = it.value();
+ }
+// std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
+ }
+ res.finalize();
+ other = res.markAsRValue();
+ }
+};
+
+} // end namespace internal
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename ExpressionType,unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(SparseMatrixBase<OtherDerived>& other) const
+{
+ eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
+ eigen_assert( (!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+
+// enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
+
+// typedef typename internal::conditional<copy,
+// typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+// OtherCopy otherCopy(other.derived());
+
+ internal::sparse_solve_triangular_sparse_selector<ExpressionType, OtherDerived, Mode>::run(derived().nestedExpression(), other.derived());
+
+// if (copy)
+// other = otherCopy;
+}
+#endif
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSETRIANGULARSOLVER_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
new file mode 100644
index 0000000..0c8d893
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
@@ -0,0 +1,923 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_LU_H
+#define EIGEN_SPARSE_LU_H
+
+namespace Eigen {
+
+template <typename _MatrixType, typename _OrderingType = COLAMDOrdering<typename _MatrixType::StorageIndex> > class SparseLU;
+template <typename MappedSparseMatrixType> struct SparseLUMatrixLReturnType;
+template <typename MatrixLType, typename MatrixUType> struct SparseLUMatrixUReturnType;
+
+template <bool Conjugate,class SparseLUType>
+class SparseLUTransposeView : public SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> >
+{
+protected:
+ typedef SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> > APIBase;
+ using APIBase::m_isInitialized;
+public:
+ typedef typename SparseLUType::Scalar Scalar;
+ typedef typename SparseLUType::StorageIndex StorageIndex;
+ typedef typename SparseLUType::MatrixType MatrixType;
+ typedef typename SparseLUType::OrderingType OrderingType;
+
+ enum {
+ ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+ MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+ };
+
+ SparseLUTransposeView() : m_sparseLU(NULL) {}
+ SparseLUTransposeView(const SparseLUTransposeView& view) {
+ this->m_sparseLU = view.m_sparseLU;
+ }
+ void setIsInitialized(const bool isInitialized) {this->m_isInitialized = isInitialized;}
+ void setSparseLU(SparseLUType* sparseLU) {m_sparseLU = sparseLU;}
+ using APIBase::_solve_impl;
+ template<typename Rhs, typename Dest>
+ bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
+ {
+ Dest& X(X_base.derived());
+ eigen_assert(m_sparseLU->info() == Success && "The matrix should be factorized first");
+ EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
+ THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+
+
+ // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
+ for(Index j = 0; j < B.cols(); ++j){
+ X.col(j) = m_sparseLU->colsPermutation() * B.const_cast_derived().col(j);
+ }
+ //Forward substitution with transposed or adjoint of U
+ m_sparseLU->matrixU().template solveTransposedInPlace<Conjugate>(X);
+
+ //Backward substitution with transposed or adjoint of L
+ m_sparseLU->matrixL().template solveTransposedInPlace<Conjugate>(X);
+
+ // Permute back the solution
+ for (Index j = 0; j < B.cols(); ++j)
+ X.col(j) = m_sparseLU->rowsPermutation().transpose() * X.col(j);
+ return true;
+ }
+ inline Index rows() const { return m_sparseLU->rows(); }
+ inline Index cols() const { return m_sparseLU->cols(); }
+
+private:
+ SparseLUType *m_sparseLU;
+ SparseLUTransposeView& operator=(const SparseLUTransposeView&);
+};
+
+
+/** \ingroup SparseLU_Module
+ * \class SparseLU
+ *
+ * \brief Sparse supernodal LU factorization for general matrices
+ *
+ * This class implements the supernodal LU factorization for general matrices.
+ * It uses the main techniques from the sequential SuperLU package
+ * (http://crd-legacy.lbl.gov/~xiaoye/SuperLU/). It handles transparently real
+ * and complex arithmetic with single and double precision, depending on the
+ * scalar type of your input matrix.
+ * The code has been optimized to provide BLAS-3 operations during supernode-panel updates.
+ * It benefits directly from the built-in high-performant Eigen BLAS routines.
+ * Moreover, when the size of a supernode is very small, the BLAS calls are avoided to
+ * enable a better optimization from the compiler. For best performance,
+ * you should compile it with NDEBUG flag to avoid the numerous bounds checking on vectors.
+ *
+ * An important parameter of this class is the ordering method. It is used to reorder the columns
+ * (and eventually the rows) of the matrix to reduce the number of new elements that are created during
+ * numerical factorization. The cheapest method available is COLAMD.
+ * See \link OrderingMethods_Module the OrderingMethods module \endlink for the list of
+ * built-in and external ordering methods.
+ *
+ * Simple example with key steps
+ * \code
+ * VectorXd x(n), b(n);
+ * SparseMatrix<double> A;
+ * SparseLU<SparseMatrix<double>, COLAMDOrdering<int> > solver;
+ * // fill A and b;
+ * // Compute the ordering permutation vector from the structural pattern of A
+ * solver.analyzePattern(A);
+ * // Compute the numerical factorization
+ * solver.factorize(A);
+ * //Use the factors to solve the linear system
+ * x = solver.solve(b);
+ * \endcode
+ *
+ * \warning The input matrix A should be in a \b compressed and \b column-major form.
+ * Otherwise an expensive copy will be made. You can call the inexpensive makeCompressed() to get a compressed matrix.
+ *
+ * \note Unlike the initial SuperLU implementation, there is no step to equilibrate the matrix.
+ * For badly scaled matrices, this step can be useful to reduce the pivoting during factorization.
+ * If this is the case for your matrices, you can try the basic scaling method at
+ * "unsupported/Eigen/src/IterativeSolvers/Scaling.h"
+ *
+ * \tparam _MatrixType The type of the sparse matrix. It must be a column-major SparseMatrix<>
+ * \tparam _OrderingType The ordering method to use, either AMD, COLAMD or METIS. Default is COLMAD
+ *
+ * \implsparsesolverconcept
+ *
+ * \sa \ref TutorialSparseSolverConcept
+ * \sa \ref OrderingMethods_Module
+ */
+template <typename _MatrixType, typename _OrderingType>
+class SparseLU : public SparseSolverBase<SparseLU<_MatrixType,_OrderingType> >, public internal::SparseLUImpl<typename _MatrixType::Scalar, typename _MatrixType::StorageIndex>
+{
+ protected:
+ typedef SparseSolverBase<SparseLU<_MatrixType,_OrderingType> > APIBase;
+ using APIBase::m_isInitialized;
+ public:
+ using APIBase::_solve_impl;
+
+ typedef _MatrixType MatrixType;
+ typedef _OrderingType OrderingType;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> NCMatrix;
+ typedef internal::MappedSuperNodalMatrix<Scalar, StorageIndex> SCMatrix;
+ typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+ typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+ typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+ typedef internal::SparseLUImpl<Scalar, StorageIndex> Base;
+
+ enum {
+ ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+ MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+ };
+
+ public:
+
+ SparseLU():m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
+ {
+ initperfvalues();
+ }
+ explicit SparseLU(const MatrixType& matrix)
+ : m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
+ {
+ initperfvalues();
+ compute(matrix);
+ }
+
+ ~SparseLU()
+ {
+ // Free all explicit dynamic pointers
+ }
+
+ void analyzePattern (const MatrixType& matrix);
+ void factorize (const MatrixType& matrix);
+ void simplicialfactorize(const MatrixType& matrix);
+
+ /**
+ * Compute the symbolic and numeric factorization of the input sparse matrix.
+ * The input matrix should be in column-major storage.
+ */
+ void compute (const MatrixType& matrix)
+ {
+ // Analyze
+ analyzePattern(matrix);
+ //Factorize
+ factorize(matrix);
+ }
+
+ /** \returns an expression of the transposed of the factored matrix.
+ *
+ * A typical usage is to solve for the transposed problem A^T x = b:
+ * \code
+ * solver.compute(A);
+ * x = solver.transpose().solve(b);
+ * \endcode
+ *
+ * \sa adjoint(), solve()
+ */
+ const SparseLUTransposeView<false,SparseLU<_MatrixType,_OrderingType> > transpose()
+ {
+ SparseLUTransposeView<false, SparseLU<_MatrixType,_OrderingType> > transposeView;
+ transposeView.setSparseLU(this);
+ transposeView.setIsInitialized(this->m_isInitialized);
+ return transposeView;
+ }
+
+
+ /** \returns an expression of the adjoint of the factored matrix
+ *
+ * A typical usage is to solve for the adjoint problem A' x = b:
+ * \code
+ * solver.compute(A);
+ * x = solver.adjoint().solve(b);
+ * \endcode
+ *
+ * For real scalar types, this function is equivalent to transpose().
+ *
+ * \sa transpose(), solve()
+ */
+ const SparseLUTransposeView<true, SparseLU<_MatrixType,_OrderingType> > adjoint()
+ {
+ SparseLUTransposeView<true, SparseLU<_MatrixType,_OrderingType> > adjointView;
+ adjointView.setSparseLU(this);
+ adjointView.setIsInitialized(this->m_isInitialized);
+ return adjointView;
+ }
+
+ inline Index rows() const { return m_mat.rows(); }
+ inline Index cols() const { return m_mat.cols(); }
+ /** Indicate that the pattern of the input matrix is symmetric */
+ void isSymmetric(bool sym)
+ {
+ m_symmetricmode = sym;
+ }
+
+ /** \returns an expression of the matrix L, internally stored as supernodes
+ * The only operation available with this expression is the triangular solve
+ * \code
+ * y = b; matrixL().solveInPlace(y);
+ * \endcode
+ */
+ SparseLUMatrixLReturnType<SCMatrix> matrixL() const
+ {
+ return SparseLUMatrixLReturnType<SCMatrix>(m_Lstore);
+ }
+ /** \returns an expression of the matrix U,
+ * The only operation available with this expression is the triangular solve
+ * \code
+ * y = b; matrixU().solveInPlace(y);
+ * \endcode
+ */
+ SparseLUMatrixUReturnType<SCMatrix,MappedSparseMatrix<Scalar,ColMajor,StorageIndex> > matrixU() const
+ {
+ return SparseLUMatrixUReturnType<SCMatrix, MappedSparseMatrix<Scalar,ColMajor,StorageIndex> >(m_Lstore, m_Ustore);
+ }
+
+ /**
+ * \returns a reference to the row matrix permutation \f$ P_r \f$ such that \f$P_r A P_c^T = L U\f$
+ * \sa colsPermutation()
+ */
+ inline const PermutationType& rowsPermutation() const
+ {
+ return m_perm_r;
+ }
+ /**
+ * \returns a reference to the column matrix permutation\f$ P_c^T \f$ such that \f$P_r A P_c^T = L U\f$
+ * \sa rowsPermutation()
+ */
+ inline const PermutationType& colsPermutation() const
+ {
+ return m_perm_c;
+ }
+ /** Set the threshold used for a diagonal entry to be an acceptable pivot. */
+ void setPivotThreshold(const RealScalar& thresh)
+ {
+ m_diagpivotthresh = thresh;
+ }
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+ /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+ *
+ * \warning the destination matrix X in X = this->solve(B) must be colmun-major.
+ *
+ * \sa compute()
+ */
+ template<typename Rhs>
+ inline const Solve<SparseLU, Rhs> solve(const MatrixBase<Rhs>& B) const;
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+ /** \brief Reports whether previous computation was successful.
+ *
+ * \returns \c Success if computation was successful,
+ * \c NumericalIssue if the LU factorization reports a problem, zero diagonal for instance
+ * \c InvalidInput if the input matrix is invalid
+ *
+ * \sa iparm()
+ */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+ return m_info;
+ }
+
+ /**
+ * \returns A string describing the type of error
+ */
+ std::string lastErrorMessage() const
+ {
+ return m_lastError;
+ }
+
+ template<typename Rhs, typename Dest>
+ bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
+ {
+ Dest& X(X_base.derived());
+ eigen_assert(m_factorizationIsOk && "The matrix should be factorized first");
+ EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
+ THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+
+ // Permute the right hand side to form X = Pr*B
+ // on return, X is overwritten by the computed solution
+ X.resize(B.rows(),B.cols());
+
+ // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
+ for(Index j = 0; j < B.cols(); ++j)
+ X.col(j) = rowsPermutation() * B.const_cast_derived().col(j);
+
+ //Forward substitution with L
+ this->matrixL().solveInPlace(X);
+ this->matrixU().solveInPlace(X);
+
+ // Permute back the solution
+ for (Index j = 0; j < B.cols(); ++j)
+ X.col(j) = colsPermutation().inverse() * X.col(j);
+
+ return true;
+ }
+
+ /**
+ * \returns the absolute value of the determinant of the matrix of which
+ * *this is the QR decomposition.
+ *
+ * \warning a determinant can be very big or small, so for matrices
+ * of large enough dimension, there is a risk of overflow/underflow.
+ * One way to work around that is to use logAbsDeterminant() instead.
+ *
+ * \sa logAbsDeterminant(), signDeterminant()
+ */
+ Scalar absDeterminant()
+ {
+ using std::abs;
+ eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+ // Initialize with the determinant of the row matrix
+ Scalar det = Scalar(1.);
+ // Note that the diagonal blocks of U are stored in supernodes,
+ // which are available in the L part :)
+ for (Index j = 0; j < this->cols(); ++j)
+ {
+ for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+ {
+ if(it.index() == j)
+ {
+ det *= abs(it.value());
+ break;
+ }
+ }
+ }
+ return det;
+ }
+
+ /** \returns the natural log of the absolute value of the determinant of the matrix
+ * of which **this is the QR decomposition
+ *
+ * \note This method is useful to work around the risk of overflow/underflow that's
+ * inherent to the determinant computation.
+ *
+ * \sa absDeterminant(), signDeterminant()
+ */
+ Scalar logAbsDeterminant() const
+ {
+ using std::log;
+ using std::abs;
+
+ eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+ Scalar det = Scalar(0.);
+ for (Index j = 0; j < this->cols(); ++j)
+ {
+ for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+ {
+ if(it.row() < j) continue;
+ if(it.row() == j)
+ {
+ det += log(abs(it.value()));
+ break;
+ }
+ }
+ }
+ return det;
+ }
+
+ /** \returns A number representing the sign of the determinant
+ *
+ * \sa absDeterminant(), logAbsDeterminant()
+ */
+ Scalar signDeterminant()
+ {
+ eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+ // Initialize with the determinant of the row matrix
+ Index det = 1;
+ // Note that the diagonal blocks of U are stored in supernodes,
+ // which are available in the L part :)
+ for (Index j = 0; j < this->cols(); ++j)
+ {
+ for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+ {
+ if(it.index() == j)
+ {
+ if(it.value()<0)
+ det = -det;
+ else if(it.value()==0)
+ return 0;
+ break;
+ }
+ }
+ }
+ return det * m_detPermR * m_detPermC;
+ }
+
+ /** \returns The determinant of the matrix.
+ *
+ * \sa absDeterminant(), logAbsDeterminant()
+ */
+ Scalar determinant()
+ {
+ eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+ // Initialize with the determinant of the row matrix
+ Scalar det = Scalar(1.);
+ // Note that the diagonal blocks of U are stored in supernodes,
+ // which are available in the L part :)
+ for (Index j = 0; j < this->cols(); ++j)
+ {
+ for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+ {
+ if(it.index() == j)
+ {
+ det *= it.value();
+ break;
+ }
+ }
+ }
+ return (m_detPermR * m_detPermC) > 0 ? det : -det;
+ }
+
+ Index nnzL() const { return m_nnzL; };
+ Index nnzU() const { return m_nnzU; };
+
+ protected:
+ // Functions
+ void initperfvalues()
+ {
+ m_perfv.panel_size = 16;
+ m_perfv.relax = 1;
+ m_perfv.maxsuper = 128;
+ m_perfv.rowblk = 16;
+ m_perfv.colblk = 8;
+ m_perfv.fillfactor = 20;
+ }
+
+ // Variables
+ mutable ComputationInfo m_info;
+ bool m_factorizationIsOk;
+ bool m_analysisIsOk;
+ std::string m_lastError;
+ NCMatrix m_mat; // The input (permuted ) matrix
+ SCMatrix m_Lstore; // The lower triangular matrix (supernodal)
+ MappedSparseMatrix<Scalar,ColMajor,StorageIndex> m_Ustore; // The upper triangular matrix
+ PermutationType m_perm_c; // Column permutation
+ PermutationType m_perm_r ; // Row permutation
+ IndexVector m_etree; // Column elimination tree
+
+ typename Base::GlobalLU_t m_glu;
+
+ // SparseLU options
+ bool m_symmetricmode;
+ // values for performance
+ internal::perfvalues m_perfv;
+ RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot
+ Index m_nnzL, m_nnzU; // Nonzeros in L and U factors
+ Index m_detPermR, m_detPermC; // Determinants of the permutation matrices
+ private:
+ // Disable copy constructor
+ SparseLU (const SparseLU& );
+}; // End class SparseLU
+
+
+
+// Functions needed by the anaysis phase
+/**
+ * Compute the column permutation to minimize the fill-in
+ *
+ * - Apply this permutation to the input matrix -
+ *
+ * - Compute the column elimination tree on the permuted matrix
+ *
+ * - Postorder the elimination tree and the column permutation
+ *
+ */
+template <typename MatrixType, typename OrderingType>
+void SparseLU<MatrixType, OrderingType>::analyzePattern(const MatrixType& mat)
+{
+
+ //TODO It is possible as in SuperLU to compute row and columns scaling vectors to equilibrate the matrix mat.
+
+ // Firstly, copy the whole input matrix.
+ m_mat = mat;
+
+ // Compute fill-in ordering
+ OrderingType ord;
+ ord(m_mat,m_perm_c);
+
+ // Apply the permutation to the column of the input matrix
+ if (m_perm_c.size())
+ {
+ m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers. FIXME : This vector is filled but not subsequently used.
+ // Then, permute only the column pointers
+ ei_declare_aligned_stack_constructed_variable(StorageIndex,outerIndexPtr,mat.cols()+1,mat.isCompressed()?const_cast<StorageIndex*>(mat.outerIndexPtr()):0);
+
+ // If the input matrix 'mat' is uncompressed, then the outer-indices do not match the ones of m_mat, and a copy is thus needed.
+ if(!mat.isCompressed())
+ IndexVector::Map(outerIndexPtr, mat.cols()+1) = IndexVector::Map(m_mat.outerIndexPtr(),mat.cols()+1);
+
+ // Apply the permutation and compute the nnz per column.
+ for (Index i = 0; i < mat.cols(); i++)
+ {
+ m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
+ m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+ }
+ }
+
+ // Compute the column elimination tree of the permuted matrix
+ IndexVector firstRowElt;
+ internal::coletree(m_mat, m_etree,firstRowElt);
+
+ // In symmetric mode, do not do postorder here
+ if (!m_symmetricmode) {
+ IndexVector post, iwork;
+ // Post order etree
+ internal::treePostorder(StorageIndex(m_mat.cols()), m_etree, post);
+
+
+ // Renumber etree in postorder
+ Index m = m_mat.cols();
+ iwork.resize(m+1);
+ for (Index i = 0; i < m; ++i) iwork(post(i)) = post(m_etree(i));
+ m_etree = iwork;
+
+ // Postmultiply A*Pc by post, i.e reorder the matrix according to the postorder of the etree
+ PermutationType post_perm(m);
+ for (Index i = 0; i < m; i++)
+ post_perm.indices()(i) = post(i);
+
+ // Combine the two permutations : postorder the permutation for future use
+ if(m_perm_c.size()) {
+ m_perm_c = post_perm * m_perm_c;
+ }
+
+ } // end postordering
+
+ m_analysisIsOk = true;
+}
+
+// Functions needed by the numerical factorization phase
+
+
+/**
+ * - Numerical factorization
+ * - Interleaved with the symbolic factorization
+ * On exit, info is
+ *
+ * = 0: successful factorization
+ *
+ * > 0: if info = i, and i is
+ *
+ * <= A->ncol: U(i,i) is exactly zero. The factorization has
+ * been completed, but the factor U is exactly singular,
+ * and division by zero will occur if it is used to solve a
+ * system of equations.
+ *
+ * > A->ncol: number of bytes allocated when memory allocation
+ * failure occurred, plus A->ncol. If lwork = -1, it is
+ * the estimated amount of space needed, plus A->ncol.
+ */
+template <typename MatrixType, typename OrderingType>
+void SparseLU<MatrixType, OrderingType>::factorize(const MatrixType& matrix)
+{
+ using internal::emptyIdxLU;
+ eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
+ eigen_assert((matrix.rows() == matrix.cols()) && "Only for squared matrices");
+
+ m_isInitialized = true;
+
+ // Apply the column permutation computed in analyzepattern()
+ // m_mat = matrix * m_perm_c.inverse();
+ m_mat = matrix;
+ if (m_perm_c.size())
+ {
+ m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers.
+ //Then, permute only the column pointers
+ const StorageIndex * outerIndexPtr;
+ if (matrix.isCompressed()) outerIndexPtr = matrix.outerIndexPtr();
+ else
+ {
+ StorageIndex* outerIndexPtr_t = new StorageIndex[matrix.cols()+1];
+ for(Index i = 0; i <= matrix.cols(); i++) outerIndexPtr_t[i] = m_mat.outerIndexPtr()[i];
+ outerIndexPtr = outerIndexPtr_t;
+ }
+ for (Index i = 0; i < matrix.cols(); i++)
+ {
+ m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
+ m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+ }
+ if(!matrix.isCompressed()) delete[] outerIndexPtr;
+ }
+ else
+ { //FIXME This should not be needed if the empty permutation is handled transparently
+ m_perm_c.resize(matrix.cols());
+ for(StorageIndex i = 0; i < matrix.cols(); ++i) m_perm_c.indices()(i) = i;
+ }
+
+ Index m = m_mat.rows();
+ Index n = m_mat.cols();
+ Index nnz = m_mat.nonZeros();
+ Index maxpanel = m_perfv.panel_size * m;
+ // Allocate working storage common to the factor routines
+ Index lwork = 0;
+ Index info = Base::memInit(m, n, nnz, lwork, m_perfv.fillfactor, m_perfv.panel_size, m_glu);
+ if (info)
+ {
+ m_lastError = "UNABLE TO ALLOCATE WORKING MEMORY\n\n" ;
+ m_factorizationIsOk = false;
+ return ;
+ }
+
+ // Set up pointers for integer working arrays
+ IndexVector segrep(m); segrep.setZero();
+ IndexVector parent(m); parent.setZero();
+ IndexVector xplore(m); xplore.setZero();
+ IndexVector repfnz(maxpanel);
+ IndexVector panel_lsub(maxpanel);
+ IndexVector xprune(n); xprune.setZero();
+ IndexVector marker(m*internal::LUNoMarker); marker.setZero();
+
+ repfnz.setConstant(-1);
+ panel_lsub.setConstant(-1);
+
+ // Set up pointers for scalar working arrays
+ ScalarVector dense;
+ dense.setZero(maxpanel);
+ ScalarVector tempv;
+ tempv.setZero(internal::LUnumTempV(m, m_perfv.panel_size, m_perfv.maxsuper, /*m_perfv.rowblk*/m) );
+
+ // Compute the inverse of perm_c
+ PermutationType iperm_c(m_perm_c.inverse());
+
+ // Identify initial relaxed snodes
+ IndexVector relax_end(n);
+ if ( m_symmetricmode == true )
+ Base::heap_relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
+ else
+ Base::relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
+
+
+ m_perm_r.resize(m);
+ m_perm_r.indices().setConstant(-1);
+ marker.setConstant(-1);
+ m_detPermR = 1; // Record the determinant of the row permutation
+
+ m_glu.supno(0) = emptyIdxLU; m_glu.xsup.setConstant(0);
+ m_glu.xsup(0) = m_glu.xlsub(0) = m_glu.xusub(0) = m_glu.xlusup(0) = Index(0);
+
+ // Work on one 'panel' at a time. A panel is one of the following :
+ // (a) a relaxed supernode at the bottom of the etree, or
+ // (b) panel_size contiguous columns, <panel_size> defined by the user
+ Index jcol;
+ Index pivrow; // Pivotal row number in the original row matrix
+ Index nseg1; // Number of segments in U-column above panel row jcol
+ Index nseg; // Number of segments in each U-column
+ Index irep;
+ Index i, k, jj;
+ for (jcol = 0; jcol < n; )
+ {
+ // Adjust panel size so that a panel won't overlap with the next relaxed snode.
+ Index panel_size = m_perfv.panel_size; // upper bound on panel width
+ for (k = jcol + 1; k < (std::min)(jcol+panel_size, n); k++)
+ {
+ if (relax_end(k) != emptyIdxLU)
+ {
+ panel_size = k - jcol;
+ break;
+ }
+ }
+ if (k == n)
+ panel_size = n - jcol;
+
+ // Symbolic outer factorization on a panel of columns
+ Base::panel_dfs(m, panel_size, jcol, m_mat, m_perm_r.indices(), nseg1, dense, panel_lsub, segrep, repfnz, xprune, marker, parent, xplore, m_glu);
+
+ // Numeric sup-panel updates in topological order
+ Base::panel_bmod(m, panel_size, jcol, nseg1, dense, tempv, segrep, repfnz, m_glu);
+
+ // Sparse LU within the panel, and below the panel diagonal
+ for ( jj = jcol; jj< jcol + panel_size; jj++)
+ {
+ k = (jj - jcol) * m; // Column index for w-wide arrays
+
+ nseg = nseg1; // begin after all the panel segments
+ //Depth-first-search for the current column
+ VectorBlock<IndexVector> panel_lsubk(panel_lsub, k, m);
+ VectorBlock<IndexVector> repfnz_k(repfnz, k, m);
+ info = Base::column_dfs(m, jj, m_perm_r.indices(), m_perfv.maxsuper, nseg, panel_lsubk, segrep, repfnz_k, xprune, marker, parent, xplore, m_glu);
+ if ( info )
+ {
+ m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_DFS() ";
+ m_info = NumericalIssue;
+ m_factorizationIsOk = false;
+ return;
+ }
+ // Numeric updates to this column
+ VectorBlock<ScalarVector> dense_k(dense, k, m);
+ VectorBlock<IndexVector> segrep_k(segrep, nseg1, m-nseg1);
+ info = Base::column_bmod(jj, (nseg - nseg1), dense_k, tempv, segrep_k, repfnz_k, jcol, m_glu);
+ if ( info )
+ {
+ m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_BMOD() ";
+ m_info = NumericalIssue;
+ m_factorizationIsOk = false;
+ return;
+ }
+
+ // Copy the U-segments to ucol(*)
+ info = Base::copy_to_ucol(jj, nseg, segrep, repfnz_k ,m_perm_r.indices(), dense_k, m_glu);
+ if ( info )
+ {
+ m_lastError = "UNABLE TO EXPAND MEMORY IN COPY_TO_UCOL() ";
+ m_info = NumericalIssue;
+ m_factorizationIsOk = false;
+ return;
+ }
+
+ // Form the L-segment
+ info = Base::pivotL(jj, m_diagpivotthresh, m_perm_r.indices(), iperm_c.indices(), pivrow, m_glu);
+ if ( info )
+ {
+ m_lastError = "THE MATRIX IS STRUCTURALLY SINGULAR ... ZERO COLUMN AT ";
+ std::ostringstream returnInfo;
+ returnInfo << info;
+ m_lastError += returnInfo.str();
+ m_info = NumericalIssue;
+ m_factorizationIsOk = false;
+ return;
+ }
+
+ // Update the determinant of the row permutation matrix
+ // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot.
+ if (pivrow != jj) m_detPermR = -m_detPermR;
+
+ // Prune columns (0:jj-1) using column jj
+ Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu);
+
+ // Reset repfnz for this column
+ for (i = 0; i < nseg; i++)
+ {
+ irep = segrep(i);
+ repfnz_k(irep) = emptyIdxLU;
+ }
+ } // end SparseLU within the panel
+ jcol += panel_size; // Move to the next panel
+ } // end for -- end elimination
+
+ m_detPermR = m_perm_r.determinant();
+ m_detPermC = m_perm_c.determinant();
+
+ // Count the number of nonzeros in factors
+ Base::countnz(n, m_nnzL, m_nnzU, m_glu);
+ // Apply permutation to the L subscripts
+ Base::fixupL(n, m_perm_r.indices(), m_glu);
+
+ // Create supernode matrix L
+ m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup);
+ // Create the column major upper sparse matrix U;
+ new (&m_Ustore) MappedSparseMatrix<Scalar, ColMajor, StorageIndex> ( m, n, m_nnzU, m_glu.xusub.data(), m_glu.usub.data(), m_glu.ucol.data() );
+
+ m_info = Success;
+ m_factorizationIsOk = true;
+}
+
+template<typename MappedSupernodalType>
+struct SparseLUMatrixLReturnType : internal::no_assignment_operator
+{
+ typedef typename MappedSupernodalType::Scalar Scalar;
+ explicit SparseLUMatrixLReturnType(const MappedSupernodalType& mapL) : m_mapL(mapL)
+ { }
+ Index rows() const { return m_mapL.rows(); }
+ Index cols() const { return m_mapL.cols(); }
+ template<typename Dest>
+ void solveInPlace( MatrixBase<Dest> &X) const
+ {
+ m_mapL.solveInPlace(X);
+ }
+ template<bool Conjugate, typename Dest>
+ void solveTransposedInPlace( MatrixBase<Dest> &X) const
+ {
+ m_mapL.template solveTransposedInPlace<Conjugate>(X);
+ }
+
+ const MappedSupernodalType& m_mapL;
+};
+
+template<typename MatrixLType, typename MatrixUType>
+struct SparseLUMatrixUReturnType : internal::no_assignment_operator
+{
+ typedef typename MatrixLType::Scalar Scalar;
+ SparseLUMatrixUReturnType(const MatrixLType& mapL, const MatrixUType& mapU)
+ : m_mapL(mapL),m_mapU(mapU)
+ { }
+ Index rows() const { return m_mapL.rows(); }
+ Index cols() const { return m_mapL.cols(); }
+
+ template<typename Dest> void solveInPlace(MatrixBase<Dest> &X) const
+ {
+ Index nrhs = X.cols();
+ Index n = X.rows();
+ // Backward solve with U
+ for (Index k = m_mapL.nsuper(); k >= 0; k--)
+ {
+ Index fsupc = m_mapL.supToCol()[k];
+ Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
+ Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+ Index luptr = m_mapL.colIndexPtr()[fsupc];
+
+ if (nsupc == 1)
+ {
+ for (Index j = 0; j < nrhs; j++)
+ {
+ X(fsupc, j) /= m_mapL.valuePtr()[luptr];
+ }
+ }
+ else
+ {
+ // FIXME: the following lines should use Block expressions and not Map!
+ Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+ Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X.coeffRef(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+ U = A.template triangularView<Upper>().solve(U);
+ }
+
+ for (Index j = 0; j < nrhs; ++j)
+ {
+ for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
+ {
+ typename MatrixUType::InnerIterator it(m_mapU, jcol);
+ for ( ; it; ++it)
+ {
+ Index irow = it.index();
+ X(irow, j) -= X(jcol, j) * it.value();
+ }
+ }
+ }
+ } // End For U-solve
+ }
+
+ template<bool Conjugate, typename Dest> void solveTransposedInPlace(MatrixBase<Dest> &X) const
+ {
+ using numext::conj;
+ Index nrhs = X.cols();
+ Index n = X.rows();
+ // Forward solve with U
+ for (Index k = 0; k <= m_mapL.nsuper(); k++)
+ {
+ Index fsupc = m_mapL.supToCol()[k];
+ Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
+ Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+ Index luptr = m_mapL.colIndexPtr()[fsupc];
+
+ for (Index j = 0; j < nrhs; ++j)
+ {
+ for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
+ {
+ typename MatrixUType::InnerIterator it(m_mapU, jcol);
+ for ( ; it; ++it)
+ {
+ Index irow = it.index();
+ X(jcol, j) -= X(irow, j) * (Conjugate? conj(it.value()): it.value());
+ }
+ }
+ }
+ if (nsupc == 1)
+ {
+ for (Index j = 0; j < nrhs; j++)
+ {
+ X(fsupc, j) /= (Conjugate? conj(m_mapL.valuePtr()[luptr]) : m_mapL.valuePtr()[luptr]);
+ }
+ }
+ else
+ {
+ Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+ Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+ if(Conjugate)
+ U = A.adjoint().template triangularView<Lower>().solve(U);
+ else
+ U = A.transpose().template triangularView<Lower>().solve(U);
+ }
+ }// End For U-solve
+ }
+
+
+ const MatrixLType& m_mapL;
+ const MatrixUType& m_mapU;
+};
+
+} // End namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
new file mode 100644
index 0000000..fc0cfc4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
@@ -0,0 +1,66 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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 SPARSELU_IMPL_H
+#define SPARSELU_IMPL_H
+
+namespace Eigen {
+namespace internal {
+
+/** \ingroup SparseLU_Module
+ * \class SparseLUImpl
+ * Base class for sparseLU
+ */
+template <typename Scalar, typename StorageIndex>
+class SparseLUImpl
+{
+ public:
+ typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+ typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+ typedef Matrix<Scalar,Dynamic,Dynamic,ColMajor> ScalarMatrix;
+ typedef Map<ScalarMatrix, 0, OuterStride<> > MappedMatrixBlock;
+ typedef typename ScalarVector::RealScalar RealScalar;
+ typedef Ref<Matrix<Scalar,Dynamic,1> > BlockScalarVector;
+ typedef Ref<Matrix<StorageIndex,Dynamic,1> > BlockIndexVector;
+ typedef LU_GlobalLU_t<IndexVector, ScalarVector> GlobalLU_t;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> MatrixType;
+
+ protected:
+ template <typename VectorType>
+ Index expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions);
+ Index memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu);
+ template <typename VectorType>
+ Index memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions);
+ void heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end);
+ void relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end);
+ Index snode_dfs(const Index jcol, const Index kcol,const MatrixType& mat, IndexVector& xprune, IndexVector& marker, GlobalLU_t& glu);
+ Index snode_bmod (const Index jcol, const Index fsupc, ScalarVector& dense, GlobalLU_t& glu);
+ Index pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu);
+ template <typename Traits>
+ void dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
+ Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
+ Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
+ IndexVector& xplore, GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits);
+ void panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+
+ void panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg, ScalarVector& dense, ScalarVector& tempv, IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu);
+ Index column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg, BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+ Index column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv, BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu);
+ Index copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu);
+ void pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg, const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu);
+ void countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu);
+ void fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu);
+
+ template<typename , typename >
+ friend struct column_dfs_traits;
+};
+
+} // end namespace internal
+} // namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
new file mode 100644
index 0000000..349bfd5
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]memory.c files in SuperLU
+
+ * -- SuperLU routine (version 3.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * August 1, 2008
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef EIGEN_SPARSELU_MEMORY
+#define EIGEN_SPARSELU_MEMORY
+
+namespace Eigen {
+namespace internal {
+
+enum { LUNoMarker = 3 };
+enum {emptyIdxLU = -1};
+inline Index LUnumTempV(Index& m, Index& w, Index& t, Index& b)
+{
+ return (std::max)(m, (t+b)*w);
+}
+
+template< typename Scalar>
+inline Index LUTempSpace(Index&m, Index& w)
+{
+ return (2*w + 4 + LUNoMarker) * m * sizeof(Index) + (w + 1) * m * sizeof(Scalar);
+}
+
+
+
+
+/**
+ * Expand the existing storage to accommodate more fill-ins
+ * \param vec Valid pointer to the vector to allocate or expand
+ * \param[in,out] length At input, contain the current length of the vector that is to be increased. At output, length of the newly allocated vector
+ * \param[in] nbElts Current number of elements in the factors
+ * \param keep_prev 1: use length and do not expand the vector; 0: compute new_len and expand
+ * \param[in,out] num_expansions Number of times the memory has been expanded
+ */
+template <typename Scalar, typename StorageIndex>
+template <typename VectorType>
+Index SparseLUImpl<Scalar,StorageIndex>::expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions)
+{
+
+ float alpha = 1.5; // Ratio of the memory increase
+ Index new_len; // New size of the allocated memory
+
+ if(num_expansions == 0 || keep_prev)
+ new_len = length ; // First time allocate requested
+ else
+ new_len = (std::max)(length+1,Index(alpha * length));
+
+ VectorType old_vec; // Temporary vector to hold the previous values
+ if (nbElts > 0 )
+ old_vec = vec.segment(0,nbElts);
+
+ //Allocate or expand the current vector
+#ifdef EIGEN_EXCEPTIONS
+ try
+#endif
+ {
+ vec.resize(new_len);
+ }
+#ifdef EIGEN_EXCEPTIONS
+ catch(std::bad_alloc& )
+#else
+ if(!vec.size())
+#endif
+ {
+ if (!num_expansions)
+ {
+ // First time to allocate from LUMemInit()
+ // Let LUMemInit() deals with it.
+ return -1;
+ }
+ if (keep_prev)
+ {
+ // In this case, the memory length should not not be reduced
+ return new_len;
+ }
+ else
+ {
+ // Reduce the size and increase again
+ Index tries = 0; // Number of attempts
+ do
+ {
+ alpha = (alpha + 1)/2;
+ new_len = (std::max)(length+1,Index(alpha * length));
+#ifdef EIGEN_EXCEPTIONS
+ try
+#endif
+ {
+ vec.resize(new_len);
+ }
+#ifdef EIGEN_EXCEPTIONS
+ catch(std::bad_alloc& )
+#else
+ if (!vec.size())
+#endif
+ {
+ tries += 1;
+ if ( tries > 10) return new_len;
+ }
+ } while (!vec.size());
+ }
+ }
+ //Copy the previous values to the newly allocated space
+ if (nbElts > 0)
+ vec.segment(0, nbElts) = old_vec;
+
+
+ length = new_len;
+ if(num_expansions) ++num_expansions;
+ return 0;
+}
+
+/**
+ * \brief Allocate various working space for the numerical factorization phase.
+ * \param m number of rows of the input matrix
+ * \param n number of columns
+ * \param annz number of initial nonzeros in the matrix
+ * \param lwork if lwork=-1, this routine returns an estimated size of the required memory
+ * \param glu persistent data to facilitate multiple factors : will be deleted later ??
+ * \param fillratio estimated ratio of fill in the factors
+ * \param panel_size Size of a panel
+ * \return an estimated size of the required memory if lwork = -1; otherwise, return the size of actually allocated memory when allocation failed, and 0 on success
+ * \note Unlike SuperLU, this routine does not support successive factorization with the same pattern and the same row permutation
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu)
+{
+ Index& num_expansions = glu.num_expansions; //No memory expansions so far
+ num_expansions = 0;
+ glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U
+ glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated nnz in L factor
+ // Return the estimated size to the user if necessary
+ Index tempSpace;
+ tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar);
+ if (lwork == emptyIdxLU)
+ {
+ Index estimated_size;
+ estimated_size = (5 * n + 5) * sizeof(Index) + tempSpace
+ + (glu.nzlmax + glu.nzumax) * sizeof(Index) + (glu.nzlumax+glu.nzumax) * sizeof(Scalar) + n;
+ return estimated_size;
+ }
+
+ // Setup the required space
+
+ // First allocate Integer pointers for L\U factors
+ glu.xsup.resize(n+1);
+ glu.supno.resize(n+1);
+ glu.xlsub.resize(n+1);
+ glu.xlusup.resize(n+1);
+ glu.xusub.resize(n+1);
+
+ // Reserve memory for L/U factors
+ do
+ {
+ if( (expand<ScalarVector>(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0)
+ || (expand<ScalarVector>(glu.ucol, glu.nzumax, 0, 0, num_expansions)<0)
+ || (expand<IndexVector> (glu.lsub, glu.nzlmax, 0, 0, num_expansions)<0)
+ || (expand<IndexVector> (glu.usub, glu.nzumax, 0, 1, num_expansions)<0) )
+ {
+ //Reduce the estimated size and retry
+ glu.nzlumax /= 2;
+ glu.nzumax /= 2;
+ glu.nzlmax /= 2;
+ if (glu.nzlumax < annz ) return glu.nzlumax;
+ }
+ } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size());
+
+ ++num_expansions;
+ return 0;
+
+} // end LuMemInit
+
+/**
+ * \brief Expand the existing storage
+ * \param vec vector to expand
+ * \param[in,out] maxlen On input, previous size of vec (Number of elements to copy ). on output, new size
+ * \param nbElts current number of elements in the vector.
+ * \param memtype Type of the element to expand
+ * \param num_expansions Number of expansions
+ * \return 0 on success, > 0 size of the memory allocated so far
+ */
+template <typename Scalar, typename StorageIndex>
+template <typename VectorType>
+Index SparseLUImpl<Scalar,StorageIndex>::memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions)
+{
+ Index failed_size;
+ if (memtype == USUB)
+ failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 1, num_expansions);
+ else
+ failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 0, num_expansions);
+
+ if (failed_size)
+ return failed_size;
+
+ return 0 ;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_SPARSELU_MEMORY
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
new file mode 100644
index 0000000..cf5ec44
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
@@ -0,0 +1,110 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+ * NOTE: This file comes from a partly modified version of files slu_[s,d,c,z]defs.h
+ * -- SuperLU routine (version 4.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November, 2010
+ *
+ * Global data structures used in LU factorization -
+ *
+ * nsuper: #supernodes = nsuper + 1, numbered [0, nsuper].
+ * (xsup,supno): supno[i] is the supernode no to which i belongs;
+ * xsup(s) points to the beginning of the s-th supernode.
+ * e.g. supno 0 1 2 2 3 3 3 4 4 4 4 4 (n=12)
+ * xsup 0 1 2 4 7 12
+ * Note: dfs will be performed on supernode rep. relative to the new
+ * row pivoting ordering
+ *
+ * (xlsub,lsub): lsub[*] contains the compressed subscript of
+ * rectangular supernodes; xlsub[j] points to the starting
+ * location of the j-th column in lsub[*]. Note that xlsub
+ * is indexed by column.
+ * Storage: original row subscripts
+ *
+ * During the course of sparse LU factorization, we also use
+ * (xlsub,lsub) for the purpose of symmetric pruning. For each
+ * supernode {s,s+1,...,t=s+r} with first column s and last
+ * column t, the subscript set
+ * lsub[j], j=xlsub[s], .., xlsub[s+1]-1
+ * is the structure of column s (i.e. structure of this supernode).
+ * It is used for the storage of numerical values.
+ * Furthermore,
+ * lsub[j], j=xlsub[t], .., xlsub[t+1]-1
+ * is the structure of the last column t of this supernode.
+ * It is for the purpose of symmetric pruning. Therefore, the
+ * structural subscripts can be rearranged without making physical
+ * interchanges among the numerical values.
+ *
+ * However, if the supernode has only one column, then we
+ * only keep one set of subscripts. For any subscript interchange
+ * performed, similar interchange must be done on the numerical
+ * values.
+ *
+ * The last column structures (for pruning) will be removed
+ * after the numercial LU factorization phase.
+ *
+ * (xlusup,lusup): lusup[*] contains the numerical values of the
+ * rectangular supernodes; xlusup[j] points to the starting
+ * location of the j-th column in storage vector lusup[*]
+ * Note: xlusup is indexed by column.
+ * Each rectangular supernode is stored by column-major
+ * scheme, consistent with Fortran 2-dim array storage.
+ *
+ * (xusub,ucol,usub): ucol[*] stores the numerical values of
+ * U-columns outside the rectangular supernodes. The row
+ * subscript of nonzero ucol[k] is stored in usub[k].
+ * xusub[i] points to the starting location of column i in ucol.
+ * Storage: new row subscripts; that is subscripts of PA.
+ */
+
+#ifndef EIGEN_LU_STRUCTS
+#define EIGEN_LU_STRUCTS
+namespace Eigen {
+namespace internal {
+
+typedef enum {LUSUP, UCOL, LSUB, USUB, LLVL, ULVL} MemType;
+
+template <typename IndexVector, typename ScalarVector>
+struct LU_GlobalLU_t {
+ typedef typename IndexVector::Scalar StorageIndex;
+ IndexVector xsup; //First supernode column ... xsup(s) points to the beginning of the s-th supernode
+ IndexVector supno; // Supernode number corresponding to this column (column to supernode mapping)
+ ScalarVector lusup; // nonzero values of L ordered by columns
+ IndexVector lsub; // Compressed row indices of L rectangular supernodes.
+ IndexVector xlusup; // pointers to the beginning of each column in lusup
+ IndexVector xlsub; // pointers to the beginning of each column in lsub
+ Index nzlmax; // Current max size of lsub
+ Index nzlumax; // Current max size of lusup
+ ScalarVector ucol; // nonzero values of U ordered by columns
+ IndexVector usub; // row indices of U columns in ucol
+ IndexVector xusub; // Pointers to the beginning of each column of U in ucol
+ Index nzumax; // Current max size of ucol
+ Index n; // Number of columns in the matrix
+ Index num_expansions;
+};
+
+// Values to set for performance
+struct perfvalues {
+ Index panel_size; // a panel consists of at most <panel_size> consecutive columns
+ Index relax; // To control degree of relaxing supernodes. If the number of nodes (columns)
+ // in a subtree of the elimination tree is less than relax, this subtree is considered
+ // as one supernode regardless of the row structures of those columns
+ Index maxsuper; // The maximum size for a supernode in complete LU
+ Index rowblk; // The minimum row dimension for 2-D blocking to be used;
+ Index colblk; // The minimum column dimension for 2-D blocking to be used;
+ Index fillfactor; // The estimated fills factors for L and U, compared with A
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_LU_STRUCTS
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
new file mode 100644
index 0000000..0be293d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
@@ -0,0 +1,375 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_SUPERNODAL_MATRIX_H
+#define EIGEN_SPARSELU_SUPERNODAL_MATRIX_H
+
+namespace Eigen {
+namespace internal {
+
+/** \ingroup SparseLU_Module
+ * \brief a class to manipulate the L supernodal factor from the SparseLU factorization
+ *
+ * This class contain the data to easily store
+ * and manipulate the supernodes during the factorization and solution phase of Sparse LU.
+ * Only the lower triangular matrix has supernodes.
+ *
+ * NOTE : This class corresponds to the SCformat structure in SuperLU
+ *
+ */
+/* TODO
+ * InnerIterator as for sparsematrix
+ * SuperInnerIterator to iterate through all supernodes
+ * Function for triangular solve
+ */
+template <typename _Scalar, typename _StorageIndex>
+class MappedSuperNodalMatrix
+{
+ public:
+ typedef _Scalar Scalar;
+ typedef _StorageIndex StorageIndex;
+ typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+ typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+ public:
+ MappedSuperNodalMatrix()
+ {
+
+ }
+ MappedSuperNodalMatrix(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+ IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
+ {
+ setInfos(m, n, nzval, nzval_colptr, rowind, rowind_colptr, col_to_sup, sup_to_col);
+ }
+
+ ~MappedSuperNodalMatrix()
+ {
+
+ }
+ /**
+ * Set appropriate pointers for the lower triangular supernodal matrix
+ * These infos are available at the end of the numerical factorization
+ * FIXME This class will be modified such that it can be use in the course
+ * of the factorization.
+ */
+ void setInfos(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+ IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
+ {
+ m_row = m;
+ m_col = n;
+ m_nzval = nzval.data();
+ m_nzval_colptr = nzval_colptr.data();
+ m_rowind = rowind.data();
+ m_rowind_colptr = rowind_colptr.data();
+ m_nsuper = col_to_sup(n);
+ m_col_to_sup = col_to_sup.data();
+ m_sup_to_col = sup_to_col.data();
+ }
+
+ /**
+ * Number of rows
+ */
+ Index rows() const { return m_row; }
+
+ /**
+ * Number of columns
+ */
+ Index cols() const { return m_col; }
+
+ /**
+ * Return the array of nonzero values packed by column
+ *
+ * The size is nnz
+ */
+ Scalar* valuePtr() { return m_nzval; }
+
+ const Scalar* valuePtr() const
+ {
+ return m_nzval;
+ }
+ /**
+ * Return the pointers to the beginning of each column in \ref valuePtr()
+ */
+ StorageIndex* colIndexPtr()
+ {
+ return m_nzval_colptr;
+ }
+
+ const StorageIndex* colIndexPtr() const
+ {
+ return m_nzval_colptr;
+ }
+
+ /**
+ * Return the array of compressed row indices of all supernodes
+ */
+ StorageIndex* rowIndex() { return m_rowind; }
+
+ const StorageIndex* rowIndex() const
+ {
+ return m_rowind;
+ }
+
+ /**
+ * Return the location in \em rowvaluePtr() which starts each column
+ */
+ StorageIndex* rowIndexPtr() { return m_rowind_colptr; }
+
+ const StorageIndex* rowIndexPtr() const
+ {
+ return m_rowind_colptr;
+ }
+
+ /**
+ * Return the array of column-to-supernode mapping
+ */
+ StorageIndex* colToSup() { return m_col_to_sup; }
+
+ const StorageIndex* colToSup() const
+ {
+ return m_col_to_sup;
+ }
+ /**
+ * Return the array of supernode-to-column mapping
+ */
+ StorageIndex* supToCol() { return m_sup_to_col; }
+
+ const StorageIndex* supToCol() const
+ {
+ return m_sup_to_col;
+ }
+
+ /**
+ * Return the number of supernodes
+ */
+ Index nsuper() const
+ {
+ return m_nsuper;
+ }
+
+ class InnerIterator;
+ template<typename Dest>
+ void solveInPlace( MatrixBase<Dest>&X) const;
+ template<bool Conjugate, typename Dest>
+ void solveTransposedInPlace( MatrixBase<Dest>&X) const;
+
+
+
+
+
+ protected:
+ Index m_row; // Number of rows
+ Index m_col; // Number of columns
+ Index m_nsuper; // Number of supernodes
+ Scalar* m_nzval; //array of nonzero values packed by column
+ StorageIndex* m_nzval_colptr; //nzval_colptr[j] Stores the location in nzval[] which starts column j
+ StorageIndex* m_rowind; // Array of compressed row indices of rectangular supernodes
+ StorageIndex* m_rowind_colptr; //rowind_colptr[j] stores the location in rowind[] which starts column j
+ StorageIndex* m_col_to_sup; // col_to_sup[j] is the supernode number to which column j belongs
+ StorageIndex* m_sup_to_col; //sup_to_col[s] points to the starting column of the s-th supernode
+
+ private :
+};
+
+/**
+ * \brief InnerIterator class to iterate over nonzero values of the current column in the supernodal matrix L
+ *
+ */
+template<typename Scalar, typename StorageIndex>
+class MappedSuperNodalMatrix<Scalar,StorageIndex>::InnerIterator
+{
+ public:
+ InnerIterator(const MappedSuperNodalMatrix& mat, Index outer)
+ : m_matrix(mat),
+ m_outer(outer),
+ m_supno(mat.colToSup()[outer]),
+ m_idval(mat.colIndexPtr()[outer]),
+ m_startidval(m_idval),
+ m_endidval(mat.colIndexPtr()[outer+1]),
+ m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]),
+ m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1])
+ {}
+ inline InnerIterator& operator++()
+ {
+ m_idval++;
+ m_idrow++;
+ return *this;
+ }
+ inline Scalar value() const { return m_matrix.valuePtr()[m_idval]; }
+
+ inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix.valuePtr()[m_idval]); }
+
+ inline Index index() const { return m_matrix.rowIndex()[m_idrow]; }
+ inline Index row() const { return index(); }
+ inline Index col() const { return m_outer; }
+
+ inline Index supIndex() const { return m_supno; }
+
+ inline operator bool() const
+ {
+ return ( (m_idval < m_endidval) && (m_idval >= m_startidval)
+ && (m_idrow < m_endidrow) );
+ }
+
+ protected:
+ const MappedSuperNodalMatrix& m_matrix; // Supernodal lower triangular matrix
+ const Index m_outer; // Current column
+ const Index m_supno; // Current SuperNode number
+ Index m_idval; // Index to browse the values in the current column
+ const Index m_startidval; // Start of the column value
+ const Index m_endidval; // End of the column value
+ Index m_idrow; // Index to browse the row indices
+ Index m_endidrow; // End index of row indices of the current column
+};
+
+/**
+ * \brief Solve with the supernode triangular matrix
+ *
+ */
+template<typename Scalar, typename Index_>
+template<typename Dest>
+void MappedSuperNodalMatrix<Scalar,Index_>::solveInPlace( MatrixBase<Dest>&X) const
+{
+ /* Explicit type conversion as the Index type of MatrixBase<Dest> may be wider than Index */
+// eigen_assert(X.rows() <= NumTraits<Index>::highest());
+// eigen_assert(X.cols() <= NumTraits<Index>::highest());
+ Index n = int(X.rows());
+ Index nrhs = Index(X.cols());
+ const Scalar * Lval = valuePtr(); // Nonzero values
+ Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs); // working vector
+ work.setZero();
+ for (Index k = 0; k <= nsuper(); k ++)
+ {
+ Index fsupc = supToCol()[k]; // First column of the current supernode
+ Index istart = rowIndexPtr()[fsupc]; // Pointer index to the subscript of the current column
+ Index nsupr = rowIndexPtr()[fsupc+1] - istart; // Number of rows in the current supernode
+ Index nsupc = supToCol()[k+1] - fsupc; // Number of columns in the current supernode
+ Index nrow = nsupr - nsupc; // Number of rows in the non-diagonal part of the supernode
+ Index irow; //Current index row
+
+ if (nsupc == 1 )
+ {
+ for (Index j = 0; j < nrhs; j++)
+ {
+ InnerIterator it(*this, fsupc);
+ ++it; // Skip the diagonal element
+ for (; it; ++it)
+ {
+ irow = it.row();
+ X(irow, j) -= X(fsupc, j) * it.value();
+ }
+ }
+ }
+ else
+ {
+ // The supernode has more than one column
+ Index luptr = colIndexPtr()[fsupc];
+ Index lda = colIndexPtr()[fsupc+1] - luptr;
+
+ // Triangular solve
+ Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+ Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+ U = A.template triangularView<UnitLower>().solve(U);
+
+ // Matrix-vector product
+ new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+ work.topRows(nrow).noalias() = A * U;
+
+ //Begin Scatter
+ for (Index j = 0; j < nrhs; j++)
+ {
+ Index iptr = istart + nsupc;
+ for (Index i = 0; i < nrow; i++)
+ {
+ irow = rowIndex()[iptr];
+ X(irow, j) -= work(i, j); // Scatter operation
+ work(i, j) = Scalar(0);
+ iptr++;
+ }
+ }
+ }
+ }
+}
+
+template<typename Scalar, typename Index_>
+template<bool Conjugate, typename Dest>
+void MappedSuperNodalMatrix<Scalar,Index_>::solveTransposedInPlace( MatrixBase<Dest>&X) const
+{
+ using numext::conj;
+ Index n = int(X.rows());
+ Index nrhs = Index(X.cols());
+ const Scalar * Lval = valuePtr(); // Nonzero values
+ Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs); // working vector
+ work.setZero();
+ for (Index k = nsuper(); k >= 0; k--)
+ {
+ Index fsupc = supToCol()[k]; // First column of the current supernode
+ Index istart = rowIndexPtr()[fsupc]; // Pointer index to the subscript of the current column
+ Index nsupr = rowIndexPtr()[fsupc+1] - istart; // Number of rows in the current supernode
+ Index nsupc = supToCol()[k+1] - fsupc; // Number of columns in the current supernode
+ Index nrow = nsupr - nsupc; // Number of rows in the non-diagonal part of the supernode
+ Index irow; //Current index row
+
+ if (nsupc == 1 )
+ {
+ for (Index j = 0; j < nrhs; j++)
+ {
+ InnerIterator it(*this, fsupc);
+ ++it; // Skip the diagonal element
+ for (; it; ++it)
+ {
+ irow = it.row();
+ X(fsupc,j) -= X(irow, j) * (Conjugate?conj(it.value()):it.value());
+ }
+ }
+ }
+ else
+ {
+ // The supernode has more than one column
+ Index luptr = colIndexPtr()[fsupc];
+ Index lda = colIndexPtr()[fsupc+1] - luptr;
+
+ //Begin Gather
+ for (Index j = 0; j < nrhs; j++)
+ {
+ Index iptr = istart + nsupc;
+ for (Index i = 0; i < nrow; i++)
+ {
+ irow = rowIndex()[iptr];
+ work.topRows(nrow)(i,j)= X(irow,j); // Gather operation
+ iptr++;
+ }
+ }
+
+ // Matrix-vector product with transposed submatrix
+ Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+ Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+ if(Conjugate)
+ U = U - A.adjoint() * work.topRows(nrow);
+ else
+ U = U - A.transpose() * work.topRows(nrow);
+
+ // Triangular solve (of transposed diagonal block)
+ new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+ if(Conjugate)
+ U = A.adjoint().template triangularView<UnitUpper>().solve(U);
+ else
+ U = A.transpose().template triangularView<UnitUpper>().solve(U);
+
+ }
+
+ }
+}
+
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSELU_MATRIX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
new file mode 100644
index 0000000..9e3dab4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
@@ -0,0 +1,80 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_SPARSELU_UTILS_H
+#define EIGEN_SPARSELU_UTILS_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Count Nonzero elements in the factors
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu)
+{
+ nnzL = 0;
+ nnzU = (glu.xusub)(n);
+ Index nsuper = (glu.supno)(n);
+ Index jlen;
+ Index i, j, fsupc;
+ if (n <= 0 ) return;
+ // For each supernode
+ for (i = 0; i <= nsuper; i++)
+ {
+ fsupc = glu.xsup(i);
+ jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc);
+
+ for (j = fsupc; j < glu.xsup(i+1); j++)
+ {
+ nnzL += jlen;
+ nnzU += j - fsupc + 1;
+ jlen--;
+ }
+ }
+}
+
+/**
+ * \brief Fix up the data storage lsub for L-subscripts.
+ *
+ * It removes the subscripts sets for structural pruning,
+ * and applies permutation to the remaining subscripts
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu)
+{
+ Index fsupc, i, j, k, jstart;
+
+ StorageIndex nextl = 0;
+ Index nsuper = (glu.supno)(n);
+
+ // For each supernode
+ for (i = 0; i <= nsuper; i++)
+ {
+ fsupc = glu.xsup(i);
+ jstart = glu.xlsub(fsupc);
+ glu.xlsub(fsupc) = nextl;
+ for (j = jstart; j < glu.xlsub(fsupc + 1); j++)
+ {
+ glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A
+ nextl++;
+ }
+ for (k = fsupc+1; k < glu.xsup(i+1); k++)
+ glu.xlsub(k) = nextl; // other columns in supernode i
+ }
+
+ glu.xlsub(n) = nextl;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_SPARSELU_UTILS_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
new file mode 100644
index 0000000..b57f068
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
@@ -0,0 +1,181 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of xcolumn_bmod.c file in SuperLU
+
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COLUMN_BMOD_H
+#define SPARSELU_COLUMN_BMOD_H
+
+namespace Eigen {
+
+namespace internal {
+/**
+ * \brief Performs numeric block updates (sup-col) in topological order
+ *
+ * \param jcol current column to update
+ * \param nseg Number of segments in the U part
+ * \param dense Store the full representation of the column
+ * \param tempv working array
+ * \param segrep segment representative ...
+ * \param repfnz ??? First nonzero column in each row ??? ...
+ * \param fpanelc First column in the current panel
+ * \param glu Global LU data.
+ * \return 0 - successful return
+ * > 0 - number of bytes allocated when run out of space
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv,
+ BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu)
+{
+ Index jsupno, k, ksub, krep, ksupno;
+ Index lptr, nrow, isub, irow, nextlu, new_next, ufirst;
+ Index fsupc, nsupc, nsupr, luptr, kfnz, no_zeros;
+ /* krep = representative of current k-th supernode
+ * fsupc = first supernodal column
+ * nsupc = number of columns in a supernode
+ * nsupr = number of rows in a supernode
+ * luptr = location of supernodal LU-block in storage
+ * kfnz = first nonz in the k-th supernodal segment
+ * no_zeros = no lf leading zeros in a supernodal U-segment
+ */
+
+ jsupno = glu.supno(jcol);
+ // For each nonzero supernode segment of U[*,j] in topological order
+ k = nseg - 1;
+ Index d_fsupc; // distance between the first column of the current panel and the
+ // first column of the current snode
+ Index fst_col; // First column within small LU update
+ Index segsize;
+ for (ksub = 0; ksub < nseg; ksub++)
+ {
+ krep = segrep(k); k--;
+ ksupno = glu.supno(krep);
+ if (jsupno != ksupno )
+ {
+ // outside the rectangular supernode
+ fsupc = glu.xsup(ksupno);
+ fst_col = (std::max)(fsupc, fpanelc);
+
+ // Distance from the current supernode to the current panel;
+ // d_fsupc = 0 if fsupc > fpanelc
+ d_fsupc = fst_col - fsupc;
+
+ luptr = glu.xlusup(fst_col) + d_fsupc;
+ lptr = glu.xlsub(fsupc) + d_fsupc;
+
+ kfnz = repfnz(krep);
+ kfnz = (std::max)(kfnz, fpanelc);
+
+ segsize = krep - kfnz + 1;
+ nsupc = krep - fst_col + 1;
+ nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc);
+ nrow = nsupr - d_fsupc - nsupc;
+ Index lda = glu.xlusup(fst_col+1) - glu.xlusup(fst_col);
+
+
+ // Perform a triangular solver and block update,
+ // then scatter the result of sup-col update to dense
+ no_zeros = kfnz - fst_col;
+ if(segsize==1)
+ LU_kernel_bmod<1>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ else
+ LU_kernel_bmod<Dynamic>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ } // end if jsupno
+ } // end for each segment
+
+ // Process the supernodal portion of L\U[*,j]
+ nextlu = glu.xlusup(jcol);
+ fsupc = glu.xsup(jsupno);
+
+ // copy the SPA dense into L\U[*,j]
+ Index mem;
+ new_next = nextlu + glu.xlsub(fsupc + 1) - glu.xlsub(fsupc);
+ Index offset = internal::first_multiple<Index>(new_next, internal::packet_traits<Scalar>::size) - new_next;
+ if(offset)
+ new_next += offset;
+ while (new_next > glu.nzlumax )
+ {
+ mem = memXpand<ScalarVector>(glu.lusup, glu.nzlumax, nextlu, LUSUP, glu.num_expansions);
+ if (mem) return mem;
+ }
+
+ for (isub = glu.xlsub(fsupc); isub < glu.xlsub(fsupc+1); isub++)
+ {
+ irow = glu.lsub(isub);
+ glu.lusup(nextlu) = dense(irow);
+ dense(irow) = Scalar(0.0);
+ ++nextlu;
+ }
+
+ if(offset)
+ {
+ glu.lusup.segment(nextlu,offset).setZero();
+ nextlu += offset;
+ }
+ glu.xlusup(jcol + 1) = StorageIndex(nextlu); // close L\U(*,jcol);
+
+ /* For more updates within the panel (also within the current supernode),
+ * should start from the first column of the panel, or the first column
+ * of the supernode, whichever is bigger. There are two cases:
+ * 1) fsupc < fpanelc, then fst_col <-- fpanelc
+ * 2) fsupc >= fpanelc, then fst_col <-- fsupc
+ */
+ fst_col = (std::max)(fsupc, fpanelc);
+
+ if (fst_col < jcol)
+ {
+ // Distance between the current supernode and the current panel
+ // d_fsupc = 0 if fsupc >= fpanelc
+ d_fsupc = fst_col - fsupc;
+
+ lptr = glu.xlsub(fsupc) + d_fsupc;
+ luptr = glu.xlusup(fst_col) + d_fsupc;
+ nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); // leading dimension
+ nsupc = jcol - fst_col; // excluding jcol
+ nrow = nsupr - d_fsupc - nsupc;
+
+ // points to the beginning of jcol in snode L\U(jsupno)
+ ufirst = glu.xlusup(jcol) + d_fsupc;
+ Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol);
+ MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+ VectorBlock<ScalarVector> u(glu.lusup, ufirst, nsupc);
+ u = A.template triangularView<UnitLower>().solve(u);
+
+ new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+ VectorBlock<ScalarVector> l(glu.lusup, ufirst+nsupc, nrow);
+ l.noalias() -= A * u;
+
+ } // End if fst_col
+ return 0;
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_COLUMN_BMOD_H
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
new file mode 100644
index 0000000..5a2c941
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
@@ -0,0 +1,179 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]column_dfs.c file in SuperLU
+
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COLUMN_DFS_H
+#define SPARSELU_COLUMN_DFS_H
+
+template <typename Scalar, typename StorageIndex> class SparseLUImpl;
+namespace Eigen {
+
+namespace internal {
+
+template<typename IndexVector, typename ScalarVector>
+struct column_dfs_traits : no_assignment_operator
+{
+ typedef typename ScalarVector::Scalar Scalar;
+ typedef typename IndexVector::Scalar StorageIndex;
+ column_dfs_traits(Index jcol, Index& jsuper, typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& glu, SparseLUImpl<Scalar, StorageIndex>& luImpl)
+ : m_jcol(jcol), m_jsuper_ref(jsuper), m_glu(glu), m_luImpl(luImpl)
+ {}
+ bool update_segrep(Index /*krep*/, Index /*jj*/)
+ {
+ return true;
+ }
+ void mem_expand(IndexVector& lsub, Index& nextl, Index chmark)
+ {
+ if (nextl >= m_glu.nzlmax)
+ m_luImpl.memXpand(lsub, m_glu.nzlmax, nextl, LSUB, m_glu.num_expansions);
+ if (chmark != (m_jcol-1)) m_jsuper_ref = emptyIdxLU;
+ }
+ enum { ExpandMem = true };
+
+ Index m_jcol;
+ Index& m_jsuper_ref;
+ typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& m_glu;
+ SparseLUImpl<Scalar, StorageIndex>& m_luImpl;
+};
+
+
+/**
+ * \brief Performs a symbolic factorization on column jcol and decide the supernode boundary
+ *
+ * A supernode representative is the last column of a supernode.
+ * The nonzeros in U[*,j] are segments that end at supernodes representatives.
+ * The routine returns a list of the supernodal representatives
+ * in topological order of the dfs that generates them.
+ * The location of the first nonzero in each supernodal segment
+ * (supernodal entry location) is also returned.
+ *
+ * \param m number of rows in the matrix
+ * \param jcol Current column
+ * \param perm_r Row permutation
+ * \param maxsuper Maximum number of column allowed in a supernode
+ * \param [in,out] nseg Number of segments in current U[*,j] - new segments appended
+ * \param lsub_col defines the rhs vector to start the dfs
+ * \param [in,out] segrep Segment representatives - new segments appended
+ * \param repfnz First nonzero location in each row
+ * \param xprune
+ * \param marker marker[i] == jj, if i was visited during dfs of current column jj;
+ * \param parent
+ * \param xplore working array
+ * \param glu global LU data
+ * \return 0 success
+ * > 0 number of bytes allocated when run out of space
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,
+ BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune,
+ IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
+{
+
+ Index jsuper = glu.supno(jcol);
+ Index nextl = glu.xlsub(jcol);
+ VectorBlock<IndexVector> marker2(marker, 2*m, m);
+
+
+ column_dfs_traits<IndexVector, ScalarVector> traits(jcol, jsuper, glu, *this);
+
+ // For each nonzero in A(*,jcol) do dfs
+ for (Index k = 0; ((k < m) ? lsub_col[k] != emptyIdxLU : false) ; k++)
+ {
+ Index krow = lsub_col(k);
+ lsub_col(k) = emptyIdxLU;
+ Index kmark = marker2(krow);
+
+ // krow was visited before, go to the next nonz;
+ if (kmark == jcol) continue;
+
+ dfs_kernel(StorageIndex(jcol), perm_r, nseg, glu.lsub, segrep, repfnz, xprune, marker2, parent,
+ xplore, glu, nextl, krow, traits);
+ } // for each nonzero ...
+
+ Index fsupc;
+ StorageIndex nsuper = glu.supno(jcol);
+ StorageIndex jcolp1 = StorageIndex(jcol) + 1;
+ Index jcolm1 = jcol - 1;
+
+ // check to see if j belongs in the same supernode as j-1
+ if ( jcol == 0 )
+ { // Do nothing for column 0
+ nsuper = glu.supno(0) = 0 ;
+ }
+ else
+ {
+ fsupc = glu.xsup(nsuper);
+ StorageIndex jptr = glu.xlsub(jcol); // Not yet compressed
+ StorageIndex jm1ptr = glu.xlsub(jcolm1);
+
+ // Use supernodes of type T2 : see SuperLU paper
+ if ( (nextl-jptr != jptr-jm1ptr-1) ) jsuper = emptyIdxLU;
+
+ // Make sure the number of columns in a supernode doesn't
+ // exceed threshold
+ if ( (jcol - fsupc) >= maxsuper) jsuper = emptyIdxLU;
+
+ /* If jcol starts a new supernode, reclaim storage space in
+ * glu.lsub from previous supernode. Note we only store
+ * the subscript set of the first and last columns of
+ * a supernode. (first for num values, last for pruning)
+ */
+ if (jsuper == emptyIdxLU)
+ { // starts a new supernode
+ if ( (fsupc < jcolm1-1) )
+ { // >= 3 columns in nsuper
+ StorageIndex ito = glu.xlsub(fsupc+1);
+ glu.xlsub(jcolm1) = ito;
+ StorageIndex istop = ito + jptr - jm1ptr;
+ xprune(jcolm1) = istop; // initialize xprune(jcol-1)
+ glu.xlsub(jcol) = istop;
+
+ for (StorageIndex ifrom = jm1ptr; ifrom < nextl; ++ifrom, ++ito)
+ glu.lsub(ito) = glu.lsub(ifrom);
+ nextl = ito; // = istop + length(jcol)
+ }
+ nsuper++;
+ glu.supno(jcol) = nsuper;
+ } // if a new supernode
+ } // end else: jcol > 0
+
+ // Tidy up the pointers before exit
+ glu.xsup(nsuper+1) = jcolp1;
+ glu.supno(jcolp1) = nsuper;
+ xprune(jcol) = StorageIndex(nextl); // Initialize upper bound for pruning
+ glu.xlsub(jcolp1) = StorageIndex(nextl);
+
+ return 0;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
new file mode 100644
index 0000000..c32d8d8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
@@ -0,0 +1,107 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU
+
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COPY_TO_UCOL_H
+#define SPARSELU_COPY_TO_UCOL_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Performs numeric block updates (sup-col) in topological order
+ *
+ * \param jcol current column to update
+ * \param nseg Number of segments in the U part
+ * \param segrep segment representative ...
+ * \param repfnz First nonzero column in each row ...
+ * \param perm_r Row permutation
+ * \param dense Store the full representation of the column
+ * \param glu Global LU data.
+ * \return 0 - successful return
+ * > 0 - number of bytes allocated when run out of space
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep,
+ BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu)
+{
+ Index ksub, krep, ksupno;
+
+ Index jsupno = glu.supno(jcol);
+
+ // For each nonzero supernode segment of U[*,j] in topological order
+ Index k = nseg - 1, i;
+ StorageIndex nextu = glu.xusub(jcol);
+ Index kfnz, isub, segsize;
+ Index new_next,irow;
+ Index fsupc, mem;
+ for (ksub = 0; ksub < nseg; ksub++)
+ {
+ krep = segrep(k); k--;
+ ksupno = glu.supno(krep);
+ if (jsupno != ksupno ) // should go into ucol();
+ {
+ kfnz = repfnz(krep);
+ if (kfnz != emptyIdxLU)
+ { // Nonzero U-segment
+ fsupc = glu.xsup(ksupno);
+ isub = glu.xlsub(fsupc) + kfnz - fsupc;
+ segsize = krep - kfnz + 1;
+ new_next = nextu + segsize;
+ while (new_next > glu.nzumax)
+ {
+ mem = memXpand<ScalarVector>(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions);
+ if (mem) return mem;
+ mem = memXpand<IndexVector>(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions);
+ if (mem) return mem;
+
+ }
+
+ for (i = 0; i < segsize; i++)
+ {
+ irow = glu.lsub(isub);
+ glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order
+ glu.ucol(nextu) = dense(irow);
+ dense(irow) = Scalar(0.0);
+ nextu++;
+ isub++;
+ }
+
+ } // end nonzero U-segment
+
+ } // end if jsupno
+
+ } // end for each segment
+ glu.xusub(jcol + 1) = nextu; // close U(*,jcol)
+ return 0;
+}
+
+} // namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_COPY_TO_UCOL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
new file mode 100644
index 0000000..e37c2fe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
@@ -0,0 +1,280 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_GEMM_KERNEL_H
+#define EIGEN_SPARSELU_GEMM_KERNEL_H
+
+namespace Eigen {
+
+namespace internal {
+
+
+/** \internal
+ * A general matrix-matrix product kernel optimized for the SparseLU factorization.
+ * - A, B, and C must be column major
+ * - lda and ldc must be multiples of the respective packet size
+ * - C must have the same alignment as A
+ */
+template<typename Scalar>
+EIGEN_DONT_INLINE
+void sparselu_gemm(Index m, Index n, Index d, const Scalar* A, Index lda, const Scalar* B, Index ldb, Scalar* C, Index ldc)
+{
+ using namespace Eigen::internal;
+
+ typedef typename packet_traits<Scalar>::type Packet;
+ enum {
+ NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
+ PacketSize = packet_traits<Scalar>::size,
+ PM = 8, // peeling in M
+ RN = 2, // register blocking
+ RK = NumberOfRegisters>=16 ? 4 : 2, // register blocking
+ BM = 4096/sizeof(Scalar), // number of rows of A-C per chunk
+ SM = PM*PacketSize // step along M
+ };
+ Index d_end = (d/RK)*RK; // number of columns of A (rows of B) suitable for full register blocking
+ Index n_end = (n/RN)*RN; // number of columns of B-C suitable for processing RN columns at once
+ Index i0 = internal::first_default_aligned(A,m);
+
+ eigen_internal_assert(((lda%PacketSize)==0) && ((ldc%PacketSize)==0) && (i0==internal::first_default_aligned(C,m)));
+
+ // handle the non aligned rows of A and C without any optimization:
+ for(Index i=0; i<i0; ++i)
+ {
+ for(Index j=0; j<n; ++j)
+ {
+ Scalar c = C[i+j*ldc];
+ for(Index k=0; k<d; ++k)
+ c += B[k+j*ldb] * A[i+k*lda];
+ C[i+j*ldc] = c;
+ }
+ }
+ // process the remaining rows per chunk of BM rows
+ for(Index ib=i0; ib<m; ib+=BM)
+ {
+ Index actual_b = std::min<Index>(BM, m-ib); // actual number of rows
+ Index actual_b_end1 = (actual_b/SM)*SM; // actual number of rows suitable for peeling
+ Index actual_b_end2 = (actual_b/PacketSize)*PacketSize; // actual number of rows suitable for vectorization
+
+ // Let's process two columns of B-C at once
+ for(Index j=0; j<n_end; j+=RN)
+ {
+ const Scalar* Bc0 = B+(j+0)*ldb;
+ const Scalar* Bc1 = B+(j+1)*ldb;
+
+ for(Index k=0; k<d_end; k+=RK)
+ {
+
+ // load and expand a RN x RK block of B
+ Packet b00, b10, b20, b30, b01, b11, b21, b31;
+ { b00 = pset1<Packet>(Bc0[0]); }
+ { b10 = pset1<Packet>(Bc0[1]); }
+ if(RK==4) { b20 = pset1<Packet>(Bc0[2]); }
+ if(RK==4) { b30 = pset1<Packet>(Bc0[3]); }
+ { b01 = pset1<Packet>(Bc1[0]); }
+ { b11 = pset1<Packet>(Bc1[1]); }
+ if(RK==4) { b21 = pset1<Packet>(Bc1[2]); }
+ if(RK==4) { b31 = pset1<Packet>(Bc1[3]); }
+
+ Packet a0, a1, a2, a3, c0, c1, t0, t1;
+
+ const Scalar* A0 = A+ib+(k+0)*lda;
+ const Scalar* A1 = A+ib+(k+1)*lda;
+ const Scalar* A2 = A+ib+(k+2)*lda;
+ const Scalar* A3 = A+ib+(k+3)*lda;
+
+ Scalar* C0 = C+ib+(j+0)*ldc;
+ Scalar* C1 = C+ib+(j+1)*ldc;
+
+ a0 = pload<Packet>(A0);
+ a1 = pload<Packet>(A1);
+ if(RK==4)
+ {
+ a2 = pload<Packet>(A2);
+ a3 = pload<Packet>(A3);
+ }
+ else
+ {
+ // workaround "may be used uninitialized in this function" warning
+ a2 = a3 = a0;
+ }
+
+#define KMADD(c, a, b, tmp) {tmp = b; tmp = pmul(a,tmp); c = padd(c,tmp);}
+#define WORK(I) \
+ c0 = pload<Packet>(C0+i+(I)*PacketSize); \
+ c1 = pload<Packet>(C1+i+(I)*PacketSize); \
+ KMADD(c0, a0, b00, t0) \
+ KMADD(c1, a0, b01, t1) \
+ a0 = pload<Packet>(A0+i+(I+1)*PacketSize); \
+ KMADD(c0, a1, b10, t0) \
+ KMADD(c1, a1, b11, t1) \
+ a1 = pload<Packet>(A1+i+(I+1)*PacketSize); \
+ if(RK==4){ KMADD(c0, a2, b20, t0) }\
+ if(RK==4){ KMADD(c1, a2, b21, t1) }\
+ if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize); }\
+ if(RK==4){ KMADD(c0, a3, b30, t0) }\
+ if(RK==4){ KMADD(c1, a3, b31, t1) }\
+ if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize); }\
+ pstore(C0+i+(I)*PacketSize, c0); \
+ pstore(C1+i+(I)*PacketSize, c1)
+
+ // process rows of A' - C' with aggressive vectorization and peeling
+ for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
+ {
+ EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL1");
+ prefetch((A0+i+(5)*PacketSize));
+ prefetch((A1+i+(5)*PacketSize));
+ if(RK==4) prefetch((A2+i+(5)*PacketSize));
+ if(RK==4) prefetch((A3+i+(5)*PacketSize));
+
+ WORK(0);
+ WORK(1);
+ WORK(2);
+ WORK(3);
+ WORK(4);
+ WORK(5);
+ WORK(6);
+ WORK(7);
+ }
+ // process the remaining rows with vectorization only
+ for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
+ {
+ WORK(0);
+ }
+#undef WORK
+ // process the remaining rows without vectorization
+ for(Index i=actual_b_end2; i<actual_b; ++i)
+ {
+ if(RK==4)
+ {
+ C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
+ C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1]+A2[i]*Bc1[2]+A3[i]*Bc1[3];
+ }
+ else
+ {
+ C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
+ C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1];
+ }
+ }
+
+ Bc0 += RK;
+ Bc1 += RK;
+ } // peeled loop on k
+ } // peeled loop on the columns j
+ // process the last column (we now perform a matrix-vector product)
+ if((n-n_end)>0)
+ {
+ const Scalar* Bc0 = B+(n-1)*ldb;
+
+ for(Index k=0; k<d_end; k+=RK)
+ {
+
+ // load and expand a 1 x RK block of B
+ Packet b00, b10, b20, b30;
+ b00 = pset1<Packet>(Bc0[0]);
+ b10 = pset1<Packet>(Bc0[1]);
+ if(RK==4) b20 = pset1<Packet>(Bc0[2]);
+ if(RK==4) b30 = pset1<Packet>(Bc0[3]);
+
+ Packet a0, a1, a2, a3, c0, t0/*, t1*/;
+
+ const Scalar* A0 = A+ib+(k+0)*lda;
+ const Scalar* A1 = A+ib+(k+1)*lda;
+ const Scalar* A2 = A+ib+(k+2)*lda;
+ const Scalar* A3 = A+ib+(k+3)*lda;
+
+ Scalar* C0 = C+ib+(n_end)*ldc;
+
+ a0 = pload<Packet>(A0);
+ a1 = pload<Packet>(A1);
+ if(RK==4)
+ {
+ a2 = pload<Packet>(A2);
+ a3 = pload<Packet>(A3);
+ }
+ else
+ {
+ // workaround "may be used uninitialized in this function" warning
+ a2 = a3 = a0;
+ }
+
+#define WORK(I) \
+ c0 = pload<Packet>(C0+i+(I)*PacketSize); \
+ KMADD(c0, a0, b00, t0) \
+ a0 = pload<Packet>(A0+i+(I+1)*PacketSize); \
+ KMADD(c0, a1, b10, t0) \
+ a1 = pload<Packet>(A1+i+(I+1)*PacketSize); \
+ if(RK==4){ KMADD(c0, a2, b20, t0) }\
+ if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize); }\
+ if(RK==4){ KMADD(c0, a3, b30, t0) }\
+ if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize); }\
+ pstore(C0+i+(I)*PacketSize, c0);
+
+ // aggressive vectorization and peeling
+ for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
+ {
+ EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL2");
+ WORK(0);
+ WORK(1);
+ WORK(2);
+ WORK(3);
+ WORK(4);
+ WORK(5);
+ WORK(6);
+ WORK(7);
+ }
+ // vectorization only
+ for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
+ {
+ WORK(0);
+ }
+ // remaining scalars
+ for(Index i=actual_b_end2; i<actual_b; ++i)
+ {
+ if(RK==4)
+ C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
+ else
+ C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
+ }
+
+ Bc0 += RK;
+#undef WORK
+ }
+ }
+
+ // process the last columns of A, corresponding to the last rows of B
+ Index rd = d-d_end;
+ if(rd>0)
+ {
+ for(Index j=0; j<n; ++j)
+ {
+ enum {
+ Alignment = PacketSize>1 ? Aligned : 0
+ };
+ typedef Map<Matrix<Scalar,Dynamic,1>, Alignment > MapVector;
+ typedef Map<const Matrix<Scalar,Dynamic,1>, Alignment > ConstMapVector;
+ if(rd==1) MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b);
+
+ else if(rd==2) MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
+ + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b);
+
+ else MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
+ + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b)
+ + B[2+d_end+j*ldb] * ConstMapVector(A+(d_end+2)*lda+ib, actual_b);
+ }
+ }
+
+ } // blocking on the rows of A and C
+}
+#undef KMADD
+
+} // namespace internal
+
+} // namespace Eigen
+
+#endif // EIGEN_SPARSELU_GEMM_KERNEL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
new file mode 100644
index 0000000..6f75d50
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -0,0 +1,126 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* This file is a modified version of heap_relax_snode.c file in SuperLU
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef SPARSELU_HEAP_RELAX_SNODE_H
+#define SPARSELU_HEAP_RELAX_SNODE_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Identify the initial relaxed supernodes
+ *
+ * This routine applied to a symmetric elimination tree.
+ * It assumes that the matrix has been reordered according to the postorder of the etree
+ * \param n The number of columns
+ * \param et elimination tree
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode
+ * \param descendants Number of descendants of each node in the etree
+ * \param relax_end last column in a supernode
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
+{
+
+ // The etree may not be postordered, but its heap ordered
+ IndexVector post;
+ internal::treePostorder(StorageIndex(n), et, post); // Post order etree
+ IndexVector inv_post(n+1);
+ for (StorageIndex i = 0; i < n+1; ++i) inv_post(post(i)) = i; // inv_post = post.inverse()???
+
+ // Renumber etree in postorder
+ IndexVector iwork(n);
+ IndexVector et_save(n+1);
+ for (Index i = 0; i < n; ++i)
+ {
+ iwork(post(i)) = post(et(i));
+ }
+ et_save = et; // Save the original etree
+ et = iwork;
+
+ // compute the number of descendants of each node in the etree
+ relax_end.setConstant(emptyIdxLU);
+ Index j, parent;
+ descendants.setZero();
+ for (j = 0; j < n; j++)
+ {
+ parent = et(j);
+ if (parent != n) // not the dummy root
+ descendants(parent) += descendants(j) + 1;
+ }
+ // Identify the relaxed supernodes by postorder traversal of the etree
+ Index snode_start; // beginning of a snode
+ StorageIndex k;
+ Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree
+ Index nsuper_et = 0; // Number of relaxed snodes in the original etree
+ StorageIndex l;
+ for (j = 0; j < n; )
+ {
+ parent = et(j);
+ snode_start = j;
+ while ( parent != n && descendants(parent) < relax_columns )
+ {
+ j = parent;
+ parent = et(j);
+ }
+ // Found a supernode in postordered etree, j is the last column
+ ++nsuper_et_post;
+ k = StorageIndex(n);
+ for (Index i = snode_start; i <= j; ++i)
+ k = (std::min)(k, inv_post(i));
+ l = inv_post(j);
+ if ( (l - k) == (j - snode_start) ) // Same number of columns in the snode
+ {
+ // This is also a supernode in the original etree
+ relax_end(k) = l; // Record last column
+ ++nsuper_et;
+ }
+ else
+ {
+ for (Index i = snode_start; i <= j; ++i)
+ {
+ l = inv_post(i);
+ if (descendants(i) == 0)
+ {
+ relax_end(l) = l;
+ ++nsuper_et;
+ }
+ }
+ }
+ j++;
+ // Search for a new leaf
+ while (descendants(j) != 0 && j < n) j++;
+ } // End postorder traversal of the etree
+
+ // Recover the original etree
+ et = et_save;
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // SPARSELU_HEAP_RELAX_SNODE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
new file mode 100644
index 0000000..8c1b3e8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
@@ -0,0 +1,130 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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 SPARSELU_KERNEL_BMOD_H
+#define SPARSELU_KERNEL_BMOD_H
+
+namespace Eigen {
+namespace internal {
+
+template <int SegSizeAtCompileTime> struct LU_kernel_bmod
+{
+ /** \internal
+ * \brief Performs numeric block updates from a given supernode to a single column
+ *
+ * \param segsize Size of the segment (and blocks ) to use for updates
+ * \param[in,out] dense Packed values of the original matrix
+ * \param tempv temporary vector to use for updates
+ * \param lusup array containing the supernodes
+ * \param lda Leading dimension in the supernode
+ * \param nrow Number of rows in the rectangular part of the supernode
+ * \param lsub compressed row subscripts of supernodes
+ * \param lptr pointer to the first column of the current supernode in lsub
+ * \param no_zeros Number of nonzeros elements before the diagonal part of the supernode
+ */
+ template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+ static EIGEN_DONT_INLINE void run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
+ const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+};
+
+template <int SegSizeAtCompileTime>
+template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+EIGEN_DONT_INLINE void LU_kernel_bmod<SegSizeAtCompileTime>::run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
+ const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
+{
+ typedef typename ScalarVector::Scalar Scalar;
+ // First, copy U[*,j] segment from dense(*) to tempv(*)
+ // The result of triangular solve is in tempv[*];
+ // The result of matric-vector update is in dense[*]
+ Index isub = lptr + no_zeros;
+ Index i;
+ Index irow;
+ for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
+ {
+ irow = lsub(isub);
+ tempv(i) = dense(irow);
+ ++isub;
+ }
+ // Dense triangular solve -- start effective triangle
+ luptr += lda * no_zeros + no_zeros;
+ // Form Eigen matrix and vector
+ Map<Matrix<Scalar,SegSizeAtCompileTime,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) );
+ Map<Matrix<Scalar,SegSizeAtCompileTime,1> > u(tempv.data(), segsize);
+
+ u = A.template triangularView<UnitLower>().solve(u);
+
+ // Dense matrix-vector product y <-- B*x
+ luptr += segsize;
+ const Index PacketSize = internal::packet_traits<Scalar>::size;
+ Index ldl = internal::first_multiple(nrow, PacketSize);
+ Map<Matrix<Scalar,Dynamic,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) );
+ Index aligned_offset = internal::first_default_aligned(tempv.data()+segsize, PacketSize);
+ Index aligned_with_B_offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize))%PacketSize;
+ Map<Matrix<Scalar,Dynamic,1>, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) );
+
+ l.setZero();
+ internal::sparselu_gemm<Scalar>(l.rows(), l.cols(), B.cols(), B.data(), B.outerStride(), u.data(), u.outerStride(), l.data(), l.outerStride());
+
+ // Scatter tempv[] into SPA dense[] as a temporary storage
+ isub = lptr + no_zeros;
+ for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
+ {
+ irow = lsub(isub++);
+ dense(irow) = tempv(i);
+ }
+
+ // Scatter l into SPA dense[]
+ for (i = 0; i < nrow; i++)
+ {
+ irow = lsub(isub++);
+ dense(irow) -= l(i);
+ }
+}
+
+template <> struct LU_kernel_bmod<1>
+{
+ template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+ static EIGEN_DONT_INLINE void run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
+ const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+};
+
+
+template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+EIGEN_DONT_INLINE void LU_kernel_bmod<1>::run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
+ const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
+{
+ typedef typename ScalarVector::Scalar Scalar;
+ typedef typename IndexVector::Scalar StorageIndex;
+ Scalar f = dense(lsub(lptr + no_zeros));
+ luptr += lda * no_zeros + no_zeros + 1;
+ const Scalar* a(lusup.data() + luptr);
+ const StorageIndex* irow(lsub.data()+lptr + no_zeros + 1);
+ Index i = 0;
+ for (; i+1 < nrow; i+=2)
+ {
+ Index i0 = *(irow++);
+ Index i1 = *(irow++);
+ Scalar a0 = *(a++);
+ Scalar a1 = *(a++);
+ Scalar d0 = dense.coeff(i0);
+ Scalar d1 = dense.coeff(i1);
+ d0 -= f*a0;
+ d1 -= f*a1;
+ dense.coeffRef(i0) = d0;
+ dense.coeffRef(i1) = d1;
+ }
+ if(i<nrow)
+ dense.coeffRef(*(irow++)) -= f * *(a++);
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // SPARSELU_KERNEL_BMOD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
new file mode 100644
index 0000000..f052001
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
@@ -0,0 +1,223 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]panel_bmod.c file in SuperLU
+
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PANEL_BMOD_H
+#define SPARSELU_PANEL_BMOD_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Performs numeric block updates (sup-panel) in topological order.
+ *
+ * Before entering this routine, the original nonzeros in the panel
+ * were already copied into the spa[m,w]
+ *
+ * \param m number of rows in the matrix
+ * \param w Panel size
+ * \param jcol Starting column of the panel
+ * \param nseg Number of segments in the U part
+ * \param dense Store the full representation of the panel
+ * \param tempv working array
+ * \param segrep segment representative... first row in the segment
+ * \param repfnz First nonzero rows
+ * \param glu Global LU data.
+ *
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::panel_bmod(const Index m, const Index w, const Index jcol,
+ const Index nseg, ScalarVector& dense, ScalarVector& tempv,
+ IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu)
+{
+
+ Index ksub,jj,nextl_col;
+ Index fsupc, nsupc, nsupr, nrow;
+ Index krep, kfnz;
+ Index lptr; // points to the row subscripts of a supernode
+ Index luptr; // ...
+ Index segsize,no_zeros ;
+ // For each nonz supernode segment of U[*,j] in topological order
+ Index k = nseg - 1;
+ const Index PacketSize = internal::packet_traits<Scalar>::size;
+
+ for (ksub = 0; ksub < nseg; ksub++)
+ { // For each updating supernode
+ /* krep = representative of current k-th supernode
+ * fsupc = first supernodal column
+ * nsupc = number of columns in a supernode
+ * nsupr = number of rows in a supernode
+ */
+ krep = segrep(k); k--;
+ fsupc = glu.xsup(glu.supno(krep));
+ nsupc = krep - fsupc + 1;
+ nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc);
+ nrow = nsupr - nsupc;
+ lptr = glu.xlsub(fsupc);
+
+ // loop over the panel columns to detect the actual number of columns and rows
+ Index u_rows = 0;
+ Index u_cols = 0;
+ for (jj = jcol; jj < jcol + w; jj++)
+ {
+ nextl_col = (jj-jcol) * m;
+ VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+
+ kfnz = repfnz_col(krep);
+ if ( kfnz == emptyIdxLU )
+ continue; // skip any zero segment
+
+ segsize = krep - kfnz + 1;
+ u_cols++;
+ u_rows = (std::max)(segsize,u_rows);
+ }
+
+ if(nsupc >= 2)
+ {
+ Index ldu = internal::first_multiple<Index>(u_rows, PacketSize);
+ Map<ScalarMatrix, Aligned, OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu));
+
+ // gather U
+ Index u_col = 0;
+ for (jj = jcol; jj < jcol + w; jj++)
+ {
+ nextl_col = (jj-jcol) * m;
+ VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+ VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+
+ kfnz = repfnz_col(krep);
+ if ( kfnz == emptyIdxLU )
+ continue; // skip any zero segment
+
+ segsize = krep - kfnz + 1;
+ luptr = glu.xlusup(fsupc);
+ no_zeros = kfnz - fsupc;
+
+ Index isub = lptr + no_zeros;
+ Index off = u_rows-segsize;
+ for (Index i = 0; i < off; i++) U(i,u_col) = 0;
+ for (Index i = 0; i < segsize; i++)
+ {
+ Index irow = glu.lsub(isub);
+ U(i+off,u_col) = dense_col(irow);
+ ++isub;
+ }
+ u_col++;
+ }
+ // solve U = A^-1 U
+ luptr = glu.xlusup(fsupc);
+ Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc);
+ no_zeros = (krep - u_rows + 1) - fsupc;
+ luptr += lda * no_zeros + no_zeros;
+ MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) );
+ U = A.template triangularView<UnitLower>().solve(U);
+
+ // update
+ luptr += u_rows;
+ MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) );
+ eigen_assert(tempv.size()>w*ldu + nrow*w + 1);
+
+ Index ldl = internal::first_multiple<Index>(nrow, PacketSize);
+ Index offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize)) % PacketSize;
+ MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl));
+
+ L.setZero();
+ internal::sparselu_gemm<Scalar>(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride());
+
+ // scatter U and L
+ u_col = 0;
+ for (jj = jcol; jj < jcol + w; jj++)
+ {
+ nextl_col = (jj-jcol) * m;
+ VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+ VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+
+ kfnz = repfnz_col(krep);
+ if ( kfnz == emptyIdxLU )
+ continue; // skip any zero segment
+
+ segsize = krep - kfnz + 1;
+ no_zeros = kfnz - fsupc;
+ Index isub = lptr + no_zeros;
+
+ Index off = u_rows-segsize;
+ for (Index i = 0; i < segsize; i++)
+ {
+ Index irow = glu.lsub(isub++);
+ dense_col(irow) = U.coeff(i+off,u_col);
+ U.coeffRef(i+off,u_col) = 0;
+ }
+
+ // Scatter l into SPA dense[]
+ for (Index i = 0; i < nrow; i++)
+ {
+ Index irow = glu.lsub(isub++);
+ dense_col(irow) -= L.coeff(i,u_col);
+ L.coeffRef(i,u_col) = 0;
+ }
+ u_col++;
+ }
+ }
+ else // level 2 only
+ {
+ // Sequence through each column in the panel
+ for (jj = jcol; jj < jcol + w; jj++)
+ {
+ nextl_col = (jj-jcol) * m;
+ VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+ VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+
+ kfnz = repfnz_col(krep);
+ if ( kfnz == emptyIdxLU )
+ continue; // skip any zero segment
+
+ segsize = krep - kfnz + 1;
+ luptr = glu.xlusup(fsupc);
+
+ Index lda = glu.xlusup(fsupc+1)-glu.xlusup(fsupc);// nsupr
+
+ // Perform a trianglar solve and block update,
+ // then scatter the result of sup-col update to dense[]
+ no_zeros = kfnz - fsupc;
+ if(segsize==1) LU_kernel_bmod<1>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ else if(segsize==2) LU_kernel_bmod<2>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ else if(segsize==3) LU_kernel_bmod<3>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ else LU_kernel_bmod<Dynamic>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+ } // End for each column in the panel
+ }
+
+ } // End for each updating supernode
+} // end panel bmod
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // SPARSELU_PANEL_BMOD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
new file mode 100644
index 0000000..155df73
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
@@ -0,0 +1,258 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]panel_dfs.c file in SuperLU
+
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PANEL_DFS_H
+#define SPARSELU_PANEL_DFS_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename IndexVector>
+struct panel_dfs_traits
+{
+ typedef typename IndexVector::Scalar StorageIndex;
+ panel_dfs_traits(Index jcol, StorageIndex* marker)
+ : m_jcol(jcol), m_marker(marker)
+ {}
+ bool update_segrep(Index krep, StorageIndex jj)
+ {
+ if(m_marker[krep]<m_jcol)
+ {
+ m_marker[krep] = jj;
+ return true;
+ }
+ return false;
+ }
+ void mem_expand(IndexVector& /*glu.lsub*/, Index /*nextl*/, Index /*chmark*/) {}
+ enum { ExpandMem = false };
+ Index m_jcol;
+ StorageIndex* m_marker;
+};
+
+
+template <typename Scalar, typename StorageIndex>
+template <typename Traits>
+void SparseLUImpl<Scalar,StorageIndex>::dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
+ Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
+ Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
+ IndexVector& xplore, GlobalLU_t& glu,
+ Index& nextl_col, Index krow, Traits& traits
+ )
+{
+
+ StorageIndex kmark = marker(krow);
+
+ // For each unmarked krow of jj
+ marker(krow) = jj;
+ StorageIndex kperm = perm_r(krow);
+ if (kperm == emptyIdxLU ) {
+ // krow is in L : place it in structure of L(*, jj)
+ panel_lsub(nextl_col++) = StorageIndex(krow); // krow is indexed into A
+
+ traits.mem_expand(panel_lsub, nextl_col, kmark);
+ }
+ else
+ {
+ // krow is in U : if its supernode-representative krep
+ // has been explored, update repfnz(*)
+ // krep = supernode representative of the current row
+ StorageIndex krep = glu.xsup(glu.supno(kperm)+1) - 1;
+ // First nonzero element in the current column:
+ StorageIndex myfnz = repfnz_col(krep);
+
+ if (myfnz != emptyIdxLU )
+ {
+ // Representative visited before
+ if (myfnz > kperm ) repfnz_col(krep) = kperm;
+
+ }
+ else
+ {
+ // Otherwise, perform dfs starting at krep
+ StorageIndex oldrep = emptyIdxLU;
+ parent(krep) = oldrep;
+ repfnz_col(krep) = kperm;
+ StorageIndex xdfs = glu.xlsub(krep);
+ Index maxdfs = xprune(krep);
+
+ StorageIndex kpar;
+ do
+ {
+ // For each unmarked kchild of krep
+ while (xdfs < maxdfs)
+ {
+ StorageIndex kchild = glu.lsub(xdfs);
+ xdfs++;
+ StorageIndex chmark = marker(kchild);
+
+ if (chmark != jj )
+ {
+ marker(kchild) = jj;
+ StorageIndex chperm = perm_r(kchild);
+
+ if (chperm == emptyIdxLU)
+ {
+ // case kchild is in L: place it in L(*, j)
+ panel_lsub(nextl_col++) = kchild;
+ traits.mem_expand(panel_lsub, nextl_col, chmark);
+ }
+ else
+ {
+ // case kchild is in U :
+ // chrep = its supernode-rep. If its rep has been explored,
+ // update its repfnz(*)
+ StorageIndex chrep = glu.xsup(glu.supno(chperm)+1) - 1;
+ myfnz = repfnz_col(chrep);
+
+ if (myfnz != emptyIdxLU)
+ { // Visited before
+ if (myfnz > chperm)
+ repfnz_col(chrep) = chperm;
+ }
+ else
+ { // Cont. dfs at snode-rep of kchild
+ xplore(krep) = xdfs;
+ oldrep = krep;
+ krep = chrep; // Go deeper down G(L)
+ parent(krep) = oldrep;
+ repfnz_col(krep) = chperm;
+ xdfs = glu.xlsub(krep);
+ maxdfs = xprune(krep);
+
+ } // end if myfnz != -1
+ } // end if chperm == -1
+
+ } // end if chmark !=jj
+ } // end while xdfs < maxdfs
+
+ // krow has no more unexplored nbrs :
+ // Place snode-rep krep in postorder DFS, if this
+ // segment is seen for the first time. (Note that
+ // "repfnz(krep)" may change later.)
+ // Baktrack dfs to its parent
+ if(traits.update_segrep(krep,jj))
+ //if (marker1(krep) < jcol )
+ {
+ segrep(nseg) = krep;
+ ++nseg;
+ //marker1(krep) = jj;
+ }
+
+ kpar = parent(krep); // Pop recursion, mimic recursion
+ if (kpar == emptyIdxLU)
+ break; // dfs done
+ krep = kpar;
+ xdfs = xplore(krep);
+ maxdfs = xprune(krep);
+
+ } while (kpar != emptyIdxLU); // Do until empty stack
+
+ } // end if (myfnz = -1)
+
+ } // end if (kperm == -1)
+}
+
+/**
+ * \brief Performs a symbolic factorization on a panel of columns [jcol, jcol+w)
+ *
+ * A supernode representative is the last column of a supernode.
+ * The nonzeros in U[*,j] are segments that end at supernodes representatives
+ *
+ * The routine returns a list of the supernodal representatives
+ * in topological order of the dfs that generates them. This list is
+ * a superset of the topological order of each individual column within
+ * the panel.
+ * The location of the first nonzero in each supernodal segment
+ * (supernodal entry location) is also returned. Each column has
+ * a separate list for this purpose.
+ *
+ * Two markers arrays are used for dfs :
+ * marker[i] == jj, if i was visited during dfs of current column jj;
+ * marker1[i] >= jcol, if i was visited by earlier columns in this panel;
+ *
+ * \param[in] m number of rows in the matrix
+ * \param[in] w Panel size
+ * \param[in] jcol Starting column of the panel
+ * \param[in] A Input matrix in column-major storage
+ * \param[in] perm_r Row permutation
+ * \param[out] nseg Number of U segments
+ * \param[out] dense Accumulate the column vectors of the panel
+ * \param[out] panel_lsub Subscripts of the row in the panel
+ * \param[out] segrep Segment representative i.e first nonzero row of each segment
+ * \param[out] repfnz First nonzero location in each row
+ * \param[out] xprune The pruned elimination tree
+ * \param[out] marker work vector
+ * \param parent The elimination tree
+ * \param xplore work vector
+ * \param glu The global data structure
+ *
+ */
+
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
+{
+ Index nextl_col; // Next available position in panel_lsub[*,jj]
+
+ // Initialize pointers
+ VectorBlock<IndexVector> marker1(marker, m, m);
+ nseg = 0;
+
+ panel_dfs_traits<IndexVector> traits(jcol, marker1.data());
+
+ // For each column in the panel
+ for (StorageIndex jj = StorageIndex(jcol); jj < jcol + w; jj++)
+ {
+ nextl_col = (jj - jcol) * m;
+
+ VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero location in each row
+ VectorBlock<ScalarVector> dense_col(dense,nextl_col, m); // Accumulate a column vector here
+
+
+ // For each nnz in A[*, jj] do depth first search
+ for (typename MatrixType::InnerIterator it(A, jj); it; ++it)
+ {
+ Index krow = it.row();
+ dense_col(krow) = it.value();
+
+ StorageIndex kmark = marker(krow);
+ if (kmark == jj)
+ continue; // krow visited before, go to the next nonzero
+
+ dfs_kernel(jj, perm_r, nseg, panel_lsub, segrep, repfnz_col, xprune, marker, parent,
+ xplore, glu, nextl_col, krow, traits);
+ }// end for nonzeros in column jj
+
+ } // end for column jj
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PANEL_DFS_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
new file mode 100644
index 0000000..a86dac9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
@@ -0,0 +1,137 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of xpivotL.c file in SuperLU
+
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PIVOTL_H
+#define SPARSELU_PIVOTL_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Performs the numerical pivotin on the current column of L, and the CDIV operation.
+ *
+ * Pivot policy :
+ * (1) Compute thresh = u * max_(i>=j) abs(A_ij);
+ * (2) IF user specifies pivot row k and abs(A_kj) >= thresh THEN
+ * pivot row = k;
+ * ELSE IF abs(A_jj) >= thresh THEN
+ * pivot row = j;
+ * ELSE
+ * pivot row = m;
+ *
+ * Note: If you absolutely want to use a given pivot order, then set u=0.0.
+ *
+ * \param jcol The current column of L
+ * \param diagpivotthresh diagonal pivoting threshold
+ * \param[in,out] perm_r Row permutation (threshold pivoting)
+ * \param[in] iperm_c column permutation - used to finf diagonal of Pc*A*Pc'
+ * \param[out] pivrow The pivot row
+ * \param glu Global LU data
+ * \return 0 if success, i > 0 if U(i,i) is exactly zero
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu)
+{
+
+ Index fsupc = (glu.xsup)((glu.supno)(jcol)); // First column in the supernode containing the column jcol
+ Index nsupc = jcol - fsupc; // Number of columns in the supernode portion, excluding jcol; nsupc >=0
+ Index lptr = glu.xlsub(fsupc); // pointer to the starting location of the row subscripts for this supernode portion
+ Index nsupr = glu.xlsub(fsupc+1) - lptr; // Number of rows in the supernode
+ Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); // leading dimension
+ Scalar* lu_sup_ptr = &(glu.lusup.data()[glu.xlusup(fsupc)]); // Start of the current supernode
+ Scalar* lu_col_ptr = &(glu.lusup.data()[glu.xlusup(jcol)]); // Start of jcol in the supernode
+ StorageIndex* lsub_ptr = &(glu.lsub.data()[lptr]); // Start of row indices of the supernode
+
+ // Determine the largest abs numerical value for partial pivoting
+ Index diagind = iperm_c(jcol); // diagonal index
+ RealScalar pivmax(-1.0);
+ Index pivptr = nsupc;
+ Index diag = emptyIdxLU;
+ RealScalar rtemp;
+ Index isub, icol, itemp, k;
+ for (isub = nsupc; isub < nsupr; ++isub) {
+ using std::abs;
+ rtemp = abs(lu_col_ptr[isub]);
+ if (rtemp > pivmax) {
+ pivmax = rtemp;
+ pivptr = isub;
+ }
+ if (lsub_ptr[isub] == diagind) diag = isub;
+ }
+
+ // Test for singularity
+ if ( pivmax <= RealScalar(0.0) ) {
+ // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero
+ pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr];
+ perm_r(pivrow) = StorageIndex(jcol);
+ return (jcol+1);
+ }
+
+ RealScalar thresh = diagpivotthresh * pivmax;
+
+ // Choose appropriate pivotal element
+
+ {
+ // Test if the diagonal element can be used as a pivot (given the threshold value)
+ if (diag >= 0 )
+ {
+ // Diagonal element exists
+ using std::abs;
+ rtemp = abs(lu_col_ptr[diag]);
+ if (rtemp != RealScalar(0.0) && rtemp >= thresh) pivptr = diag;
+ }
+ pivrow = lsub_ptr[pivptr];
+ }
+
+ // Record pivot row
+ perm_r(pivrow) = StorageIndex(jcol);
+ // Interchange row subscripts
+ if (pivptr != nsupc )
+ {
+ std::swap( lsub_ptr[pivptr], lsub_ptr[nsupc] );
+ // Interchange numerical values as well, for the two rows in the whole snode
+ // such that L is indexed the same way as A
+ for (icol = 0; icol <= nsupc; icol++)
+ {
+ itemp = pivptr + icol * lda;
+ std::swap(lu_sup_ptr[itemp], lu_sup_ptr[nsupc + icol * lda]);
+ }
+ }
+ // cdiv operations
+ Scalar temp = Scalar(1.0) / lu_col_ptr[nsupc];
+ for (k = nsupc+1; k < nsupr; k++)
+ lu_col_ptr[k] *= temp;
+ return 0;
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PIVOTL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
new file mode 100644
index 0000000..ad32fed
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
@@ -0,0 +1,136 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/*
+
+ * NOTE: This file is the modified version of [s,d,c,z]pruneL.c file in SuperLU
+
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PRUNEL_H
+#define SPARSELU_PRUNEL_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Prunes the L-structure.
+ *
+ * It prunes the L-structure of supernodes whose L-structure contains the current pivot row "pivrow"
+ *
+ *
+ * \param jcol The current column of L
+ * \param[in] perm_r Row permutation
+ * \param[out] pivrow The pivot row
+ * \param nseg Number of segments
+ * \param segrep
+ * \param repfnz
+ * \param[out] xprune
+ * \param glu Global LU data
+ *
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg,
+ const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu)
+{
+ // For each supernode-rep irep in U(*,j]
+ Index jsupno = glu.supno(jcol);
+ Index i,irep,irep1;
+ bool movnum, do_prune = false;
+ Index kmin = 0, kmax = 0, minloc, maxloc,krow;
+ for (i = 0; i < nseg; i++)
+ {
+ irep = segrep(i);
+ irep1 = irep + 1;
+ do_prune = false;
+
+ // Don't prune with a zero U-segment
+ if (repfnz(irep) == emptyIdxLU) continue;
+
+ // If a snode overlaps with the next panel, then the U-segment
+ // is fragmented into two parts -- irep and irep1. We should let
+ // pruning occur at the rep-column in irep1s snode.
+ if (glu.supno(irep) == glu.supno(irep1) ) continue; // don't prune
+
+ // If it has not been pruned & it has a nonz in row L(pivrow,i)
+ if (glu.supno(irep) != jsupno )
+ {
+ if ( xprune (irep) >= glu.xlsub(irep1) )
+ {
+ kmin = glu.xlsub(irep);
+ kmax = glu.xlsub(irep1) - 1;
+ for (krow = kmin; krow <= kmax; krow++)
+ {
+ if (glu.lsub(krow) == pivrow)
+ {
+ do_prune = true;
+ break;
+ }
+ }
+ }
+
+ if (do_prune)
+ {
+ // do a quicksort-type partition
+ // movnum=true means that the num values have to be exchanged
+ movnum = false;
+ if (irep == glu.xsup(glu.supno(irep)) ) // Snode of size 1
+ movnum = true;
+
+ while (kmin <= kmax)
+ {
+ if (perm_r(glu.lsub(kmax)) == emptyIdxLU)
+ kmax--;
+ else if ( perm_r(glu.lsub(kmin)) != emptyIdxLU)
+ kmin++;
+ else
+ {
+ // kmin below pivrow (not yet pivoted), and kmax
+ // above pivrow: interchange the two suscripts
+ std::swap(glu.lsub(kmin), glu.lsub(kmax));
+
+ // If the supernode has only one column, then we
+ // only keep one set of subscripts. For any subscript
+ // intercnahge performed, similar interchange must be
+ // done on the numerical values.
+ if (movnum)
+ {
+ minloc = glu.xlusup(irep) + ( kmin - glu.xlsub(irep) );
+ maxloc = glu.xlusup(irep) + ( kmax - glu.xlsub(irep) );
+ std::swap(glu.lusup(minloc), glu.lusup(maxloc));
+ }
+ kmin++;
+ kmax--;
+ }
+ } // end while
+
+ xprune(irep) = StorageIndex(kmin); //Pruning
+ } // end if do_prune
+ } // end pruning
+ } // End for each U-segment
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PRUNEL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
new file mode 100644
index 0000000..c408d01
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
@@ -0,0 +1,83 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* This file is a modified version of heap_relax_snode.c file in SuperLU
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation. All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef SPARSELU_RELAX_SNODE_H
+#define SPARSELU_RELAX_SNODE_H
+
+namespace Eigen {
+
+namespace internal {
+
+/**
+ * \brief Identify the initial relaxed supernodes
+ *
+ * This routine is applied to a column elimination tree.
+ * It assumes that the matrix has been reordered according to the postorder of the etree
+ * \param n the number of columns
+ * \param et elimination tree
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode
+ * \param descendants Number of descendants of each node in the etree
+ * \param relax_end last column in a supernode
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
+{
+
+ // compute the number of descendants of each node in the etree
+ Index parent;
+ relax_end.setConstant(emptyIdxLU);
+ descendants.setZero();
+ for (Index j = 0; j < n; j++)
+ {
+ parent = et(j);
+ if (parent != n) // not the dummy root
+ descendants(parent) += descendants(j) + 1;
+ }
+ // Identify the relaxed supernodes by postorder traversal of the etree
+ Index snode_start; // beginning of a snode
+ for (Index j = 0; j < n; )
+ {
+ parent = et(j);
+ snode_start = j;
+ while ( parent != n && descendants(parent) < relax_columns )
+ {
+ j = parent;
+ parent = et(j);
+ }
+ // Found a supernode in postordered etree, j is the last column
+ relax_end(snode_start) = StorageIndex(j); // Record last column
+ j++;
+ // Search for a new leaf
+ while (descendants(j) != 0 && j < n) j++;
+ } // End postorder traversal of the etree
+
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif
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
new file mode 100644
index 0000000..d1fb96f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
@@ -0,0 +1,758 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012-2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_QR_H
+#define EIGEN_SPARSE_QR_H
+
+namespace Eigen {
+
+template<typename MatrixType, typename OrderingType> class SparseQR;
+template<typename SparseQRType> struct SparseQRMatrixQReturnType;
+template<typename SparseQRType> struct SparseQRMatrixQTransposeReturnType;
+template<typename SparseQRType, typename Derived> struct SparseQR_QProduct;
+namespace internal {
+ template <typename SparseQRType> struct traits<SparseQRMatrixQReturnType<SparseQRType> >
+ {
+ typedef typename SparseQRType::MatrixType ReturnType;
+ typedef typename ReturnType::StorageIndex StorageIndex;
+ typedef typename ReturnType::StorageKind StorageKind;
+ enum {
+ RowsAtCompileTime = Dynamic,
+ ColsAtCompileTime = Dynamic
+ };
+ };
+ template <typename SparseQRType> struct traits<SparseQRMatrixQTransposeReturnType<SparseQRType> >
+ {
+ typedef typename SparseQRType::MatrixType ReturnType;
+ };
+ template <typename SparseQRType, typename Derived> struct traits<SparseQR_QProduct<SparseQRType, Derived> >
+ {
+ typedef typename Derived::PlainObject ReturnType;
+ };
+} // End namespace internal
+
+/**
+ * \ingroup SparseQR_Module
+ * \class SparseQR
+ * \brief Sparse left-looking QR factorization with numerical column pivoting
+ *
+ * This class implements a left-looking QR decomposition of sparse matrices
+ * with numerical column pivoting.
+ * When a column has a norm less than a given tolerance
+ * it is implicitly permuted to the end. The QR factorization thus obtained is
+ * given by A*P = Q*R where R is upper triangular or trapezoidal.
+ *
+ * P is the column permutation which is the product of the fill-reducing and the
+ * numerical permutations. Use colsPermutation() to get it.
+ *
+ * Q is the orthogonal matrix represented as products of Householder reflectors.
+ * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint.
+ * You can then apply it to a vector.
+ *
+ * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
+ * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank.
+ *
+ * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<>
+ * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module
+ * OrderingMethods \endlink module for the list of built-in and external ordering methods.
+ *
+ * \implsparsesolverconcept
+ *
+ * The numerical pivoting strategy and default threshold are the same as in SuiteSparse QR, and
+ * detailed in the following paper:
+ * <i>
+ * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+ * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011.
+ * </i>
+ * 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
+ * it is thus strongly recommended to check the accuracy of the computed solution. If it
+ * failed, it usually helps to increase the threshold with setPivotThreshold.
+ *
+ * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
+ * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix.
+ *
+ */
+template<typename _MatrixType, typename _OrderingType>
+class SparseQR : public SparseSolverBase<SparseQR<_MatrixType,_OrderingType> >
+{
+ protected:
+ typedef SparseSolverBase<SparseQR<_MatrixType,_OrderingType> > Base;
+ using Base::m_isInitialized;
+ public:
+ using Base::_solve_impl;
+ typedef _MatrixType MatrixType;
+ typedef _OrderingType OrderingType;
+ typedef typename MatrixType::Scalar Scalar;
+ typedef typename MatrixType::RealScalar RealScalar;
+ typedef typename MatrixType::StorageIndex StorageIndex;
+ typedef SparseMatrix<Scalar,ColMajor,StorageIndex> QRMatrixType;
+ typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+ typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
+ typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+ enum {
+ ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+ MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+ };
+
+ public:
+ SparseQR () : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
+ { }
+
+ /** Construct a QR factorization of the matrix \a mat.
+ *
+ * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+ *
+ * \sa compute()
+ */
+ explicit SparseQR(const MatrixType& mat) : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
+ {
+ compute(mat);
+ }
+
+ /** Computes the QR factorization of the sparse matrix \a mat.
+ *
+ * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+ *
+ * \sa analyzePattern(), factorize()
+ */
+ void compute(const MatrixType& mat)
+ {
+ analyzePattern(mat);
+ factorize(mat);
+ }
+ void analyzePattern(const MatrixType& mat);
+ void factorize(const MatrixType& mat);
+
+ /** \returns the number of rows of the represented matrix.
+ */
+ inline Index rows() const { return m_pmat.rows(); }
+
+ /** \returns the number of columns of the represented matrix.
+ */
+ inline Index cols() const { return m_pmat.cols();}
+
+ /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization.
+ * \warning The entries of the returned matrix are not sorted. This means that using it in algorithms
+ * expecting sorted entries will fail. This include random coefficient accesses (SpaseMatrix::coeff()),
+ * and coefficient-wise operations. Matrix products and triangular solves are fine though.
+ *
+ * To sort the entries, you can assign it to a row-major matrix, and if a column-major matrix
+ * is required, you can copy it again:
+ * \code
+ * SparseMatrix<double> R = qr.matrixR(); // column-major, not sorted!
+ * SparseMatrix<double,RowMajor> Rr = qr.matrixR(); // row-major, sorted
+ * SparseMatrix<double> Rc = Rr; // column-major, sorted
+ * \endcode
+ */
+ const QRMatrixType& matrixR() const { return m_R; }
+
+ /** \returns the number of non linearly dependent columns as determined by the pivoting threshold.
+ *
+ * \sa setPivotThreshold()
+ */
+ Index rank() const
+ {
+ eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+ return m_nonzeropivots;
+ }
+
+ /** \returns an expression of the matrix Q as products of sparse Householder reflectors.
+ * The common usage of this function is to apply it to a dense matrix or vector
+ * \code
+ * VectorXd B1, B2;
+ * // Initialize B1
+ * B2 = matrixQ() * B1;
+ * \endcode
+ *
+ * To get a plain SparseMatrix representation of Q:
+ * \code
+ * SparseMatrix<double> Q;
+ * Q = SparseQR<SparseMatrix<double> >(A).matrixQ();
+ * \endcode
+ * Internally, this call simply performs a sparse product between the matrix Q
+ * and a sparse identity matrix. However, due to the fact that the sparse
+ * reflectors are stored unsorted, two transpositions are needed to sort
+ * them before performing the product.
+ */
+ SparseQRMatrixQReturnType<SparseQR> matrixQ() const
+ { return SparseQRMatrixQReturnType<SparseQR>(*this); }
+
+ /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R
+ * It is the combination of the fill-in reducing permutation and numerical column pivoting.
+ */
+ const PermutationType& colsPermutation() const
+ {
+ eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+ return m_outputPerm_c;
+ }
+
+ /** \returns A string describing the type of error.
+ * This method is provided to ease debugging, not to handle errors.
+ */
+ std::string lastErrorMessage() const { return m_lastError; }
+
+ /** \internal */
+ template<typename Rhs, typename Dest>
+ bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &dest) const
+ {
+ eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+ eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+
+ Index rank = this->rank();
+
+ // Compute Q^* * b;
+ typename Dest::PlainObject y, b;
+ y = this->matrixQ().adjoint() * B;
+ b = y;
+
+ // Solve with the triangular matrix R
+ y.resize((std::max<Index>)(cols(),y.rows()),y.cols());
+ y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(b.topRows(rank));
+ y.bottomRows(y.rows()-rank).setZero();
+
+ // Apply the column permutation
+ if (m_perm_c.size()) dest = colsPermutation() * y.topRows(cols());
+ else dest = y.topRows(cols());
+
+ m_info = Success;
+ return true;
+ }
+
+ /** Sets the threshold that is used to determine linearly dependent columns during the factorization.
+ *
+ * In practice, if during the factorization the norm of the column that has to be eliminated is below
+ * this threshold, then the entire column is treated as zero, and it is moved at the end.
+ */
+ void setPivotThreshold(const RealScalar& threshold)
+ {
+ m_useDefaultThreshold = false;
+ m_threshold = threshold;
+ }
+
+ /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+ *
+ * \sa compute()
+ */
+ template<typename Rhs>
+ inline const Solve<SparseQR, Rhs> solve(const MatrixBase<Rhs>& B) const
+ {
+ eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+ eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+ return Solve<SparseQR, Rhs>(*this, B.derived());
+ }
+ template<typename Rhs>
+ inline const Solve<SparseQR, Rhs> solve(const SparseMatrixBase<Rhs>& B) const
+ {
+ eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+ eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+ return Solve<SparseQR, Rhs>(*this, B.derived());
+ }
+
+ /** \brief Reports whether previous computation was successful.
+ *
+ * \returns \c Success if computation was successful,
+ * \c NumericalIssue if the QR factorization reports a numerical problem
+ * \c InvalidInput if the input matrix is invalid
+ *
+ * \sa iparm()
+ */
+ ComputationInfo info() const
+ {
+ eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+ return m_info;
+ }
+
+
+ /** \internal */
+ inline void _sort_matrix_Q()
+ {
+ if(this->m_isQSorted) return;
+ // The matrix Q is sorted during the transposition
+ SparseMatrix<Scalar, RowMajor, Index> mQrm(this->m_Q);
+ this->m_Q = mQrm;
+ this->m_isQSorted = true;
+ }
+
+
+ protected:
+ bool m_analysisIsok;
+ bool m_factorizationIsok;
+ mutable ComputationInfo m_info;
+ std::string m_lastError;
+ QRMatrixType m_pmat; // Temporary matrix
+ QRMatrixType m_R; // The triangular factor matrix
+ QRMatrixType m_Q; // The orthogonal reflectors
+ ScalarVector m_hcoeffs; // The Householder coefficients
+ PermutationType m_perm_c; // Fill-reducing Column permutation
+ PermutationType m_pivotperm; // The permutation for rank revealing
+ PermutationType m_outputPerm_c; // The final column permutation
+ RealScalar m_threshold; // Threshold to determine null Householder reflections
+ bool m_useDefaultThreshold; // Use default threshold
+ Index m_nonzeropivots; // Number of non zero pivots found
+ IndexVector m_etree; // Column elimination tree
+ IndexVector m_firstRowElt; // First element in each row
+ bool m_isQSorted; // whether Q is sorted or not
+ bool m_isEtreeOk; // whether the elimination tree match the initial input matrix
+
+ template <typename, typename > friend struct SparseQR_QProduct;
+
+};
+
+/** \brief Preprocessing step of a QR factorization
+ *
+ * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+ *
+ * In this step, the fill-reducing permutation is computed and applied to the columns of A
+ * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
+ *
+ * \note In this step it is assumed that there is no empty row in the matrix \a mat.
+ */
+template <typename MatrixType, typename OrderingType>
+void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
+{
+ eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
+ // Copy to a column major matrix if the input is rowmajor
+ typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
+ // Compute the column fill reducing ordering
+ OrderingType ord;
+ ord(matCpy, m_perm_c);
+ Index n = mat.cols();
+ Index m = mat.rows();
+ Index diagSize = (std::min)(m,n);
+
+ if (!m_perm_c.size())
+ {
+ m_perm_c.resize(n);
+ m_perm_c.indices().setLinSpaced(n, 0,StorageIndex(n-1));
+ }
+
+ // Compute the column elimination tree of the permuted matrix
+ m_outputPerm_c = m_perm_c.inverse();
+ internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+ m_isEtreeOk = true;
+
+ m_R.resize(m, n);
+ m_Q.resize(m, diagSize);
+
+ // Allocate space for nonzero elements: rough estimation
+ m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
+ m_Q.reserve(2*mat.nonZeros());
+ m_hcoeffs.resize(diagSize);
+ m_analysisIsok = true;
+}
+
+/** \brief Performs the numerical QR factorization of the input matrix
+ *
+ * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
+ * a matrix having the same sparsity pattern than \a mat.
+ *
+ * \param mat The sparse column-major matrix
+ */
+template <typename MatrixType, typename OrderingType>
+void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
+{
+ using std::abs;
+
+ eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
+ StorageIndex m = StorageIndex(mat.rows());
+ StorageIndex n = StorageIndex(mat.cols());
+ StorageIndex diagSize = (std::min)(m,n);
+ IndexVector mark((std::max)(m,n)); mark.setConstant(-1); // Record the visited nodes
+ 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();
+ m_pmat = mat;
+ if(!m_isEtreeOk)
+ {
+ m_outputPerm_c = m_perm_c.inverse();
+ internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+ m_isEtreeOk = true;
+ }
+
+ m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
+
+ // Apply the fill-in reducing permutation lazily:
+ {
+ // If the input is row major, copy the original column indices,
+ // otherwise directly use the input matrix
+ //
+ IndexVector originalOuterIndicesCpy;
+ const StorageIndex *originalOuterIndices = mat.outerIndexPtr();
+ if(MatrixType::IsRowMajor)
+ {
+ originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
+ originalOuterIndices = originalOuterIndicesCpy.data();
+ }
+
+ for (int i = 0; i < n; i++)
+ {
+ Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
+ m_pmat.outerIndexPtr()[p] = originalOuterIndices[i];
+ m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i];
+ }
+ }
+
+ /* Compute the default threshold as in MatLab, see:
+ * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+ * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3
+ */
+ 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<RealScalar>::epsilon();
+ }
+
+ // Initialize the numerical permutation
+ m_pivotperm.setIdentity(n);
+
+ StorageIndex nonzeroCol = 0; // Record the number of valid pivots
+ m_Q.startVec(0);
+
+ // Left looking rank-revealing QR factorization: compute a column of R and Q at a time
+ for (StorageIndex col = 0; col < n; ++col)
+ {
+ mark.setConstant(-1);
+ m_R.startVec(col);
+ mark(nonzeroCol) = col;
+ Qidx(0) = nonzeroCol;
+ nzcolR = 0; nzcolQ = 1;
+ bool found_diag = nonzeroCol>=m;
+ tval.setZero();
+
+ // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
+ // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
+ // Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
+ // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
+ for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
+ {
+ StorageIndex curIdx = nonzeroCol;
+ if(itp) curIdx = StorageIndex(itp.row());
+ if(curIdx == nonzeroCol) found_diag = true;
+
+ // Get the nonzeros indexes of the current column of R
+ StorageIndex st = m_firstRowElt(curIdx); // The traversal of the etree starts here
+ if (st < 0 )
+ {
+ m_lastError = "Empty row found during numerical factorization";
+ m_info = InvalidInput;
+ return;
+ }
+
+ // Traverse the etree
+ Index bi = nzcolR;
+ for (; mark(st) != col; st = m_etree(st))
+ {
+ Ridx(nzcolR) = st; // Add this row to the list,
+ mark(st) = col; // and mark this row as visited
+ nzcolR++;
+ }
+
+ // Reverse the list to get the topological ordering
+ Index nt = nzcolR-bi;
+ for(Index i = 0; i < nt/2; i++) std::swap(Ridx(bi+i), Ridx(nzcolR-i-1));
+
+ // Copy the current (curIdx,pcol) value of the input matrix
+ if(itp) tval(curIdx) = itp.value();
+ else tval(curIdx) = Scalar(0);
+
+ // Compute the pattern of Q(:,k)
+ if(curIdx > nonzeroCol && mark(curIdx) != col )
+ {
+ Qidx(nzcolQ) = curIdx; // Add this row to the pattern of Q,
+ mark(curIdx) = col; // and mark it as visited
+ nzcolQ++;
+ }
+ }
+
+ // Browse all the indexes of R(:,col) in reverse order
+ for (Index i = nzcolR-1; i >= 0; i--)
+ {
+ Index curIdx = Ridx(i);
+
+ // Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
+ Scalar tdot(0);
+
+ // First compute q' * tval
+ tdot = m_Q.col(curIdx).dot(tval);
+
+ tdot *= m_hcoeffs(curIdx);
+
+ // Then update tval = tval - q * tau
+ // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense ?= sparse")
+ for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
+ tval(itq.row()) -= itq.value() * tdot;
+
+ // Detect fill-in for the current column of Q
+ if(m_etree(Ridx(i)) == nonzeroCol)
+ {
+ for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
+ {
+ StorageIndex iQ = StorageIndex(itq.row());
+ if (mark(iQ) != col)
+ {
+ Qidx(nzcolQ++) = iQ; // Add this row to the pattern of Q,
+ mark(iQ) = col; // and mark it as visited
+ }
+ }
+ }
+ } // End update current column
+
+ Scalar tau = RealScalar(0);
+ RealScalar beta = 0;
+
+ if(nonzeroCol < diagSize)
+ {
+ // Compute the Householder reflection that eliminate the current column
+ // FIXME this step should call the Householder module.
+ Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
+
+ // First, the squared norm of Q((col+1):m, col)
+ RealScalar sqrNorm = 0.;
+ for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
+ if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
+ {
+ beta = numext::real(c0);
+ tval(Qidx(0)) = 1;
+ }
+ else
+ {
+ using std::sqrt;
+ beta = sqrt(numext::abs2(c0) + sqrNorm);
+ if(numext::real(c0) >= RealScalar(0))
+ beta = -beta;
+ tval(Qidx(0)) = 1;
+ for (Index itq = 1; itq < nzcolQ; ++itq)
+ tval(Qidx(itq)) /= (c0 - beta);
+ tau = numext::conj((beta-c0) / beta);
+
+ }
+ }
+
+ // Insert values in R
+ for (Index i = nzcolR-1; i >= 0; i--)
+ {
+ Index curIdx = Ridx(i);
+ if(curIdx < nonzeroCol)
+ {
+ m_R.insertBackByOuterInnerUnordered(col, curIdx) = tval(curIdx);
+ tval(curIdx) = Scalar(0.);
+ }
+ }
+
+ if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
+ {
+ m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
+ // The householder coefficient
+ m_hcoeffs(nonzeroCol) = tau;
+ // Record the householder reflections
+ for (Index itq = 0; itq < nzcolQ; ++itq)
+ {
+ Index iQ = Qidx(itq);
+ m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
+ tval(iQ) = Scalar(0.);
+ }
+ nonzeroCol++;
+ if(nonzeroCol<diagSize)
+ m_Q.startVec(nonzeroCol);
+ }
+ else
+ {
+ // Zero pivot found: move implicitly this column to the end
+ for (Index j = nonzeroCol; j < n-1; j++)
+ std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
+
+ // Recompute the column elimination tree
+ internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
+ m_isEtreeOk = false;
+ }
+ }
+
+ m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
+
+ // Finalize the column pointers of the sparse matrices R and Q
+ m_Q.finalize();
+ m_Q.makeCompressed();
+ m_R.finalize();
+ m_R.makeCompressed();
+ m_isQSorted = false;
+
+ m_nonzeropivots = nonzeroCol;
+
+ if(nonzeroCol<n)
+ {
+ // Permute the triangular factor to put the 'dead' columns to the end
+ QRMatrixType tempR(m_R);
+ m_R = tempR * m_pivotperm;
+
+ // Update the column permutation
+ m_outputPerm_c = m_outputPerm_c * m_pivotperm;
+ }
+
+ m_isInitialized = true;
+ m_factorizationIsok = true;
+ m_info = Success;
+}
+
+template <typename SparseQRType, typename Derived>
+struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived> >
+{
+ typedef typename SparseQRType::QRMatrixType MatrixType;
+ typedef typename SparseQRType::Scalar Scalar;
+ // Get the references
+ SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) :
+ m_qr(qr),m_other(other),m_transpose(transpose) {}
+ inline Index rows() const { return m_qr.matrixQ().rows(); }
+ inline Index cols() const { return m_other.cols(); }
+
+ // Assign to a vector
+ template<typename DesType>
+ void evalTo(DesType& res) const
+ {
+ Index m = m_qr.rows();
+ Index n = m_qr.cols();
+ Index diagSize = (std::min)(m,n);
+ res = m_other;
+ if (m_transpose)
+ {
+ eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
+ //Compute res = Q' * other column by column
+ for(Index j = 0; j < res.cols(); j++){
+ for (Index k = 0; k < diagSize; k++)
+ {
+ Scalar tau = Scalar(0);
+ tau = m_qr.m_Q.col(k).dot(res.col(j));
+ if(tau==Scalar(0)) continue;
+ tau = tau * m_qr.m_hcoeffs(k);
+ res.col(j) -= tau * m_qr.m_Q.col(k);
+ }
+ }
+ }
+ else
+ {
+ eigen_assert(m_qr.matrixQ().cols() == m_other.rows() && "Non conforming object sizes");
+
+ res.conservativeResize(rows(), cols());
+
+ // Compute res = Q * other column by column
+ for(Index j = 0; j < res.cols(); j++)
+ {
+ Index start_k = internal::is_identity<Derived>::value ? numext::mini(j,diagSize-1) : diagSize-1;
+ for (Index k = start_k; k >=0; k--)
+ {
+ Scalar tau = Scalar(0);
+ tau = m_qr.m_Q.col(k).dot(res.col(j));
+ if(tau==Scalar(0)) continue;
+ tau = tau * numext::conj(m_qr.m_hcoeffs(k));
+ res.col(j) -= tau * m_qr.m_Q.col(k);
+ }
+ }
+ }
+ }
+
+ const SparseQRType& m_qr;
+ const Derived& m_other;
+ bool m_transpose; // TODO this actually means adjoint
+};
+
+template<typename SparseQRType>
+struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<SparseQRType> >
+{
+ typedef typename SparseQRType::Scalar Scalar;
+ typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
+ enum {
+ RowsAtCompileTime = Dynamic,
+ ColsAtCompileTime = Dynamic
+ };
+ explicit SparseQRMatrixQReturnType(const SparseQRType& qr) : m_qr(qr) {}
+ template<typename Derived>
+ SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other)
+ {
+ return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(),false);
+ }
+ // To use for operations with the adjoint of Q
+ SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const
+ {
+ return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
+ }
+ inline Index rows() const { return m_qr.rows(); }
+ inline Index cols() const { return m_qr.rows(); }
+ // To use for operations with the transpose of Q FIXME this is the same as adjoint at the moment
+ SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
+ {
+ return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
+ }
+ const SparseQRType& m_qr;
+};
+
+// TODO this actually represents the adjoint of Q
+template<typename SparseQRType>
+struct SparseQRMatrixQTransposeReturnType
+{
+ explicit SparseQRMatrixQTransposeReturnType(const SparseQRType& qr) : m_qr(qr) {}
+ template<typename Derived>
+ SparseQR_QProduct<SparseQRType,Derived> operator*(const MatrixBase<Derived>& other)
+ {
+ return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(), true);
+ }
+ const SparseQRType& m_qr;
+};
+
+namespace internal {
+
+template<typename SparseQRType>
+struct evaluator_traits<SparseQRMatrixQReturnType<SparseQRType> >
+{
+ typedef typename SparseQRType::MatrixType MatrixType;
+ typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
+ typedef SparseShape Shape;
+};
+
+template< typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Sparse>
+{
+ typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
+ typedef typename DstXprType::Scalar Scalar;
+ typedef typename DstXprType::StorageIndex StorageIndex;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
+ {
+ typename DstXprType::PlainObject idMat(src.rows(), src.cols());
+ idMat.setIdentity();
+ // Sort the sparse householder reflectors if needed
+ const_cast<SparseQRType *>(&src.m_qr)->_sort_matrix_Q();
+ dst = SparseQR_QProduct<SparseQRType, DstXprType>(src.m_qr, idMat, false);
+ }
+};
+
+template< typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Dense>
+{
+ typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
+ typedef typename DstXprType::Scalar Scalar;
+ typedef typename DstXprType::StorageIndex StorageIndex;
+ static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
+ {
+ dst = src.m_qr.matrixQ() * DstXprType::Identity(src.m_qr.rows(), src.m_qr.rows());
+ }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
new file mode 100644
index 0000000..cb28ff0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
@@ -0,0 +1,104 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_HPP
+#define _gcem_HPP
+
+#include "gcem_incl/gcem_options.hpp"
+
+namespace gcem
+{
+ #include "gcem_incl/quadrature/gauss_legendre_50.hpp"
+
+ #include "gcem_incl/is_inf.hpp"
+ #include "gcem_incl/is_nan.hpp"
+ #include "gcem_incl/is_finite.hpp"
+
+ #include "gcem_incl/signbit.hpp"
+ #include "gcem_incl/copysign.hpp"
+ #include "gcem_incl/neg_zero.hpp"
+ #include "gcem_incl/sgn.hpp"
+
+ #include "gcem_incl/abs.hpp"
+ #include "gcem_incl/ceil.hpp"
+ #include "gcem_incl/floor.hpp"
+ #include "gcem_incl/trunc.hpp"
+ #include "gcem_incl/is_odd.hpp"
+ #include "gcem_incl/is_even.hpp"
+ #include "gcem_incl/max.hpp"
+ #include "gcem_incl/min.hpp"
+ #include "gcem_incl/sqrt.hpp"
+ #include "gcem_incl/inv_sqrt.hpp"
+ #include "gcem_incl/hypot.hpp"
+
+ #include "gcem_incl/find_exponent.hpp"
+ #include "gcem_incl/find_fraction.hpp"
+ #include "gcem_incl/find_whole.hpp"
+ #include "gcem_incl/mantissa.hpp"
+ #include "gcem_incl/round.hpp"
+ #include "gcem_incl/fmod.hpp"
+
+ #include "gcem_incl/pow_integral.hpp"
+ #include "gcem_incl/exp.hpp"
+ #include "gcem_incl/expm1.hpp"
+ #include "gcem_incl/log.hpp"
+ #include "gcem_incl/log1p.hpp"
+ #include "gcem_incl/log2.hpp"
+ #include "gcem_incl/log10.hpp"
+ #include "gcem_incl/pow.hpp"
+
+ #include "gcem_incl/gcd.hpp"
+ #include "gcem_incl/lcm.hpp"
+
+ #include "gcem_incl/tan.hpp"
+ #include "gcem_incl/cos.hpp"
+ #include "gcem_incl/sin.hpp"
+
+ #include "gcem_incl/atan.hpp"
+ #include "gcem_incl/atan2.hpp"
+ #include "gcem_incl/acos.hpp"
+ #include "gcem_incl/asin.hpp"
+
+ #include "gcem_incl/tanh.hpp"
+ #include "gcem_incl/cosh.hpp"
+ #include "gcem_incl/sinh.hpp"
+
+ #include "gcem_incl/atanh.hpp"
+ #include "gcem_incl/acosh.hpp"
+ #include "gcem_incl/asinh.hpp"
+
+ #include "gcem_incl/binomial_coef.hpp"
+ #include "gcem_incl/lgamma.hpp"
+ #include "gcem_incl/tgamma.hpp"
+ #include "gcem_incl/factorial.hpp"
+ #include "gcem_incl/lbeta.hpp"
+ #include "gcem_incl/beta.hpp"
+ #include "gcem_incl/lmgamma.hpp"
+ #include "gcem_incl/log_binomial_coef.hpp"
+
+ #include "gcem_incl/erf.hpp"
+ #include "gcem_incl/erf_inv.hpp"
+ #include "gcem_incl/incomplete_beta.hpp"
+ #include "gcem_incl/incomplete_beta_inv.hpp"
+ #include "gcem_incl/incomplete_gamma.hpp"
+ #include "gcem_incl/incomplete_gamma_inv.hpp"
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
new file mode 100644
index 0000000..0a3ada6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
@@ -0,0 +1,45 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_abs_HPP
+#define _gcem_abs_HPP
+
+/**
+ * Compile-time absolute value function
+ *
+ * @param x a real-valued input.
+ * @return the absolute value of \c x, \f$ |x| \f$.
+ */
+
+template<typename T>
+constexpr
+T
+abs(const T x)
+noexcept
+{
+ return( // deal with signed-zeros
+ x == T(0) ? \
+ T(0) :
+ // else
+ x < T(0) ? \
+ - x : x );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
new file mode 100644
index 0000000..9d3bc07
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
@@ -0,0 +1,84 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time arccosine function
+ */
+
+#ifndef _gcem_acos_HPP
+#define _gcem_acos_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+acos_compute(const T x)
+noexcept
+{
+ return( // only defined on [-1,1]
+ abs(x) > T(1) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from one or zero
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(x) ? \
+ T(GCEM_HALF_PI) :
+ // else
+ atan( sqrt(T(1) - x*x)/x ) );
+}
+
+template<typename T>
+constexpr
+T
+acos_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ x > T(0) ? \
+ // if
+ acos_compute(x) :
+ // else
+ T(GCEM_PI) - acos_compute(-x) );
+}
+
+}
+
+/**
+ * Compile-time arccosine function
+ *
+ * @param x a real-valued input, where \f$ x \in [-1,1] \f$.
+ * @return the inverse cosine function using \f[ \text{acos}(x) = \text{atan} \left( \frac{\sqrt{1-x^2}}{x} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+acos(const T x)
+noexcept
+{
+ return internal::acos_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
new file mode 100644
index 0000000..79579d7
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
@@ -0,0 +1,68 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic cosine function
+ */
+
+#ifndef _gcem_acosh_HPP
+#define _gcem_acosh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+acosh_compute(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // function defined for x >= 1
+ x < T(1) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from 1
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(0) :
+ // else
+ log( x + sqrt(x*x - T(1)) ) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic cosine function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic cosine function using \f[ \text{acosh}(x) = \ln \left( x + \sqrt{x^2 - 1} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+acosh(const T x)
+noexcept
+{
+ return internal::acosh_compute( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
new file mode 100644
index 0000000..210d9fc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time arcsine function
+ */
+
+#ifndef _gcem_asin_HPP
+#define _gcem_asin_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+asin_compute(const T x)
+noexcept
+{
+ return( // only defined on [-1,1]
+ x > T(1) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from one or zero
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(GCEM_HALF_PI) :
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ atan( x/sqrt(T(1) - x*x) ) );
+}
+
+template<typename T>
+constexpr
+T
+asin_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ x < T(0) ? \
+ - asin_compute(-x) :
+ asin_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time arcsine function
+ *
+ * @param x a real-valued input, where \f$ x \in [-1,1] \f$.
+ * @return the inverse sine function using \f[ \text{asin}(x) = \text{atan} \left( \frac{x}{\sqrt{1-x^2}} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+asin(const T x)
+noexcept
+{
+ return internal::asin_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
new file mode 100644
index 0000000..dfad57e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
@@ -0,0 +1,66 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic sine function
+ */
+
+#ifndef _gcem_asinh_HPP
+#define _gcem_asinh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+asinh_compute(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ log( x + sqrt(x*x + T(1)) ) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic sine function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic sine function using \f[ \text{asinh}(x) = \ln \left( x + \sqrt{x^2 + 1} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+asinh(const T x)
+noexcept
+{
+ return internal::asinh_compute( static_cast<return_t<T>>(x) );
+}
+
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
new file mode 100644
index 0000000..9aea85b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
@@ -0,0 +1,155 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time arctangent function
+ */
+
+// see
+// http://functions.wolfram.com/ElementaryFunctions/ArcTan/10/0001/
+// http://functions.wolfram.com/ElementaryFunctions/ArcTan/06/01/06/01/0002/
+
+#ifndef _gcem_atan_HPP
+#define _gcem_atan_HPP
+
+namespace internal
+{
+
+// Series
+
+template<typename T>
+constexpr
+T
+atan_series_order_calc(const T x, const T x_pow, const uint_t order)
+noexcept
+{
+ return( T(1)/( T((order-1)*4 - 1) * x_pow ) \
+ - T(1)/( T((order-1)*4 + 1) * x_pow*x) );
+}
+
+template<typename T>
+constexpr
+T
+atan_series_order(const T x, const T x_pow, const uint_t order, const uint_t max_order)
+noexcept
+{
+ return( order == 1 ? \
+ GCEM_HALF_PI - T(1)/x + atan_series_order(x*x,pow(x,3),order+1,max_order) :
+ // NOTE: x changes to x*x for order > 1
+ order < max_order ? \
+ atan_series_order_calc(x,x_pow,order) \
+ + atan_series_order(x,x_pow*x*x,order+1,max_order) :
+ // order == max_order
+ atan_series_order_calc(x,x_pow,order) );
+}
+
+template<typename T>
+constexpr
+T
+atan_series_main(const T x)
+noexcept
+{
+ return( x < T(3) ? atan_series_order(x,x,1U,10U) : // O(1/x^39)
+ x < T(4) ? atan_series_order(x,x,1U,9U) : // O(1/x^35)
+ x < T(5) ? atan_series_order(x,x,1U,8U) : // O(1/x^31)
+ x < T(7) ? atan_series_order(x,x,1U,7U) : // O(1/x^27)
+ x < T(11) ? atan_series_order(x,x,1U,6U) : // O(1/x^23)
+ x < T(25) ? atan_series_order(x,x,1U,5U) : // O(1/x^19)
+ x < T(100) ? atan_series_order(x,x,1U,4U) : // O(1/x^15)
+ x < T(1000) ? atan_series_order(x,x,1U,3U) : // O(1/x^11)
+ atan_series_order(x,x,1U,2U) ); // O(1/x^7)
+}
+
+// CF
+
+template<typename T>
+constexpr
+T
+atan_cf_recur(const T xx, const uint_t depth, const uint_t max_depth)
+noexcept
+{
+ return( depth < max_depth ? \
+ // if
+ T(2*depth - 1) + depth*depth*xx/atan_cf_recur(xx,depth+1,max_depth) :
+ // else
+ T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+atan_cf_main(const T x)
+noexcept
+{
+ return( x < T(0.5) ? x/atan_cf_recur(x*x,1U, 15U ) :
+ x < T(1) ? x/atan_cf_recur(x*x,1U, 25U ) :
+ x < T(1.5) ? x/atan_cf_recur(x*x,1U, 35U ) :
+ x < T(2) ? x/atan_cf_recur(x*x,1U, 45U ) :
+ x/atan_cf_recur(x*x,1U, 52U ) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+atan_begin(const T x)
+noexcept
+{
+ return( x > T(2.5) ? atan_series_main(x) : atan_cf_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+atan_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // negative or positive
+ x < T(0) ? \
+ - atan_begin(-x) :
+ atan_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time arctangent function
+ *
+ * @param x a real-valued input.
+ * @return the inverse tangent function using \f[ \text{atan}(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{4x^2}{5 + \dfrac{9x^2}{7 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+atan(const T x)
+noexcept
+{
+ return internal::atan_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
new file mode 100644
index 0000000..97c8d6a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
@@ -0,0 +1,88 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time two-argument arctangent function
+ */
+
+#ifndef _gcem_atan2_HPP
+#define _gcem_atan2_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+atan2_compute(const T y, const T x)
+noexcept
+{
+ return( // NaN check
+ any_nan(y,x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ GCLIM<T>::min() > abs(x) ? \
+ //
+ GCLIM<T>::min() > abs(y) ? \
+ neg_zero(y) ? \
+ neg_zero(x) ? - T(GCEM_PI) : - T(0) :
+ neg_zero(x) ? T(GCEM_PI) : T(0) :
+ y > T(0) ? \
+ T(GCEM_HALF_PI) : - T(GCEM_HALF_PI) :
+ //
+ x < T(0) ? \
+ y < T(0) ? \
+ atan(y/x) - T(GCEM_PI) :
+ atan(y/x) + T(GCEM_PI) :
+ //
+ atan(y/x) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+atan2_type_check(const T1 y, const T2 x)
+noexcept
+{
+ return atan2_compute(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time two-argument arctangent function
+ *
+ * @param y a real-valued input.
+ * @param x a real-valued input.
+ * @return \f[ \text{atan2}(y,x) = \begin{cases} \text{atan}(y/x) & \text{ if } x > 0 \\ \text{atan}(y/x) + \pi & \text{ if } x < 0 \text{ and } y \geq 0 \\ \text{atan}(y/x) - \pi & \text{ if } x < 0 \text{ and } y < 0 \\ + \pi/2 & \text{ if } x = 0 \text{ and } y > 0 \\ - \pi/2 & \text{ if } x = 0 \text{ and } y < 0 \end{cases} \f]
+ * The function is undefined at the origin, however the following conventions are used.
+ * \f[ \text{atan2}(y,x) = \begin{cases} +0 & \text{ if } x = +0 \text{ and } y = +0 \\ -0 & \text{ if } x = +0 \text{ and } y = -0 \\ +\pi & \text{ if } x = -0 \text{ and } y = +0 \\ - \pi & \text{ if } x = -0 \text{ and } y = -0 \end{cases} \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+atan2(const T1 y, const T2 x)
+noexcept
+{
+ return internal::atan2_type_check(x,y);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
new file mode 100644
index 0000000..2e1cb76
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
@@ -0,0 +1,79 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic tangent function
+ */
+
+#ifndef _gcem_atanh_HPP
+#define _gcem_atanh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+atanh_compute(const T x)
+noexcept
+{
+ return( log( (T(1) + x)/(T(1) - x) ) / T(2) );
+}
+
+template<typename T>
+constexpr
+T
+atanh_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // function is defined for |x| < 1
+ T(1) < abs(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ GCLIM<T>::min() > (T(1) - abs(x)) ? \
+ sgn(x)*GCLIM<T>::infinity() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ atanh_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic tangent function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic tangent function using \f[ \text{atanh}(x) = \frac{1}{2} \ln \left( \frac{1+x}{1-x} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+atanh(const T x)
+noexcept
+{
+ return internal::atanh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
new file mode 100644
index 0000000..e888e47
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_beta_HPP
+#define _gcem_beta_HPP
+
+/**
+ * Compile-time beta function
+ *
+ * @param a a real-valued input.
+ * @param b a real-valued input.
+ * @return the beta function using \f[ \text{B}(\alpha,\beta) := \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \frac{\Gamma(\alpha)\Gamma(\beta)}{\Gamma(\alpha + \beta)} \f]
+ * where \f$ \Gamma \f$ denotes the gamma function.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+beta(const T1 a, const T2 b)
+noexcept
+{
+ return exp( lbeta(a,b) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
new file mode 100644
index 0000000..fb050a2
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
@@ -0,0 +1,91 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_binomial_coef_HPP
+#define _gcem_binomial_coef_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+binomial_coef_recur(const T n, const T k)
+noexcept
+{
+ return( // edge cases
+ (k == T(0) || n == k) ? T(1) : // deals with 0 choose 0 case
+ n == T(0) ? T(0) :
+ // else
+ binomial_coef_recur(n-1,k-1) + binomial_coef_recur(n-1,k) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+binomial_coef_check(const T n, const T k)
+noexcept
+{
+ return binomial_coef_recur(n,k);
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+binomial_coef_check(const T n, const T k)
+noexcept
+{
+ return( // NaN check; removed due to MSVC problems; template not being ignored in <int> cases
+ // (is_nan(n) || is_nan(k)) ? GCLIM<T>::quiet_NaN() :
+ //
+ static_cast<T>(binomial_coef_recur(static_cast<ullint_t>(n),static_cast<ullint_t>(k))) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+binomial_coef_type_check(const T1 n, const T2 k)
+noexcept
+{
+ return binomial_coef_check(static_cast<TC>(n),static_cast<TC>(k));
+}
+
+}
+
+/**
+ * Compile-time binomial coefficient
+ *
+ * @param n integral-valued input.
+ * @param k integral-valued input.
+ * @return computes the Binomial coefficient
+ * \f[ \binom{n}{k} = \frac{n!}{k!(n-k)!} \f]
+ * also known as '\c n choose \c k '.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+binomial_coef(const T1 n, const T2 k)
+noexcept
+{
+ return internal::binomial_coef_type_check(n,k);
+}
+
+#endif
\ No newline at end of file
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
new file mode 100644
index 0000000..e8570ab
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
@@ -0,0 +1,130 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_ceil_HPP
+#define _gcem_ceil_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+int
+ceil_resid(const T x, const T x_whole)
+noexcept
+{
+ return( (x > T(0)) && (x > x_whole) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_int(const T x, const T x_whole)
+noexcept
+{
+ return( x_whole + static_cast<T>(ceil_resid(x,x_whole)) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_check_internal(const T x)
+noexcept
+{
+ return x;
+}
+
+template<>
+constexpr
+float
+ceil_check_internal<float>(const float x)
+noexcept
+{
+ return( abs(x) >= 8388608.f ? \
+ // if
+ x : \
+ // else
+ ceil_int(x, float(static_cast<int>(x))) );
+}
+
+template<>
+constexpr
+double
+ceil_check_internal<double>(const double x)
+noexcept
+{
+ return( abs(x) >= 4503599627370496. ? \
+ // if
+ x : \
+ // else
+ ceil_int(x, double(static_cast<llint_t>(x))) );
+}
+
+template<>
+constexpr
+long double
+ceil_check_internal<long double>(const long double x)
+noexcept
+{
+ return( abs(x) >= 9223372036854775808.l ? \
+ // if
+ x : \
+ // else
+ ceil_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/- infinite
+ !is_finite(x) ? \
+ x :
+ // signed-zero cases
+ GCLIM<T>::min() > abs(x) ? \
+ x :
+ // else
+ ceil_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time ceil function
+ *
+ * @param x a real-valued input.
+ * @return computes the ceiling-value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+ceil(const T x)
+noexcept
+{
+ return internal::ceil_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
new file mode 100644
index 0000000..a3bab74
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_copysign_HPP
+#define _gcem_copysign_HPP
+
+/**
+ * Compile-time copy sign function
+ *
+ * @param x a real-valued input
+ * @param y a real-valued input
+ * @return replace the signbit of \c x with the signbit of \c y.
+ */
+
+template <typename T1, typename T2>
+constexpr
+T1
+copysign(const T1 x, const T2 y)
+noexcept
+{
+ return( signbit(x) != signbit(y) ? -x : x );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
new file mode 100644
index 0000000..0e98012
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
@@ -0,0 +1,83 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time cosine function using tan(x/2)
+ */
+
+#ifndef _gcem_cos_HPP
+#define _gcem_cos_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+cos_compute(const T x)
+noexcept
+{
+ return( T(1) - x*x)/(T(1) + x*x );
+}
+
+template<typename T>
+constexpr
+T
+cos_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from 0
+ GCLIM<T>::min() > abs(x) ?
+ T(1) :
+ // special cases: pi/2 and pi
+ GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
+ - T(1) :
+ GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
+ - T(1) :
+ // else
+ cos_compute( tan(x/T(2)) ) );
+}
+
+}
+
+/**
+ * Compile-time cosine function
+ *
+ * @param x a real-valued input.
+ * @return the cosine function using \f[ \cos(x) = \frac{1-\tan^2(x/2)}{1+\tan^2(x/2)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+cos(const T x)
+noexcept
+{
+ return internal::cos_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
new file mode 100644
index 0000000..b3cebb8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time hyperbolic cosine function
+ */
+
+#ifndef _gcem_cosh_HPP
+#define _gcem_cosh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+cosh_compute(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(1) :
+ // else
+ (exp(x) + exp(-x)) / T(2) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic cosine function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic cosine function using \f[ \cosh(x) = \frac{\exp(x) + \exp(-x)}{2} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+cosh(const T x)
+noexcept
+{
+ return internal::cosh_compute( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
new file mode 100644
index 0000000..afec09e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
@@ -0,0 +1,143 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time error function
+ */
+
+#ifndef _gcem_erf_HPP
+#define _gcem_erf_HPP
+
+namespace internal
+{
+
+// see
+// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0007/
+
+template<typename T>
+constexpr
+T
+erf_cf_large_recur(const T x, const int depth)
+noexcept
+{
+ return( depth < GCEM_ERF_MAX_ITER ? \
+ // if
+ x + 2*depth/erf_cf_large_recur(x,depth+1) :
+ // else
+ x );
+}
+
+template<typename T>
+constexpr
+T
+erf_cf_large_main(const T x)
+noexcept
+{
+ return( T(1) - T(2) * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
+ / erf_cf_large_recur(T(2)*x,1) );
+}
+
+// see
+// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0005/
+
+template<typename T>
+constexpr
+T
+erf_cf_small_recur(const T xx, const int depth)
+noexcept
+{
+ return( depth < GCEM_ERF_MAX_ITER ? \
+ // if
+ (2*depth - T(1)) - 2*xx \
+ + 4*depth*xx / erf_cf_small_recur(xx,depth+1) :
+ // else
+ (2*depth - T(1)) - 2*xx );
+}
+
+template<typename T>
+constexpr
+T
+erf_cf_small_main(const T x)
+noexcept
+{
+ return( T(2) * x * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
+ / erf_cf_small_recur(x*x,1) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+erf_begin(const T x)
+noexcept
+{
+ return( x > T(2.1) ? \
+ // if
+ erf_cf_large_main(x) :
+ // else
+ erf_cf_small_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+erf_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/-Inf
+ is_posinf(x) ? \
+ T(1) :
+ is_neginf(x) ? \
+ - T(1) :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ x < T(0) ? \
+ - erf_begin(-x) :
+ erf_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time Gaussian error function
+ *
+ * @param x a real-valued input.
+ * @return computes the Gaussian error function
+ * \f[ \text{erf}(x) = \frac{2}{\sqrt{\pi}} \int_0^x \exp( - t^2) dt \f]
+ * using a continued fraction representation:
+ * \f[ \text{erf}(x) = \frac{2x}{\sqrt{\pi}} \exp(-x^2) \dfrac{1}{1 - 2x^2 + \dfrac{4x^2}{3 - 2x^2 + \dfrac{8x^2}{5 - 2x^2 + \dfrac{12x^2}{7 - 2x^2 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+erf(const T x)
+noexcept
+{
+ return internal::erf_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
new file mode 100644
index 0000000..a93f8db
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
@@ -0,0 +1,264 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time inverse error function
+ *
+ * Initial approximation based on:
+ * 'Approximating the erfinv function' by Mike Giles
+ */
+
+#ifndef _gcem_erf_inv_HPP
+#define _gcem_erf_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T erf_inv_decision(const T value, const T p, const T direc, const int iter_count) noexcept;
+
+//
+// initial value
+
+// two cases: (1) a < 5; and (2) otherwise
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_coef_2(const T a, const T p_term, const int order)
+noexcept
+{
+ return( order == 1 ? T(-0.000200214257L) :
+ order == 2 ? T( 0.000100950558L) + a*p_term :
+ order == 3 ? T( 0.00134934322L) + a*p_term :
+ order == 4 ? T(-0.003673428440L) + a*p_term :
+ order == 5 ? T( 0.005739507730L) + a*p_term :
+ order == 6 ? T(-0.00762246130L) + a*p_term :
+ order == 7 ? T( 0.009438870470L) + a*p_term :
+ order == 8 ? T( 1.001674060000L) + a*p_term :
+ order == 9 ? T( 2.83297682000L) + a*p_term :
+ p_term );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_case_2(const T a, const T p_term, const int order)
+noexcept
+{
+ return( order == 9 ? \
+ // if
+ erf_inv_initial_val_coef_2(a,p_term,order) :
+ // else
+ erf_inv_initial_val_case_2(a,erf_inv_initial_val_coef_2(a,p_term,order),order+1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_coef_1(const T a, const T p_term, const int order)
+noexcept
+{
+ return( order == 1 ? T( 2.81022636e-08L) :
+ order == 2 ? T( 3.43273939e-07L) + a*p_term :
+ order == 3 ? T(-3.5233877e-06L) + a*p_term :
+ order == 4 ? T(-4.39150654e-06L) + a*p_term :
+ order == 5 ? T( 0.00021858087L) + a*p_term :
+ order == 6 ? T(-0.00125372503L) + a*p_term :
+ order == 7 ? T(-0.004177681640L) + a*p_term :
+ order == 8 ? T( 0.24664072700L) + a*p_term :
+ order == 9 ? T( 1.50140941000L) + a*p_term :
+ p_term );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_case_1(const T a, const T p_term, const int order)
+noexcept
+{
+ return( order == 9 ? \
+ // if
+ erf_inv_initial_val_coef_1(a,p_term,order) :
+ // else
+ erf_inv_initial_val_case_1(a,erf_inv_initial_val_coef_1(a,p_term,order),order+1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_int(const T a)
+noexcept
+{
+ return( a < T(5) ? \
+ // if
+ erf_inv_initial_val_case_1(a-T(2.5),T(0),1) :
+ // else
+ erf_inv_initial_val_case_2(sqrt(a)-T(3),T(0),1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val(const T x)
+noexcept
+{
+ return x*erf_inv_initial_val_int( -log( (T(1) - x)*(T(1) + x) ) );
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+erf_inv_err_val(const T value, const T p)
+noexcept
+{ // err_val = f(x)
+ return( erf(value) - p );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_deriv_1(const T value)
+noexcept
+{ // derivative of the error function w.r.t. x
+ return( exp( -value*value ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_deriv_2(const T value, const T deriv_1)
+noexcept
+{ // second derivative of the error function w.r.t. x
+ return( deriv_1*( -T(2)*value ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_ratio_val_1(const T value, const T p, const T deriv_1)
+noexcept
+{
+ return( erf_inv_err_val(value,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_ratio_val_2(const T value, const T deriv_1)
+noexcept
+{
+ return( erf_inv_deriv_2(value,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+ return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_recur(const T value, const T p, const T deriv_1, const int iter_count)
+noexcept
+{
+ return erf_inv_decision( value, p,
+ erf_inv_halley(erf_inv_ratio_val_1(value,p,deriv_1),
+ erf_inv_ratio_val_2(value,deriv_1)),
+ iter_count );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_decision(const T value, const T p, const T direc, const int iter_count)
+noexcept
+{
+ return( iter_count < GCEM_ERF_INV_MAX_ITER ? \
+ // if
+ erf_inv_recur(value-direc,p, erf_inv_deriv_1(value), iter_count+1) :
+ // else
+ value - direc );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_recur_begin(const T initial_val, const T p)
+noexcept
+{
+ return erf_inv_recur(initial_val,p,erf_inv_deriv_1(initial_val),1);
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_begin(const T p)
+noexcept
+{
+ return( // NaN check
+ is_nan(p) ? \
+ GCLIM<T>::quiet_NaN() :
+ // bad values
+ abs(p) > T(1) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from 1
+ GCLIM<T>::min() > abs(T(1) - p) ? \
+ GCLIM<T>::infinity() :
+ // indistinguishable from - 1
+ GCLIM<T>::min() > abs(T(1) + p) ? \
+ - GCLIM<T>::infinity() :
+ // else
+ erf_inv_recur_begin(erf_inv_initial_val(p),p) );
+}
+
+}
+
+/**
+ * Compile-time inverse Gaussian error function
+ *
+ * @param p a real-valued input with values in the unit-interval.
+ * @return Computes the inverse Gaussian error function, a value \f$ x \f$ such that
+ * \f[ f(x) := \text{erf}(x) - p \f]
+ * is equal to zero, for a given \c p.
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \text{erf}(x) = \exp(-x^2), \ \ \frac{\partial^2}{\partial x^2} \text{erf}(x) = -2x\exp(-x^2) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+erf_inv(const T p)
+noexcept
+{
+ return internal::erf_inv_begin( static_cast<return_t<T>>(p) );
+}
+
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
new file mode 100644
index 0000000..0c66829
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
@@ -0,0 +1,106 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time exponential function
+ */
+
+#ifndef _gcem_exp_HPP
+#define _gcem_exp_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+exp_cf_recur(const T x, const int depth)
+noexcept
+{
+ return( depth < GCEM_EXP_MAX_ITER_SMALL ? \
+ // if
+ depth == 1 ? \
+ T(1) - x/exp_cf_recur(x,depth+1) :
+ T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) :
+ // else
+ T(1) );
+}
+
+template<typename T>
+constexpr
+T
+exp_cf(const T x)
+noexcept
+{
+ return( T(1)/exp_cf_recur(x,1) );
+}
+
+template<typename T>
+constexpr
+T
+exp_split(const T x)
+noexcept
+{
+ return( static_cast<T>(pow_integral(GCEM_E,find_whole(x))) * exp_cf(find_fraction(x)) );
+}
+
+template<typename T>
+constexpr
+T
+exp_check(const T x)
+noexcept
+{
+ return( is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ is_neginf(x) ? \
+ T(0) :
+ //
+ GCLIM<T>::min() > abs(x) ? \
+ T(1) :
+ //
+ is_posinf(x) ? \
+ GCLIM<T>::infinity() :
+ //
+ abs(x) < T(2) ? \
+ exp_cf(x) : \
+ exp_split(x) );
+}
+
+}
+
+/**
+ * Compile-time exponential function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \exp(x) \f$ using \f[ \exp(x) = \dfrac{1}{1-\dfrac{x}{1+x-\dfrac{\frac{1}{2}x}{1 + \frac{1}{2}x - \dfrac{\frac{1}{3}x}{1 + \frac{1}{3}x - \ddots}}}} \f]
+ * The continued fraction argument is split into two parts: \f$ x = n + r \f$, where \f$ n \f$ is an integer and \f$ r \in [-0.5,0.5] \f$.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+exp(const T x)
+noexcept
+{
+ return internal::exp_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
new file mode 100644
index 0000000..11b2eb9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
@@ -0,0 +1,76 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time exponential function
+ */
+
+#ifndef _gcem_expm1_HPP
+#define _gcem_expm1_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+expm1_compute(const T x)
+noexcept
+{
+ // return x * ( T(1) + x * ( T(1)/T(2) + x * ( T(1)/T(6) + x * ( T(1)/T(24) + x/T(120) ) ) ) ); // O(x^6)
+ return x + x * ( x/T(2) + x * ( x/T(6) + x * ( x/T(24) + x*x/T(120) ) ) ); // O(x^6)
+}
+
+template<typename T>
+constexpr
+T
+expm1_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ abs(x) > T(1e-04) ? \
+ // if
+ exp(x) - T(1) :
+ // else
+ expm1_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time exponential-minus-1 function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \exp(x) - 1 \f$ using \f[ \exp(x) = \sum_{k=0}^\infty \dfrac{x^k}{k!} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+expm1(const T x)
+noexcept
+{
+ return internal::expm1_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
new file mode 100644
index 0000000..539c3f3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
@@ -0,0 +1,98 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time factorial function
+ */
+
+#ifndef _gcem_factorial_HPP
+#define _gcem_factorial_HPP
+
+namespace internal
+{
+
+// T should be int, long int, unsigned int, etc.
+
+template<typename T>
+constexpr
+T
+factorial_table(const T x)
+noexcept
+{ // table for x! when x = {2,...,16}
+ return( x == T(2) ? T(2) : x == T(3) ? T(6) :
+ x == T(4) ? T(24) : x == T(5) ? T(120) :
+ x == T(6) ? T(720) : x == T(7) ? T(5040) :
+ x == T(8) ? T(40320) : x == T(9) ? T(362880) :
+ //
+ x == T(10) ? T(3628800) :
+ x == T(11) ? T(39916800) :
+ x == T(12) ? T(479001600) :
+ x == T(13) ? T(6227020800) :
+ x == T(14) ? T(87178291200) :
+ x == T(15) ? T(1307674368000) :
+ T(20922789888000) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+factorial_recur(const T x)
+noexcept
+{
+ return( x == T(0) ? T(1) :
+ x == T(1) ? x :
+ //
+ x < T(17) ? \
+ // if
+ factorial_table(x) :
+ // else
+ x*factorial_recur(x-1) );
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+factorial_recur(const T x)
+noexcept
+{
+ return tgamma(x + 1);
+}
+
+}
+
+/**
+ * Compile-time factorial function
+ *
+ * @param x a real-valued input.
+ * @return Computes the factorial value \f$ x! \f$.
+ * When \c x is an integral type (\c int, <tt>long int</tt>, etc.), a simple recursion method is used, along with table values.
+ * When \c x is real-valued, <tt>factorial(x) = tgamma(x+1)</tt>.
+ */
+
+template<typename T>
+constexpr
+T
+factorial(const T x)
+noexcept
+{
+ return internal::factorial_recur(x);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
new file mode 100644
index 0000000..710adce
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
@@ -0,0 +1,47 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time find_exponent function
+ */
+
+#ifndef _gcem_find_exponent_HPP
+#define _gcem_find_exponent_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+llint_t
+find_exponent(const T x, const llint_t exponent)
+noexcept
+{
+ return( x < T(1) ? \
+ find_exponent(x*T(10),exponent - llint_t(1)) :
+ x > T(10) ? \
+ find_exponent(x/T(10),exponent + llint_t(1)) :
+ // else
+ exponent );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
new file mode 100644
index 0000000..d9769e6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
@@ -0,0 +1,46 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * find the fraction part of x = n + r, where -0.5 <= r <= 0.5
+ */
+
+#ifndef _gcem_find_fraction_HPP
+#define _gcem_find_fraction_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+find_fraction(const T x)
+noexcept
+{
+ return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+ // if
+ x - internal::floor_check(x) - sgn(x) :
+ //else
+ x - internal::floor_check(x) );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
new file mode 100644
index 0000000..bd5e0b9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
@@ -0,0 +1,46 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * find the whole number part of x = n + r, where -0.5 <= r <= 0.5
+ */
+
+#ifndef _gcem_find_whole_HPP
+#define _gcem_find_whole_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+llint_t
+find_whole(const T x)
+noexcept
+{
+ return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+ // if
+ static_cast<llint_t>(internal::floor_check(x) + sgn(x)) :
+ // else
+ static_cast<llint_t>(internal::floor_check(x)) );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
new file mode 100644
index 0000000..c60ff6a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
@@ -0,0 +1,130 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_floor_HPP
+#define _gcem_floor_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+int
+floor_resid(const T x, const T x_whole)
+noexcept
+{
+ return( (x < T(0)) && (x < x_whole) );
+}
+
+template<typename T>
+constexpr
+T
+floor_int(const T x, const T x_whole)
+noexcept
+{
+ return( x_whole - static_cast<T>(floor_resid(x,x_whole)) );
+}
+
+template<typename T>
+constexpr
+T
+floor_check_internal(const T x)
+noexcept
+{
+ return x;
+}
+
+template<>
+constexpr
+float
+floor_check_internal<float>(const float x)
+noexcept
+{
+ return( abs(x) >= 8388608.f ? \
+ // if
+ x : \
+ // else
+ floor_int(x, float(static_cast<int>(x))) );
+}
+
+template<>
+constexpr
+double
+floor_check_internal<double>(const double x)
+noexcept
+{
+ return( abs(x) >= 4503599627370496. ? \
+ // if
+ x : \
+ // else
+ floor_int(x, double(static_cast<llint_t>(x))) );
+}
+
+template<>
+constexpr
+long double
+floor_check_internal<long double>(const long double x)
+noexcept
+{
+ return( abs(x) >= 9223372036854775808.l ? \
+ // if
+ x : \
+ // else
+ floor_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
+}
+
+template<typename T>
+constexpr
+T
+floor_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/- infinite
+ !is_finite(x) ? \
+ x :
+ // signed-zero cases
+ GCLIM<T>::min() > abs(x) ? \
+ x :
+ // else
+ floor_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time floor function
+ *
+ * @param x a real-valued input.
+ * @return computes the floor-value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+floor(const T x)
+noexcept
+{
+ return internal::floor_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
new file mode 100644
index 0000000..c804ce6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
@@ -0,0 +1,70 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_fmod_HPP
+#define _gcem_fmod_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+fmod_check(const T x, const T y)
+noexcept
+{
+ return( // NaN check
+ any_nan(x, y) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/- infinite
+ !all_finite(x, y) ? \
+ GCLIM<T>::quiet_NaN() :
+ // else
+ x - trunc(x/y)*y );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+fmod_type_check(const T1 x, const T2 y)
+noexcept
+{
+ return fmod_check(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time remainder of division function
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return computes the floating-point remainder of \f$ x / y \f$ (rounded towards zero) using \f[ \text{fmod}(x,y) = x - \text{trunc}(x/y) \times y \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+fmod(const T1 x, const T2 y)
+noexcept
+{
+ return internal::fmod_type_check(x,y);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
new file mode 100644
index 0000000..4a10bbe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_gcd_HPP
+#define _gcem_gcd_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+gcd_recur(const T a, const T b)
+noexcept
+{
+ return( b == T(0) ? a : gcd_recur(b, a % b) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+gcd_int_check(const T a, const T b)
+noexcept
+{
+ return gcd_recur(a,b);
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+gcd_int_check(const T a, const T b)
+noexcept
+{
+ return gcd_recur( static_cast<ullint_t>(a), static_cast<ullint_t>(b) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+gcd_type_check(const T1 a, const T2 b)
+noexcept
+{
+ return gcd_int_check( static_cast<TC>(abs(a)), static_cast<TC>(abs(b)) );
+}
+
+}
+
+/**
+ * Compile-time greatest common divisor (GCD) function
+ *
+ * @param a integral-valued input.
+ * @param b integral-valued input.
+ * @return the greatest common divisor between integers \c a and \c b using a Euclidean algorithm.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+gcd(const T1 a, const T2 b)
+noexcept
+{
+ return internal::gcd_type_check(a,b);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
new file mode 100644
index 0000000..cd2747c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
@@ -0,0 +1,213 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#include <cstddef> // size_t
+#include <limits>
+#include <type_traits>
+
+// undef some functions from math.h
+// see issue #29
+
+#ifdef abs
+ #undef abs
+#endif
+
+#ifdef min
+ #undef min
+#endif
+
+#ifdef max
+ #undef max
+#endif
+
+#ifdef round
+ #undef round
+#endif
+
+#ifdef signbit
+ #undef signbit
+#endif
+
+//
+// version
+
+#ifndef GCEM_VERSION_MAJOR
+ #define GCEM_VERSION_MAJOR 1
+#endif
+
+#ifndef GCEM_VERSION_MINOR
+ #define GCEM_VERSION_MINOR 16
+#endif
+
+#ifndef GCEM_VERSION_PATCH
+ #define GCEM_VERSION_PATCH 0
+#endif
+
+//
+// types
+
+namespace gcem
+{
+ using uint_t = unsigned int;
+ using ullint_t = unsigned long long int;
+
+ using llint_t = long long int;
+
+ template<class T>
+ using GCLIM = std::numeric_limits<T>;
+
+ template<typename T>
+ using return_t = typename std::conditional<std::is_integral<T>::value,double,T>::type;
+
+ template<typename ...T>
+ using common_t = typename std::common_type<T...>::type;
+
+ template<typename ...T>
+ using common_return_t = return_t<common_t<T...>>;
+}
+
+//
+// constants
+
+#ifndef GCEM_LOG_2
+ #define GCEM_LOG_2 0.6931471805599453094172321214581765680755L
+#endif
+
+#ifndef GCEM_LOG_10
+ #define GCEM_LOG_10 2.3025850929940456840179914546843642076011L
+#endif
+
+#ifndef GCEM_PI
+ #define GCEM_PI 3.1415926535897932384626433832795028841972L
+#endif
+
+#ifndef GCEM_LOG_PI
+ #define GCEM_LOG_PI 1.1447298858494001741434273513530587116473L
+#endif
+
+#ifndef GCEM_LOG_2PI
+ #define GCEM_LOG_2PI 1.8378770664093454835606594728112352797228L
+#endif
+
+#ifndef GCEM_LOG_SQRT_2PI
+ #define GCEM_LOG_SQRT_2PI 0.9189385332046727417803297364056176398614L
+#endif
+
+#ifndef GCEM_SQRT_2
+ #define GCEM_SQRT_2 1.4142135623730950488016887242096980785697L
+#endif
+
+#ifndef GCEM_HALF_PI
+ #define GCEM_HALF_PI 1.5707963267948966192313216916397514420986L
+#endif
+
+#ifndef GCEM_SQRT_PI
+ #define GCEM_SQRT_PI 1.7724538509055160272981674833411451827975L
+#endif
+
+#ifndef GCEM_SQRT_HALF_PI
+ #define GCEM_SQRT_HALF_PI 1.2533141373155002512078826424055226265035L
+#endif
+
+#ifndef GCEM_E
+ #define GCEM_E 2.7182818284590452353602874713526624977572L
+#endif
+
+//
+// convergence settings
+
+#ifndef GCEM_ERF_MAX_ITER
+ #define GCEM_ERF_MAX_ITER 60
+#endif
+
+#ifndef GCEM_ERF_INV_MAX_ITER
+ #define GCEM_ERF_INV_MAX_ITER 60
+#endif
+
+#ifndef GCEM_EXP_MAX_ITER_SMALL
+ #define GCEM_EXP_MAX_ITER_SMALL 25
+#endif
+
+// #ifndef GCEM_LOG_TOL
+// #define GCEM_LOG_TOL 1E-14
+// #endif
+
+#ifndef GCEM_LOG_MAX_ITER_SMALL
+ #define GCEM_LOG_MAX_ITER_SMALL 25
+#endif
+
+#ifndef GCEM_LOG_MAX_ITER_BIG
+ #define GCEM_LOG_MAX_ITER_BIG 255
+#endif
+
+#ifndef GCEM_INCML_BETA_TOL
+ #define GCEM_INCML_BETA_TOL 1E-15
+#endif
+
+#ifndef GCEM_INCML_BETA_MAX_ITER
+ #define GCEM_INCML_BETA_MAX_ITER 205
+#endif
+
+#ifndef GCEM_INCML_BETA_INV_MAX_ITER
+ #define GCEM_INCML_BETA_INV_MAX_ITER 35
+#endif
+
+#ifndef GCEM_INCML_GAMMA_MAX_ITER
+ #define GCEM_INCML_GAMMA_MAX_ITER 55
+#endif
+
+#ifndef GCEM_INCML_GAMMA_INV_MAX_ITER
+ #define GCEM_INCML_GAMMA_INV_MAX_ITER 35
+#endif
+
+#ifndef GCEM_SQRT_MAX_ITER
+ #define GCEM_SQRT_MAX_ITER 100
+#endif
+
+#ifndef GCEM_INV_SQRT_MAX_ITER
+ #define GCEM_INV_SQRT_MAX_ITER 100
+#endif
+
+#ifndef GCEM_TAN_MAX_ITER
+ #define GCEM_TAN_MAX_ITER 35
+#endif
+
+#ifndef GCEM_TANH_MAX_ITER
+ #define GCEM_TANH_MAX_ITER 35
+#endif
+
+//
+// Macros
+
+#ifdef _MSC_VER
+ #ifndef GCEM_SIGNBIT
+ #define GCEM_SIGNBIT(x) _signbit(x)
+ #endif
+ #ifndef GCEM_COPYSIGN
+ #define GCEM_COPYSIGN(x,y) _copysign(x,y)
+ #endif
+#else
+ #ifndef GCEM_SIGNBIT
+ #define GCEM_SIGNBIT(x) __builtin_signbit(x)
+ #endif
+ #ifndef GCEM_COPYSIGN
+ #define GCEM_COPYSIGN(x,y) __builtin_copysign(x,y)
+ #endif
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
new file mode 100644
index 0000000..5a805ed
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
@@ -0,0 +1,90 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time Pythagorean addition function
+ */
+
+// see: https://en.wikipedia.org/wiki/Pythagorean_addition
+
+#ifndef _gcem_hypot_HPP
+#define _gcem_hypot_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+hypot_compute(const T x, const T ydx)
+noexcept
+{
+ return abs(x) * sqrt( T(1) + (ydx * ydx) );
+}
+
+template<typename T>
+constexpr
+T
+hypot_vals_check(const T x, const T y)
+noexcept
+{
+ return( any_nan(x, y) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ any_inf(x,y) ? \
+ GCLIM<T>::infinity() :
+ // indistinguishable from zero or one
+ GCLIM<T>::min() > abs(x) ? \
+ abs(y) :
+ GCLIM<T>::min() > abs(y) ? \
+ abs(x) :
+ // else
+ hypot_compute(x, y/x) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+hypot_type_check(const T1 x, const T2 y)
+noexcept
+{
+ return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time Pythagorean addition function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes \f$ x \oplus y = \sqrt{x^2 + y^2} \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+hypot(const T1 x, const T2 y)
+noexcept
+{
+ return internal::hypot_type_check(x,y);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
new file mode 100644
index 0000000..5645dbe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
@@ -0,0 +1,194 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time incomplete beta function
+ *
+ * see eq. 18.5.17a in the Handbook of Continued Fractions for Special Functions
+ */
+
+#ifndef _gcem_incomplete_beta_HPP
+#define _gcem_incomplete_beta_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth) noexcept;
+
+//
+// coefficients; see eq. 18.5.17b
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef_even(const T a, const T b, const T z, const int k)
+noexcept
+{
+ return( -z*(a + k)*(a + b + k)/( (a + 2*k)*(a + 2*k + T(1)) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef_odd(const T a, const T b, const T z, const int k)
+noexcept
+{
+ return( z*k*(b - k)/((a + 2*k - T(1))*(a + 2*k)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef(const T a, const T b, const T z, const int depth)
+noexcept
+{
+ return( !is_odd(depth) ? incomplete_beta_coef_even(a,b,z,depth/2) :
+ incomplete_beta_coef_odd(a,b,z,(depth+1)/2) );
+}
+
+//
+// update formulae for the modified Lentz method
+
+template<typename T>
+constexpr
+T
+incomplete_beta_c_update(const T a, const T b, const T z, const T c_j, const int depth)
+noexcept
+{
+ return( T(1) + incomplete_beta_coef(a,b,z,depth)/c_j );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_d_update(const T a, const T b, const T z, const T d_j, const int depth)
+noexcept
+{
+ return( T(1) / (T(1) + incomplete_beta_coef(a,b,z,depth)*d_j) );
+}
+
+//
+// convergence-type condition
+
+template<typename T>
+constexpr
+T
+incomplete_beta_decision(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
+noexcept
+{
+ return( // tolerance check
+ abs(c_j*d_j - T(1)) < GCEM_INCML_BETA_TOL ? f_j*c_j*d_j :
+ // max_iter check
+ depth < GCEM_INCML_BETA_MAX_ITER ? \
+ // if
+ incomplete_beta_cf(a,b,z,c_j,d_j,f_j*c_j*d_j,depth+1) :
+ // else
+ f_j*c_j*d_j );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
+noexcept
+{
+ return incomplete_beta_decision(a,b,z,
+ incomplete_beta_c_update(a,b,z,c_j,depth),
+ incomplete_beta_d_update(a,b,z,d_j,depth),
+ f_j,depth);
+}
+
+//
+// x^a (1-x)^{b} / (a beta(a,b)) * cf
+
+template<typename T>
+constexpr
+T
+incomplete_beta_begin(const T a, const T b, const T z)
+noexcept
+{
+ return ( (exp(a*log(z) + b*log(T(1)-z) - lbeta(a,b)) / a) * \
+ incomplete_beta_cf(a,b,z,T(1),
+ incomplete_beta_d_update(a,b,z,T(1),0),
+ incomplete_beta_d_update(a,b,z,T(1),0),1)
+ );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_check(const T a, const T b, const T z)
+noexcept
+{
+ return( // NaN check
+ any_nan(a, b, z) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > z ? \
+ T(0) :
+ // parameter check for performance
+ (a + T(1))/(a + b + T(2)) > z ? \
+ incomplete_beta_begin(a,b,z) :
+ T(1) - incomplete_beta_begin(b,a,T(1) - z) );
+}
+
+template<typename T1, typename T2, typename T3, typename TC = common_return_t<T1,T2,T3>>
+constexpr
+TC
+incomplete_beta_type_check(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+ return incomplete_beta_check(static_cast<TC>(a),
+ static_cast<TC>(b),
+ static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time regularized incomplete beta function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param b a real-valued, non-negative input.
+ * @param z a real-valued, non-negative input.
+ *
+ * @return computes the regularized incomplete beta function,
+ * \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{1}{\text{B}(\alpha,\beta)}\int_0^z t^{a-1} (1-t)^{\beta-1} dt \f]
+ * using a continued fraction representation, found in the Handbook of Continued Fractions for Special Functions, and a modified Lentz method.
+ * \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{z^{\alpha} (1-t)^{\beta}}{\alpha \text{B}(\alpha,\beta)} \dfrac{a_1}{1 + \dfrac{a_2}{1 + \dfrac{a_3}{1 + \dfrac{a_4}{1 + \ddots}}}} \f]
+ * where \f$ a_1 = 1 \f$ and
+ * \f[ a_{2m+2} = - \frac{(\alpha + m)(\alpha + \beta + m)}{(\alpha + 2m)(\alpha + 2m + 1)}, \ m \geq 0 \f]
+ * \f[ a_{2m+1} = \frac{m(\beta - m)}{(\alpha + 2m - 1)(\alpha + 2m)}, \ m \geq 1 \f]
+ * The Lentz method works as follows: let \f$ f_j \f$ denote the value of the continued fraction up to the first \f$ j \f$ terms; \f$ f_j \f$ is updated as follows:
+ * \f[ c_j = 1 + a_j / c_{j-1}, \ \ d_j = 1 / (1 + a_j d_{j-1}) \f]
+ * \f[ f_j = c_j d_j f_{j-1} \f]
+ */
+
+template<typename T1, typename T2, typename T3>
+constexpr
+common_return_t<T1,T2,T3>
+incomplete_beta(const T1 a, const T2 b, const T3 z)
+noexcept
+{
+ return internal::incomplete_beta_type_check(a,b,z);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
new file mode 100644
index 0000000..f7fdfa0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
@@ -0,0 +1,352 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * inverse of the incomplete beta function
+ */
+
+#ifndef _gcem_incomplete_beta_inv_HPP
+#define _gcem_incomplete_beta_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p,
+ const T direc, const T lb_val, const int iter_count) noexcept;
+
+//
+// initial value for Halley
+
+//
+// a,b > 1 case
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_tval(const T p)
+noexcept
+{ // a > 1.0
+ return( p > T(0.5) ? \
+ // if
+ sqrt(-T(2)*log(T(1) - p)) :
+ // else
+ sqrt(-T(2)*log(p)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_begin(const T t_val)
+noexcept
+{ // internal for a > 1.0
+ return( t_val - ( T(2.515517) + T(0.802853)*t_val + T(0.010328)*t_val*t_val ) \
+ / ( T(1) + T(1.432788)*t_val + T(0.189269)*t_val*t_val + T(0.001308)*t_val*t_val*t_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_ab1(const T alpha_par, const T beta_par)
+noexcept
+{
+ return( T(1)/(2*alpha_par - T(1)) + T(1)/(2*beta_par - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_ab2(const T alpha_par, const T beta_par)
+noexcept
+{
+ return( T(1)/(2*beta_par - T(1)) - T(1)/(2*alpha_par - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_h(const T ab_term_1)
+noexcept
+{
+ return( T(2) / ab_term_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_w(const T value, const T ab_term_2, const T h_term)
+noexcept
+{
+ // return( value * sqrt(h_term + lambda)/h_term - ab_term_2*(lambda + 5.0/6.0 -2.0/(3.0*h_term)) );
+ return( value * sqrt(h_term + (value*value - T(3))/T(6))/h_term \
+ - ab_term_2*((value*value - T(3))/T(6) + T(5)/T(6) - T(2)/(T(3)*h_term)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_end(const T alpha_par, const T beta_par, const T w_term)
+noexcept
+{
+ return( alpha_par / (alpha_par + beta_par*exp(2*w_term)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1(const T alpha_par, const T beta_par, const T t_val, const T sgn_term)
+noexcept
+{ // a > 1.0
+ return incomplete_beta_inv_initial_val_1_int_end( alpha_par, beta_par,
+ incomplete_beta_inv_initial_val_1_int_w(
+ sgn_term*incomplete_beta_inv_initial_val_1_int_begin(t_val),
+ incomplete_beta_inv_initial_val_1_int_ab2(alpha_par,beta_par),
+ incomplete_beta_inv_initial_val_1_int_h(
+ incomplete_beta_inv_initial_val_1_int_ab1(alpha_par,beta_par)
+ )
+ )
+ );
+}
+
+//
+// a,b else
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2_s1(const T alpha_par, const T beta_par)
+noexcept
+{
+ return( pow(alpha_par/(alpha_par+beta_par),alpha_par) / alpha_par );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2_s2(const T alpha_par, const T beta_par)
+noexcept
+{
+ return( pow(beta_par/(alpha_par+beta_par),beta_par) / beta_par );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2(const T alpha_par, const T beta_par, const T p, const T s_1, const T s_2)
+noexcept
+{
+ return( p <= s_1/(s_1 + s_2) ? pow(p*(s_1+s_2)*alpha_par,T(1)/alpha_par) :
+ T(1) - pow(p*(s_1+s_2)*beta_par,T(1)/beta_par) );
+}
+
+// initial value
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val(const T alpha_par, const T beta_par, const T p)
+noexcept
+{
+ return( (alpha_par > T(1) && beta_par > T(1)) ?
+ // if
+ incomplete_beta_inv_initial_val_1(alpha_par,beta_par,
+ incomplete_beta_inv_initial_val_1_tval(p),
+ p < T(0.5) ? T(1) : T(-1) ) :
+ // else
+ p > T(0.5) ?
+ // if
+ T(1) - incomplete_beta_inv_initial_val_2(beta_par,alpha_par,T(1) - p,
+ incomplete_beta_inv_initial_val_2_s1(beta_par,alpha_par),
+ incomplete_beta_inv_initial_val_2_s2(beta_par,alpha_par)) :
+ // else
+ incomplete_beta_inv_initial_val_2(alpha_par,beta_par,p,
+ incomplete_beta_inv_initial_val_2_s1(alpha_par,beta_par),
+ incomplete_beta_inv_initial_val_2_s2(alpha_par,beta_par))
+ );
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_err_val(const T value, const T alpha_par, const T beta_par, const T p)
+noexcept
+{ // err_val = f(x)
+ return( incomplete_beta(alpha_par,beta_par,value) - p );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_deriv_1(const T value, const T alpha_par, const T beta_par, const T lb_val)
+noexcept
+{ // derivative of the incomplete beta function w.r.t. x
+ return( // indistinguishable from zero or one
+ GCLIM<T>::min() > abs(value) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(T(1) - value) ? \
+ T(0) :
+ // else
+ exp( (alpha_par - T(1))*log(value) + (beta_par - T(1))*log(T(1) - value) - lb_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_deriv_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
+noexcept
+{ // second derivative of the incomplete beta function w.r.t. x
+ return( deriv_1*((alpha_par - T(1))/value - (beta_par - T(1))/(T(1) - value)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_ratio_val_1(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1)
+noexcept
+{
+ return( incomplete_beta_inv_err_val(value,alpha_par,beta_par,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_ratio_val_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
+noexcept
+{
+ return( incomplete_beta_inv_deriv_2(value,alpha_par,beta_par,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+ return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_recur(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1,
+ const T lb_val, const int iter_count)
+noexcept
+{
+ return( // derivative = 0
+ GCLIM<T>::min() > abs(deriv_1) ? \
+ incomplete_beta_inv_decision( value, alpha_par, beta_par, p, T(0), lb_val,
+ GCEM_INCML_BETA_INV_MAX_ITER+1) :
+ // else
+ incomplete_beta_inv_decision( value, alpha_par, beta_par, p,
+ incomplete_beta_inv_halley(
+ incomplete_beta_inv_ratio_val_1(value,alpha_par,beta_par,p,deriv_1),
+ incomplete_beta_inv_ratio_val_2(value,alpha_par,beta_par,deriv_1)
+ ), lb_val, iter_count) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p, const T direc,
+ const T lb_val, const int iter_count)
+noexcept
+{
+ return( iter_count <= GCEM_INCML_BETA_INV_MAX_ITER ?
+ // if
+ incomplete_beta_inv_recur(value-direc,alpha_par,beta_par,p,
+ incomplete_beta_inv_deriv_1(value,alpha_par,beta_par,lb_val),
+ lb_val, iter_count+1) :
+ // else
+ value - direc );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_begin(const T initial_val, const T alpha_par, const T beta_par, const T p, const T lb_val)
+noexcept
+{
+ return incomplete_beta_inv_recur(initial_val,alpha_par,beta_par,p,
+ incomplete_beta_inv_deriv_1(initial_val,alpha_par,beta_par,lb_val),
+ lb_val,1);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_check(const T alpha_par, const T beta_par, const T p)
+noexcept
+{
+ return( // NaN check
+ any_nan(alpha_par, beta_par, p) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero or one
+ GCLIM<T>::min() > p ? \
+ T(0) :
+ GCLIM<T>::min() > abs(T(1) - p) ? \
+ T(1) :
+ // else
+ incomplete_beta_inv_begin(incomplete_beta_inv_initial_val(alpha_par,beta_par,p),
+ alpha_par,beta_par,p,lbeta(alpha_par,beta_par)) );
+}
+
+template<typename T1, typename T2, typename T3, typename TC = common_t<T1,T2,T3>>
+constexpr
+TC
+incomplete_beta_inv_type_check(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+ return incomplete_beta_inv_check(static_cast<TC>(a),
+ static_cast<TC>(b),
+ static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time inverse incomplete beta function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param b a real-valued, non-negative input.
+ * @param p a real-valued input with values in the unit-interval.
+ *
+ * @return Computes the inverse incomplete beta function, a value \f$ x \f$ such that
+ * \f[ f(x) := \frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)} - p \f]
+ * equal to zero, for a given \c p.
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \f]
+ * \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \left( \frac{\alpha-1}{x} - \frac{\beta-1}{1 - x} \right) \f]
+ */
+
+template<typename T1, typename T2, typename T3>
+constexpr
+common_t<T1,T2,T3>
+incomplete_beta_inv(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+ return internal::incomplete_beta_inv_type_check(a,b,p);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
new file mode 100644
index 0000000..38734a5
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
@@ -0,0 +1,247 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time (regularized) incomplete gamma function
+ */
+
+#ifndef _gcem_incomplete_gamma_HPP
+#define _gcem_incomplete_gamma_HPP
+
+namespace internal
+{
+
+// 50 point Gauss-Legendre quadrature
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_inp_vals(const T lb, const T ub, const int counter)
+noexcept
+{
+ return (ub-lb) * gauss_legendre_50_points[counter] / T(2) + (ub + lb) / T(2);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_weight_vals(const T lb, const T ub, const int counter)
+noexcept
+{
+ return (ub-lb) * gauss_legendre_50_weights[counter] / T(2);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_fn(const T x, const T a, const T lg_term)
+noexcept
+{
+ return exp( -x + (a-T(1))*log(x) - lg_term );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_recur(const T lb, const T ub, const T a, const T lg_term, const int counter)
+noexcept
+{
+ return( counter < 49 ? \
+ // if
+ incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
+ * incomplete_gamma_quad_weight_vals(lb,ub,counter) \
+ + incomplete_gamma_quad_recur(lb,ub,a,lg_term,counter+1) :
+ // else
+ incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
+ * incomplete_gamma_quad_weight_vals(lb,ub,counter) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_lb(const T a, const T z)
+noexcept
+{
+ return( a > T(1000) ? max(T(0),min(z,a) - 11*sqrt(a)) : // break integration into ranges
+ a > T(800) ? max(T(0),min(z,a) - 11*sqrt(a)) :
+ a > T(500) ? max(T(0),min(z,a) - 10*sqrt(a)) :
+ a > T(300) ? max(T(0),min(z,a) - 10*sqrt(a)) :
+ a > T(100) ? max(T(0),min(z,a) - 9*sqrt(a)) :
+ a > T(90) ? max(T(0),min(z,a) - 9*sqrt(a)) :
+ a > T(70) ? max(T(0),min(z,a) - 8*sqrt(a)) :
+ a > T(50) ? max(T(0),min(z,a) - 7*sqrt(a)) :
+ a > T(40) ? max(T(0),min(z,a) - 6*sqrt(a)) :
+ a > T(30) ? max(T(0),min(z,a) - 5*sqrt(a)) :
+ // else
+ max(T(0),min(z,a)-4*sqrt(a)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_ub(const T a, const T z)
+noexcept
+{
+ return( a > T(1000) ? min(z, a + 10*sqrt(a)) :
+ a > T(800) ? min(z, a + 10*sqrt(a)) :
+ a > T(500) ? min(z, a + 9*sqrt(a)) :
+ a > T(300) ? min(z, a + 9*sqrt(a)) :
+ a > T(100) ? min(z, a + 8*sqrt(a)) :
+ a > T(90) ? min(z, a + 8*sqrt(a)) :
+ a > T(70) ? min(z, a + 7*sqrt(a)) :
+ a > T(50) ? min(z, a + 6*sqrt(a)) :
+ // else
+ min(z, a + 5*sqrt(a)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad(const T a, const T z)
+noexcept
+{
+ return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a,z), incomplete_gamma_quad_ub(a,z), a,lgamma(a),0);
+}
+
+// reverse cf expansion
+// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_2_recur(const T a, const T z, const int depth)
+noexcept
+{
+ return( depth < 100 ? \
+ // if
+ (1 + (depth-1)*2 - a + z) + depth*(a - depth)/incomplete_gamma_cf_2_recur(a,z,depth+1) :
+ // else
+ (1 + (depth-1)*2 - a + z) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_2(const T a, const T z)
+noexcept
+{ // lower (regularized) incomplete gamma function
+ return( T(1.0) - exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_2_recur(a,z,1) );
+}
+
+// cf expansion
+// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1_coef(const T a, const T z, const int depth)
+noexcept
+{
+ return( is_odd(depth) ? - (a - 1 + T(depth+1)/T(2)) * z : T(depth)/T(2) * z );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1_recur(const T a, const T z, const int depth)
+noexcept
+{
+ return( depth < GCEM_INCML_GAMMA_MAX_ITER ? \
+ // if
+ (a + depth - 1) + incomplete_gamma_cf_1_coef(a,z,depth)/incomplete_gamma_cf_1_recur(a,z,depth+1) :
+ // else
+ (a + depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1(const T a, const T z)
+noexcept
+{ // lower (regularized) incomplete gamma function
+ return( exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a,z,1) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_check(const T a, const T z)
+noexcept
+{
+ return( // NaN check
+ any_nan(a, z) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ a < T(0) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ GCLIM<T>::min() > z ? \
+ T(0) :
+ //
+ GCLIM<T>::min() > a ? \
+ T(1) :
+ // cf or quadrature
+ (a < T(10)) && (z - a < T(10)) ?
+ incomplete_gamma_cf_1(a,z) :
+ (a < T(10)) || (z/a > T(3)) ?
+ incomplete_gamma_cf_2(a,z) :
+ // else
+ incomplete_gamma_quad(a,z) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+incomplete_gamma_type_check(const T1 a, const T2 p)
+noexcept
+{
+ return incomplete_gamma_check(static_cast<TC>(a),
+ static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time regularized lower incomplete gamma function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param x a real-valued, non-negative input.
+ *
+ * @return the regularized lower incomplete gamma function evaluated at (\c a, \c x),
+ * \f[ \frac{\gamma(a,x)}{\Gamma(a)} = \frac{1}{\Gamma(a)} \int_0^x t^{a-1} \exp(-t) dt \f]
+ * When \c a is not too large, the value is computed using the continued fraction representation of the upper incomplete gamma function, \f$ \Gamma(a,x) \f$, using
+ * \f[ \Gamma(a,x) = \Gamma(a) - \dfrac{x^a\exp(-x)}{a - \dfrac{ax}{a + 1 + \dfrac{x}{a + 2 - \dfrac{(a+1)x}{a + 3 + \dfrac{2x}{a + 4 - \ddots}}}}} \f]
+ * where \f$ \gamma(a,x) \f$ and \f$ \Gamma(a,x) \f$ are connected via
+ * \f[ \frac{\gamma(a,x)}{\Gamma(a)} + \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f]
+ * When \f$ a > 10 \f$, a 50-point Gauss-Legendre quadrature scheme is employed.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+incomplete_gamma(const T1 a, const T2 x)
+noexcept
+{
+ return internal::incomplete_gamma_type_check(a,x);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
new file mode 100644
index 0000000..1e57fc1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
@@ -0,0 +1,271 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * inverse of the incomplete gamma function
+ */
+
+#ifndef _gcem_incomplete_gamma_inv_HPP
+#define _gcem_incomplete_gamma_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count) noexcept;
+
+//
+// initial value for Halley
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_t_val_1(const T p)
+noexcept
+{ // a > 1.0
+ return( p > T(0.5) ? sqrt(-2*log(T(1) - p)) : sqrt(-2*log(p)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_t_val_2(const T a)
+noexcept
+{ // a <= 1.0
+ return( T(1) - T(0.253) * a - T(0.12) * a*a );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1_int_begin(const T t_val)
+noexcept
+{ // internal for a > 1.0
+ return( t_val - (T(2.515517L) + T(0.802853L)*t_val + T(0.010328L)*t_val*t_val) \
+ / (T(1) + T(1.432788L)*t_val + T(0.189269L)*t_val*t_val + T(0.001308L)*t_val*t_val*t_val) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1_int_end(const T value_inp, const T a)
+noexcept
+{ // internal for a > 1.0
+ return max( T(1E-04), a*pow(T(1) - T(1)/(9*a) - value_inp/(3*sqrt(a)), 3) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1(const T a, const T t_val, const T sgn_term)
+noexcept
+{ // a > 1.0
+ return incomplete_gamma_inv_initial_val_1_int_end(sgn_term*incomplete_gamma_inv_initial_val_1_int_begin(t_val), a);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_2(const T a, const T p, const T t_val)
+noexcept
+{ // a <= 1.0
+ return( p < t_val ? \
+ // if
+ pow(p/t_val,T(1)/a) :
+ // else
+ T(1) - log(T(1) - (p - t_val)/(T(1) - t_val)) );
+}
+
+// initial value
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val(const T a, const T p)
+noexcept
+{
+ return( a > T(1) ? \
+ // if
+ incomplete_gamma_inv_initial_val_1(a,
+ incomplete_gamma_inv_t_val_1(p),
+ p > T(0.5) ? T(-1) : T(1)) :
+ // else
+ incomplete_gamma_inv_initial_val_2(a,p,
+ incomplete_gamma_inv_t_val_2(a)));
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_err_val(const T value, const T a, const T p)
+noexcept
+{ // err_val = f(x)
+ return( incomplete_gamma(a,value) - p );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_deriv_1(const T value, const T a, const T lg_val)
+noexcept
+{ // derivative of the incomplete gamma function w.r.t. x
+ return( exp( - value + (a - T(1))*log(value) - lg_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_deriv_2(const T value, const T a, const T deriv_1)
+noexcept
+{ // second derivative of the incomplete gamma function w.r.t. x
+ return( deriv_1*((a - T(1))/value - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_ratio_val_1(const T value, const T a, const T p, const T deriv_1)
+noexcept
+{
+ return( incomplete_gamma_inv_err_val(value,a,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_ratio_val_2(const T value, const T a, const T deriv_1)
+noexcept
+{
+ return( incomplete_gamma_inv_deriv_2(value,a,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+ return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_recur(const T value, const T a, const T p, const T deriv_1, const T lg_val, const int iter_count)
+noexcept
+{
+ return incomplete_gamma_inv_decision(value, a, p,
+ incomplete_gamma_inv_halley(incomplete_gamma_inv_ratio_val_1(value,a,p,deriv_1),
+ incomplete_gamma_inv_ratio_val_2(value,a,deriv_1)),
+ lg_val, iter_count);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count)
+noexcept
+{
+ // return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? incomplete_gamma_inv_recur(value - direc, a, p, incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc );
+ return( iter_count <= GCEM_INCML_GAMMA_INV_MAX_ITER ? \
+ // if
+ incomplete_gamma_inv_recur(value-direc,a,p,
+ incomplete_gamma_inv_deriv_1(value,a,lg_val),
+ lg_val,iter_count+1) :
+ // else
+ value - direc );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_begin(const T initial_val, const T a, const T p, const T lg_val)
+noexcept
+{
+ return incomplete_gamma_inv_recur(initial_val,a,p,
+ incomplete_gamma_inv_deriv_1(initial_val,a,lg_val), lg_val,1);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_check(const T a, const T p)
+noexcept
+{
+ return( // NaN check
+ any_nan(a, p) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ GCLIM<T>::min() > p ? \
+ T(0) :
+ p > T(1) ? \
+ GCLIM<T>::quiet_NaN() :
+ GCLIM<T>::min() > abs(T(1) - p) ? \
+ GCLIM<T>::infinity() :
+ //
+ GCLIM<T>::min() > a ? \
+ T(0) :
+ // else
+ incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a,p),a,p,lgamma(a)) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+incomplete_gamma_inv_type_check(const T1 a, const T2 p)
+noexcept
+{
+ return incomplete_gamma_inv_check(static_cast<TC>(a),
+ static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time inverse incomplete gamma function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param p a real-valued input with values in the unit-interval.
+ *
+ * @return Computes the inverse incomplete gamma function, a value \f$ x \f$ such that
+ * \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f]
+ * equal to zero, for a given \c p.
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \f]
+ * \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+incomplete_gamma_inv(const T1 a, const T2 p)
+noexcept
+{
+ return internal::incomplete_gamma_inv_type_check(a,p);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
new file mode 100644
index 0000000..0200f11
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
@@ -0,0 +1,88 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time inverse-square-root function
+ */
+
+#ifndef _gcem_inv_sqrt_HPP
+#define _gcem_inv_sqrt_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+inv_sqrt_recur(const T x, const T xn, const int count)
+noexcept
+{
+ return( abs( xn - T(1)/(x*xn) ) / (T(1) + xn) < GCLIM<T>::min() ? \
+ // if
+ xn :
+ count < GCEM_INV_SQRT_MAX_ITER ? \
+ // else
+ inv_sqrt_recur(x, T(0.5)*(xn + T(1)/(x*xn)), count+1) :
+ xn );
+}
+
+template<typename T>
+constexpr
+T
+inv_sqrt_check(const T x)
+noexcept
+{
+ return( is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ x < T(0) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ is_posinf(x) ? \
+ T(0) :
+ // indistinguishable from zero or one
+ GCLIM<T>::min() > abs(x) ? \
+ GCLIM<T>::infinity() :
+ GCLIM<T>::min() > abs(T(1) - x) ? \
+ x :
+ // else
+ inv_sqrt_recur(x, x/T(2), 0) );
+}
+
+}
+
+
+/**
+ * Compile-time inverse-square-root function
+ *
+ * @param x a real-valued input.
+ * @return Computes \f$ 1 / \sqrt{x} \f$ using a Newton-Raphson approach.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+inv_sqrt(const T x)
+noexcept
+{
+ return internal::inv_sqrt_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
new file mode 100644
index 0000000..fa925bb
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time check if integer is even
+ */
+
+#ifndef _gcem_is_even_HPP
+#define _gcem_is_even_HPP
+
+namespace internal
+{
+
+constexpr
+bool
+is_even(const llint_t x)
+noexcept
+{
+ return !is_odd(x);
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
new file mode 100644
index 0000000..25f2e3c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
@@ -0,0 +1,78 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time check if a float is not NaN-valued or +/-Inf
+ */
+
+#ifndef _gcem_is_finite_HPP
+#define _gcem_is_finite_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+bool
+is_finite(const T x)
+noexcept
+{
+ return (!is_nan(x)) && (!is_inf(x));
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_finite(const T1 x, const T2 y)
+noexcept
+{
+ return( is_finite(x) || is_finite(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_finite(const T1 x, const T2 y)
+noexcept
+{
+ return( is_finite(x) && is_finite(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_finite(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_finite(x) || is_finite(y) || is_finite(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_finite(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_finite(x) && is_finite(y) && is_finite(z) );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
new file mode 100644
index 0000000..627c509
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
@@ -0,0 +1,172 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time check if a float is +/-Inf
+ */
+
+#ifndef _gcem_is_inf_HPP
+#define _gcem_is_inf_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+bool
+is_neginf(const T x)
+noexcept
+{
+ return x == - GCLIM<T>::infinity();
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_neginf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_neginf(x) || is_neginf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_neginf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_neginf(x) && is_neginf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_neginf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_neginf(x) || is_neginf(y) || is_neginf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_neginf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_neginf(x) && is_neginf(y) && is_neginf(z) );
+}
+
+//
+
+template<typename T>
+constexpr
+bool
+is_posinf(const T x)
+noexcept
+{
+ return x == GCLIM<T>::infinity();
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_posinf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_posinf(x) || is_posinf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_posinf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_posinf(x) && is_posinf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_posinf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_posinf(x) || is_posinf(y) || is_posinf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_posinf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_posinf(x) && is_posinf(y) && is_posinf(z) );
+}
+
+//
+
+template<typename T>
+constexpr
+bool
+is_inf(const T x)
+noexcept
+{
+ return( is_neginf(x) || is_posinf(x) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_inf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_inf(x) || is_inf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_inf(const T1 x, const T2 y)
+noexcept
+{
+ return( is_inf(x) && is_inf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_inf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_inf(x) || is_inf(y) || is_inf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_inf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_inf(x) && is_inf(y) && is_inf(z) );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
new file mode 100644
index 0000000..a7a1af3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time check if a float is NaN-valued
+ */
+
+#ifndef _gcem_is_nan_HPP
+#define _gcem_is_nan_HPP
+
+namespace internal
+{
+
+// future: consider using __builtin_isnan(__x)
+
+template<typename T>
+constexpr
+bool
+is_nan(const T x)
+noexcept
+{
+ return x != x;
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_nan(const T1 x, const T2 y)
+noexcept
+{
+ return( is_nan(x) || is_nan(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_nan(const T1 x, const T2 y)
+noexcept
+{
+ return( is_nan(x) && is_nan(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_nan(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_nan(x) || is_nan(y) || is_nan(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_nan(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+ return( is_nan(x) && is_nan(y) && is_nan(z) );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
new file mode 100644
index 0000000..e6da720
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time check if integer is odd
+ */
+
+#ifndef _gcem_is_odd_HPP
+#define _gcem_is_odd_HPP
+
+namespace internal
+{
+
+constexpr
+bool
+is_odd(const llint_t x)
+noexcept
+{
+ // return( x % llint_t(2) == llint_t(0) ? false : true );
+ return (x & 1U) != 0;
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
new file mode 100644
index 0000000..2213849
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_lbeta_HPP
+#define _gcem_lbeta_HPP
+
+/**
+ * Compile-time log-beta function
+ *
+ * @param a a real-valued input.
+ * @param b a real-valued input.
+ * @return the log-beta function using \f[ \ln \text{B}(\alpha,\beta) := \ln \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \ln \Gamma(\alpha) + \ln \Gamma(\beta) - \ln \Gamma(\alpha + \beta) \f]
+ * where \f$ \Gamma \f$ denotes the gamma function.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+lbeta(const T1 a, const T2 b)
+noexcept
+{
+ return( (lgamma(a) + lgamma(b)) - lgamma(a+b) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
new file mode 100644
index 0000000..b0d8fb4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_lcm_HPP
+#define _gcem_lcm_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+lcm_compute(const T a, const T b)
+noexcept
+{
+ return abs(a * (b / gcd(a,b)));
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+lcm_type_check(const T1 a, const T2 b)
+noexcept
+{
+ return lcm_compute(static_cast<TC>(a),static_cast<TC>(b));
+}
+
+}
+
+/**
+ * Compile-time least common multiple (LCM) function
+ *
+ * @param a integral-valued input.
+ * @param b integral-valued input.
+ * @return the least common multiple between integers \c a and \c b using the representation \f[ \text{lcm}(a,b) = \dfrac{| a b |}{\text{gcd}(a,b)} \f]
+ * where \f$ \text{gcd}(a,b) \f$ denotes the greatest common divisor between \f$ a \f$ and \f$ b \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+lcm(const T1 a, const T2 b)
+noexcept
+{
+ return internal::lcm_type_check(a,b);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
new file mode 100644
index 0000000..5d78eb3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
@@ -0,0 +1,135 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time log-gamma function
+ *
+ * for coefficient values, see:
+ * http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+#ifndef _gcem_lgamma_HPP
+#define _gcem_lgamma_HPP
+
+namespace internal
+{
+
+// P. Godfrey's coefficients:
+//
+// 0.99999999999999709182
+// 57.156235665862923517
+// -59.597960355475491248
+// 14.136097974741747174
+// -0.49191381609762019978
+// .33994649984811888699e-4
+// .46523628927048575665e-4
+// -.98374475304879564677e-4
+// .15808870322491248884e-3
+// -.21026444172410488319e-3
+// .21743961811521264320e-3
+// -.16431810653676389022e-3
+// .84418223983852743293e-4
+// -.26190838401581408670e-4
+// .36899182659531622704e-5
+
+constexpr
+long double
+lgamma_coef_term(const long double x)
+noexcept
+{
+ return( 0.99999999999999709182L + 57.156235665862923517L / (x+1) \
+ - 59.597960355475491248L / (x+2) + 14.136097974741747174L / (x+3) \
+ - 0.49191381609762019978L / (x+4) + .33994649984811888699e-4L / (x+5) \
+ + .46523628927048575665e-4L / (x+6) - .98374475304879564677e-4L / (x+7) \
+ + .15808870322491248884e-3L / (x+8) - .21026444172410488319e-3L / (x+9) \
+ + .21743961811521264320e-3L / (x+10) - .16431810653676389022e-3L / (x+11) \
+ + .84418223983852743293e-4L / (x+12) - .26190838401581408670e-4L / (x+13) \
+ + .36899182659531622704e-5L / (x+14) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_term_2(const T x)
+noexcept
+{ //
+ return( T(GCEM_LOG_SQRT_2PI) + log(T(lgamma_coef_term(x))) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_term_1(const T x)
+noexcept
+{ // note: 607/128 + 0.5 = 5.2421875
+ return( (x + T(0.5))*log(x + T(5.2421875L)) - (x + T(5.2421875L)) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_begin(const T x)
+noexcept
+{ // returns lngamma(x+1)
+ return( lgamma_term_1(x) + lgamma_term_2(x) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from one or <= zero
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(0) :
+ GCLIM<T>::min() > x ? \
+ GCLIM<T>::infinity() :
+ // else
+ lgamma_begin(x - T(1)) );
+}
+
+}
+
+/**
+ * Compile-time log-gamma function
+ *
+ * @param x a real-valued input.
+ * @return computes the log-gamma function
+ * \f[ \ln \Gamma(x) = \ln \int_0^\infty y^{x-1} \exp(-y) dy \f]
+ * using a polynomial form:
+ * \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
+ * where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
+ * are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+lgamma(const T x)
+noexcept
+{
+ return internal::lgamma_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
new file mode 100644
index 0000000..76bf833
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
@@ -0,0 +1,73 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * log multivariate gamma function
+ */
+
+#ifndef _gcem_lmgamma_HPP
+#define _gcem_lmgamma_HPP
+
+namespace internal
+{
+
+// see https://en.wikipedia.org/wiki/Multivariate_gamma_function
+
+template<typename T1, typename T2>
+constexpr
+T1
+lmgamma_recur(const T1 a, const T2 p)
+noexcept
+{
+ return( // NaN check
+ is_nan(a) ? \
+ GCLIM<T1>::quiet_NaN() :
+ //
+ p == T2(1) ? \
+ lgamma(a) :
+ p < T2(1) ? \
+ GCLIM<T1>::quiet_NaN() :
+ // else
+ T1(GCEM_LOG_PI) * (p - T1(1))/T1(2) \
+ + lgamma(a) + lmgamma_recur(a - T1(0.5),p-T2(1)) );
+}
+
+}
+
+/**
+ * Compile-time log multivariate gamma function
+ *
+ * @param a a real-valued input.
+ * @param p integral-valued input.
+ * @return computes log-multivariate gamma function via recursion
+ * \f[ \Gamma_p(a) = \pi^{(p-1)/2} \Gamma(a) \Gamma_{p-1}(a-0.5) \f]
+ * where \f$ \Gamma_1(a) = \Gamma(a) \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+return_t<T1>
+lmgamma(const T1 a, const T2 p)
+noexcept
+{
+ return internal::lmgamma_recur(static_cast<return_t<T1>>(a),p);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
new file mode 100644
index 0000000..0d83e97
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
@@ -0,0 +1,162 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time natural logarithm function
+ */
+
+#ifndef _gcem_log_HPP
+#define _gcem_log_HPP
+
+namespace internal
+{
+
+// continued fraction seems to be a better approximation for small x
+// see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
+
+template<typename T>
+constexpr
+T
+log_cf_main(const T xx, const int depth)
+noexcept
+{
+ return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
+ // if
+ T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
+ // else
+ T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+log_cf_begin(const T x)
+noexcept
+{
+ return( T(2)*x/log_cf_main(x*x,1) );
+}
+
+template<typename T>
+constexpr
+T
+log_main(const T x)
+noexcept
+{
+ return( log_cf_begin((x - T(1))/(x + T(1))) );
+}
+
+constexpr
+long double
+log_mantissa_integer(const int x)
+noexcept
+{
+ return( x == 2 ? 0.6931471805599453094172321214581765680755L :
+ x == 3 ? 1.0986122886681096913952452369225257046475L :
+ x == 4 ? 1.3862943611198906188344642429163531361510L :
+ x == 5 ? 1.6094379124341003746007593332261876395256L :
+ x == 6 ? 1.7917594692280550008124773583807022727230L :
+ x == 7 ? 1.9459101490553133051053527434431797296371L :
+ x == 8 ? 2.0794415416798359282516963643745297042265L :
+ x == 9 ? 2.1972245773362193827904904738450514092950L :
+ x == 10 ? 2.3025850929940456840179914546843642076011L :
+ 0.0L );
+}
+
+template<typename T>
+constexpr
+T
+log_mantissa(const T x)
+noexcept
+{ // divide by the integer part of x, which will be in [1,10], then adjust using tables
+ return( log_main(x/T(static_cast<int>(x))) + T(log_mantissa_integer(static_cast<int>(x))) );
+}
+
+template<typename T>
+constexpr
+T
+log_breakup(const T x)
+noexcept
+{ // x = a*b, where b = 10^c
+ return( log_mantissa(mantissa(x)) + T(GCEM_LOG_10)*T(find_exponent(x,0)) );
+}
+
+template<typename T>
+constexpr
+T
+log_check(const T x)
+noexcept
+{
+ return( is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // x < 0
+ x < T(0) ? \
+ GCLIM<T>::quiet_NaN() :
+ // x ~= 0
+ GCLIM<T>::min() > x ? \
+ - GCLIM<T>::infinity() :
+ // indistinguishable from 1
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(0) :
+ //
+ x == GCLIM<T>::infinity() ? \
+ GCLIM<T>::infinity() :
+ // else
+ (x < T(0.5) || x > T(1.5)) ?
+ // if
+ log_breakup(x) :
+ // else
+ log_main(x) );
+}
+
+template<typename T>
+constexpr
+return_t<T>
+log_integral_check(const T x)
+noexcept
+{
+ return( std::is_integral<T>::value ? \
+ x == T(0) ? \
+ - GCLIM<return_t<T>>::infinity() :
+ x > T(1) ? \
+ log_check( static_cast<return_t<T>>(x) ) :
+ static_cast<return_t<T>>(0) :
+ log_check( static_cast<return_t<T>>(x) ) );
+}
+
+}
+
+/**
+ * Compile-time natural logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_e(x) \f$ using \f[ \log\left(\frac{1+x}{1-x}\right) = \dfrac{2x}{1-\dfrac{x^2}{3-\dfrac{4x^2}{5 - \dfrac{9x^3}{7 - \ddots}}}}, \ \ x \in [-1,1] \f]
+ * The continued fraction argument is split into two parts: \f$ x = a \times 10^c \f$, where \f$ c \f$ is an integer.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log(const T x)
+noexcept
+{
+ return internal::log_integral_check( x );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
new file mode 100644
index 0000000..4a3c37d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
@@ -0,0 +1,59 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time common logarithm function
+ */
+
+#ifndef _gcem_log10_HPP
+#define _gcem_log10_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+return_t<T>
+log10_check(const T x)
+noexcept
+{
+ // log_10(x) = ln(x) / ln(10)
+ return return_t<T>(log(x) / GCEM_LOG_10);
+}
+
+}
+
+/**
+ * Compile-time common logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_{10}(x) \f$ using \f[ \log_{10}(x) = \frac{\log_e(x)}{\log_e(10)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log10(const T x)
+noexcept
+{
+ return internal::log10_check( x );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
new file mode 100644
index 0000000..3883b22
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time natural logarithm(x+1) function
+ */
+
+#ifndef _gcem_log1p_HPP
+#define _gcem_log1p_HPP
+
+namespace internal
+{
+
+// see:
+// http://functions.wolfram.com/ElementaryFunctions/Log/06/01/04/01/0003/
+
+
+template<typename T>
+constexpr
+T
+log1p_compute(const T x)
+noexcept
+{
+ // return x * ( T(1) + x * ( -T(1)/T(2) + x * ( T(1)/T(3) + x * ( -T(1)/T(4) + x/T(5) ) ) ) ); // O(x^6)
+ return x + x * ( - x/T(2) + x * ( x/T(3) + x * ( -x/T(4) + x*x/T(5) ) ) ); // O(x^6)
+}
+
+template<typename T>
+constexpr
+T
+log1p_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ abs(x) > T(1e-04) ? \
+ // if
+ log(T(1) + x) :
+ // else
+ log1p_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time natural-logarithm-plus-1 function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_e(x+1) \f$ using \f[ \log(x+1) = \sum_{k=1}^\infty \dfrac{(-1)^{k-1}x^k}{k}, \ \ |x| < 1 \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log1p(const T x)
+noexcept
+{
+ return internal::log1p_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
new file mode 100644
index 0000000..56b7f8e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
@@ -0,0 +1,59 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time binary logarithm function
+ */
+
+#ifndef _gcem_log2_HPP
+#define _gcem_log2_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+return_t<T>
+log2_check(const T x)
+noexcept
+{
+ // log_2(x) = ln(x) / ln(2)
+ return return_t<T>(log(x) / GCEM_LOG_2);
+}
+
+}
+
+/**
+ * Compile-time binary logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_2(x) \f$ using \f[ \log_{2}(x) = \frac{\log_e(x)}{\log_e(2)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log2(const T x)
+noexcept
+{
+ return internal::log2_check( x );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
new file mode 100644
index 0000000..7aa9a2b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_log_binomial_coef_HPP
+#define _gcem_log_binomial_coef_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+log_binomial_coef_compute(const T n, const T k)
+noexcept
+{
+ return( lgamma(n+1) - (lgamma(k+1) + lgamma(n-k+1)) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+log_binomial_coef_type_check(const T1 n, const T2 k)
+noexcept
+{
+ return log_binomial_coef_compute(static_cast<TC>(n),static_cast<TC>(k));
+}
+
+}
+
+/**
+ * Compile-time log binomial coefficient
+ *
+ * @param n integral-valued input.
+ * @param k integral-valued input.
+ * @return computes the log Binomial coefficient
+ * \f[ \ln \frac{n!}{k!(n-k)!} = \ln \Gamma(n+1) - [ \ln \Gamma(k+1) + \ln \Gamma(n-k+1) ] \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+log_binomial_coef(const T1 n, const T2 k)
+noexcept
+{
+ return internal::log_binomial_coef_type_check(n,k);
+}
+
+#endif
\ No newline at end of file
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
new file mode 100644
index 0000000..df6152b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
@@ -0,0 +1,47 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time mantissa function
+ */
+
+#ifndef _gcem_mantissa_HPP
+#define _gcem_mantissa_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+mantissa(const T x)
+noexcept
+{
+ return( x < T(1) ? \
+ mantissa(x*T(10)) :
+ x > T(10) ? \
+ mantissa(x/T(10)) :
+ // else
+ x );
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
new file mode 100644
index 0000000..4aed84f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_max_HPP
+#define _gcem_max_HPP
+
+/**
+ * Compile-time pairwise maximum function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes the maximum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+max(const T1 x, const T2 y)
+noexcept
+{
+ return( y < x ? x : y );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
new file mode 100644
index 0000000..d593dbc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_min_HPP
+#define _gcem_min_HPP
+
+/**
+ * Compile-time pairwise minimum function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes the minimum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+min(const T1 x, const T2 y)
+noexcept
+{
+ return( y > x ? x : y );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
new file mode 100644
index 0000000..db33f87
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
@@ -0,0 +1,37 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * extract signbit for signed zeros
+ */
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+bool
+neg_zero(const T x)
+noexcept
+{
+ return( (x == T(0.0)) && (copysign(T(1.0), x) == T(-1.0)) );
+}
+
+}
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
new file mode 100644
index 0000000..166a8c1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time power function
+ */
+
+#ifndef _gcem_pow_HPP
+#define _gcem_pow_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+pow_dbl(const T base, const T exp_term)
+noexcept
+{
+ return exp(exp_term*log(base));
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>,
+ typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+TC
+pow_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ return( base < T1(0) ? \
+ GCLIM<TC>::quiet_NaN() :
+ //
+ pow_dbl(static_cast<TC>(base),static_cast<TC>(exp_term)) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>,
+ typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+TC
+pow_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ return pow_integral(base,exp_term);
+}
+
+}
+
+/**
+ * Compile-time power function
+ *
+ * @param base a real-valued input.
+ * @param exp_term a real-valued input.
+ * @return Computes \c base raised to the power \c exp_term. In the case where \c exp_term is integral-valued, recursion by squaring is used, otherwise \f$ \text{base}^{\text{exp\_term}} = e^{\text{exp\_term} \log(\text{base})} \f$
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+pow(const T1 base, const T2 exp_term)
+noexcept
+{
+ return internal::pow_check(base,exp_term);
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
new file mode 100644
index 0000000..3a902ca
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
@@ -0,0 +1,128 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time power function
+ */
+
+#ifndef _gcem_pow_integral_HPP
+#define _gcem_pow_integral_HPP
+
+namespace internal
+{
+
+template<typename T1, typename T2>
+constexpr T1 pow_integral_compute(const T1 base, const T2 exp_term) noexcept;
+
+// integral-valued powers using method described in
+// https://en.wikipedia.org/wiki/Exponentiation_by_squaring
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral_compute_recur(const T1 base, const T1 val, const T2 exp_term)
+noexcept
+{
+ return( exp_term > T2(1) ? \
+ (is_odd(exp_term) ? \
+ pow_integral_compute_recur(base*base,val*base,exp_term/2) :
+ pow_integral_compute_recur(base*base,val,exp_term/2)) :
+ (exp_term == T2(1) ? val*base : val) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<std::is_signed<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_sgn_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ return( exp_term < T2(0) ? \
+ //
+ T1(1) / pow_integral_compute(base, - exp_term) :
+ //
+ pow_integral_compute_recur(base,T1(1),exp_term) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<!std::is_signed<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_sgn_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ return( pow_integral_compute_recur(base,T1(1),exp_term) );
+}
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral_compute(const T1 base, const T2 exp_term)
+noexcept
+{
+ return( exp_term == T2(3) ? \
+ base*base*base :
+ exp_term == T2(2) ? \
+ base*base :
+ exp_term == T2(1) ? \
+ base :
+ exp_term == T2(0) ? \
+ T1(1) :
+ // check for overflow
+ exp_term == GCLIM<T2>::min() ? \
+ T1(0) :
+ exp_term == GCLIM<T2>::max() ? \
+ GCLIM<T1>::infinity() :
+ // else
+ pow_integral_sgn_check(base,exp_term) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_type_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ return pow_integral_compute(base,exp_term);
+}
+
+template<typename T1, typename T2, typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_type_check(const T1 base, const T2 exp_term)
+noexcept
+{
+ // return GCLIM<return_t<T1>>::quiet_NaN();
+ return pow_integral_compute(base,static_cast<llint_t>(exp_term));
+}
+
+//
+// main function
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral(const T1 base, const T2 exp_term)
+noexcept
+{
+ return internal::pow_integral_type_check(base,exp_term);
+}
+
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
new file mode 100644
index 0000000..e609b89
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
@@ -0,0 +1,91 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * Gauss-Legendre quadrature: 30 points
+ */
+
+static const long double gauss_legendre_30_points[30] = \
+{
+ -0.05147184255531769583302521316672L,
+ 0.05147184255531769583302521316672L,
+ -0.15386991360858354696379467274326L,
+ 0.15386991360858354696379467274326L,
+ -0.25463692616788984643980512981781L,
+ 0.25463692616788984643980512981781L,
+ -0.35270472553087811347103720708937L,
+ 0.35270472553087811347103720708937L,
+ -0.44703376953808917678060990032285L,
+ 0.44703376953808917678060990032285L,
+ -0.53662414814201989926416979331107L,
+ 0.53662414814201989926416979331107L,
+ -0.62052618298924286114047755643119L,
+ 0.62052618298924286114047755643119L,
+ -0.69785049479331579693229238802664L,
+ 0.69785049479331579693229238802664L,
+ -0.76777743210482619491797734097450L,
+ 0.76777743210482619491797734097450L,
+ -0.82956576238276839744289811973250L,
+ 0.82956576238276839744289811973250L,
+ -0.88256053579205268154311646253023L,
+ 0.88256053579205268154311646253023L,
+ -0.92620004742927432587932427708047L,
+ 0.92620004742927432587932427708047L,
+ -0.96002186496830751221687102558180L,
+ 0.96002186496830751221687102558180L,
+ -0.98366812327974720997003258160566L,
+ 0.98366812327974720997003258160566L,
+ -0.99689348407464954027163005091870L,
+ 0.99689348407464954027163005091870L\
+};
+
+static const long double gauss_legendre_30_weights[30] = \
+{
+ 0.10285265289355884034128563670542L,
+ 0.10285265289355884034128563670542L,
+ 0.10176238974840550459642895216855L,
+ 0.10176238974840550459642895216855L,
+ 0.09959342058679526706278028210357L,
+ 0.09959342058679526706278028210357L,
+ 0.09636873717464425963946862635181L,
+ 0.09636873717464425963946862635181L,
+ 0.09212252223778612871763270708762L,
+ 0.09212252223778612871763270708762L,
+ 0.08689978720108297980238753071513L,
+ 0.08689978720108297980238753071513L,
+ 0.08075589522942021535469493846053L,
+ 0.08075589522942021535469493846053L,
+ 0.07375597473770520626824385002219L,
+ 0.07375597473770520626824385002219L,
+ 0.06597422988218049512812851511596L,
+ 0.06597422988218049512812851511596L,
+ 0.05749315621761906648172168940206L,
+ 0.05749315621761906648172168940206L,
+ 0.04840267283059405290293814042281L,
+ 0.04840267283059405290293814042281L,
+ 0.03879919256962704959680193644635L,
+ 0.03879919256962704959680193644635L,
+ 0.02878470788332336934971917961129L,
+ 0.02878470788332336934971917961129L,
+ 0.01846646831109095914230213191205L,
+ 0.01846646831109095914230213191205L,
+ 0.00796819249616660561546588347467L,
+ 0.00796819249616660561546588347467L\
+};
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
new file mode 100644
index 0000000..44281f9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
@@ -0,0 +1,131 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * Gauss-Legendre quadrature: 50 points
+ */
+
+static const long double gauss_legendre_50_points[50] = \
+{
+ -0.03109833832718887611232898966595L,
+ 0.03109833832718887611232898966595L,
+ -0.09317470156008614085445037763960L,
+ 0.09317470156008614085445037763960L,
+ -0.15489058999814590207162862094111L,
+ 0.15489058999814590207162862094111L,
+ -0.21600723687604175684728453261710L,
+ 0.21600723687604175684728453261710L,
+ -0.27628819377953199032764527852113L,
+ 0.27628819377953199032764527852113L,
+ -0.33550024541943735683698825729107L,
+ 0.33550024541943735683698825729107L,
+ -0.39341431189756512739422925382382L,
+ 0.39341431189756512739422925382382L,
+ -0.44980633497403878914713146777838L,
+ 0.44980633497403878914713146777838L,
+ -0.50445814490746420165145913184914L,
+ 0.50445814490746420165145913184914L,
+ -0.55715830451465005431552290962580L,
+ 0.55715830451465005431552290962580L,
+ -0.60770292718495023918038179639183L,
+ 0.60770292718495023918038179639183L,
+ -0.65589646568543936078162486400368L,
+ 0.65589646568543936078162486400368L,
+ -0.70155246870682225108954625788366L,
+ 0.70155246870682225108954625788366L,
+ -0.74449430222606853826053625268219L,
+ 0.74449430222606853826053625268219L,
+ -0.78455583290039926390530519634099L,
+ 0.78455583290039926390530519634099L,
+ -0.82158207085933594835625411087394L,
+ 0.82158207085933594835625411087394L,
+ -0.85542976942994608461136264393476L,
+ 0.85542976942994608461136264393476L,
+ -0.88596797952361304863754098246675L,
+ 0.88596797952361304863754098246675L,
+ -0.91307855665579189308973564277166L,
+ 0.91307855665579189308973564277166L,
+ -0.93665661894487793378087494727250L,
+ 0.93665661894487793378087494727250L,
+ -0.95661095524280794299774564415662L,
+ 0.95661095524280794299774564415662L,
+ -0.97286438510669207371334410460625L,
+ 0.97286438510669207371334410460625L,
+ -0.98535408404800588230900962563249L,
+ 0.98535408404800588230900962563249L,
+ -0.99403196943209071258510820042069L,
+ 0.99403196943209071258510820042069L,
+ -0.99886640442007105018545944497422L,
+ 0.99886640442007105018545944497422L\
+};
+
+static const long double gauss_legendre_50_weights[50] = \
+{
+ 0.06217661665534726232103310736061L,
+ 0.06217661665534726232103310736061L,
+ 0.06193606742068324338408750978083L,
+ 0.06193606742068324338408750978083L,
+ 0.06145589959031666375640678608392L,
+ 0.06145589959031666375640678608392L,
+ 0.06073797084177021603175001538481L,
+ 0.06073797084177021603175001538481L,
+ 0.05978505870426545750957640531259L,
+ 0.05978505870426545750957640531259L,
+ 0.05860084981322244583512243663085L,
+ 0.05860084981322244583512243663085L,
+ 0.05718992564772838372302931506599L,
+ 0.05718992564772838372302931506599L,
+ 0.05555774480621251762356742561227L,
+ 0.05555774480621251762356742561227L,
+ 0.05371062188899624652345879725566L,
+ 0.05371062188899624652345879725566L,
+ 0.05165570306958113848990529584010L,
+ 0.05165570306958113848990529584010L,
+ 0.04940093844946631492124358075143L,
+ 0.04940093844946631492124358075143L,
+ 0.04695505130394843296563301363499L,
+ 0.04695505130394843296563301363499L,
+ 0.04432750433880327549202228683039L,
+ 0.04432750433880327549202228683039L,
+ 0.04152846309014769742241197896407L,
+ 0.04152846309014769742241197896407L,
+ 0.03856875661258767524477015023639L,
+ 0.03856875661258767524477015023639L,
+ 0.03545983561514615416073461100098L,
+ 0.03545983561514615416073461100098L,
+ 0.03221372822357801664816582732300L,
+ 0.03221372822357801664816582732300L,
+ 0.02884299358053519802990637311323L,
+ 0.02884299358053519802990637311323L,
+ 0.02536067357001239044019487838544L,
+ 0.02536067357001239044019487838544L,
+ 0.02178024317012479298159206906269L,
+ 0.02178024317012479298159206906269L,
+ 0.01811556071348939035125994342235L,
+ 0.01811556071348939035125994342235L,
+ 0.01438082276148557441937890892732L,
+ 0.01438082276148557441937890892732L,
+ 0.01059054838365096926356968149924L,
+ 0.01059054838365096926356968149924L,
+ 0.00675979919574540150277887817799L,
+ 0.00675979919574540150277887817799L,
+ 0.00290862255315514095840072434286L,
+ 0.00290862255315514095840072434286L\
+};
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
new file mode 100644
index 0000000..43d7a5e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
@@ -0,0 +1,125 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_round_HPP
+#define _gcem_round_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+round_int(const T x)
+noexcept
+{
+ return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+ // if
+ internal::floor_check(x) + sgn(x) : \
+ // else
+ internal::floor_check(x) );
+}
+
+template<typename T>
+constexpr
+T
+round_check_internal(const T x)
+noexcept
+{
+ return x;
+}
+
+template<>
+constexpr
+float
+round_check_internal<float>(const float x)
+noexcept
+{
+ return( abs(x) >= 8388608.f ? \
+ // if
+ x : \
+ //else
+ round_int(x) );
+}
+
+template<>
+constexpr
+double
+round_check_internal<double>(const double x)
+noexcept
+{
+ return( abs(x) >= 4503599627370496. ? \
+ // if
+ x : \
+ // else
+ round_int(x) );
+}
+
+template<>
+constexpr
+long double
+round_check_internal<long double>(const long double x)
+noexcept
+{
+ return( abs(x) >= 9223372036854775808.l ? \
+ // if
+ x : \
+ // else
+ round_int(x) );
+}
+
+template<typename T>
+constexpr
+T
+round_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/- infinite
+ !is_finite(x) ? \
+ x :
+ // signed-zero cases
+ GCLIM<T>::min() > abs(x) ? \
+ x :
+ // else
+ sgn(x) * round_check_internal(abs(x)) );
+}
+
+}
+
+/**
+ * Compile-time round function
+ *
+ * @param x a real-valued input.
+ * @return computes the rounding value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+round(const T x)
+noexcept
+{
+ return internal::round_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
new file mode 100644
index 0000000..605a35a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
@@ -0,0 +1,45 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_sgn_HPP
+#define _gcem_sgn_HPP
+
+/**
+ * Compile-time sign function
+ *
+ * @param x a real-valued input
+ * @return a value \f$ y \f$ such that \f[ y = \begin{cases} 1 \ &\text{ if } x > 0 \\ 0 \ &\text{ if } x = 0 \\ -1 \ &\text{ if } x < 0 \end{cases} \f]
+ */
+
+template<typename T>
+constexpr
+int
+sgn(const T x)
+noexcept
+{
+ return( // positive
+ x > T(0) ? 1 :
+ // negative
+ x < T(0) ? -1 :
+ // else
+ 0 );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
new file mode 100644
index 0000000..e207a5a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
@@ -0,0 +1,44 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_signbit_HPP
+#define _gcem_signbit_HPP
+
+/**
+ * Compile-time sign bit detection function
+ *
+ * @param x a real-valued input
+ * @return return true if \c x is negative, otherwise return false.
+ */
+
+template <typename T>
+constexpr
+bool
+signbit(const T x)
+noexcept
+{
+#ifdef _MSC_VER
+ return( (x == T(0)) ? (_fpclass(x) == _FPCLASS_NZ) : (x < T(0)) );
+#else
+ return GCEM_SIGNBIT(x);
+#endif
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
new file mode 100644
index 0000000..128cd32
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
@@ -0,0 +1,85 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time sine function using tan(x/2)
+ *
+ * see eq. 5.4.8 in Numerical Recipes
+ */
+
+#ifndef _gcem_sin_HPP
+#define _gcem_sin_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sin_compute(const T x)
+noexcept
+{
+ return T(2)*x/(T(1) + x*x);
+}
+
+template<typename T>
+constexpr
+T
+sin_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // special cases: pi/2 and pi
+ GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+ T(1) :
+ GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
+ - T(1) :
+ GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
+ - T(0) :
+ // else
+ sin_compute( tan(x/T(2)) ) );
+}
+
+}
+
+/**
+ * Compile-time sine function
+ *
+ * @param x a real-valued input.
+ * @return the sine function using \f[ \sin(x) = \frac{2\tan(x/2)}{1+\tan^2(x/2)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sin(const T x)
+noexcept
+{
+ return internal::sin_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
new file mode 100644
index 0000000..5355301
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time hyperbolic sine function
+ */
+
+#ifndef _gcem_sinh_HPP
+#define _gcem_sinh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sinh_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ (exp(x) - exp(-x))/T(2) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic sine function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic sine function using \f[ \sinh(x) = \frac{\exp(x) - \exp(-x)}{2} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sinh(const T x)
+noexcept
+{
+ return internal::sinh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
new file mode 100644
index 0000000..0fd559d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
@@ -0,0 +1,90 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time square-root function
+ */
+
+#ifndef _gcem_sqrt_HPP
+#define _gcem_sqrt_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sqrt_recur(const T x, const T xn, const int count)
+noexcept
+{
+ return( abs(xn - x/xn) / (T(1) + xn) < GCLIM<T>::min() ? \
+ // if
+ xn :
+ count < GCEM_SQRT_MAX_ITER ? \
+ // else
+ sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
+ xn );
+}
+
+template<typename T>
+constexpr
+T
+sqrt_check(const T x, const T m_val)
+noexcept
+{
+ return( is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ x < T(0) ? \
+ GCLIM<T>::quiet_NaN() :
+ //
+ is_posinf(x) ? \
+ x :
+ // indistinguishable from zero or one
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ GCLIM<T>::min() > abs(T(1) - x) ? \
+ x :
+ // else
+ x > T(4) ? \
+ sqrt_check(x/T(4), T(2)*m_val) :
+ m_val * sqrt_recur(x, x/T(2), 0) );
+}
+
+}
+
+
+/**
+ * Compile-time square-root function
+ *
+ * @param x a real-valued input.
+ * @return Computes \f$ \sqrt{x} \f$ using a Newton-Raphson approach.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sqrt(const T x)
+noexcept
+{
+ return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
new file mode 100644
index 0000000..e53f5c8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
@@ -0,0 +1,140 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time tangent function
+ */
+
+#ifndef _gcem_tan_HPP
+#define _gcem_tan_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tan_series_exp_long(const T z)
+noexcept
+{ // this is based on a fourth-order expansion of tan(z) using Bernoulli numbers
+ return( - 1/z + (z/3 + (pow_integral(z,3)/45 + (2*pow_integral(z,5)/945 + pow_integral(z,7)/4725))) );
+}
+
+template<typename T>
+constexpr
+T
+tan_series_exp(const T x)
+noexcept
+{
+ return( GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+ // the value tan(pi/2) is somewhat of a convention;
+ // technically the function is not defined at EXACTLY pi/2,
+ // but this is floating point pi/2
+ T(1.633124e+16) :
+ // otherwise we use an expansion around pi/2
+ tan_series_exp_long(x - T(GCEM_HALF_PI))
+ );
+}
+
+template<typename T>
+constexpr
+T
+tan_cf_recur(const T xx, const int depth, const int max_depth)
+noexcept
+{
+ return( depth < max_depth ? \
+ // if
+ T(2*depth - 1) - xx/tan_cf_recur(xx,depth+1,max_depth) :
+ // else
+ T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+tan_cf_main(const T x)
+noexcept
+{
+ return( (x > T(1.55) && x < T(1.60)) ? \
+ tan_series_exp(x) : // deals with a singularity at tan(pi/2)
+ //
+ x > T(1.4) ? \
+ x/tan_cf_recur(x*x,1,45) :
+ x > T(1) ? \
+ x/tan_cf_recur(x*x,1,35) :
+ // else
+ x/tan_cf_recur(x*x,1,25) );
+}
+
+template<typename T>
+constexpr
+T
+tan_begin(const T x, const int count = 0)
+noexcept
+{ // tan(x) = tan(x + pi)
+ return( x > T(GCEM_PI) ? \
+ // if
+ count > 1 ? GCLIM<T>::quiet_NaN() : // protect against undefined behavior
+ tan_begin( x - T(GCEM_PI) * internal::floor_check(x/T(GCEM_PI)), count+1 ) :
+ // else
+ tan_cf_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+tan_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ x < T(0) ? \
+ - tan_begin(-x) :
+ tan_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time tangent function
+ *
+ * @param x a real-valued input.
+ * @return the tangent function using
+ * \f[ \tan(x) = \dfrac{x}{1 - \dfrac{x^2}{3 - \dfrac{x^2}{5 - \ddots}}} \f]
+ * To deal with a singularity at \f$ \pi / 2 \f$, the following expansion is employed:
+ * \f[ \tan(x) = - \frac{1}{x-\pi/2} - \sum_{k=1}^\infty \frac{(-1)^k 2^{2k} B_{2k}}{(2k)!} (x - \pi/2)^{2k - 1} \f]
+ * where \f$ B_n \f$ is the n-th Bernoulli number.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tan(const T x)
+noexcept
+{
+ return internal::tan_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
new file mode 100644
index 0000000..109d751
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
@@ -0,0 +1,89 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * compile-time hyperbolic tangent function
+ */
+
+#ifndef _gcem_tanh_HPP
+#define _gcem_tanh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tanh_cf(const T xx, const int depth)
+noexcept
+{
+ return( depth < GCEM_TANH_MAX_ITER ? \
+ // if
+ (2*depth - 1) + xx/tanh_cf(xx,depth+1) :
+ // else
+ T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+tanh_begin(const T x)
+noexcept
+{
+ return( x/tanh_cf(x*x,1) );
+}
+
+template<typename T>
+constexpr
+T
+tanh_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from zero
+ GCLIM<T>::min() > abs(x) ? \
+ T(0) :
+ // else
+ x < T(0) ? \
+ - tanh_begin(-x) :
+ tanh_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic tangent function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic tangent function using \f[ \tanh(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{x^2}{5 + \dfrac{x^2}{7 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tanh(const T x)
+noexcept
+{
+ return internal::tanh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
new file mode 100644
index 0000000..5a9ae97
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+/*
+ * the ('true') gamma function
+ */
+
+#ifndef _gcem_tgamma_HPP
+#define _gcem_tgamma_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tgamma_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // indistinguishable from one or zero
+ GCLIM<T>::min() > abs(x - T(1)) ? \
+ T(1) :
+ GCLIM<T>::min() > abs(x) ? \
+ GCLIM<T>::infinity() :
+ // negative numbers
+ x < T(0) ? \
+ // check for integer
+ GCLIM<T>::min() > abs(x - find_whole(x)) ? \
+ GCLIM<T>::quiet_NaN() :
+ // else
+ tgamma_check(x+T(1)) / x :
+
+ // else
+ exp(lgamma(x)) );
+}
+
+}
+
+/**
+ * Compile-time gamma function
+ *
+ * @param x a real-valued input.
+ * @return computes the `true' gamma function
+ * \f[ \Gamma(x) = \int_0^\infty y^{x-1} \exp(-y) dy \f]
+ * using a polynomial form:
+ * \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
+ * where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
+ * are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tgamma(const T x)
+noexcept
+{
+ return internal::tgamma_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
new file mode 100644
index 0000000..4e19ef9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
@@ -0,0 +1,121 @@
+/*################################################################################
+ ##
+ ## Copyright (C) 2016-2022 Keith O'Hara
+ ##
+ ## This file is part of the GCE-Math C++ library.
+ ##
+ ## Licensed under the Apache License, Version 2.0 (the "License");
+ ## you may not use this file except in compliance with the License.
+ ## You may obtain a copy of the License at
+ ##
+ ## http://www.apache.org/licenses/LICENSE-2.0
+ ##
+ ## Unless required by applicable law or agreed to in writing, software
+ ## distributed under the License is distributed on an "AS IS" BASIS,
+ ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ ## See the License for the specific language governing permissions and
+ ## limitations under the License.
+ ##
+ ################################################################################*/
+
+#ifndef _gcem_trunc_HPP
+#define _gcem_trunc_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+trunc_int(const T x)
+noexcept
+{
+ return( T(static_cast<llint_t>(x)) );
+}
+
+template<typename T>
+constexpr
+T
+trunc_check_internal(const T x)
+noexcept
+{
+ return x;
+}
+
+template<>
+constexpr
+float
+trunc_check_internal<float>(const float x)
+noexcept
+{
+ return( abs(x) >= 8388608.f ? \
+ // if
+ x : \
+ // else
+ trunc_int(x) );
+}
+
+template<>
+constexpr
+double
+trunc_check_internal<double>(const double x)
+noexcept
+{
+ return( abs(x) >= 4503599627370496. ? \
+ // if
+ x : \
+ // else
+ trunc_int(x) );
+}
+
+template<>
+constexpr
+long double
+trunc_check_internal<long double>(const long double x)
+noexcept
+{
+ return( abs(x) >= 9223372036854775808.l ? \
+ // if
+ x : \
+ // else
+ ((long double)static_cast<ullint_t>(abs(x))) * sgn(x) );
+}
+
+template<typename T>
+constexpr
+T
+trunc_check(const T x)
+noexcept
+{
+ return( // NaN check
+ is_nan(x) ? \
+ GCLIM<T>::quiet_NaN() :
+ // +/- infinite
+ !is_finite(x) ? \
+ x :
+ // signed-zero cases
+ GCLIM<T>::min() > abs(x) ? \
+ x :
+ // else
+ trunc_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time trunc function
+ *
+ * @param x a real-valued input.
+ * @return computes the trunc-value of the input, essentially returning the integer part of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+trunc(const T x)
+noexcept
+{
+ return internal::trunc_check( static_cast<return_t<T>>(x) );
+}
+
+#endif