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/.styleguide b/wpimath/.styleguide
index b9044c9..6ae8bbf 100644
--- a/wpimath/.styleguide
+++ b/wpimath/.styleguide
@@ -1,5 +1,6 @@
cppHeaderFileInclude {
\.h$
+ \.hpp$
\.inc$
\.inl$
}
@@ -13,12 +14,10 @@
}
generatedFileExclude {
- src/main/native/cpp/drake/
- src/main/native/eigeninclude/
- src/main/native/include/drake/
src/main/native/include/units/base\.h$
src/main/native/include/units/units\.h$
src/main/native/include/unsupported/
+ src/main/native/thirdparty/
src/test/native/cpp/UnitsTest\.cpp$
src/test/native/cpp/drake/
src/test/native/include/drake/
@@ -40,6 +39,7 @@
}
includeProject {
+ ^gcem/
^drake/
^Eigen/
^units/
diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index 4e1cac3..e3d90cd 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -89,7 +89,8 @@
endif()
-file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
+file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp
+ src/main/native/thirdparty/drake/src/*.cpp)
list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src})
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE)
@@ -100,29 +101,37 @@
set_property(TARGET wpimath PROPERTY FOLDER "libraries")
target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS)
-target_compile_features(wpimath PUBLIC cxx_std_17)
+target_compile_features(wpimath PUBLIC cxx_std_20)
if (MSVC)
target_compile_options(wpimath PUBLIC /bigobj)
endif()
wpilib_target_warnings(wpimath)
target_link_libraries(wpimath wpiutil)
-if (NOT USE_VCPKG_EIGEN)
- install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpimath")
+if (NOT USE_SYSTEM_EIGEN)
+ install(DIRECTORY src/main/native/thirdparty/eigen/include/ DESTINATION "${include_dest}/wpimath")
target_include_directories(wpimath SYSTEM PUBLIC
- $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/eigeninclude>
- $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/eigen/include>
+ $<INSTALL_INTERFACE:${include_dest}/wpimath>)
else()
find_package(Eigen3 CONFIG REQUIRED)
target_link_libraries (wpimath Eigen3::Eigen)
endif()
+install(DIRECTORY src/main/native/thirdparty/drake/include/ DESTINATION "${include_dest}/wpimath")
+target_include_directories(wpimath SYSTEM PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/drake/include>)
+
+install(DIRECTORY src/main/native/thirdparty/gcem/include/ DESTINATION "${include_dest}/wpimath")
+target_include_directories(wpimath SYSTEM PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/thirdparty/gcem/include>)
+
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
target_include_directories(wpimath PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
$<INSTALL_INTERFACE:${include_dest}/wpimath>)
install(TARGETS wpimath EXPORT wpimath DESTINATION "${main_lib_dest}")
-install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
if (WITH_JAVA AND MSVC)
install(TARGETS wpimath RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md
new file mode 100644
index 0000000..04cdf94
--- /dev/null
+++ b/wpimath/algorithms.md
@@ -0,0 +1,88 @@
+# Algorithms
+
+## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
+
+### Derivation
+
+Model is
+
+```
+ dx/dt = Ax + Bu
+ y = Cx + Du
+```
+
+where A = 0, B = 0, C = I, and D = 0.
+
+The optimal cost-to-go is the P that satisfies
+
+```
+ AᵀP + PA − PBR⁻¹BᵀP + Q = 0
+```
+
+Let A = Aᵀ and B = Cᵀ for state observers.
+
+```
+ AP + PAᵀ − PCᵀR⁻¹CP + Q = 0
+```
+
+Let A = 0, C = I.
+
+```
+ −PR⁻¹P + Q = 0
+```
+
+Solve for P. P, Q, and R are all diagonal, so this can be solved element-wise.
+
+```
+ −pr⁻¹p + q = 0
+ −pr⁻¹p = −q
+ pr⁻¹p = q
+ p²r⁻¹ = q
+ p² = qr
+ p = sqrt(qr)
+```
+
+Now solve for the Kalman gain.
+
+```
+ K = PCᵀ(CPCᵀ + R)⁻¹
+ K = P(P + R)⁻¹
+ k = p(p + r)⁻¹
+ k = sqrt(qr)(sqrt(qr) + r)⁻¹
+ k = sqrt(qr)/(sqrt(qr) + r)
+```
+
+Multiply by sqrt(q/r)/sqrt(q/r).
+
+```
+ k = q/(q + r sqrt(q/r))
+ k = q/(q + sqrt(qr²/r))
+ k = q/(q + sqrt(qr))
+```
+
+### Corner cases
+
+For q = 0 and r ≠ 0,
+
+```
+ k = 0/(0 + sqrt(0))
+ k = 0/0
+```
+
+Apply L'Hôpital's rule to k with respect to q.
+
+```
+ k = 1/(1 + r/(2sqrt(qr)))
+ k = 2sqrt(qr)/(2sqrt(qr) + r)
+ k = 2sqrt(0)/(2sqrt(0) + r)
+ k = 0/r
+ k = 0
+```
+
+For q ≠ 0 and r = 0,
+
+```
+ k = q / (q + sqrt(0))
+ k = q / q
+ k = 1
+```
diff --git a/wpimath/build.gradle b/wpimath/build.gradle
index c31f04c..9e7c3a8 100644
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle
@@ -9,12 +9,34 @@
nativeName = 'wpimath'
devMain = 'edu.wpi.first.math.DevMain'
+
+ splitSetup = {
+ it.sources {
+ drakeCpp(CppSourceSet) {
+ source {
+ srcDirs 'src/main/native/thirdparty/drake/src'
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/native/thirdparty/drake/include',
+ 'src/main/native/thirdparty/eigen/include',
+ 'src/main/native/thirdparty/gcem/include'
+ }
+ }
+ }
+ }
}
apply from: "${rootDir}/shared/jni/setupBuild.gradle"
cppHeadersZip {
- from('src/main/native/eigeninclude') {
+ from('src/main/native/thirdparty/drake/include') {
+ into '/'
+ }
+ from('src/main/native/thirdparty/eigen/include') {
+ into '/'
+ }
+ from('src/main/native/thirdparty/gcem/include') {
into '/'
}
}
@@ -24,7 +46,10 @@
all {
it.sources.each {
it.exportedHeaders {
- srcDirs 'src/main/native/include', 'src/main/native/eigeninclude'
+ srcDirs 'src/main/native/include',
+ 'src/main/native/thirdparty/drake/include',
+ 'src/main/native/thirdparty/eigen/include',
+ 'src/main/native/thirdparty/gcem/include'
}
}
}
diff --git a/wpimath/generate_numbers.py b/wpimath/generate_numbers.py
index c52ddb4..c9da1a4 100644
--- a/wpimath/generate_numbers.py
+++ b/wpimath/generate_numbers.py
@@ -29,9 +29,11 @@
dirname, _ = os.path.split(os.path.abspath(__file__))
cmake_binary_dir = sys.argv[1]
- env = Environment(loader=FileSystemLoader(f"{dirname}/src/generate"),
- autoescape=False,
- keep_trailing_newline=True)
+ env = Environment(
+ loader=FileSystemLoader(f"{dirname}/src/generate"),
+ autoescape=False,
+ keep_trailing_newline=True,
+ )
template = env.get_template("GenericNumber.java.jinja")
rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/math/numbers"
diff --git a/wpimath/src/dev/native/cpp/main.cpp b/wpimath/src/dev/native/cpp/main.cpp
index 54952b7..447d3f2 100644
--- a/wpimath/src/dev/native/cpp/main.cpp
+++ b/wpimath/src/dev/native/cpp/main.cpp
@@ -2,9 +2,10 @@
// 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 <numbers>
+
#include <fmt/core.h>
-#include <wpi/numbers>
int main() {
- fmt::print("{}\n", wpi::numbers::pi);
+ fmt::print("{}\n", std::numbers::pi);
}
diff --git a/wpimath/src/generate/Nat.java.jinja b/wpimath/src/generate/Nat.java.jinja
index 31451d2..cbc0ddb 100644
--- a/wpimath/src/generate/Nat.java.jinja
+++ b/wpimath/src/generate/Nat.java.jinja
@@ -16,7 +16,6 @@
*
* @param <T> The {@link Num} this represents.
*/
-@SuppressWarnings({"MethodName", "unused"})
public interface Nat<T extends Num> {
/**
* The number this interface represents.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
new file mode 100644
index 0000000..7b1c377
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+public final class ComputerVisionUtil {
+ private ComputerVisionUtil() {
+ throw new AssertionError("utility class");
+ }
+
+ /**
+ * 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.
+ *
+ * <p>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 new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0,
+ * new Rotation3d()).
+ * @return The robot's field-relative pose.
+ */
+ public static Pose3d objectToRobotPose(
+ Pose3d objectInField, Transform3d cameraToObject, Transform3d robotToCamera) {
+ final var objectToCamera = cameraToObject.inverse();
+ final var cameraToRobot = robotToCamera.inverse();
+ return objectInField.plus(objectToCamera).plus(cameraToRobot);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
index 766f7e9..55bc5b2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
@@ -10,7 +10,7 @@
private Drake() {}
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Solves the discrete algebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
@@ -18,7 +18,6 @@
* @param R Input cost matrix.
* @return Solution of DARE.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
var S = new SimpleMatrix(A.numRows(), A.numCols());
@@ -34,7 +33,7 @@
}
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Solves the discrete algebraic Riccati equation.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
@@ -44,7 +43,6 @@
* @param R Input cost matrix.
* @return Solution of DARE.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
@@ -57,7 +55,7 @@
}
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Solves the discrete algebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
@@ -66,7 +64,6 @@
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
// See
@@ -88,7 +85,7 @@
}
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Solves the discrete algebraic Riccati equation.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
@@ -99,7 +96,6 @@
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
new file mode 100644
index 0000000..a7b922c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
+ private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
+
+ /**
+ * Inserts a key-value pair.
+ *
+ * @param key The key.
+ * @param value The value.
+ */
+ public void put(K key, Matrix<R, C> value) {
+ m_map.put(key, value);
+ }
+
+ /**
+ * Returns the value associated with a given key.
+ *
+ * <p>If there's no matching key, the value returned will be a linear interpolation between the
+ * keys before and after the provided one.
+ *
+ * @param key The key.
+ * @return The value associated with the given key.
+ */
+ public Matrix<R, C> get(K key) {
+ Matrix<R, C> val = m_map.get(key);
+ if (val == null) {
+ K ceilingKey = m_map.ceilingKey(key);
+ K floorKey = m_map.floorKey(key);
+
+ if (ceilingKey == null && floorKey == null) {
+ return null;
+ }
+ if (ceilingKey == null) {
+ return m_map.get(floorKey);
+ }
+ if (floorKey == null) {
+ return m_map.get(ceilingKey);
+ }
+ Matrix<R, C> floor = m_map.get(floorKey);
+ Matrix<R, C> ceiling = m_map.get(ceilingKey);
+
+ return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
+ } else {
+ return val;
+ }
+ }
+
+ /**
+ * Return the value interpolated between val1 and val2 by the interpolant d.
+ *
+ * @param val1 The lower part of the interpolation range.
+ * @param val2 The upper part of the interpolation range.
+ * @param d The interpolant in the range [0, 1].
+ * @return The interpolated value.
+ */
+ public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
+ var dydx = val2.minus(val1);
+ return dydx.times(d).plus(val1);
+ }
+
+ /**
+ * Return where within interpolation range [0, 1] q is between down and up.
+ *
+ * @param up Upper part of interpolation range.
+ * @param q Query.
+ * @param down Lower part of interpolation range.
+ * @return Interpolant in range [0, 1].
+ */
+ public double inverseInterpolate(K up, K q, K down) {
+ double upperToLower = up.doubleValue() - down.doubleValue();
+ if (upperToLower <= 0) {
+ return 0.0;
+ }
+ double queryToLower = q.doubleValue() - down.doubleValue();
+ if (queryToLower <= 0) {
+ return 0.0;
+ }
+ return queryToLower / upperToLower;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
index e5b1952..2ee010f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
@@ -24,7 +24,6 @@
* @param data The data to fill the matrix with.
* @return The constructed matrix.
*/
- @SuppressWarnings("LineLength")
public final Matrix<R, C> fill(double... data) {
if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
throw new IllegalArgumentException(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 25b9f92..95ed5bf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -35,6 +35,63 @@
/**
* 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 deadband Range around zero.
+ * @param maxMagnitude The maximum magnitude of the input. Can be infinite.
+ * @return The value after the deadband is applied.
+ */
+ public static double applyDeadband(double value, double deadband, double maxMagnitude) {
+ if (Math.abs(value) > 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 > 0.0 ? value - deadband : value + deadband;
+ }
+ if (value > 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 0.0;
+ }
+ }
+
+ /**
+ * 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.
*
* @param value Value to clip.
@@ -42,15 +99,7 @@
* @return The value after the deadband is applied.
*/
public static double applyDeadband(double value, double deadband) {
- if (Math.abs(value) > deadband) {
- if (value > 0.0) {
- return (value - deadband) / (1.0 - deadband);
- } else {
- return (value + deadband) / (1.0 - deadband);
- }
- } else {
- return 0.0;
- }
+ return applyDeadband(value, deadband, 1);
}
/**
@@ -93,7 +142,6 @@
* @param t How far between the two values to interpolate. This is clamped to [0, 1].
* @return The interpolated value.
*/
- @SuppressWarnings("ParameterName")
public static double interpolate(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index 113758b..b8e7c28 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -323,16 +323,42 @@
* <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
* is used if A is not square.
*
+ * <p>Note that this method does not support solving using a QR decomposition with full-pivoting,
+ * as only column-pivoting is supported. For full-pivoting, use {@link
+ * #solveFullPivHouseholderQr}.
+ *
* @param <C2> Columns in b.
* @param b The right-hand side of the equation to solve.
* @return The solution to the linear system.
*/
- @SuppressWarnings("ParameterName")
public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
}
/**
+ * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this
+ * matrix is A.
+ *
+ * @param <R2> Number of rows in B.
+ * @param <C2> Number of columns in B.
+ * @param other The B matrix.
+ * @return The solution matrix.
+ */
+ public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
+ Matrix<R2, C2> other) {
+ Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
+ WPIMathJNI.solveFullPivHouseholderQr(
+ this.getData(),
+ this.getNumRows(),
+ this.getNumCols(),
+ other.getData(),
+ other.getNumRows(),
+ other.getNumCols(),
+ solution.getData());
+ return solution;
+ }
+
+ /**
* Computes the matrix exponential using Eigen's solver. This method only works for square
* matrices, and will otherwise throw an {@link MatrixDimensionException}.
*
@@ -437,7 +463,6 @@
* @param b Scalar.
* @return The element by element power of "this" and b.
*/
- @SuppressWarnings("ParameterName")
public final Matrix<R, C> elementPower(double b) {
return new Matrix<>(this.m_storage.elementPower(b));
}
@@ -450,7 +475,6 @@
* @param b Scalar.
* @return The element by element power of "this" and b.
*/
- @SuppressWarnings("ParameterName")
public final Matrix<R, C> elementPower(int b) {
return new Matrix<>(this.m_storage.elementPower((double) b));
}
@@ -549,10 +573,9 @@
* Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will
* return the zero matrix.
*
- * @param lowerTriangular Whether or not we want to decompose to the lower triangular Cholesky
- * matrix.
+ * @param lowerTriangular Whether we want to decompose to the lower triangular Cholesky matrix.
* @return The decomposed matrix.
- * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
+ * @throws RuntimeException if the matrix could not be decomposed(i.e. is not positive
* semidefinite).
*/
public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
@@ -646,10 +669,10 @@
* same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or
* Double.NEGATIVE_INFINITY.
*
- * <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this method when
+ * <p>NOTE:It is recommended to use {@link Matrix#isEqual(Matrix, double)} over this method when
* checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} will return false
* if an element is uncountable. This method should only be used when uncountable elements need to
- * compared.
+ * be compared.
*
* @param other The {@link Matrix} to check against this one.
* @param tolerance The tolerance to check equality with.
@@ -677,6 +700,20 @@
this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
}
+ /**
+ * Performs an inplace Cholesky rank update (or downdate).
+ *
+ * <p>If this matrix contains L where A = LLᵀ before the update, it will contain L where LLᵀ = A +
+ * σvvᵀ after the update.
+ *
+ * @param v Vector to use for the update.
+ * @param sigma Sigma to use for the update.
+ * @param lowerTriangular Whether this matrix is lower triangular.
+ */
+ public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
+ WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
+ }
+
@Override
public String toString() {
return m_storage.toString();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
deleted file mode 100644
index 7600e31..0000000
--- a/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
+++ /dev/null
@@ -1,80 +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.
-
-package edu.wpi.first.math;
-
-import edu.wpi.first.math.numbers.N1;
-import java.util.Objects;
-import org.ejml.simple.SimpleMatrix;
-
-@Deprecated
-public final class MatrixUtils {
- private MatrixUtils() {
- throw new AssertionError("utility class");
- }
-
- /**
- * Creates a new matrix of zeros.
- *
- * @param rows The number of rows in the matrix.
- * @param cols The number of columns in the matrix.
- * @param <R> The number of rows in the matrix as a generic.
- * @param <C> The number of columns in the matrix as a generic.
- * @return An RxC matrix filled with zeros.
- */
- @SuppressWarnings("LineLength")
- public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
- return new Matrix<>(
- new SimpleMatrix(
- Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
- }
-
- /**
- * Creates a new vector of zeros.
- *
- * @param nums The size of the desired vector.
- * @param <N> The size of the desired vector as a generic.
- * @return A vector of size N filled with zeros.
- */
- public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
- return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
- }
-
- /**
- * Creates the identity matrix of the given dimension.
- *
- * @param dim The dimension of the desired matrix.
- * @param <D> The dimension of the desired matrix as a generic.
- * @return The DxD identity matrix.
- */
- public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
- return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
- }
-
- /**
- * Entrypoint to the MatBuilder class for creation of custom matrices with the given dimensions
- * and contents.
- *
- * @param rows The number of rows of the desired matrix.
- * @param cols The number of columns of the desired matrix.
- * @param <R> The number of rows of the desired matrix as a generic.
- * @param <C> The number of columns of the desired matrix as a generic.
- * @return A builder to construct the matrix.
- */
- public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
- return new MatBuilder<>(rows, cols);
- }
-
- /**
- * Entrypoint to the VecBuilder class for creation of custom vectors with the given size and
- * contents.
- *
- * @param dim The dimension of the vector.
- * @param <D> The dimension of the vector as a generic.
- * @return A builder to construct the vector.
- */
- public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
- return new VecBuilder<>(dim);
- }
-}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Pair.java b/wpimath/src/main/java/edu/wpi/first/math/Pair.java
index d1a68c7..8ddf1ae 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Pair.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Pair.java
@@ -21,7 +21,6 @@
return m_second;
}
- @SuppressWarnings("ParameterName")
public static <A, B> Pair<A, B> of(A a, B b) {
return new Pair<>(a, b);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
index fc78dd1..e1680eb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
@@ -21,36 +21,34 @@
* @param matrix The matrix to compute the exponential of.
* @return The resultant matrix.
*/
- @SuppressWarnings({"LocalVariableName", "LineLength"})
public static SimpleMatrix expm(SimpleMatrix matrix) {
BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
SimpleMatrix A = matrix;
double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
- int n_squarings = 0;
+ int nSquarings = 0;
if (A_L1 < 1.495585217958292e-002) {
- Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
- return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+ Pair<SimpleMatrix, SimpleMatrix> pair = pade3(A);
+ return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
} else if (A_L1 < 2.539398330063230e-001) {
- Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
- return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+ Pair<SimpleMatrix, SimpleMatrix> pair = pade5(A);
+ return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
} else if (A_L1 < 9.504178996162932e-001) {
- Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
- return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+ Pair<SimpleMatrix, SimpleMatrix> pair = pade7(A);
+ return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
} else if (A_L1 < 2.097847961257068e+000) {
- Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
- return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+ Pair<SimpleMatrix, SimpleMatrix> pair = pade9(A);
+ return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
} else {
double maxNorm = 5.371920351148152;
double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
- n_squarings = (int) Math.max(0, Math.ceil(log));
- A = A.divide(Math.pow(2.0, n_squarings));
- Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
- return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+ nSquarings = (int) Math.max(0, Math.ceil(log));
+ A = A.divide(Math.pow(2.0, nSquarings));
+ Pair<SimpleMatrix, SimpleMatrix> pair = pade13(A);
+ return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
}
}
- @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
private static SimpleMatrix dispatchPade(
SimpleMatrix U,
SimpleMatrix V,
@@ -68,8 +66,7 @@
return R;
}
- @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
- private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
+ private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
double[] b = new double[] {120, 60, 12, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
@@ -79,8 +76,7 @@
return new Pair<>(U, V);
}
- @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
- private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
+ private static Pair<SimpleMatrix, SimpleMatrix> pade5(SimpleMatrix A) {
double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
@@ -92,8 +88,7 @@
return new Pair<>(U, V);
}
- @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
- private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
+ private static Pair<SimpleMatrix, SimpleMatrix> pade7(SimpleMatrix A) {
double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
@@ -108,8 +103,7 @@
return new Pair<>(U, V);
}
- @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
- private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
+ private static Pair<SimpleMatrix, SimpleMatrix> pade9(SimpleMatrix A) {
double[] b =
new double[] {
17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
@@ -137,8 +131,7 @@
return new Pair<>(U, V);
}
- @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
- private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
+ private static Pair<SimpleMatrix, SimpleMatrix> pade13(SimpleMatrix A) {
double[] b =
new double[] {
64764752532480000.0,
@@ -199,7 +192,7 @@
*
* @param src The matrix to decompose.
* @return The decomposed matrix.
- * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+ * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
* semidefinite).
*/
public static SimpleMatrix lltDecompose(SimpleMatrix src) {
@@ -213,7 +206,7 @@
* @param src The matrix to decompose.
* @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
* @return The decomposed matrix.
- * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+ * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
* semidefinite).
*/
public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
@@ -245,7 +238,6 @@
* @param A the matrix to exponentiate.
* @return the exponential of A.
*/
- @SuppressWarnings("ParameterName")
public static SimpleMatrix exp(SimpleMatrix A) {
SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index 69430eb..a041845 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -11,7 +11,6 @@
import java.util.Random;
import org.ejml.simple.SimpleMatrix;
-@SuppressWarnings("ParameterName")
public final class StateSpaceUtil {
private static Random rand = new Random();
@@ -31,7 +30,6 @@
* output measurement.
* @return Process noise or measurement noise covariance matrix.
*/
- @SuppressWarnings("MethodTypeParameterName")
public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
Nat<States> states, Matrix<States, N1> stdDevs) {
var result = new Matrix<>(states, states);
@@ -61,24 +59,28 @@
/**
* Creates a cost matrix from the given vector for use with LQR.
*
- * <p>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.
+ * <p>The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is
+ * placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to
+ * zero.
*
- * @param <States> Nat representing the states of the system.
- * @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 <Elements> Nat representing the number of system states or inputs.
+ * @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.
*/
- @SuppressWarnings("MethodTypeParameterName")
- public static <States extends Num> Matrix<States, States> makeCostMatrix(
- Matrix<States, N1> costs) {
- Matrix<States, States> result =
- new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
+ public static <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
+ Matrix<Elements, N1> tolerances) {
+ Matrix<Elements, Elements> result =
+ new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows()));
result.fill(0.0);
- for (int i = 0; i < costs.getNumRows(); i++) {
- result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
+ for (int i = 0; i < tolerances.getNumRows(); i++) {
+ if (tolerances.get(i, 0) == Double.POSITIVE_INFINITY) {
+ result.set(i, i, 0.0);
+ } else {
+ result.set(i, i, 1.0 / (Math.pow(tolerances.get(i, 0), 2)));
+ }
}
return result;
@@ -97,7 +99,6 @@
* @param B Input matrix.
* @return If the system is stabilizable.
*/
- @SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Inputs extends Num> boolean isStabilizable(
Matrix<States, States> A, Matrix<States, Inputs> B) {
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
@@ -116,7 +117,6 @@
* @param C Output matrix.
* @return If the system is detectable.
*/
- @SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Outputs extends Num> boolean isDetectable(
Matrix<States, States> A, Matrix<Outputs, States> C) {
return WPIMathJNI.isStabilizable(
@@ -142,7 +142,6 @@
* @param <I> The number of inputs.
* @return The clamped input.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 9b06e71..1aebdde 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -61,4 +61,35 @@
public Vector<R> div(double value) {
return new Vector<>(this.m_storage.divide(value));
}
+
+ /**
+ * Returns the dot product of this vector with another.
+ *
+ * @param other The other vector.
+ * @return The dot product.
+ */
+ public double dot(Vector<R> other) {
+ double dot = 0.0;
+
+ for (int i = 0; i < getNumRows(); ++i) {
+ dot += get(i, 0) * other.get(i, 0);
+ }
+
+ return dot;
+ }
+
+ /**
+ * Returns the norm of this vector.
+ *
+ * @return The norm.
+ */
+ public double norm() {
+ double squaredNorm = 0.0;
+
+ for (int i = 0; i < getNumRows(); ++i) {
+ squaredNorm += get(i, 0) * get(i, 0);
+ }
+
+ return Math.sqrt(squaredNorm);
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 54445d3..40a5c63 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -125,6 +125,19 @@
*/
public static native String serializeTrajectory(double[] elements);
+ /**
+ * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+ * matrix.
+ *
+ * @param mat Array of elements of the matrix to be updated.
+ * @param lowerTriangular Whether mat is lower triangular.
+ * @param rows How many rows there are.
+ * @param vec Vector to use for the rank update.
+ * @param sigma Sigma value to use for the rank update.
+ */
+ public static native void rankUpdate(
+ double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
@@ -136,4 +149,18 @@
extractOnStaticLoad.set(load);
}
}
+
+ /**
+ * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+ *
+ * @param A Array of elements of the A matrix.
+ * @param Arows Number of rows of the A matrix.
+ * @param Acols Number of rows of the A matrix.
+ * @param B Array of elements of the B matrix.
+ * @param Brows Number of rows of the B matrix.
+ * @param Bcols Number of rows of the B matrix.
+ * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+ */
+ public static native void solveFullPivHouseholderQr(
+ double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
index 2991511..2e9a8d8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -8,10 +8,9 @@
* A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
* against the force of gravity on a beam suspended at an angle).
*/
-@SuppressWarnings("MemberName")
public class ArmFeedforward {
public final double ks;
- public final double kcos;
+ public final double kg;
public final double kv;
public final double ka;
@@ -20,13 +19,13 @@
* units of the computed feedforward.
*
* @param ks The static gain.
- * @param kcos The gravity gain.
+ * @param kg The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
*/
- public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+ public ArmFeedforward(double ks, double kg, double kv, double ka) {
this.ks = ks;
- this.kcos = kcos;
+ this.kg = kg;
this.kv = kv;
this.ka = ka;
}
@@ -36,17 +35,19 @@
* Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
- * @param kcos The gravity gain.
+ * @param kg The gravity gain.
* @param kv The velocity gain.
*/
- public ArmFeedforward(double ks, double kcos, double kv) {
- this(ks, kcos, kv, 0);
+ public ArmFeedforward(double ks, double kg, double kv) {
+ this(ks, kg, kv, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
- * @param positionRadians The position (angle) setpoint.
+ * @param positionRadians The position (angle) setpoint. This angle should be measured from the
+ * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+ * your encoder does not follow this convention, an offset should be added.
* @param velocityRadPerSec The velocity setpoint.
* @param accelRadPerSecSquared The acceleration setpoint.
* @return The computed feedforward.
@@ -54,7 +55,7 @@
public double calculate(
double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
return ks * Math.signum(velocityRadPerSec)
- + kcos * Math.cos(positionRadians)
+ + kg * Math.cos(positionRadians)
+ kv * velocityRadPerSec
+ ka * accelRadPerSecSquared;
}
@@ -63,7 +64,9 @@
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
- * @param positionRadians The position (angle) setpoint.
+ * @param positionRadians The position (angle) setpoint. This angle should be measured from the
+ * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+ * your encoder does not follow this convention, an offset should be added.
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
@@ -81,13 +84,15 @@
* 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 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 with 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.
*/
public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
// Assume max velocity is positive
- return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+ return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv;
}
/**
@@ -97,13 +102,15 @@
* 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 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 with 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.
*/
public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
// Assume min velocity is negative, ks flips sign
- return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+ return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv;
}
/**
@@ -113,12 +120,14 @@
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle 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 with 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.
*/
public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
- return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
+ return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
}
/**
@@ -128,7 +137,9 @@
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
- * @param angle The angle 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 with 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.
*/
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
index d4beabd..385c5c0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
@@ -29,16 +29,13 @@
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
/** The current reference state. */
- @SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/** The computed feedforward. */
private Matrix<Inputs, N1> m_uff;
- @SuppressWarnings("MemberName")
private final Matrix<States, Inputs> m_B;
private final Nat<Inputs> m_inputs;
@@ -181,7 +178,6 @@
* @param nextR The reference state of the future timestep (k + dt).
* @return The calculated feedforward.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
var rDot = (nextR.minus(r)).div(m_dt);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
new file mode 100644
index 0000000..8c47765
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
@@ -0,0 +1,130 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and angular acceleration.
+ *
+ * <p>The differential drive model can be created via the functions in {@link
+ * edu.wpi.first.math.system.plant.LinearSystemId}.
+ */
+public class DifferentialDriveAccelerationLimiter {
+ private final LinearSystem<N2, N2, N2> m_system;
+ private final double m_trackwidth;
+ private final double m_minLinearAccel;
+ private final double m_maxLinearAccel;
+ private final double m_maxAngularAccel;
+
+ /**
+ * Constructs a DifferentialDriveAccelerationLimiter.
+ *
+ * @param system The differential drive dynamics.
+ * @param trackwidth The distance between the differential drive's left and right wheels in
+ * meters.
+ * @param maxLinearAccel The maximum linear acceleration in meters per second squared.
+ * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+ */
+ public DifferentialDriveAccelerationLimiter(
+ LinearSystem<N2, N2, N2> system,
+ double trackwidth,
+ double maxLinearAccel,
+ double maxAngularAccel) {
+ this(system, trackwidth, -maxLinearAccel, maxLinearAccel, maxAngularAccel);
+ }
+
+ /**
+ * Constructs a DifferentialDriveAccelerationLimiter.
+ *
+ * @param system The differential drive dynamics.
+ * @param trackwidth The distance between the differential drive's left and right wheels in
+ * meters.
+ * @param minLinearAccel The minimum (most negative) linear acceleration in meters per second
+ * squared.
+ * @param maxLinearAccel The maximum (most positive) linear acceleration in meters per second
+ * squared.
+ * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+ * @throws IllegalArgumentException if minimum linear acceleration is greater than maximum linear
+ * acceleration
+ */
+ public DifferentialDriveAccelerationLimiter(
+ LinearSystem<N2, N2, N2> system,
+ double trackwidth,
+ double minLinearAccel,
+ double maxLinearAccel,
+ double maxAngularAccel) {
+ if (minLinearAccel > maxLinearAccel) {
+ throw new IllegalArgumentException("maxLinearAccel must be greater than minLinearAccel");
+ }
+ m_system = system;
+ m_trackwidth = trackwidth;
+ m_minLinearAccel = minLinearAccel;
+ m_maxLinearAccel = maxLinearAccel;
+ m_maxAngularAccel = maxAngularAccel;
+ }
+
+ /**
+ * Returns the next voltage pair subject to acceleration constraints.
+ *
+ * @param leftVelocity The left wheel velocity in meters per second.
+ * @param rightVelocity The right wheel velocity in meters per second.
+ * @param leftVoltage The unconstrained left motor voltage.
+ * @param rightVoltage The unconstrained right motor voltage.
+ * @return The constrained wheel voltages.
+ */
+ public DifferentialDriveWheelVoltages calculate(
+ double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage);
+
+ // Find unconstrained wheel accelerations
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity);
+ var dxdt = m_system.getA().times(x).plus(m_system.getB().times(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]
+ var M =
+ new MatBuilder<>(Nat.N2(), Nat.N2())
+ .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
+ var accels = M.times(dxdt);
+
+ // Constrain the linear and angular accelerations
+ if (accels.get(0, 0) > m_maxLinearAccel) {
+ accels.set(0, 0, m_maxLinearAccel);
+ } else if (accels.get(0, 0) < m_minLinearAccel) {
+ accels.set(0, 0, m_minLinearAccel);
+ }
+ if (accels.get(1, 0) > m_maxAngularAccel) {
+ accels.set(1, 0, m_maxAngularAccel);
+ } else if (accels.get(1, 0) < -m_maxAngularAccel) {
+ accels.set(1, 0, -m_maxAngularAccel);
+ }
+
+ // Convert the constrained linear and angular accelerations back to wheel
+ // accelerations
+ dxdt = M.solve(accels);
+
+ // Find voltages for the given wheel accelerations
+ //
+ // dx/dt = Ax + Bu
+ // u = B⁻¹(dx/dt - Ax)
+ u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x)));
+
+ return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
new file mode 100644
index 0000000..4d0c94f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
+public class DifferentialDriveFeedforward {
+ private final LinearSystem<N2, N2, N2> m_plant;
+
+ /**
+ * 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.
+ */
+ public DifferentialDriveFeedforward(
+ double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+ m_plant =
+ LinearSystemId.identifyDrivetrainSystem(
+ kVLinear, kALinear, kVAngular, kAAngular, 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).
+ */
+ public DifferentialDriveFeedforward(
+ double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+ m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, 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 dtSeconds Discretization timestep.
+ * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
+ */
+ public DifferentialDriveWheelVoltages calculate(
+ double currentLeftVelocity,
+ double nextLeftVelocity,
+ double currentRightVelocity,
+ double nextRightVelocity,
+ double dtSeconds) {
+ var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
+ var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
+ var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
+ var u = feedforward.calculate(r, nextR);
+ return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
new file mode 100644
index 0000000..2fbd0aa
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+/** Motor voltages for a differential drive. */
+public class DifferentialDriveWheelVoltages {
+ public double left;
+ public double right;
+
+ public DifferentialDriveWheelVoltages() {}
+
+ public DifferentialDriveWheelVoltages(double left, double right) {
+ this.left = left;
+ this.right = right;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
index 248015f..9f4dd86 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -8,7 +8,6 @@
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
* against the force of gravity).
*/
-@SuppressWarnings("MemberName")
public class ElevatorFeedforward {
public final double ks;
public final double kg;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index be813cc..e43c6ff 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -8,6 +8,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.util.Units;
/**
* This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
@@ -20,7 +21,6 @@
* are decoupled from translations, users can specify a custom heading that the drivetrain should
* point toward. This heading reference is profiled for smoothness.
*/
-@SuppressWarnings("MemberName")
public class HolonomicDriveController {
private Pose2d m_poseError = new Pose2d();
private Rotation2d m_rotationError = new Rotation2d();
@@ -40,12 +40,12 @@
* @param yController A PID Controller to respond to error in the field-relative y direction.
* @param thetaController A profiled PID controller to respond to error in angle.
*/
- @SuppressWarnings("ParameterName")
public HolonomicDriveController(
PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
m_xController = xController;
m_yController = yController;
m_thetaController = thetaController;
+ m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0));
}
/**
@@ -75,15 +75,17 @@
/**
* Returns the next output of the holonomic drive controller.
*
- * @param currentPose The current pose.
- * @param poseRef The desired pose.
- * @param linearVelocityRefMeters The linear velocity reference.
- * @param angleRef The angular reference.
+ * @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 desiredLinearVelocityMetersPerSecond The desired linear velocity.
+ * @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
- @SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
- Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
+ Pose2d currentPose,
+ Pose2d trajectoryPose,
+ double desiredLinearVelocityMetersPerSecond,
+ 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) {
@@ -92,21 +94,22 @@
}
// Calculate feedforward velocities (field-relative).
- double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
- double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
+ double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos();
+ double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin();
double thetaFF =
- m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
+ m_thetaController.calculate(
+ currentPose.getRotation().getRadians(), desiredHeading.getRadians());
- m_poseError = poseRef.relativeTo(currentPose);
- m_rotationError = angleRef.minus(currentPose.getRotation());
+ m_poseError = trajectoryPose.relativeTo(currentPose);
+ m_rotationError = desiredHeading.minus(currentPose.getRotation());
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
- double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
- double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
+ double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
+ double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
return ChassisSpeeds.fromFieldRelativeSpeeds(
@@ -116,15 +119,15 @@
/**
* Returns the next output of the holonomic drive controller.
*
- * @param currentPose The current pose.
- * @param desiredState The desired trajectory state.
- * @param angleRef The desired end-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.
*/
public ChassisSpeeds calculate(
- Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
+ Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
return calculate(
- currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
+ currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading);
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
new file mode 100644
index 0000000..7aa4cfa
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Contains the controller coefficients and logic for an implicit model follower.
+ *
+ * <p>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.
+ *
+ * <p>For more on the underlying math, read appendix B.3 in
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
+ // Computed controller output
+ private Matrix<Inputs, N1> m_u;
+
+ // State space conversion gain
+ private Matrix<Inputs, States> m_A;
+
+ // Input space conversion gain
+ private Matrix<Inputs, Inputs> m_B;
+
+ /**
+ * Constructs a controller with the given coefficients and plant.
+ *
+ * @param plant The plant being controlled.
+ * @param plantRef The plant whose dynamics should be followed.
+ */
+ public ImplicitModelFollower(
+ LinearSystem<States, Inputs, Outputs> plant, LinearSystem<States, Inputs, Outputs> plantRef) {
+ this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB());
+ }
+
+ /**
+ * 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.
+ */
+ public ImplicitModelFollower(
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<States, States> Aref,
+ Matrix<States, Inputs> Bref) {
+ m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+ // 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.solve(A.minus(Aref)).times(-1.0);
+ m_B = B.solve(Bref);
+
+ reset();
+ }
+
+ /**
+ * Returns the control input vector u.
+ *
+ * @return The control input.
+ */
+ public Matrix<Inputs, N1> getU() {
+ 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.
+ */
+ public double getU(int i) {
+ return m_u.get(i, 0);
+ }
+
+ /** Resets the controller. */
+ public void reset() {
+ m_u.fill(0.0);
+ }
+
+ /**
+ * Returns the next output of the controller.
+ *
+ * @param x The current state x.
+ * @param u The current input for the original model.
+ * @return The next controller output.
+ */
+ public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
+ m_u = m_A.times(x).plus(m_B.times(u));
+ return m_u;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
new file mode 100644
index 0000000..46254a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -0,0 +1,270 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.InterpolatingMatrixTreeMap;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * 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.
+ *
+ * <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
+ * shown in theorem 8.7.4.
+ */
+public class LTVDifferentialDriveController {
+ private final double m_trackwidth;
+
+ // LUT from drivetrain linear velocity to LQR gain
+ private final InterpolatingMatrixTreeMap<Double, N2, N5> m_table =
+ new InterpolatingMatrixTreeMap<>();
+
+ private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
+ private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
+
+ /** States of the drivetrain system. */
+ private enum State {
+ kX(0),
+ kY(1),
+ kHeading(2),
+ kLeftVelocity(3),
+ kRightVelocity(4);
+
+ public final int value;
+
+ State(int i) {
+ this.value = i;
+ }
+ }
+
+ /**
+ * 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 in
+ * meters.
+ * @param qelems The maximum desired error tolerance for each state.
+ * @param relems The maximum desired control effort for each input.
+ * @param dt Discretization timestep in seconds.
+ */
+ public LTVDifferentialDriveController(
+ LinearSystem<N2, N2, N2> plant,
+ double trackwidth,
+ Vector<N5> qelems,
+ Vector<N2> relems,
+ double dt) {
+ m_trackwidth = trackwidth;
+
+ // Control law derivation is in section 8.7 of
+ // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+ var A =
+ new MatBuilder<>(Nat.N5(), Nat.N5())
+ .fill(
+ 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,
+ 1.0 / m_trackwidth,
+ 0.0,
+ 0.0,
+ 0.0,
+ plant.getA(0, 0),
+ plant.getA(0, 1),
+ 0.0,
+ 0.0,
+ 0.0,
+ plant.getA(1, 0),
+ plant.getA(1, 1));
+ var B =
+ new MatBuilder<>(Nat.N5(), Nat.N2())
+ .fill(
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ plant.getB(0, 0),
+ plant.getB(0, 1),
+ plant.getB(1, 0),
+ plant.getB(1, 1));
+ var Q = StateSpaceUtil.makeCostMatrix(qelems);
+ var R = StateSpaceUtil.makeCostMatrix(relems);
+
+ // dx/dt = Ax + Bu
+ // 0 = Ax + Bu
+ // Ax = -Bu
+ // x = -A⁻¹Bu
+ double maxV =
+ plant
+ .getA()
+ .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)))
+ .times(-1.0)
+ .get(0, 0);
+
+ for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
+ // The DARE is ill-conditioned if the velocity is close to zero, so don't
+ // let the system stop.
+ if (Math.abs(velocity) < 1e-4) {
+ m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
+ } else {
+ A.set(State.kY.value, State.kHeading.value, velocity);
+ m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
+ }
+ }
+ }
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ *
+ * @return True if the pose error is within tolerance of the reference.
+ */
+ public boolean atReference() {
+ return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0)
+ && Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0)
+ && Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0)
+ && Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0)
+ && Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0);
+ }
+
+ /**
+ * 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 in meters per second.
+ * @param rightVelocityTolerance Right velocity error which is tolerable in meters per second.
+ */
+ public void setTolerance(
+ Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
+ m_tolerance =
+ new MatBuilder<>(Nat.N5(), Nat.N1())
+ .fill(
+ poseTolerance.getX(),
+ poseTolerance.getY(),
+ poseTolerance.getRotation().getRadians(),
+ leftVelocityTolerance,
+ rightVelocityTolerance);
+ }
+
+ /**
+ * Returns the left and right output voltages of the LTV controller.
+ *
+ * <p>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 in meters per second.
+ * @param rightVelocity The current right velocity in meters per second.
+ * @param poseRef The desired pose.
+ * @param leftVelocityRef The desired left velocity in meters per second.
+ * @param rightVelocityRef The desired right velocity in meters per second.
+ * @return Left and right output voltages of the LTV controller.
+ */
+ public DifferentialDriveWheelVoltages calculate(
+ Pose2d currentPose,
+ double leftVelocity,
+ double rightVelocity,
+ Pose2d poseRef,
+ double leftVelocityRef,
+ double rightVelocityRef) {
+ // This implements the linear time-varying differential drive controller in
+ // theorem 9.6.3 of https://tavsys.net/controls-in-frc.
+ var x =
+ new MatBuilder<>(Nat.N5(), Nat.N1())
+ .fill(
+ currentPose.getX(),
+ currentPose.getY(),
+ currentPose.getRotation().getRadians(),
+ leftVelocity,
+ rightVelocity);
+
+ var inRobotFrame = Matrix.eye(Nat.N5());
+ inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
+ inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0)));
+ inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0)));
+ inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
+
+ var r =
+ new MatBuilder<>(Nat.N5(), Nat.N1())
+ .fill(
+ poseRef.getX(),
+ poseRef.getY(),
+ poseRef.getRotation().getRadians(),
+ leftVelocityRef,
+ rightVelocityRef);
+ m_error = r.minus(x);
+ m_error.set(
+ State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));
+
+ double velocity = (leftVelocity + rightVelocity) / 2.0;
+ var K = m_table.get(velocity);
+
+ var u = K.times(inRobotFrame).times(m_error);
+
+ return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+ }
+
+ /**
+ * Returns the left and right output voltages of the LTV controller.
+ *
+ * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+ * trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param leftVelocity The left velocity in meters per second.
+ * @param rightVelocity The right velocity in meters per second.
+ * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
+ * @return The left and right output voltages of the LTV controller.
+ */
+ public DifferentialDriveWheelVoltages calculate(
+ Pose2d currentPose,
+ double leftVelocity,
+ double rightVelocity,
+ 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.poseMeters,
+ desiredState.velocityMetersPerSecond
+ * (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)),
+ desiredState.velocityMetersPerSecond
+ * (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
new file mode 100644
index 0000000..701f21b
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -0,0 +1,226 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.InterpolatingMatrixTreeMap;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * 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.
+ *
+ * <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
+ * shown in theorem 8.9.1.
+ */
+public class LTVUnicycleController {
+ // LUT from drivetrain linear velocity to LQR gain
+ private final InterpolatingMatrixTreeMap<Double, N2, N3> m_table =
+ new InterpolatingMatrixTreeMap<>();
+
+ private Pose2d m_poseError;
+ private Pose2d m_poseTolerance;
+ private boolean m_enabled = true;
+
+ /** States of the drivetrain system. */
+ private enum State {
+ kX(0),
+ kY(1),
+ kHeading(2);
+
+ public final int value;
+
+ State(int i) {
+ this.value = i;
+ }
+ }
+
+ /**
+ * 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 in seconds.
+ */
+ public LTVUnicycleController(double dt) {
+ this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, 9.0);
+ }
+
+ /**
+ * 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 in seconds.
+ * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
+ * table. The default is 9 m/s.
+ */
+ public LTVUnicycleController(double dt, double maxVelocity) {
+ this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
+ }
+
+ /**
+ * 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 in seconds.
+ */
+ public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
+ this(qelems, relems, dt, 9.0);
+ }
+
+ /**
+ * 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 in seconds.
+ * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
+ * table. The default is 9 m/s.
+ */
+ public LTVUnicycleController(
+ Vector<N3> qelems, Vector<N2> relems, double dt, double 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]
+ var A = new Matrix<>(Nat.N3(), Nat.N3());
+ var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+ var Q = StateSpaceUtil.makeCostMatrix(qelems);
+ var R = StateSpaceUtil.makeCostMatrix(relems);
+
+ for (double velocity = -maxVelocity; velocity < maxVelocity; velocity += 0.01) {
+ // The DARE is ill-conditioned if the velocity is close to zero, so don't
+ // let the system stop.
+ if (Math.abs(velocity) < 1e-4) {
+ m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
+ } else {
+ A.set(State.kY.value, State.kHeading.value, velocity);
+ m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
+ }
+ }
+ }
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ *
+ * @return True if the pose error is within tolerance of the reference.
+ */
+ public boolean atReference() {
+ final var eTranslate = m_poseError.getTranslation();
+ final var eRotate = m_poseError.getRotation();
+ final var tolTranslate = m_poseTolerance.getTranslation();
+ final var tolRotate = m_poseTolerance.getRotation();
+ return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+ && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+ && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+ }
+
+ /**
+ * Sets the pose error which is considered tolerable for use with atReference().
+ *
+ * @param poseTolerance Pose error which is tolerable.
+ */
+ public void setTolerance(Pose2d poseTolerance) {
+ m_poseTolerance = poseTolerance;
+ }
+
+ /**
+ * Returns the linear and angular velocity outputs of the LTV controller.
+ *
+ * <p>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 in meters per second.
+ * @param angularVelocityRef The desired angular velocity in radians per second.
+ * @return The linear and angular velocity outputs of the LTV controller.
+ */
+ public ChassisSpeeds calculate(
+ Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
+ if (!m_enabled) {
+ return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
+ }
+
+ m_poseError = poseRef.relativeTo(currentPose);
+
+ var K = m_table.get(linearVelocityRef);
+ var e =
+ new MatBuilder<>(Nat.N3(), Nat.N1())
+ .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
+ var u = K.times(e);
+
+ return new ChassisSpeeds(
+ linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
+ }
+
+ /**
+ * Returns the next output of the LTV controller.
+ *
+ * <p>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.
+ * @return The linear and angular velocity outputs of the LTV controller.
+ */
+ public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+ return calculate(
+ currentPose,
+ desiredState.poseMeters,
+ desiredState.velocityMetersPerSecond,
+ desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
+ }
+
+ /**
+ * Enables and disables the controller for troubleshooting purposes.
+ *
+ * @param enabled If the controller is enabled or not.
+ */
+ public void setEnabled(boolean enabled) {
+ m_enabled = enabled;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
index 627c272..a762851 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
@@ -20,20 +20,16 @@
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
public class LinearPlantInversionFeedforward<
States extends Num, Inputs extends Num, Outputs extends Num> {
/** The current reference state. */
- @SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/** The computed feedforward. */
private Matrix<Inputs, N1> m_uff;
- @SuppressWarnings("MemberName")
private final Matrix<States, Inputs> m_B;
- @SuppressWarnings("MemberName")
private final Matrix<States, States> m_A;
/**
@@ -54,7 +50,6 @@
* @param B Continuous input matrix of the plant being controlled.
* @param dtSeconds Discretization timestep.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearPlantInversionFeedforward(
Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
@@ -143,7 +138,6 @@
* @param nextR The reference state of the future timestep (k + dt).
* @return The calculated feedforward.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index e244c5f..dce1748 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -7,7 +7,6 @@
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.Vector;
@@ -23,18 +22,14 @@
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
-@SuppressWarnings("ClassTypeParameterName")
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
/** The current reference state. */
- @SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/** The computed and capped controller output. */
- @SuppressWarnings("MemberName")
private Matrix<Inputs, N1> m_u;
// Controller gain.
- @SuppressWarnings("MemberName")
private Matrix<Inputs, States> m_K;
/**
@@ -44,6 +39,7 @@
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
+ * @throws IllegalArgumentException If the system is uncontrollable.
*/
public LinearQuadraticRegulator(
LinearSystem<States, Inputs, Outputs> plant,
@@ -66,8 +62,8 @@
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
+ * @throws IllegalArgumentException If the system is uncontrollable.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
@@ -90,8 +86,8 @@
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dtSeconds Discretization timestep.
+ * @throws IllegalArgumentException If the system is uncontrollable.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
@@ -141,8 +137,8 @@
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dtSeconds Discretization timestep.
+ * @throws IllegalArgumentException If the system is uncontrollable.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
@@ -172,24 +168,6 @@
}
/**
- * Constructs a controller with the given coefficients and plant.
- *
- * @param states The number of states.
- * @param inputs The number of inputs.
- * @param k The gain matrix.
- */
- @SuppressWarnings("ParameterName")
- public LinearQuadraticRegulator(
- Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs, States> k) {
- m_K = k;
-
- m_r = new Matrix<>(states, Nat.N1());
- m_u = new Matrix<>(inputs, Nat.N1());
-
- reset();
- }
-
- /**
* Returns the control input vector u.
*
* @return The control input.
@@ -248,7 +226,6 @@
* @param x The current state x.
* @return The next controller output.
*/
- @SuppressWarnings("ParameterName")
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
m_u = m_K.times(m_r.minus(x));
return m_u;
@@ -261,7 +238,6 @@
* @param nextR the next reference vector r.
* @return The next controller output.
*/
- @SuppressWarnings("ParameterName")
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
m_r = nextR;
return calculate(x);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 12c4175..5e82909 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -35,7 +35,7 @@
private double m_minimumInput;
- // Do the endpoints wrap around? eg. Absolute encoder
+ // Do the endpoints wrap around? e.g. Absolute encoder
private boolean m_continuous;
// The error at the time of the most recent call to calculate()
@@ -175,12 +175,39 @@
}
/**
+ * Returns the position tolerance of this controller.
+ *
+ * @return the position tolerance of the controller.
+ */
+ public double getPositionTolerance() {
+ return m_positionTolerance;
+ }
+
+ /**
+ * Returns the velocity tolerance of this controller.
+ *
+ * @return the velocity tolerance of the controller.
+ */
+ public double getVelocityTolerance() {
+ return m_velocityTolerance;
+ }
+
+ /**
* Sets the setpoint for the PIDController.
*
* @param setpoint The desired setpoint.
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
+
+ if (m_continuous) {
+ double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+ m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+ } else {
+ m_positionError = m_setpoint - m_measurement;
+ }
+
+ m_velocityError = (m_positionError - m_prevError) / m_period;
}
/**
@@ -200,18 +227,8 @@
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint() {
- double positionError;
- if (m_continuous) {
- double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
- positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
- } else {
- positionError = m_setpoint - m_measurement;
- }
-
- double velocityError = (positionError - m_prevError) / m_period;
-
- return Math.abs(positionError) < m_positionTolerance
- && Math.abs(velocityError) < m_velocityTolerance;
+ return Math.abs(m_positionError) < m_positionTolerance
+ && Math.abs(m_velocityError) < m_velocityTolerance;
}
/**
@@ -303,8 +320,7 @@
* @return The next controller output.
*/
public double calculate(double measurement, double setpoint) {
- // Set setpoint to provided value
- setSetpoint(setpoint);
+ m_setpoint = setpoint;
return calculate(measurement);
}
@@ -322,7 +338,7 @@
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError = MathUtil.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;
@@ -340,8 +356,10 @@
/** Resets the previous error and the integral term. */
public void reset() {
+ m_positionError = 0;
m_prevError = 0;
m_totalError = 0;
+ m_velocityError = 0;
}
@Override
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 3ebcbd8..02cc17d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -10,6 +10,7 @@
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
@@ -33,7 +34,6 @@
* @param Kd The derivative coefficient.
* @param constraints Velocity and acceleration constraints for goal.
*/
- @SuppressWarnings("ParameterName")
public ProfiledPIDController(
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
this(Kp, Ki, Kd, constraints, 0.02);
@@ -48,12 +48,13 @@
* @param constraints Velocity and acceleration constraints for goal.
* @param period The period between controller updates in seconds. The default is 0.02 seconds.
*/
- @SuppressWarnings("ParameterName")
public ProfiledPIDController(
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
m_controller = new PIDController(Kp, Ki, Kd, period);
m_constraints = constraints;
instances++;
+
+ SendableRegistry.add(this, "ProfiledPIDController", instances);
MathSharedStore.reportUsage(MathUsageId.kController_ProfiledPIDController, instances);
}
@@ -66,7 +67,6 @@
* @param Ki Integral coefficient
* @param Kd Differential coefficient
*/
- @SuppressWarnings("ParameterName")
public void setPID(double Kp, double Ki, double Kd) {
m_controller.setPID(Kp, Ki, Kd);
}
@@ -76,7 +76,6 @@
*
* @param Kp proportional coefficient
*/
- @SuppressWarnings("ParameterName")
public void setP(double Kp) {
m_controller.setP(Kp);
}
@@ -86,7 +85,6 @@
*
* @param Ki integral coefficient
*/
- @SuppressWarnings("ParameterName")
public void setI(double Ki) {
m_controller.setI(Ki);
}
@@ -96,7 +94,6 @@
*
* @param Kd differential coefficient
*/
- @SuppressWarnings("ParameterName")
public void setD(double Kd) {
m_controller.setD(Kd);
}
@@ -138,6 +135,24 @@
}
/**
+ * Returns the position tolerance of this controller.
+ *
+ * @return the position tolerance of the controller.
+ */
+ public double getPositionTolerance() {
+ return m_controller.getPositionTolerance();
+ }
+
+ /**
+ * Returns the velocity tolerance of this controller.
+ *
+ * @return the velocity tolerance of the controller.
+ */
+ public double getVelocityTolerance() {
+ return m_controller.getVelocityTolerance();
+ }
+
+ /**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired goal state.
@@ -282,7 +297,7 @@
*/
public double calculate(double measurement) {
if (m_controller.isContinuousInputEnabled()) {
- // Get error which is smallest distance between goal and measurement
+ // Get error which is the smallest distance between goal and measurement
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
double goalMinDistance =
MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
index a1d6237..5683987 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
@@ -33,10 +33,8 @@
* derivation and analysis.
*/
public class RamseteController {
- @SuppressWarnings("MemberName")
private final double m_b;
- @SuppressWarnings("MemberName")
private final double m_zeta;
private Pose2d m_poseError = new Pose2d();
@@ -51,7 +49,6 @@
* @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide
* more damping in response.
*/
- @SuppressWarnings("ParameterName")
public RamseteController(double b, double zeta) {
m_b = b;
m_zeta = zeta;
@@ -101,7 +98,6 @@
* @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
* @return The next controller output.
*/
- @SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
Pose2d currentPose,
Pose2d poseRef,
@@ -120,8 +116,11 @@
final double vRef = linearVelocityRefMeters;
final double omegaRef = angularVelocityRefRadiansPerSecond;
+ // k = 2ζ√(ω_ref² + b v_ref²)
double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
+ // v_cmd = v_ref cos(e_θ) + k e_x
+ // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
return new ChassisSpeeds(
vRef * m_poseError.getRotation().getCos() + k * eX,
0.0,
@@ -138,7 +137,6 @@
* @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
* @return The next controller output.
*/
- @SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
return calculate(
currentPose,
@@ -161,7 +159,6 @@
*
* @param x Value of which to take sinc(x).
*/
- @SuppressWarnings("ParameterName")
private static double sinc(double x) {
if (Math.abs(x) < 1e-9) {
return 1.0 - 1.0 / 6.0 * x * x;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
index a1e5f68..714d705 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -9,7 +9,6 @@
import edu.wpi.first.math.system.plant.LinearSystemId;
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
-@SuppressWarnings("MemberName")
public class SimpleMotorFeedforward {
public final double ks;
public final double kv;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
index a6d8b44..0f91a1d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
@@ -87,7 +87,6 @@
* @param angleStateIdx The row containing the angles.
* @return Mean of sigma points.
*/
- @SuppressWarnings("checkstyle:ParameterName")
public static <S extends Num> Matrix<S, N1> angleMean(
Matrix<S, ?> sigmas, Matrix<?, N1> Wm, int angleStateIdx) {
double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData();
@@ -115,7 +114,6 @@
* @param angleStateIdx The row containing the angles.
* @return Function returning mean of sigma points.
*/
- @SuppressWarnings("LambdaParameterName")
public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>> angleMean(
int angleStateIdx) {
return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index b199d4d..b5fe591 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -4,158 +4,109 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link edu.wpi.first.math.estimator.UnscentedKalmanFilter Unscented Kalman
- * Filter} 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 {@link edu.wpi.first.math.kinematics.DifferentialDriveOdometry}; in fact,
- * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call
- * {@link DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
+ * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
+ * latency-compensated vision measurements with differential drive encoder measurements. It is
+ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry}; in fact, if you never
+ * call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call {@link
+ * DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
* DifferentialDriveOdometry.
*
- * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
- * loops are faster than the default then you should change the {@link
- * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
- * Matrix, double) nominal delta time}.) {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
- * can be called as infrequently as you want; if you never call it then this class will behave
- * exactly like regular encoder odometry.
+ * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop.
*
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><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.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p>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.
- *
- * <p><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.
+ * <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
+ * you want; if you never call it then this class will behave exactly like regular encoder odometry.
*/
public class DifferentialDrivePoseEstimator {
- final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
- private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
- private final KalmanFilterLatencyCompensator<N5, N3, N3> m_latencyCompensator;
+ private final DifferentialDriveKinematics m_kinematics;
+ private final DifferentialDriveOdometry m_odometry;
+ private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix<N3, N3> m_visionContR;
+ private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
- * Constructs a DifferentialDrivePoseEstimator.
+ * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
+ * vision measurements.
*
+ * <p>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 current gyro angle.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
- * @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.
*/
public DifferentialDrivePoseEstimator(
+ DifferentialDriveKinematics kinematics,
Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
- Matrix<N5, N1> stateStdDevs,
- Matrix<N3, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs) {
+ double leftDistanceMeters,
+ double rightDistanceMeters,
+ Pose2d initialPoseMeters) {
this(
+ kinematics,
gyroAngle,
+ leftDistanceMeters,
+ rightDistanceMeters,
initialPoseMeters,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
+ VecBuilder.fill(0.02, 0.02, 0.01),
+ VecBuilder.fill(0.1, 0.1, 0.1));
}
/**
* Constructs a DifferentialDrivePoseEstimator.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
- * @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 nominalDtSeconds The time in seconds between each robot loop.
+ * @param kinematics A correctly-configured kinematics object for your drivetrain.
+ * @param gyroAngle The gyro angle of the robot.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
+ * @param initialPoseMeters 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.
*/
- @SuppressWarnings("ParameterName")
public DifferentialDrivePoseEstimator(
+ DifferentialDriveKinematics kinematics,
Rotation2d gyroAngle,
+ double leftDistanceMeters,
+ double rightDistanceMeters,
Pose2d initialPoseMeters,
- Matrix<N5, N1> stateStdDevs,
- Matrix<N3, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs,
- double nominalDtSeconds) {
- m_nominalDt = nominalDtSeconds;
+ Matrix<N3, N1> stateStdDevs,
+ Matrix<N3, N1> visionMeasurementStdDevs) {
+ m_kinematics = kinematics;
+ m_odometry =
+ new DifferentialDriveOdometry(
+ gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
- m_observer =
- new UnscentedKalmanFilter<>(
- Nat.N5(),
- Nat.N3(),
- this::f,
- (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
- m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
- m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
}
/**
@@ -168,109 +119,122 @@
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
- }
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
- @SuppressWarnings({"ParameterName", "MethodName"})
- private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
- // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
- var theta = x.get(2, 0);
- var toFieldRotation =
- new MatBuilder<>(Nat.N5(), Nat.N5())
- .fill(
- Math.cos(theta),
- -Math.sin(theta),
- 0,
- 0,
- 0,
- Math.sin(theta),
- Math.cos(theta),
- 0,
- 0,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0,
- 0,
- 0,
- 0,
- 1);
- return toFieldRotation.times(
- VecBuilder.fill(u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)));
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
* 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 here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
- * @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftPositionMeters The distance traveled by the left encoder.
+ * @param rightPositionMeters The distance traveled by the right encoder.
+ * @param poseMeters The position on the field that your robot is at.
*/
- public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle,
+ double leftPositionMeters,
+ double rightPositionMeters,
+ Pose2d poseMeters) {
// Reset state estimate and error covariance
- m_observer.reset();
- m_latencyCompensator.reset();
-
- m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
+ m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
+ m_poseBuffer.clear();
}
/**
- * Gets 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 in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
+ * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#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 in this case.
+ * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
+ * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
+ * or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- m_latencyCompensator.applyPastGlobalMeasurement(
- Nat.N3(),
- m_observer,
- m_nominalDt,
- StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
- m_visionCorrect,
- timestampSeconds);
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+ var sample = m_poseBuffer.getSample(timestampSeconds);
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // 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.
+ var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+ // Step 4: Convert back to Twist2d.
+ var scaledTwist =
+ new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.resetPosition(
+ sample.get().gyroAngle,
+ sample.get().leftMeters,
+ sample.get().rightMeters,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+ // pose buffer and correct odometry.
+ for (Map.Entry<Double, InterpolationRecord> entry :
+ m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+ updateWithTime(
+ entry.getKey(),
+ entry.getValue().gyroAngle,
+ entry.getValue().leftMeters,
+ entry.getValue().rightMeters);
+ }
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
+ * <p>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.
+ *
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
@@ -278,13 +242,14 @@
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
- * DifferentialDrivePoseEstimator#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 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.
+ * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that
+ * you should use {@link edu.wpi.first.wpilibj.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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -295,78 +260,127 @@
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
- * should be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+ * loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
- * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
- * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+ * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+ * @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
- Rotation2d gyroAngle,
- DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
- double distanceLeftMeters,
- double distanceRightMeters) {
+ Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return updateWithTime(
- WPIUtilJNI.now() * 1.0e-6,
- gyroAngle,
- wheelVelocitiesMetersPerSecond,
- distanceLeftMeters,
- distanceRightMeters);
+ WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
- * should be called every loop.
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+ * loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
- * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
- * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
- * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+ * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+ * @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
public Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle,
- DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters,
double distanceRightMeters) {
- double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
-
- var angle = gyroAngle.plus(m_gyroOffset);
- // Diff drive forward kinematics:
- // v_c = (v_l + v_r) / 2
- var wheelVels = wheelVelocitiesMetersPerSecond;
- var u =
- VecBuilder.fill(
- (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2,
- 0,
- angle.minus(m_previousAngle).getRadians() / dt);
- m_previousAngle = angle;
-
- var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
- m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
return getEstimatedPosition();
}
- private static Matrix<N5, N1> fillStateVector(Pose2d pose, double leftDist, double rightDist) {
- return VecBuilder.fill(
- pose.getTranslation().getX(),
- pose.getTranslation().getY(),
- pose.getRotation().getRadians(),
- leftDist,
- rightDist);
+ /**
+ * Represents an odometry record. The record contains the inputs provided as well as the pose that
+ * was observed based on these inputs, as well as the previous record and its inputs.
+ */
+ private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+ // The pose observed given the current sensor inputs and the previous pose.
+ private final Pose2d poseMeters;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distance traveled by the left encoder.
+ private final double leftMeters;
+
+ // The distance traveled by the right encoder.
+ private final double rightMeters;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param leftMeters The distance traveled by the left encoder.
+ * @param rightMeters The distanced traveled by the right encoder.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.leftMeters = leftMeters;
+ this.rightMeters = rightMeters;
+ }
+
+ /**
+ * Return the interpolated record. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @Override
+ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ // Find the new left distance.
+ var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
+
+ // Find the new right distance.
+ var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Double.compare(leftMeters, record.leftMeters) == 0
+ && Double.compare(rightMeters, record.rightMeters) == 0
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
+ }
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
index d4f9e56..5f9e52e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -34,16 +34,13 @@
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
*/
-@SuppressWarnings("ClassTypeParameterName")
public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
private final Nat<States> m_states;
private final Nat<Outputs> m_outputs;
- @SuppressWarnings("MemberName")
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
- @SuppressWarnings("MemberName")
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
@@ -53,10 +50,8 @@
private final Matrix<States, States> m_initP;
private final Matrix<Outputs, Outputs> m_contR;
- @SuppressWarnings("MemberName")
private Matrix<States, N1> m_xHat;
- @SuppressWarnings("MemberName")
private Matrix<States, States> m_P;
private double m_dtSeconds;
@@ -73,7 +68,6 @@
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
*/
- @SuppressWarnings("ParameterName")
public ExtendedKalmanFilter(
Nat<States> states,
Nat<Inputs> inputs,
@@ -111,7 +105,6 @@
* @param addFuncX A function that adds two state vectors.
* @param dtSeconds Nominal discretization timestep.
*/
- @SuppressWarnings("ParameterName")
public ExtendedKalmanFilter(
Nat<States> states,
Nat<Inputs> inputs,
@@ -133,7 +126,7 @@
m_addFuncX = addFuncX;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
- this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+ m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_dtSeconds = dtSeconds;
reset();
@@ -207,7 +200,7 @@
* Returns an element of the state estimate x-hat.
*
* @param row Row of x-hat.
- * @return the value of the state estimate x-hat at i.
+ * @return the value of the state estimate x-hat at that row.
*/
@Override
public double getXhat(int row) {
@@ -219,7 +212,6 @@
*
* @param xHat The state estimate x-hat.
*/
- @SuppressWarnings("ParameterName")
@Override
public void setXhat(Matrix<States, N1> xHat) {
m_xHat = xHat;
@@ -248,7 +240,6 @@
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
- @SuppressWarnings("ParameterName")
@Override
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
predict(u, m_f, dtSeconds);
@@ -258,10 +249,9 @@
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
- * @param f The function used to linearlize the model.
+ * @param f The function used to linearize the model.
* @param dtSeconds Timestep for prediction.
*/
- @SuppressWarnings("ParameterName")
public void predict(
Matrix<Inputs, N1> u,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
@@ -288,7 +278,6 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
- @SuppressWarnings("ParameterName")
@Override
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(m_outputs, u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
@@ -306,16 +295,15 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
- * @param R Discrete measurement noise covariance matrix.
+ * @param contR Continuous measurement noise covariance matrix.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public <Rows extends Num> void correct(
Nat<Rows> rows,
Matrix<Inputs, N1> u,
Matrix<Rows, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
- Matrix<Rows, Rows> R) {
- correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+ Matrix<Rows, Rows> contR) {
+ correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
}
/**
@@ -330,22 +318,21 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
- * @param R Discrete measurement noise covariance matrix.
+ * @param contR Continuous measurement noise covariance matrix.
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public <Rows extends Num> void correct(
Nat<Rows> rows,
Matrix<Inputs, N1> u,
Matrix<Rows, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
- Matrix<Rows, Rows> R,
+ Matrix<Rows, Rows> contR,
BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
- final var discR = Discretization.discretizeR(R, m_dtSeconds);
+ final var discR = Discretization.discretizeR(contR, m_dtSeconds);
final var S = C.times(m_P).times(C.transpose()).plus(discR);
@@ -366,7 +353,13 @@
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
- // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
- m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+ // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+ // Use Joseph form for numerical stability
+ m_P =
+ Matrix.eye(m_states)
+ .minus(K.times(C))
+ .times(m_P)
+ .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+ .plus(K.times(discR).times(K.transpose()));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index c992d2f..24d6d91 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -29,18 +29,15 @@
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
*/
-@SuppressWarnings("ClassTypeParameterName")
public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
private final Nat<States> m_states;
private final LinearSystem<States, Inputs, Outputs> m_plant;
/** The steady-state Kalman gain matrix. */
- @SuppressWarnings("MemberName")
private final Matrix<States, Outputs> m_K;
/** The state estimate. */
- @SuppressWarnings("MemberName")
private Matrix<States, N1> m_xHat;
/**
@@ -52,8 +49,8 @@
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
+ * @throws IllegalArgumentException If the system is unobservable.
*/
- @SuppressWarnings("LocalVariableName")
public KalmanFilter(
Nat<States> states,
Nat<Outputs> outputs,
@@ -172,7 +169,7 @@
* Returns an element of the state estimate x-hat.
*
* @param row Row of x-hat.
- * @return the state estimate x-hat at i.
+ * @return the state estimate x-hat at that row.
*/
public double getXhat(int row) {
return m_xHat.get(row, 0);
@@ -184,7 +181,6 @@
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
- @SuppressWarnings("ParameterName")
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
}
@@ -195,7 +191,6 @@
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
- @SuppressWarnings("ParameterName")
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
final var C = m_plant.getC();
final var D = m_plant.getD();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
index 89f35e5..15e7bd4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -33,9 +33,8 @@
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
- * @param timestampSeconds The timesnap of the state.
+ * @param timestampSeconds The timestamp of the state.
*/
- @SuppressWarnings("ParameterName")
public void addObserverState(
KalmanTypeFilter<S, I, O> observer,
Matrix<I, N1> u,
@@ -60,7 +59,6 @@
* @param globalMeasurementCorrect The function take calls correct() on the observer.
* @param timestampSeconds The timestamp of the measurement.
*/
- @SuppressWarnings("ParameterName")
public <R extends Num> void applyPastGlobalMeasurement(
Nat<R> rows,
KalmanTypeFilter<S, I, O> observer,
@@ -119,7 +117,7 @@
// Index of snapshot taken before the global measurement. Since we already
// handled the case where the index points to the first snapshot, this
- // computation is guaranteed to be nonnegative.
+ // computation is guaranteed to be non-negative.
int prevIdx = nextIdx - 1;
// Find the snapshot closest in time to global measurement
@@ -165,14 +163,12 @@
}
/** This class contains all the information about our observer at a given time. */
- @SuppressWarnings("MemberName")
public class ObserverSnapshot {
public final Matrix<S, N1> xHat;
public final Matrix<S, S> errorCovariances;
public final Matrix<I, N1> inputs;
public final Matrix<O, N1> localMeasurements;
- @SuppressWarnings("ParameterName")
private ObserverSnapshot(
KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY) {
this.xHat = observer.getXhat();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
index 3fd3957..7a100f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
@@ -8,7 +8,6 @@
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
-@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
Matrix<States, States> getP();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index d6f1075..448b8d3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -4,157 +4,101 @@
package edu.wpi.first.math.estimator;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} 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 {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
+ * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
+ * vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
+ * measurements and encoder drift. It is intended to be a drop-in replacement for {@link
+ * edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
*
- * <p>{@link MecanumDrivePoseEstimator#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 using
- * the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
- * Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
*
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
- *
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
- * position, and heading.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
*/
public class MecanumDrivePoseEstimator {
- private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
private final MecanumDriveKinematics m_kinematics;
- private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
- private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+ private final MecanumDriveOdometry m_odometry;
+ private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix<N3, N3> m_visionContR;
+ private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
- * Constructs a MecanumDrivePoseEstimator.
+ * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
+ * vision measurements.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
+ * <p>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 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 gyroAngle The current gyro angle.
+ * @param wheelPositions The distances driven by each wheel.
+ * @param initialPoseMeters The starting pose estimate.
*/
public MecanumDrivePoseEstimator(
- Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
- Matrix<N3, N1> stateStdDevs,
- Matrix<N1, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs) {
+ Rotation2d gyroAngle,
+ MecanumDriveWheelPositions wheelPositions,
+ Pose2d initialPoseMeters) {
this(
- gyroAngle,
- initialPoseMeters,
kinematics,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
+ gyroAngle,
+ wheelPositions,
+ initialPoseMeters,
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.45, 0.45, 0.45));
}
/**
* Constructs a MecanumDrivePoseEstimator.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters 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 nominalDtSeconds The time in seconds between each robot loop.
+ * @param gyroAngle The current gyro angle.
+ * @param wheelPositions The distance measured by each wheel.
+ * @param initialPoseMeters 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.
*/
- @SuppressWarnings("ParameterName")
public MecanumDrivePoseEstimator(
- Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ MecanumDriveWheelPositions wheelPositions,
+ Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
- Matrix<N1, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs,
- double nominalDtSeconds) {
- m_nominalDt = nominalDtSeconds;
-
- m_observer =
- new UnscentedKalmanFilter<>(
- Nat.N3(),
- Nat.N1(),
- (x, u) -> u,
- (x, u) -> x.extractRowVector(2),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(0),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(0),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
+ Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
- m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+ m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
+
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> x,
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
- m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
}
/**
@@ -167,85 +111,128 @@
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
* 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 library
* automatically takes care of offsetting the gyro angle.
*
- * @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The distances driven by each wheel.
+ * @param poseMeters The position on the field that your robot is at.
*/
- public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
- m_observer.reset();
- m_latencyCompensator.reset();
-
- m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
+ m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
+ m_poseBuffer.clear();
}
/**
- * Gets 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 in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
+ * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link MecanumDrivePoseEstimator#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.
+ * don't use your own time source by calling {@link
+ * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
+ * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
+ * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
+ * your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- m_latencyCompensator.applyPastGlobalMeasurement(
- Nat.N3(),
- m_observer,
- m_nominalDt,
- StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
- m_visionCorrect,
- timestampSeconds);
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+ var sample = m_poseBuffer.getSample(timestampSeconds);
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // 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.
+ var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+ // Step 4: Convert back to Twist2d.
+ var scaledTwist =
+ new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.resetPosition(
+ sample.get().gyroAngle,
+ sample.get().wheelPositions,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+ // pose buffer and correct odometry.
+ for (Map.Entry<Double, InterpolationRecord> entry :
+ m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+ updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
+ }
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
+ * <p>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.
+ *
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link MecanumDrivePoseEstimator#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 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.
+ * don't use your own time source by calling {@link
+ * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
+ * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
+ * This means that you should use {@link edu.wpi.first.wpilibj.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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -256,50 +243,141 @@
}
/**
- * 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 driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
- public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+ public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+ return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, 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 currentTimeSeconds 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 driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
- @SuppressWarnings("LocalVariableName")
public Pose2d updateWithTime(
- double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
- double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
+ double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+ m_odometry.update(gyroAngle, wheelPositions);
- var angle = gyroAngle.plus(m_gyroOffset);
- var omega = angle.minus(m_previousAngle).getRadians() / dt;
-
- var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
- var fieldRelativeVelocities =
- new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
- .rotateBy(angle);
-
- var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
- m_previousAngle = angle;
-
- var localY = VecBuilder.fill(angle.getRadians());
- m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(),
+ gyroAngle,
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters)));
return getEstimatedPosition();
}
+
+ /**
+ * Represents an odometry record. The record contains the inputs provided as well as the pose that
+ * was observed based on these inputs, as well as the previous record and its inputs.
+ */
+ private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+ // The pose observed given the current sensor inputs and the previous pose.
+ private final Pose2d poseMeters;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distances traveled by each wheel encoder.
+ private final MecanumDriveWheelPositions wheelPositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param wheelPositions The distances traveled by each wheel encoder.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.wheelPositions = wheelPositions;
+ }
+
+ /**
+ * Return the interpolated record. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @Override
+ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distances.
+ var wheels_lerp =
+ new MecanumDriveWheelPositions(
+ MathUtil.interpolate(
+ this.wheelPositions.frontLeftMeters,
+ endValue.wheelPositions.frontLeftMeters,
+ t),
+ MathUtil.interpolate(
+ this.wheelPositions.frontRightMeters,
+ endValue.wheelPositions.frontRightMeters,
+ t),
+ MathUtil.interpolate(
+ this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
+ MathUtil.interpolate(
+ this.wheelPositions.rearRightMeters,
+ endValue.wheelPositions.rearRightMeters,
+ t));
+
+ // Find the distance travelled between this measurement and the interpolated measurement.
+ var wheels_delta =
+ new MecanumDriveWheelPositions(
+ wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
+ wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
+ wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
+ wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Objects.equals(wheelPositions, record.wheelPositions)
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, wheelPositions, poseMeters);
+ }
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
index fb0628b..24835f6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -71,16 +71,15 @@
* of the filter.
*
* @param x An array of the means.
- * @param P Covariance of the filter.
- * @return Two dimensional array of sigma points. Each column contains all of the sigmas for one
+ * @param s Square-root covariance of the filter.
+ * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
* dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
- public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
+ public Matrix<S, ?> squareRootSigmaPoints(Matrix<S, N1> x, Matrix<S, S> s) {
double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+ double eta = Math.sqrt(lambda + m_states.getNum());
- var intermediate = P.times(lambda + m_states.getNum());
- var U = intermediate.lltDecompose(true); // Lower triangular
+ Matrix<S, S> U = s.times(eta);
// 2 * states + 1 by states
Matrix<S, ?> sigmas =
@@ -101,7 +100,6 @@
*
* @param beta Incorporates prior knowledge of the distribution of the mean.
*/
- @SuppressWarnings("LocalVariableName")
private void computeWeights(double beta) {
double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
double c = 0.5 / (m_states.getNum() + lambda);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 8782b7b..57451a0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -4,157 +4,103 @@
package edu.wpi.first.math.estimator;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Arrays;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link UnscentedKalmanFilter 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 {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
+ * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is intended to be a
+ * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
*
- * <p>{@link SwerveDrivePoseEstimator#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 using
- * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d,
- * Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
*
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
- * want; if you never call it, then this class will behave mostly like regular encoder odometry.
- *
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
- * position, and heading.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ * want; if you never call it, then this class will behave as regular encoder odometry.
*/
public class SwerveDrivePoseEstimator {
- private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
private final SwerveDriveKinematics m_kinematics;
- private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
- private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+ private final SwerveDriveOdometry m_odometry;
+ private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private final int m_numModules;
+ private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix<N3, N3> m_visionContR;
+ private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
- * Constructs a SwerveDrivePoseEstimator.
+ * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
+ * measurements.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
+ * <p>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 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 gyroAngle The current gyro angle.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param initialPoseMeters The starting pose estimate.
*/
public SwerveDrivePoseEstimator(
- Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
- Matrix<N3, N1> stateStdDevs,
- Matrix<N1, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs) {
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters) {
this(
- gyroAngle,
- initialPoseMeters,
kinematics,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
+ gyroAngle,
+ modulePositions,
+ initialPoseMeters,
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
}
/**
* Constructs a SwerveDrivePoseEstimator.
*
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters 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 nominalDtSeconds The time in seconds between each robot loop.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of the swerve modules.
+ * @param initialPoseMeters 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.
*/
- @SuppressWarnings("ParameterName")
public SwerveDrivePoseEstimator(
- Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
- Matrix<N1, N1> localMeasurementStdDevs,
- Matrix<N3, N1> visionMeasurementStdDevs,
- double nominalDtSeconds) {
- m_nominalDt = nominalDtSeconds;
-
- m_observer =
- new UnscentedKalmanFilter<>(
- Nat.N3(),
- Nat.N1(),
- (x, u) -> u,
- (x, u) -> x.extractRowVector(2),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(0),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(0),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
+ Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
- m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+ m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
- // Initialize vision R
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
+
+ m_numModules = modulePositions.length;
+
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> x,
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
- m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
}
/**
@@ -167,85 +113,128 @@
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
* 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 library
* automatically takes care of offsetting the gyro angle.
*
- * @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param poseMeters The position on the field that your robot is at.
*/
- public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
- m_observer.reset();
- m_latencyCompensator.reset();
-
- m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
+ m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
+ m_poseBuffer.clear();
}
/**
- * Gets 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 in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
+ * <p>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 visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link SwerveDrivePoseEstimator#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.
+ * don't use your own time source by calling {@link
+ * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
+ * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
+ * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
+ * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
+ * or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
- m_latencyCompensator.applyPastGlobalMeasurement(
- Nat.N3(),
- m_observer,
- m_nominalDt,
- StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
- m_visionCorrect,
- timestampSeconds);
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+ var sample = m_poseBuffer.getSample(timestampSeconds);
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // 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.
+ var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+ // Step 4: Convert back to Twist2d.
+ var scaledTwist =
+ new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.resetPosition(
+ sample.get().gyroAngle,
+ sample.get().modulePositions,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+ // pose buffer and correct odometry.
+ for (Map.Entry<Double, InterpolationRecord> entry :
+ m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+ updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions);
+ }
}
/**
- * 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.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
+ * <p>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.
+ *
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
- * don't use your own time source by calling {@link SwerveDrivePoseEstimator#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 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.
+ * don't use your own time source by calling {@link
+ * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
+ * you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
+ * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
+ * This means that you should use {@link edu.wpi.first.wpilibj.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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
@@ -256,50 +245,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.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
- public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+ public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+ return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, 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 currentTimeSeconds 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.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
- @SuppressWarnings("LocalVariableName")
public Pose2d updateWithTime(
- double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
- double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
+ double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+ if (modulePositions.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
- var angle = gyroAngle.plus(m_gyroOffset);
- var omega = angle.minus(m_previousAngle).getRadians() / dt;
+ var internalModulePositions = new SwerveModulePosition[m_numModules];
- var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
- var fieldRelativeVelocities =
- new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
- .rotateBy(angle);
+ for (int i = 0; i < m_numModules; i++) {
+ internalModulePositions[i] =
+ new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
+ }
- var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
- m_previousAngle = angle;
+ m_odometry.update(gyroAngle, internalModulePositions);
- var localY = VecBuilder.fill(angle.getRadians());
- m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
return getEstimatedPosition();
}
+
+ /**
+ * Represents an odometry record. The record contains the inputs provided as well as the pose that
+ * was observed based on these inputs, as well as the previous record and its inputs.
+ */
+ private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+ // The pose observed given the current sensor inputs and the previous pose.
+ private final Pose2d poseMeters;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distances and rotations measured at each module.
+ private final SwerveModulePosition[] modulePositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param wheelPositions The distances and rotations measured at each wheel.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.modulePositions = modulePositions;
+ }
+
+ /**
+ * Return the interpolated record. This object is assumed to be the starting position, or lower
+ * bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @Override
+ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distances.
+ var modulePositions = new SwerveModulePosition[m_numModules];
+
+ // Find the distance travelled between this measurement and the interpolated measurement.
+ var moduleDeltas = new SwerveModulePosition[m_numModules];
+
+ for (int i = 0; i < m_numModules; i++) {
+ double ds =
+ MathUtil.interpolate(
+ this.modulePositions[i].distanceMeters,
+ endValue.modulePositions[i].distanceMeters,
+ t);
+ Rotation2d theta =
+ this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
+ modulePositions[i] = new SwerveModulePosition(ds, theta);
+ moduleDeltas[i] =
+ new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
+ }
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Arrays.equals(modulePositions, record.modulePositions)
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
+ }
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index ffc2c15..d668564 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -14,6 +14,7 @@
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import java.util.function.BiFunction;
+import org.ejml.dense.row.decomposition.qr.QRDecompositionHouseholder_DDRM;
import org.ejml.simple.SimpleMatrix;
/**
@@ -33,8 +34,10 @@
* <p>For more on the underlying math, read
* 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.
*/
-@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
private final Nat<States> m_states;
@@ -50,7 +53,7 @@
private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
private Matrix<States, N1> m_xHat;
- private Matrix<States, States> m_P;
+ private Matrix<States, States> m_S;
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private Matrix<States, ?> m_sigmasF;
@@ -69,7 +72,6 @@
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDtSeconds Nominal discretization timestep.
*/
- @SuppressWarnings("LambdaParameterName")
public UnscentedKalmanFilter(
Nat<States> states,
Nat<Outputs> outputs,
@@ -115,7 +117,6 @@
* @param addFuncX A function that adds two state vectors.
* @param nominalDtSeconds Nominal discretization timestep.
*/
- @SuppressWarnings("ParameterName")
public UnscentedKalmanFilter(
Nat<States> states,
Nat<Outputs> outputs,
@@ -151,15 +152,16 @@
reset();
}
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
- static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
- Nat<S> s,
- Nat<C> dim,
- Matrix<C, ?> sigmas,
- Matrix<?, N1> Wm,
- Matrix<?, N1> Wc,
- BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
- BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc) {
+ static <S extends Num, C extends Num>
+ Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
+ Nat<S> s,
+ Nat<C> dim,
+ Matrix<C, ?> sigmas,
+ Matrix<?, N1> Wm,
+ Matrix<?, N1> Wc,
+ BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+ BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
+ Matrix<C, C> squareRootR) {
if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
throw new IllegalArgumentException(
"Sigmas must be covDim by 2 * states + 1! Got "
@@ -184,28 +186,64 @@
// k=1
Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
- // New covariance is the sum of the outer product of the residuals times the
- // weights
- Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
- for (int i = 0; i < 2 * s.getNum() + 1; i++) {
- // y[:, i] = sigmas[:, i] - x
- y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
+ Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
+ for (int i = 0; i < 2 * s.getNum(); i++) {
+ Sbar.setColumn(
+ i,
+ residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
}
- Matrix<C, C> P =
- y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
- .times(Matrix.changeBoundsUnchecked(y.transpose()));
+ Sbar.assignBlock(0, 2 * s.getNum(), squareRootR);
- return new Pair<>(x, P);
+ QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
+ var qrStorage = Sbar.transpose().getStorage();
+
+ if (!qr.decompose(qrStorage.getDDRM())) {
+ throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
+ }
+
+ Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
+ newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
+
+ return new Pair<>(x, newS);
}
/**
- * Returns the error covariance matrix P.
+ * Returns the square-root error covariance matrix S.
+ *
+ * @return the square-root error covariance matrix S.
+ */
+ public Matrix<States, States> getS() {
+ return m_S;
+ }
+
+ /**
+ * Returns an element of the square-root error covariance matrix S.
+ *
+ * @param row Row of S.
+ * @param col Column of S.
+ * @return the value of the square-root error covariance matrix S at (i, j).
+ */
+ public double getS(int row, int col) {
+ return m_S.get(row, col);
+ }
+
+ /**
+ * Sets the entire square-root error covariance matrix S.
+ *
+ * @param newS The new value of S to use.
+ */
+ public void setS(Matrix<States, States> newS) {
+ m_S = newS;
+ }
+
+ /**
+ * Returns the reconstructed error covariance matrix P.
*
* @return the error covariance matrix P.
*/
@Override
public Matrix<States, States> getP() {
- return m_P;
+ return m_S.transpose().times(m_S);
}
/**
@@ -214,10 +252,12 @@
* @param row Row of P.
* @param col Column of P.
* @return the value of the error covariance matrix P at (i, j).
+ * @throws UnsupportedOperationException indexing into the reconstructed P matrix is not supported
*/
@Override
public double getP(int row, int col) {
- return m_P.get(row, col);
+ throw new UnsupportedOperationException(
+ "indexing into the reconstructed P matrix is not supported");
}
/**
@@ -227,7 +267,7 @@
*/
@Override
public void setP(Matrix<States, States> newP) {
- m_P = newP;
+ m_S = newP.lltDecompose(false);
}
/**
@@ -244,7 +284,7 @@
* Returns an element of the state estimate x-hat.
*
* @param row Row of x-hat.
- * @return the value of the state estimate x-hat at i.
+ * @return the value of the state estimate x-hat at 'i'.
*/
@Override
public double getXhat(int row) {
@@ -256,7 +296,6 @@
*
* @param xHat The state estimate x-hat.
*/
- @SuppressWarnings("ParameterName")
@Override
public void setXhat(Matrix<States, N1> xHat) {
m_xHat = xHat;
@@ -277,7 +316,7 @@
@Override
public void reset() {
m_xHat = new Matrix<>(m_states, Nat.N1());
- m_P = new Matrix<>(m_states, m_states);
+ m_S = new Matrix<>(m_states, m_states);
m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
}
@@ -287,15 +326,15 @@
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
@Override
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
// Discretize Q before projecting mean and covariance forward
Matrix<States, States> contA =
NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+ var squareRootDiscQ = discQ.lltDecompose(true);
- var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+ var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
Matrix<States, N1> x = sigmas.extractColumnVector(i);
@@ -304,17 +343,18 @@
}
var ret =
- unscentedTransform(
+ squareRootUnscentedTransform(
m_states,
m_states,
m_sigmasF,
m_pts.getWm(),
m_pts.getWc(),
m_meanFuncX,
- m_residualFuncX);
+ m_residualFuncX,
+ squareRootDiscQ);
m_xHat = ret.getFirst();
- m_P = ret.getSecond().plus(discQ);
+ m_S = ret.getSecond();
m_dtSeconds = dtSeconds;
}
@@ -324,7 +364,6 @@
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
- @SuppressWarnings("ParameterName")
@Override
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(
@@ -345,7 +384,6 @@
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Measurement noise covariance matrix (continuous-time).
*/
- @SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
public <R extends Num> void correct(
Nat<R> rows,
Matrix<Inputs, N1> u,
@@ -382,7 +420,6 @@
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
public <R extends Num> void correct(
Nat<R> rows,
Matrix<Inputs, N1> u,
@@ -394,10 +431,11 @@
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var discR = Discretization.discretizeR(R, m_dtSeconds);
+ final var squareRootDiscR = discR.lltDecompose(true);
// Transform sigma points into measurement space
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
- var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+ var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
sigmasH.setColumn(i, hRet);
@@ -405,10 +443,17 @@
// Mean and covariance of prediction passed through unscented transform
var transRet =
- unscentedTransform(
- m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
+ squareRootUnscentedTransform(
+ m_states,
+ rows,
+ sigmasH,
+ m_pts.getWm(),
+ m_pts.getWc(),
+ meanFuncY,
+ residualFuncY,
+ squareRootDiscR);
var yHat = transRet.getFirst();
- var Py = transRet.getSecond().plus(discR);
+ var Sy = transRet.getSecond();
// Compute cross covariance of the state and the measurements
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
@@ -420,17 +465,20 @@
Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
}
- // 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}ᵀ)ᵀ
- Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
+ // K = (P_{xy} / S_yᵀ) / S_y
+ // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+ // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+ Matrix<States, R> K =
+ Sy.transpose()
+ .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
+ .transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
- // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
- m_P = m_P.minus(K.times(Py).times(K.transpose()));
+ Matrix<States, R> U = K.times(Sy);
+ for (int i = 0; i < rows.getNum(); i++) {
+ m_S.rankUpdate(U.extractColumnVector(i), -1, false);
+ }
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 5eac0b3..98dd4e1 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -128,9 +128,7 @@
}
double[] ffGains = new double[taps];
- for (int i = 0; i < ffGains.length; i++) {
- ffGains[i] = 1.0 / taps;
- }
+ Arrays.fill(ffGains, 1.0 / taps);
double[] fbGains = new double[0];
@@ -144,20 +142,17 @@
* <p>Stencil points are the indices of the samples to use in the finite 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.
+ * stream-based online filtering (e.g., taking derivative of encoder samples in real-time).
*
* @param derivative The order of the derivative to compute.
- * @param samples The number of samples to use to compute the given derivative. This must be one
- * more than the order of derivative or higher.
- * @param stencil List of stencil points.
+ * @param stencil List of stencil points. Its length is the number of samples to use to compute
+ * the given derivative. This must be one more than the order of the derivative or higher.
* @param period The period in seconds between samples taken by the user.
* @return Linear filter.
* @throws IllegalArgumentException if derivative < 1, samples <= 0, or derivative >=
* samples.
*/
- @SuppressWarnings("LocalVariableName")
- public static LinearFilter finiteDifference(
- int derivative, int samples, int[] stencil, double period) {
+ public static LinearFilter finiteDifference(int derivative, int[] stencil, double period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//
@@ -170,7 +165,7 @@
// [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ] [δₙ₋₁,d]
//
// where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
- // vector a in reverse order divided by hᵈ.
+ // vector 'a' in reverse order divided by hᵈ.
//
// The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
@@ -179,6 +174,8 @@
"Order of derivative must be greater than or equal to one.");
}
+ int samples = stencil.length;
+
if (samples <= 0) {
throw new IllegalArgumentException("Number of samples must be greater than zero.");
}
@@ -236,7 +233,7 @@
stencil[i] = -(samples - 1) + i;
}
- return finiteDifference(derivative, samples, stencil, period);
+ return finiteDifference(derivative, stencil, period);
}
/** Reset the filter state. */
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
index d3aa7d8..668b8b1 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
@@ -14,29 +14,50 @@
* edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
*/
public class SlewRateLimiter {
- private final double m_rateLimit;
+ private final double m_positiveRateLimit;
+ private final double m_negativeRateLimit;
private double m_prevVal;
private double m_prevTime;
/**
- * 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 rateLimit The rate-of-change limit, in units per second.
+ * @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.
*/
- public SlewRateLimiter(double rateLimit, double initialValue) {
- m_rateLimit = rateLimit;
+ public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
+ m_positiveRateLimit = positiveRateLimit;
+ m_negativeRateLimit = negativeRateLimit;
m_prevVal = initialValue;
m_prevTime = WPIUtilJNI.now() * 1e-6;
}
/**
- * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+ * 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, in units per second.
+ * @param initalValue The initial value of the input.
+ * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double
+ * initalValue) instead.
+ */
+ @Deprecated(since = "2023", forRemoval = true)
+ public SlewRateLimiter(double rateLimit, double initalValue) {
+ this(rateLimit, -rateLimit, initalValue);
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+ * -rateLimit.
*
* @param rateLimit The rate-of-change limit, in units per second.
*/
public SlewRateLimiter(double rateLimit) {
- this(rateLimit, 0);
+ this(rateLimit, -rateLimit, 0);
}
/**
@@ -49,7 +70,10 @@
double currentTime = WPIUtilJNI.now() * 1e-6;
double elapsedTime = currentTime - m_prevTime;
m_prevVal +=
- MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime);
+ MathUtil.clamp(
+ input - m_prevVal,
+ m_negativeRateLimit * elapsedTime,
+ m_positiveRateLimit * elapsedTime);
m_prevTime = currentTime;
return m_prevVal;
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
new file mode 100644
index 0000000..d6033f8
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+
+/** A class representing a coordinate system axis within the NWU coordinate system. */
+public class CoordinateAxis {
+ private static final CoordinateAxis m_n = new CoordinateAxis(1.0, 0.0, 0.0);
+ private static final CoordinateAxis m_s = new CoordinateAxis(-1.0, 0.0, 0.0);
+ private static final CoordinateAxis m_e = new CoordinateAxis(0.0, -1.0, 0.0);
+ private static final CoordinateAxis m_w = new CoordinateAxis(0.0, 1.0, 0.0);
+ private static final CoordinateAxis m_u = new CoordinateAxis(0.0, 0.0, 1.0);
+ private static final CoordinateAxis m_d = new CoordinateAxis(0.0, 0.0, -1.0);
+
+ final Vector<N3> m_axis;
+
+ /**
+ * 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.
+ */
+ public CoordinateAxis(double x, double y, double z) {
+ double norm = Math.sqrt(x * x + y * y + z * z);
+ m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to +X in the NWU coordinate system.
+ */
+ public static CoordinateAxis N() {
+ return m_n;
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to -X in the NWU coordinate system.
+ */
+ public static CoordinateAxis S() {
+ return m_s;
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
+ */
+ public static CoordinateAxis E() {
+ return m_e;
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
+ */
+ public static CoordinateAxis W() {
+ return m_w;
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
+ */
+ public static CoordinateAxis U() {
+ return m_u;
+ }
+
+ /**
+ * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+ *
+ * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
+ */
+ public static CoordinateAxis D() {
+ return m_d;
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
new file mode 100644
index 0000000..5733177
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
@@ -0,0 +1,130 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+
+/** A helper class that converts Pose3d objects between different standard coordinate frames. */
+public class CoordinateSystem {
+ private static final CoordinateSystem m_nwu =
+ new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
+ private static final CoordinateSystem m_edn =
+ new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
+ private static final CoordinateSystem m_ned =
+ new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
+
+ // Rotation from this coordinate system to NWU coordinate system
+ private final Rotation3d m_rotation;
+
+ /**
+ * 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 IllegalArgumentException if the coordinate system isn't special orthogonal
+ */
+ public CoordinateSystem(
+ CoordinateAxis positiveX, CoordinateAxis positiveY, 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.
+ var R = new Matrix<>(Nat.N3(), Nat.N3());
+ R.assignBlock(0, 0, positiveX.m_axis);
+ R.assignBlock(0, 1, positiveY.m_axis);
+ R.assignBlock(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 = new Rotation3d(R);
+ }
+
+ /**
+ * Returns an instance of the North-West-Up (NWU) coordinate system.
+ *
+ * <p>The +X axis is north, the +Y axis is west, and the +Z axis is up.
+ *
+ * @return An instance of the North-West-Up (NWU) coordinate system.
+ */
+ public static CoordinateSystem NWU() {
+ return m_nwu;
+ }
+
+ /**
+ * Returns an instance of the East-Down-North (EDN) coordinate system.
+ *
+ * <p>The +X axis is east, the +Y axis is down, and the +Z axis is north.
+ *
+ * @return An instance of the East-Down-North (EDN) coordinate system.
+ */
+ public static CoordinateSystem EDN() {
+ return m_edn;
+ }
+
+ /**
+ * Returns an instance of the North-East-Down (NED) coordinate system.
+ *
+ * <p>The +X axis is north, the +Y axis is east, and the +Z axis is down.
+ *
+ * @return An instance of the North-East-Down (NED) coordinate system.
+ */
+ public static CoordinateSystem NED() {
+ return m_ned;
+ }
+
+ /**
+ * Converts the given translation from one coordinate system to another.
+ *
+ * @param translation The translation to convert.
+ * @param from The coordinate system the pose starts in.
+ * @param to The coordinate system to which to convert.
+ * @return The given translation in the desired coordinate system.
+ */
+ public static Translation3d convert(
+ Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
+ return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+ }
+
+ /**
+ * 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.
+ */
+ public static Rotation3d convert(
+ Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
+ return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+ }
+
+ /**
+ * 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.
+ */
+ public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
+ return new Pose3d(
+ convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, 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.
+ */
+ public static Transform3d convert(
+ Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+ return new Transform3d(
+ convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index f64eff4..bce832e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -11,17 +11,14 @@
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
-/** Represents a 2d pose containing translational and rotational elements. */
+/** Represents a 2D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d implements Interpolatable<Pose2d> {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
- /**
- * Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
- * Rotation{0})
- */
+ /** Constructs a pose at the origin facing toward the positive X axis. */
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
@@ -42,8 +39,7 @@
}
/**
- * 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.
@@ -57,8 +53,11 @@
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
- * <p>The matrix multiplication is as follows [x_new] [cos, -sin, 0][transform.x] [y_new] += [sin,
- * cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]
+ * <pre>
+ * [x_new] [cos, -sin, 0][transform.x]
+ * [y_new] += [sin, cos, 0][transform.y]
+ * [t_new] [ 0, 0, 1][transform.t]
+ * </pre>
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -117,6 +116,26 @@
}
/**
+ * Multiplies the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Pose2d.
+ */
+ public Pose2d times(double scalar) {
+ return new Pose2d(m_translation.times(scalar), m_rotation.times(scalar));
+ }
+
+ /**
+ * Divides the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Pose2d.
+ */
+ public Pose2d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
@@ -126,11 +145,11 @@
public Pose2d transformBy(Transform2d other) {
return new Pose2d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
- m_rotation.plus(other.getRotation()));
+ other.getRotation().plus(m_rotation));
}
/**
- * Returns the other pose relative to the current pose.
+ * Returns the current pose relative to the given pose.
*
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
@@ -160,8 +179,8 @@
*
* @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)}
+ * degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0,
+ * Units.degreesToRadians(0.5)).
* @return The new pose of the robot.
*/
public Pose2d exp(Twist2d twist) {
@@ -190,8 +209,8 @@
}
/**
- * Returns a Twist2d that maps this pose to the end pose. If c is the output of a.Log(b), then
- * a.Exp(c) would yield b.
+ * Returns a Twist2d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
+ * then {@code a.Exp(c)} would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
@@ -245,7 +264,6 @@
}
@Override
- @SuppressWarnings("ParameterName")
public Pose2d interpolate(Pose2d endValue, double t) {
if (t < 0) {
return this;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
new file mode 100644
index 0000000..8e3a3fe
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -0,0 +1,364 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+
+/** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose3d implements Interpolatable<Pose3d> {
+ private final Translation3d m_translation;
+ private final Rotation3d m_rotation;
+
+ /** Constructs a pose at the origin facing toward the positive X axis. */
+ public Pose3d() {
+ m_translation = new Translation3d();
+ m_rotation = new Rotation3d();
+ }
+
+ /**
+ * 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.
+ */
+ @JsonCreator
+ public Pose3d(
+ @JsonProperty(required = true, value = "translation") Translation3d translation,
+ @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
+ m_translation = translation;
+ m_rotation = 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.
+ */
+ public Pose3d(double x, double y, double z, Rotation3d rotation) {
+ m_translation = new Translation3d(x, y, z);
+ m_rotation = rotation;
+ }
+
+ /**
+ * Constructs a 3D pose from a 2D pose in the X-Y plane.
+ *
+ * @param pose The 2D pose.
+ */
+ public Pose3d(Pose2d pose) {
+ m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);
+ m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians());
+ }
+
+ /**
+ * 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.
+ */
+ public Pose3d plus(Transform3d other) {
+ return transformBy(other);
+ }
+
+ /**
+ * 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.
+ */
+ public Transform3d minus(Pose3d other) {
+ final var pose = this.relativeTo(other);
+ return new Transform3d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the pose.
+ */
+ @JsonProperty
+ public Translation3d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the X component of the pose's translation.
+ *
+ * @return The x component of the pose's translation.
+ */
+ public double getX() {
+ return m_translation.getX();
+ }
+
+ /**
+ * Returns the Y component of the pose's translation.
+ *
+ * @return The y component of the pose's translation.
+ */
+ public double getY() {
+ return m_translation.getY();
+ }
+
+ /**
+ * Returns the Z component of the pose's translation.
+ *
+ * @return The z component of the pose's translation.
+ */
+ public double getZ() {
+ return m_translation.getZ();
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return The rotational component of the pose.
+ */
+ @JsonProperty
+ public Rotation3d getRotation() {
+ return m_rotation;
+ }
+
+ /**
+ * Multiplies the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Pose3d.
+ */
+ public Pose3d times(double scalar) {
+ return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar));
+ }
+
+ /**
+ * Divides the current pose by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Pose3d.
+ */
+ public Pose3d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
+ * 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.
+ */
+ public Pose3d transformBy(Transform3d other) {
+ return new Pose3d(
+ m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+ other.getRotation().plus(m_rotation));
+ }
+
+ /**
+ * Returns the current pose relative to the given pose.
+ *
+ * <p>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.
+ */
+ public Pose3d relativeTo(Pose3d other) {
+ var transform = new Transform3d(other, this);
+ return new Pose3d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Obtain a new Pose3d from a (constant curvature) velocity.
+ *
+ * <p>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.
+ *
+ * <p>"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, 0.0, 0.0, new new
+ * Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
+ * @return The new pose of the robot.
+ */
+ public Pose3d exp(Twist3d twist) {
+ final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
+ final var OmegaSq = Omega.times(Omega);
+
+ double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
+
+ // Get left Jacobian of SO3. See first line in right column of
+ // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+ Matrix<N3, N3> J;
+ if (thetaSq < 1E-9 * 1E-9) {
+ // J = I + 0.5ω
+ J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
+ } else {
+ double theta = Math.sqrt(thetaSq);
+ // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
+ J =
+ Matrix.eye(Nat.N3())
+ .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
+ .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
+ }
+
+ // Get translation component
+ final var translation =
+ J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
+
+ final var transform =
+ new Transform3d(
+ new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
+ new Rotation3d(twist.rx, twist.ry, twist.rz));
+
+ return this.plus(transform);
+ }
+
+ /**
+ * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
+ * then {@code a.Exp(c)} would yield b.
+ *
+ * @param end The end pose for the transformation.
+ * @return The twist that maps this to end.
+ */
+ public Twist3d log(Pose3d end) {
+ final var transform = end.relativeTo(this);
+
+ final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
+
+ final var Omega = rotationVectorToMatrix(rotVec);
+ final var OmegaSq = Omega.times(Omega);
+
+ double thetaSq =
+ rotVec.get(0, 0) * rotVec.get(0, 0)
+ + rotVec.get(1, 0) * rotVec.get(1, 0)
+ + rotVec.get(2, 0) * rotVec.get(2, 0);
+
+ // Get left Jacobian inverse of SO3. See fourth line in right column of
+ // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+ Matrix<N3, N3> Jinv;
+ if (thetaSq < 1E-9 * 1E-9) {
+ // J⁻¹ = I − 0.5ω + 1/12 ω²
+ Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
+ } else {
+ double theta = Math.sqrt(thetaSq);
+ double halfTheta = 0.5 * theta;
+
+ // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
+ Jinv =
+ Matrix.eye(Nat.N3())
+ .minus(Omega.times(0.5))
+ .plus(
+ OmegaSq.times(
+ (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
+ }
+
+ // Get dtranslation component
+ final var dtranslation =
+ Jinv.times(
+ new MatBuilder<>(Nat.N3(), Nat.N1())
+ .fill(transform.getX(), transform.getY(), transform.getZ()));
+
+ return new Twist3d(
+ dtranslation.get(0, 0),
+ dtranslation.get(1, 0),
+ dtranslation.get(2, 0),
+ rotVec.get(0, 0),
+ rotVec.get(1, 0),
+ rotVec.get(2, 0));
+ }
+
+ /**
+ * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+ *
+ * @return A Pose2d representing this Pose3d projected into the X-Y plane.
+ */
+ public Pose2d toPose2d() {
+ return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Pose3d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Pose3d) {
+ return ((Pose3d) obj).m_translation.equals(m_translation)
+ && ((Pose3d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+
+ @Override
+ public Pose3d interpolate(Pose3d endValue, double t) {
+ if (t < 0) {
+ return this;
+ } else if (t >= 1) {
+ return endValue;
+ } else {
+ var twist = this.log(endValue);
+ var scaledTwist =
+ new Twist3d(
+ twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
+ return this.exp(scaledTwist);
+ }
+ }
+
+ /**
+ * Applies the hat operator to a rotation vector.
+ *
+ * <p>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.
+ */
+ private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
+ // Given a rotation vector <a, b, c>,
+ // [ 0 -c b]
+ // Omega = [ c 0 -a]
+ // [-b a 0]
+ return new MatBuilder<>(Nat.N3(), Nat.N3())
+ .fill(
+ 0.0,
+ -rotation.get(2, 0),
+ rotation.get(1, 0),
+ rotation.get(2, 0),
+ 0.0,
+ -rotation.get(0, 0),
+ -rotation.get(1, 0),
+ rotation.get(0, 0),
+ 0.0);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
new file mode 100644
index 0000000..cadfaa4
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -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.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Quaternion {
+ private final double m_r;
+ private final Vector<N3> m_v;
+
+ /** Constructs a quaternion with a default angle of 0 degrees. */
+ public Quaternion() {
+ m_r = 1.0;
+ m_v = VecBuilder.fill(0.0, 0.0, 0.0);
+ }
+
+ /**
+ * 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.
+ */
+ @JsonCreator
+ public Quaternion(
+ @JsonProperty(required = true, value = "W") double w,
+ @JsonProperty(required = true, value = "X") double x,
+ @JsonProperty(required = true, value = "Y") double y,
+ @JsonProperty(required = true, value = "Z") double z) {
+ m_r = w;
+ m_v = VecBuilder.fill(x, y, z);
+ }
+
+ /**
+ * Multiply with another quaternion.
+ *
+ * @param other The other quaternion.
+ * @return The quaternion product.
+ */
+ public Quaternion times(Quaternion other) {
+ // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+ final var r1 = m_r;
+ final var v1 = m_v;
+ final var r2 = other.m_r;
+ final var v2 = other.m_v;
+
+ // v₁ x v₂
+ var cross =
+ VecBuilder.fill(
+ v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
+ v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
+ v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
+
+ // v = r₁v₂ + r₂v₁ + v₁ x v₂
+ final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
+
+ return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
+ }
+
+ /**
+ * Checks equality between this Quaternion and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Quaternion) {
+ var other = (Quaternion) obj;
+
+ return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_r, m_v);
+ }
+
+ /**
+ * Returns the inverse of the quaternion.
+ *
+ * @return The inverse quaternion.
+ */
+ public Quaternion inverse() {
+ return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
+ }
+
+ /**
+ * Normalizes the quaternion.
+ *
+ * @return The normalized quaternion.
+ */
+ public Quaternion normalize() {
+ double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+ if (norm == 0.0) {
+ return new Quaternion();
+ } else {
+ return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
+ }
+ }
+
+ /**
+ * Returns W component of the quaternion.
+ *
+ * @return W component of the quaternion.
+ */
+ @JsonProperty(value = "W")
+ public double getW() {
+ return m_r;
+ }
+
+ /**
+ * Returns X component of the quaternion.
+ *
+ * @return X component of the quaternion.
+ */
+ @JsonProperty(value = "X")
+ public double getX() {
+ return m_v.get(0, 0);
+ }
+
+ /**
+ * Returns Y component of the quaternion.
+ *
+ * @return Y component of the quaternion.
+ */
+ @JsonProperty(value = "Y")
+ public double getY() {
+ return m_v.get(1, 0);
+ }
+
+ /**
+ * Returns Z component of the quaternion.
+ *
+ * @return Z component of the quaternion.
+ */
+ @JsonProperty(value = "Z")
+ public double getZ() {
+ return m_v.get(2, 0);
+ }
+
+ /**
+ * Returns the rotation vector representation of this quaternion.
+ *
+ * <p>This is also the log operator of SO(3).
+ *
+ * @return The rotation vector representation of this quaternion.
+ */
+ public Vector<N3> toRotationVector() {
+ // 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 m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
+ } else {
+ if (getW() < 0.0) {
+ return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
+ } else {
+ return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+ }
+ }
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 08e5438..5be6156 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,9 +10,16 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.util.Units;
import java.util.Objects;
-/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
+/**
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ *
+ * <p>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.
+ */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation2d implements Interpolatable<Rotation2d> {
@@ -28,7 +35,7 @@
}
/**
- * Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
+ * Constructs a Rotation2d with the given radian value.
*
* @param value The value of the angle in radians.
*/
@@ -58,6 +65,16 @@
}
/**
+ * Constructs and returns a Rotation2d with the given radian value.
+ *
+ * @param radians The value of the angle in radians.
+ * @return The rotation object with the desired angle value.
+ */
+ public static Rotation2d fromRadians(double radians) {
+ return new Rotation2d(radians);
+ }
+
+ /**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
@@ -68,9 +85,20 @@
}
/**
+ * Constructs and returns a Rotation2d with the given number of rotations.
+ *
+ * @param rotations The value of the angle in rotations.
+ * @return The rotation object with the desired angle value.
+ */
+ public static Rotation2d fromRotations(double rotations) {
+ return new Rotation2d(Units.rotationsToRadians(rotations));
+ }
+
+ /**
* Adds two rotations together, with the result being bounded between -pi and pi.
*
- * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/2}
+ * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals
+ * <code>Rotation2d(Math.PI/2.0)</code>
*
* @param other The rotation to add.
* @return The sum of the two rotations.
@@ -82,7 +110,8 @@
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
- * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = Rotation2d{-pi/2}
+ * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
+ * equals <code>Rotation2d(-Math.PI/2.0)</code>
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
@@ -112,6 +141,16 @@
}
/**
+ * Divides the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Rotation2d.
+ */
+ public Rotation2d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* <p>The matrix multiplication is as follows:
@@ -131,9 +170,10 @@
}
/**
- * Returns the radian value of the rotation.
+ * Returns the radian value of the Rotation2d.
*
- * @return The radian value of the rotation.
+ * @return The radian value of the Rotation2d.
+ * @see edu.wpi.first.math.MathUtil#angleModulus(double) to constrain the angle within (-pi, pi]
*/
@JsonProperty
public double getRadians() {
@@ -141,36 +181,47 @@
}
/**
- * Returns the degree value of the rotation.
+ * Returns the degree value of the Rotation2d.
*
- * @return The degree value of the rotation.
+ * @return The degree value of the Rotation2d.
+ * @see edu.wpi.first.math.MathUtil#inputModulus(double, double, double) to constrain the angle
+ * within (-180, 180]
*/
public double getDegrees() {
return Math.toDegrees(m_value);
}
/**
- * Returns the cosine of the rotation.
+ * Returns the number of rotations of the Rotation2d.
*
- * @return The cosine of the rotation.
+ * @return The number of rotations of the Rotation2d.
+ */
+ public double getRotations() {
+ return Units.radiansToRotations(m_value);
+ }
+
+ /**
+ * Returns the cosine of the Rotation2d.
+ *
+ * @return The cosine of the Rotation2d.
*/
public double getCos() {
return m_cos;
}
/**
- * Returns the sine of the rotation.
+ * Returns the sine of the Rotation2d.
*
- * @return The sine of the rotation.
+ * @return The sine of the Rotation2d.
*/
public double getSin() {
return m_sin;
}
/**
- * Returns the tangent of the rotation.
+ * Returns the tangent of the Rotation2d.
*
- * @return The tangent of the rotation.
+ * @return The tangent of the Rotation2d.
*/
public double getTan() {
return m_sin / m_cos;
@@ -202,8 +253,7 @@
}
@Override
- @SuppressWarnings("ParameterName")
public Rotation2d interpolate(Rotation2d endValue, double t) {
- return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
+ return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
new file mode 100644
index 0000000..3226e31
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -0,0 +1,403 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+
+/** A rotation in a 3D coordinate frame represented by a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Rotation3d implements Interpolatable<Rotation3d> {
+ private Quaternion m_q = new Quaternion();
+
+ /** Constructs a Rotation3d with a default angle of 0 degrees. */
+ public Rotation3d() {}
+
+ /**
+ * Constructs a Rotation3d from a quaternion.
+ *
+ * @param q The quaternion.
+ */
+ @JsonCreator
+ public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
+ m_q = q.normalize();
+ }
+
+ /**
+ * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+ *
+ * <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
+ * than the body frame.
+ *
+ * <p>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) in radians.
+ * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians.
+ * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians.
+ */
+ public Rotation3d(double roll, double pitch, double yaw) {
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+ double cr = Math.cos(roll * 0.5);
+ double sr = Math.sin(roll * 0.5);
+
+ double cp = Math.cos(pitch * 0.5);
+ double sp = Math.sin(pitch * 0.5);
+
+ double cy = Math.cos(yaw * 0.5);
+ double sy = Math.sin(yaw * 0.5);
+
+ m_q =
+ new 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);
+ }
+
+ /**
+ * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
+ * normalized.
+ *
+ * @param axis The rotation axis.
+ * @param angleRadians The rotation around the axis in radians.
+ */
+ public Rotation3d(Vector<N3> axis, double angleRadians) {
+ double norm = axis.norm();
+ if (norm == 0.0) {
+ return;
+ }
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+ var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
+ m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+ }
+
+ /**
+ * Constructs a Rotation3d from a rotation matrix.
+ *
+ * @param rotationMatrix The rotation matrix.
+ * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+ */
+ public Rotation3d(Matrix<N3, N3> rotationMatrix) {
+ final var 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.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
+ var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
+ builder.append(R.getStorage().toString()).append('\n');
+
+ var msg = builder.toString();
+ MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+ throw new IllegalArgumentException(msg);
+ }
+ if (Math.abs(R.det() - 1.0) > 1e-9) {
+ var builder =
+ new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
+ builder.append(R.getStorage().toString()).append('\n');
+
+ var msg = builder.toString();
+ MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+ throw new IllegalArgumentException(msg);
+ }
+
+ // Turn rotation matrix into a quaternion
+ // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+ double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+ double w;
+ double x;
+ double y;
+ double z;
+
+ if (trace > 0.0) {
+ double s = 0.5 / Math.sqrt(trace + 1.0);
+ w = 0.25 / s;
+ x = (R.get(2, 1) - R.get(1, 2)) * s;
+ y = (R.get(0, 2) - R.get(2, 0)) * s;
+ z = (R.get(1, 0) - R.get(0, 1)) * s;
+ } else {
+ if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+ double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+ w = (R.get(2, 1) - R.get(1, 2)) / s;
+ x = 0.25 * s;
+ y = (R.get(0, 1) + R.get(1, 0)) / s;
+ z = (R.get(0, 2) + R.get(2, 0)) / s;
+ } else if (R.get(1, 1) > R.get(2, 2)) {
+ double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+ w = (R.get(0, 2) - R.get(2, 0)) / s;
+ x = (R.get(0, 1) + R.get(1, 0)) / s;
+ y = 0.25 * s;
+ z = (R.get(1, 2) + R.get(2, 1)) / s;
+ } else {
+ double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+ w = (R.get(1, 0) - R.get(0, 1)) / s;
+ x = (R.get(0, 2) + R.get(2, 0)) / s;
+ y = (R.get(1, 2) + R.get(2, 1)) / s;
+ z = 0.25 * s;
+ }
+ }
+
+ m_q = new Quaternion(w, x, y, z);
+ }
+
+ /**
+ * Constructs a Rotation3d that rotates the initial vector onto the final vector.
+ *
+ * <p>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 last The final vector.
+ */
+ public Rotation3d(Vector<N3> initial, Vector<N3> last) {
+ double dot = initial.dot(last);
+ double normProduct = initial.norm() * last.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.
+ var X =
+ new MatBuilder<>(Nat.N3(), Nat.N1())
+ .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
+ final var qr = DecompositionFactory_DDRM.qr(3, 1);
+ qr.decompose(X.getStorage().getMatrix());
+ final var Q = qr.getQ(null, false);
+
+ // w = cos(θ/2) = 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 = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1));
+ } else {
+ // initial x last
+ var axis =
+ VecBuilder.fill(
+ initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0),
+ last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0),
+ initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0));
+
+ // https://stackoverflow.com/a/11741520
+ m_q =
+ new Quaternion(normProduct + dot, axis.get(0, 0), axis.get(1, 0), axis.get(2, 0))
+ .normalize();
+ }
+ }
+
+ /**
+ * Adds two rotations together.
+ *
+ * @param other The rotation to add.
+ * @return The sum of the two rotations.
+ */
+ public Rotation3d plus(Rotation3d other) {
+ return rotateBy(other);
+ }
+
+ /**
+ * 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.
+ */
+ public Rotation3d minus(Rotation3d other) {
+ return rotateBy(other.unaryMinus());
+ }
+
+ /**
+ * Takes the inverse of the current rotation.
+ *
+ * @return The inverse of the current rotation.
+ */
+ public Rotation3d unaryMinus() {
+ return new Rotation3d(m_q.inverse());
+ }
+
+ /**
+ * Multiplies the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Rotation3d.
+ */
+ public Rotation3d times(double scalar) {
+ // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+ if (m_q.getW() >= 0.0) {
+ return new Rotation3d(
+ VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
+ 2.0 * scalar * Math.acos(m_q.getW()));
+ } else {
+ return new Rotation3d(
+ VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
+ 2.0 * scalar * Math.acos(-m_q.getW()));
+ }
+ }
+
+ /**
+ * Divides the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Rotation3d.
+ */
+ public Rotation3d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
+ * Adds the new rotation to the current rotation.
+ *
+ * @param other The rotation to rotate by.
+ * @return The new rotated Rotation3d.
+ */
+ public Rotation3d rotateBy(Rotation3d other) {
+ return new Rotation3d(other.m_q.times(m_q));
+ }
+
+ /**
+ * Returns the quaternion representation of the Rotation3d.
+ *
+ * @return The quaternion representation of the Rotation3d.
+ */
+ @JsonProperty(value = "quaternion")
+ public Quaternion getQuaternion() {
+ return m_q;
+ }
+
+ /**
+ * Returns the counterclockwise rotation angle around the X axis (roll) in radians.
+ *
+ * @return The counterclockwise rotation angle around the X axis (roll) in radians.
+ */
+ public double getX() {
+ final var w = m_q.getW();
+ final var x = m_q.getX();
+ final var y = m_q.getY();
+ final var z = m_q.getZ();
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
+ }
+
+ /**
+ * Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
+ *
+ * @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
+ */
+ public double getY() {
+ final var w = m_q.getW();
+ final var x = m_q.getX();
+ final var y = m_q.getY();
+ final var z = m_q.getZ();
+
+ // 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 (Math.abs(ratio) >= 1.0) {
+ return Math.copySign(Math.PI / 2.0, ratio);
+ } else {
+ return Math.asin(ratio);
+ }
+ }
+
+ /**
+ * Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
+ *
+ * @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
+ */
+ public double getZ() {
+ final var w = m_q.getW();
+ final var x = m_q.getX();
+ final var y = m_q.getY();
+ final var z = m_q.getZ();
+
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+ return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
+ }
+
+ /**
+ * Returns the axis in the axis-angle representation of this rotation.
+ *
+ * @return The axis in the axis-angle representation.
+ */
+ public Vector<N3> getAxis() {
+ double norm =
+ Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+ if (norm == 0.0) {
+ return VecBuilder.fill(0.0, 0.0, 0.0);
+ } else {
+ return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
+ }
+ }
+
+ /**
+ * Returns the angle in radians in the axis-angle representation of this rotation.
+ *
+ * @return The angle in radians in the axis-angle representation of this rotation.
+ */
+ public double getAngle() {
+ double norm =
+ Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+ return 2.0 * Math.atan2(norm, m_q.getW());
+ }
+
+ /**
+ * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
+ *
+ * @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
+ */
+ public Rotation2d toRotation2d() {
+ return new Rotation2d(getZ());
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Rotation3d(%s)", m_q);
+ }
+
+ /**
+ * Checks equality between this Rotation3d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Rotation3d) {
+ var other = (Rotation3d) obj;
+ return m_q.equals(other.m_q);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_q);
+ }
+
+ @Override
+ public Rotation3d interpolate(Rotation3d endValue, double t) {
+ return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index dd35670..c3c6b0c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -47,7 +47,7 @@
}
/**
- * Scales the transform by the scalar.
+ * Multiplies the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
@@ -57,6 +57,16 @@
}
/**
+ * Divides the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform2d.
+ */
+ public Transform2d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
* Composes two transformations.
*
* @param other The transform to compose with this one.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
new file mode 100644
index 0000000..4920ef6
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -0,0 +1,162 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.Objects;
+
+/** Represents a transformation for a Pose3d. */
+public class Transform3d {
+ private final Translation3d m_translation;
+ private final Rotation3d m_rotation;
+
+ /**
+ * Constructs the transform that maps the initial pose to the final pose.
+ *
+ * @param initial The initial pose for the transformation.
+ * @param last The final pose for the transformation.
+ */
+ public Transform3d(Pose3d initial, Pose3d last) {
+ // 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 =
+ last.getTranslation()
+ .minus(initial.getTranslation())
+ .rotateBy(initial.getRotation().unaryMinus());
+
+ m_rotation = last.getRotation().minus(initial.getRotation());
+ }
+
+ /**
+ * Constructs a transform with the given translation and rotation components.
+ *
+ * @param translation Translational component of the transform.
+ * @param rotation Rotational component of the transform.
+ */
+ public Transform3d(Translation3d translation, Rotation3d rotation) {
+ m_translation = translation;
+ m_rotation = rotation;
+ }
+
+ /** Constructs the identity transform -- maps an initial pose to itself. */
+ public Transform3d() {
+ m_translation = new Translation3d();
+ m_rotation = new Rotation3d();
+ }
+
+ /**
+ * Multiplies the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform3d.
+ */
+ public Transform3d times(double scalar) {
+ return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
+ }
+
+ /**
+ * Divides the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform3d.
+ */
+ public Transform3d div(double scalar) {
+ return times(1.0 / scalar);
+ }
+
+ /**
+ * Composes two transformations.
+ *
+ * @param other The transform to compose with this one.
+ * @return The composition of the two transformations.
+ */
+ public Transform3d plus(Transform3d other) {
+ return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the transform.
+ */
+ public Translation3d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the X component of the transformation's translation.
+ *
+ * @return The x component of the transformation's translation.
+ */
+ public double getX() {
+ return m_translation.getX();
+ }
+
+ /**
+ * Returns the Y component of the transformation's translation.
+ *
+ * @return The y component of the transformation's translation.
+ */
+ public double getY() {
+ return m_translation.getY();
+ }
+
+ /**
+ * Returns the Z component of the transformation's translation.
+ *
+ * @return The z component of the transformation's translation.
+ */
+ public double getZ() {
+ return m_translation.getZ();
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return Reference to the rotational component of the transform.
+ */
+ public Rotation3d getRotation() {
+ return m_rotation;
+ }
+
+ /**
+ * Invert the transformation. This is useful for undoing a transformation.
+ *
+ * @return The inverted transformation.
+ */
+ public Transform3d inverse() {
+ // 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 new Transform3d(
+ getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+ getRotation().unaryMinus());
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Transform3d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Transform3d) {
+ return ((Transform3d) obj).m_translation.equals(m_translation)
+ && ((Transform3d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 84eaea8..2d57edc 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -13,13 +13,11 @@
import java.util.Objects;
/**
- * Represents a translation in 2d space. This object can be used to represent a point or a vector.
+ * Represents a translation in 2D space. This object can be used to represent a point or a vector.
*
- * <p>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.
+ * <p>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 and left is positive Y.
*/
-@SuppressWarnings({"ParameterName", "MemberName"})
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation2d implements Interpolatable<Translation2d> {
@@ -58,10 +56,9 @@
}
/**
- * Calculates the distance between two translations in 2d space.
+ * Calculates the distance between two translations in 2D space.
*
- * <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
- * x1)^2 + (y2 - y1)^2)
+ * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
@@ -73,7 +70,7 @@
/**
* Returns the X component of the translation.
*
- * @return The x component of the translation.
+ * @return The X component of the translation.
*/
@JsonProperty
public double getX() {
@@ -83,7 +80,7 @@
/**
* Returns the Y component of the translation.
*
- * @return The y component of the translation.
+ * @return The Y component of the translation.
*/
@JsonProperty
public double getY() {
@@ -100,13 +97,27 @@
}
/**
- * 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
+ */
+ public Rotation2d getAngle() {
+ return new Rotation2d(m_x, m_y);
+ }
+
+ /**
+ * Applies a rotation to the translation in 2D space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
- * angle. [x_new] [other.cos, -other.sin][x] [y_new] = [other.sin, other.cos][y]
+ * angle.
*
- * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
- * {0, 2}.
+ * <pre>
+ * [x_new] [other.cos, -other.sin][x]
+ * [y_new] = [other.sin, other.cos][y]
+ * </pre>
+ *
+ * <p>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.
@@ -117,9 +128,9 @@
}
/**
- * 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.
*
- * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
+ * <p>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.
@@ -129,9 +140,9 @@
}
/**
- * Subtracts the other translation from the other translation and returns the difference.
+ * Returns the difference between two translations.
*
- * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
+ * <p>For example, Translation2d(5.0, 4.0) - Translation2d(1.0, 2.0) = Translation2d(4.0, 2.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
@@ -142,7 +153,7 @@
/**
* 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.
+ * flipping the point over both axes, or negating all components of the translation.
*
* @return The inverse of the current translation.
*/
@@ -151,9 +162,9 @@
}
/**
- * Multiplies the translation by a scalar and returns the new translation.
+ * Returns the translation multiplied by a scalar.
*
- * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+ * <p>For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
@@ -163,9 +174,9 @@
}
/**
- * Divides the translation by a scalar and returns the new translation.
+ * Returns the translation divided by a scalar.
*
- * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+ * <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
new file mode 100644
index 0000000..810f56c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -0,0 +1,234 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import java.util.Objects;
+
+/**
+ * Represents a translation in 3D space. This object can be used to represent a point or a vector.
+ *
+ * <p>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.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation3d implements Interpolatable<Translation3d> {
+ private final double m_x;
+ private final double m_y;
+ private final double m_z;
+
+ /** Constructs a Translation3d with X, Y, and Z components equal to zero. */
+ public Translation3d() {
+ this(0.0, 0.0, 0.0);
+ }
+
+ /**
+ * 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.
+ */
+ @JsonCreator
+ public Translation3d(
+ @JsonProperty(required = true, value = "x") double x,
+ @JsonProperty(required = true, value = "y") double y,
+ @JsonProperty(required = true, value = "z") double z) {
+ m_x = x;
+ m_y = y;
+ m_z = 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.
+ */
+ public Translation3d(double distance, Rotation3d angle) {
+ final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
+ m_x = rectangular.getX();
+ m_y = rectangular.getY();
+ m_z = rectangular.getZ();
+ }
+
+ /**
+ * Calculates the distance between two translations in 3D space.
+ *
+ * <p>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.
+ */
+ public double getDistance(Translation3d other) {
+ return Math.sqrt(
+ Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
+ }
+
+ /**
+ * Returns the X component of the translation.
+ *
+ * @return The X component of the translation.
+ */
+ @JsonProperty
+ public double getX() {
+ return m_x;
+ }
+
+ /**
+ * Returns the Y component of the translation.
+ *
+ * @return The Y component of the translation.
+ */
+ @JsonProperty
+ public double getY() {
+ return m_y;
+ }
+
+ /**
+ * Returns the Z component of the translation.
+ *
+ * @return The Z component of the translation.
+ */
+ @JsonProperty
+ public double getZ() {
+ return m_z;
+ }
+
+ /**
+ * Returns the norm, or distance from the origin to the translation.
+ *
+ * @return The norm of the translation.
+ */
+ public double getNorm() {
+ return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+ }
+
+ /**
+ * Applies a rotation to the translation in 3D space.
+ *
+ * <p>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.
+ */
+ public Translation3d rotateBy(Rotation3d other) {
+ final var p = new Quaternion(0.0, m_x, m_y, m_z);
+ final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
+ return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
+ }
+
+ /**
+ * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
+ *
+ * @return A Translation2d representing this Translation3d projected into the X-Y plane.
+ */
+ public Translation2d toTranslation2d() {
+ return new Translation2d(m_x, m_y);
+ }
+
+ /**
+ * Returns the sum of two translations in 3D space.
+ *
+ * <p>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.
+ */
+ public Translation3d plus(Translation3d other) {
+ return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
+ }
+
+ /**
+ * Returns the difference between two translations.
+ *
+ * <p>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.
+ */
+ public Translation3d minus(Translation3d other) {
+ return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
+ }
+
+ /**
+ * Returns the inverse of the current translation. This is equivalent to negating all components
+ * of the translation.
+ *
+ * @return The inverse of the current translation.
+ */
+ public Translation3d unaryMinus() {
+ return new Translation3d(-m_x, -m_y, -m_z);
+ }
+
+ /**
+ * Returns the translation multiplied by a scalar.
+ *
+ * <p>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.
+ */
+ public Translation3d times(double scalar) {
+ return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
+ }
+
+ /**
+ * Returns the translation divided by a scalar.
+ *
+ * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The reference to the new mutated object.
+ */
+ public Translation3d div(double scalar) {
+ return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
+ }
+
+ /**
+ * Checks equality between this Translation3d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Translation3d) {
+ return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
+ && Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
+ && Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_x, m_y, m_z);
+ }
+
+ @Override
+ public Translation3d interpolate(Translation3d endValue, double t) {
+ return new Translation3d(
+ MathUtil.interpolate(this.getX(), endValue.getX(), t),
+ MathUtil.interpolate(this.getY(), endValue.getY(), t),
+ MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
index c73d236..be6831e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
@@ -7,12 +7,11 @@
import java.util.Objects;
/**
- * 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 Pose2d objects from a Twist2d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
-@SuppressWarnings("MemberName")
public class Twist2d {
/** Linear "dx" component. */
public double dx;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
new file mode 100644
index 0000000..b78505e
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
@@ -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.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.Objects;
+
+/**
+ * A change in distance along a 3D arc since the last pose update. We can use ideas from
+ * differential calculus to create new Pose3d objects from a Twist3d and vise versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+public class Twist3d {
+ /** Linear "dx" component. */
+ public double dx;
+
+ /** Linear "dy" component. */
+ public double dy;
+
+ /** Linear "dz" component. */
+ public double dz;
+
+ /** Rotation vector x component (radians). */
+ public double rx;
+
+ /** Rotation vector y component (radians). */
+ public double ry;
+
+ /** Rotation vector z component (radians). */
+ public double rz;
+
+ public Twist3d() {}
+
+ /**
+ * Constructs a Twist3d with the given values.
+ *
+ * @param dx Change in x direction relative to robot.
+ * @param dy Change in y direction relative to robot.
+ * @param dz Change in z direction relative to robot.
+ * @param rx Rotation vector x component.
+ * @param ry Rotation vector y component.
+ * @param rz Rotation vector z component.
+ */
+ public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
+ this.dx = dx;
+ this.dy = dy;
+ this.dz = dz;
+ this.rx = rx;
+ this.ry = ry;
+ this.rz = rz;
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
+ dx, dy, dz, rx, ry, rz);
+ }
+
+ /**
+ * Checks equality between this Twist3d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Twist3d) {
+ return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
+ && Math.abs(((Twist3d) obj).dy - dy) < 1E-9
+ && Math.abs(((Twist3d) obj).dz - dz) < 1E-9
+ && Math.abs(((Twist3d) obj).rx - rx) < 1E-9
+ && Math.abs(((Twist3d) obj).ry - ry) < 1E-9
+ && Math.abs(((Twist3d) obj).rz - rz) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(dx, dy, dz, rx, ry, rz);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
index f10820f..9fe09fe 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
@@ -20,6 +20,5 @@
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
- @SuppressWarnings("ParameterName")
T interpolate(T endValue, double t);
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 676bd7c..7e0712d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -6,6 +6,7 @@
import edu.wpi.first.math.MathUtil;
import java.util.NavigableMap;
+import java.util.Optional;
import java.util.TreeMap;
/**
@@ -16,10 +17,10 @@
*
* @param <T> The type stored in this buffer.
*/
-public class TimeInterpolatableBuffer<T> {
+public final class TimeInterpolatableBuffer<T> {
private final double m_historySize;
private final InterpolateFunction<T> m_interpolatingFunc;
- private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+ private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
private TimeInterpolatableBuffer(
InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
@@ -70,7 +71,7 @@
*/
public void addSample(double timeSeconds, T sample) {
cleanUp(timeSeconds);
- m_buffer.put(timeSeconds, sample);
+ m_pastSnapshots.put(timeSeconds, sample);
}
/**
@@ -79,10 +80,10 @@
* @param time The current timestamp.
*/
private void cleanUp(double time) {
- while (!m_buffer.isEmpty()) {
- var entry = m_buffer.firstEntry();
+ while (!m_pastSnapshots.isEmpty()) {
+ var entry = m_pastSnapshots.firstEntry();
if (time - entry.getKey() >= m_historySize) {
- m_buffer.remove(entry.getKey());
+ m_pastSnapshots.remove(entry.getKey());
} else {
return;
}
@@ -91,48 +92,58 @@
/** Clear all old samples. */
public void clear() {
- m_buffer.clear();
+ m_pastSnapshots.clear();
}
/**
- * Sample the buffer at the given time. If the buffer is empty, this will return null.
+ * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
*
* @param timeSeconds The time at which to sample.
- * @return The interpolated value at that timestamp. Might be null.
+ * @return The interpolated value at that timestamp or an empty Optional.
*/
- @SuppressWarnings("UnnecessaryParentheses")
- public T getSample(double timeSeconds) {
- if (m_buffer.isEmpty()) {
- return null;
+ public Optional<T> getSample(double timeSeconds) {
+ if (m_pastSnapshots.isEmpty()) {
+ return Optional.empty();
}
// Special case for when the requested time is the same as a sample
- var nowEntry = m_buffer.get(timeSeconds);
+ var nowEntry = m_pastSnapshots.get(timeSeconds);
if (nowEntry != null) {
- return nowEntry;
+ return Optional.of(nowEntry);
}
- var topBound = m_buffer.ceilingEntry(timeSeconds);
- var bottomBound = m_buffer.floorEntry(timeSeconds);
+ var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
+ var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
// Return null if neither sample exists, and the opposite bound if the other is null
if (topBound == null && bottomBound == null) {
- return null;
+ return Optional.empty();
} else if (topBound == null) {
- return bottomBound.getValue();
+ return Optional.of(bottomBound.getValue());
} else if (bottomBound == null) {
- return topBound.getValue();
+ return Optional.of(topBound.getValue());
} else {
// Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
// between the current time and bottom bound) and (the difference between top and bottom
// bounds).
- return m_interpolatingFunc.interpolate(
- bottomBound.getValue(),
- topBound.getValue(),
- ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+ return Optional.of(
+ m_interpolatingFunc.interpolate(
+ bottomBound.getValue(),
+ topBound.getValue(),
+ (timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
}
}
+ /**
+ * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+ * stored within this buffer.
+ *
+ * @return The internal sample buffer.
+ */
+ public NavigableMap<Double, T> getInternalBuffer() {
+ return m_pastSnapshots;
+ }
+
public interface InterpolateFunction<T> {
/**
* Return the interpolated value. This object is assumed to be the starting position, or lower
@@ -143,7 +154,6 @@
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
- @SuppressWarnings("ParameterName")
T interpolate(T start, T end, double t);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index 451c008..6c98337 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -16,7 +16,6 @@
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
-@SuppressWarnings("MemberName")
public class ChassisSpeeds {
/** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
public double vxMetersPerSecond;
@@ -69,6 +68,27 @@
omegaRadiansPerSecond);
}
+ /**
+ * 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.
+ */
+ public static ChassisSpeeds fromFieldRelativeSpeeds(
+ ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
+ return fromFieldRelativeSpeeds(
+ fieldRelativeSpeeds.vxMetersPerSecond,
+ fieldRelativeSpeeds.vyMetersPerSecond,
+ fieldRelativeSpeeds.omegaRadiansPerSecond,
+ robotAngle);
+ }
+
@Override
public String toString() {
return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index 7984e39..0dfb016 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -6,6 +6,7 @@
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Twist2d;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
@@ -15,7 +16,6 @@
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
*/
-@SuppressWarnings("MemberName")
public class DifferentialDriveKinematics {
public final double trackWidthMeters;
@@ -58,4 +58,20 @@
chassisSpeeds.vxMetersPerSecond
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
+
+ /**
+ * Performs forward kinematics to return the resulting Twist2d from the given left and right side
+ * distance deltas. This method is often used for odometry -- determining the robot's position on
+ * the field using changes in the distance driven by each wheel on the robot.
+ *
+ * @param leftDistanceMeters The distance measured by the left side encoder.
+ * @param rightDistanceMeters The distance measured by the right side encoder.
+ * @return The resulting Twist2d.
+ */
+ public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
+ return new Twist2d(
+ (leftDistanceMeters + rightDistanceMeters) / 2,
+ 0,
+ (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index 0139573..e8f97f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -33,42 +33,59 @@
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
*/
- public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+ public DifferentialDriveOdometry(
+ Rotation2d gyroAngle,
+ double leftDistanceMeters,
+ double rightDistanceMeters,
+ Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
+
+ m_prevLeftDistance = leftDistanceMeters;
+ m_prevRightDistance = rightDistanceMeters;
+
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
/**
- * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+ * Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
*/
- public DifferentialDriveOdometry(Rotation2d gyroAngle) {
- this(gyroAngle, new Pose2d());
+ public DifferentialDriveOdometry(
+ Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
+ this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
}
/**
* 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 here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
- * @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
+ * @param poseMeters The position on the field that your robot is at.
*/
- public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle,
+ double leftDistanceMeters,
+ double rightDistanceMeters,
+ Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
- m_prevLeftDistance = 0.0;
- m_prevRightDistance = 0.0;
+ m_prevLeftDistance = leftDistanceMeters;
+ m_prevRightDistance = rightDistanceMeters;
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index 20a38a9..d4b235e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -5,7 +5,6 @@
package edu.wpi.first.math.kinematics;
/** Represents the wheel speeds for a differential drive drivetrain. */
-@SuppressWarnings("MemberName")
public class DifferentialDriveWheelSpeeds {
/** Speed of the left side of the robot. */
public double leftMetersPerSecond;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 0807aba..23204d4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -7,6 +7,7 @@
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
import org.ejml.simple.SimpleMatrix;
/**
@@ -15,8 +16,8 @@
*
* <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
* uses the relative locations of the wheels with respect to the center of rotation. The center of
- * rotation for inverse kinematics is also variable. This means that you can set your set your
- * center of rotation in a corner of the robot to perform special evasion maneuvers.
+ * rotation for inverse kinematics is also variable. This means that you can set your center of
+ * rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of wheel speeds into the overall chassis motion) is
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
@@ -154,6 +155,28 @@
}
/**
+ * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
+ * method is often used for odometry -- determining the robot's position on the field using
+ * changes in the distance driven by each wheel on the robot.
+ *
+ * @param wheelDeltas The distances driven by each wheel.
+ * @return The resulting Twist2d.
+ */
+ public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
+ var wheelDeltasVector = new SimpleMatrix(4, 1);
+ wheelDeltasVector.setColumn(
+ 0,
+ 0,
+ wheelDeltas.frontLeftMeters,
+ wheelDeltas.frontRightMeters,
+ wheelDeltas.rearLeftMeters,
+ wheelDeltas.rearRightMeters);
+ var twist = m_forwardKinematics.mult(wheelDeltasVector);
+
+ return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
+ }
+
+ /**
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical center of the robot.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
index b504acc..a63eaea 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
@@ -5,7 +5,6 @@
package edu.wpi.first.math.kinematics;
/** Represents the motor voltages for a mecanum drive drivetrain. */
-@SuppressWarnings("MemberName")
public class MecanumDriveMotorVoltages {
/** Voltage of the front left motor. */
public double frontLeftVoltage;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
index b3c79c9..40a77e2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
@@ -8,8 +8,6 @@
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.util.WPIUtilJNI;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
@@ -21,7 +19,7 @@
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
- private double m_prevTimeSeconds = -1;
+ private MecanumDriveWheelPositions m_previousWheelPositions;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
@@ -31,14 +29,24 @@
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(
- MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+ MecanumDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ MecanumDriveWheelPositions wheelPositions,
+ Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
+ m_previousWheelPositions =
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
@@ -47,9 +55,13 @@
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The distances driven by each wheel.
*/
- public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
- this(kinematics, gyroAngle, new Pose2d());
+ public MecanumDriveOdometry(
+ MecanumDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ MecanumDriveWheelPositions wheelPositions) {
+ this(kinematics, gyroAngle, wheelPositions, new Pose2d());
}
/**
@@ -58,13 +70,21 @@
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelPositions The distances driven by each wheel.
+ * @param poseMeters The position on the field that your robot is at.
*/
- public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousWheelPositions =
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters);
}
/**
@@ -78,48 +98,37 @@
/**
* 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.
+ * 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 currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
- * @param wheelSpeeds The current wheel speeds.
+ * @param wheelPositions The distances driven by each wheel.
* @return The new pose of the robot.
*/
- public Pose2d updateWithTime(
- double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
- double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
- m_prevTimeSeconds = currentTimeSeconds;
-
+ public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
- var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
- var newPose =
- m_poseMeters.exp(
- new Twist2d(
- chassisState.vxMetersPerSecond * period,
- chassisState.vyMetersPerSecond * period,
- angle.minus(m_previousAngle).getRadians()));
+ var wheelDeltas =
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
+
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+ twist.dtheta = angle.minus(m_previousAngle).getRadians();
+ var newPose = m_poseMeters.exp(twist);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
- return m_poseMeters;
- }
+ m_previousWheelPositions =
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters);
- /**
- * 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 wheelSpeeds The current wheel speeds.
- * @return The new pose of the robot.
- */
- public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+ return m_poseMeters;
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
new file mode 100644
index 0000000..9ff3341
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+public class MecanumDriveWheelPositions {
+ /** Distance measured by the front left wheel. */
+ public double frontLeftMeters;
+
+ /** Distance measured by the front right wheel. */
+ public double frontRightMeters;
+
+ /** Distance measured by the rear left wheel. */
+ public double rearLeftMeters;
+
+ /** Distance measured by the rear right wheel. */
+ public double rearRightMeters;
+
+ /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
+ public MecanumDriveWheelPositions() {}
+
+ /**
+ * Constructs a MecanumDriveWheelPositions.
+ *
+ * @param frontLeftMeters Distance measured by the front left wheel.
+ * @param frontRightMeters Distance measured by the front right wheel.
+ * @param rearLeftMeters Distance measured by the rear left wheel.
+ * @param rearRightMeters Distance measured by the rear right wheel.
+ */
+ public MecanumDriveWheelPositions(
+ double frontLeftMeters,
+ double frontRightMeters,
+ double rearLeftMeters,
+ double rearRightMeters) {
+ this.frontLeftMeters = frontLeftMeters;
+ this.frontRightMeters = frontRightMeters;
+ this.rearLeftMeters = rearLeftMeters;
+ this.rearRightMeters = rearRightMeters;
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+ + "Rear Left: %.2f m, Rear Right: %.2f m)",
+ frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index ef354ef..1dcfc85 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -6,7 +6,6 @@
import java.util.stream.DoubleStream;
-@SuppressWarnings("MemberName")
public class MecanumDriveWheelSpeeds {
/** Speed of the front left wheel. */
public double frontLeftMetersPerSecond;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index dc1b9e4..98df547 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -8,6 +8,7 @@
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
@@ -18,8 +19,8 @@
*
* <p>The inverse kinematics (converting from a desired chassis velocity to individual module
* states) uses the relative locations of the modules with respect to the center of rotation. The
- * center of rotation for inverse kinematics is also variable. This means that you can set your set
- * your center of rotation in a corner of the robot to perform special evasion maneuvers.
+ * center of rotation for inverse kinematics is also variable. This means that you can set your
+ * center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of module states into the overall chassis motion) is
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
@@ -38,13 +39,15 @@
private final int m_numModules;
private final Translation2d[] m_modules;
+ private final SwerveModuleState[] m_moduleStates;
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
- * as Translation2ds. The order in which you pass in the wheel locations is the same order that
- * you will receive the module states when performing inverse kinematics. It is also expected that
- * you pass in the module states in the same order when calling the forward kinematics methods.
+ * as Translation2d objects. The order in which you pass in the wheel locations is the same order
+ * that you will receive the module states when performing inverse kinematics. It is also expected
+ * that you pass in the module states in the same order when calling the forward kinematics
+ * methods.
*
* @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
*/
@@ -54,6 +57,8 @@
}
m_numModules = wheelsMeters.length;
m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+ m_moduleStates = new SwerveModuleState[m_numModules];
+ Arrays.fill(m_moduleStates, new SwerveModuleState());
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
for (int i = 0; i < m_numModules; i++) {
@@ -74,6 +79,9 @@
* argument is defaulted to that use case. 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.
*
+ * <p>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 centerOfRotationMeters The center of rotation. For example, if you set the center of
* rotation at one corner of the robot and provide a chassis speed that only has a dtheta
@@ -83,9 +91,19 @@
* attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
* DesaturateWheelSpeeds} function to rectify this issue.
*/
- @SuppressWarnings("LocalVariableName")
+ @SuppressWarnings("PMD.MethodReturnsInternalArray")
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+ if (chassisSpeeds.vxMetersPerSecond == 0.0
+ && chassisSpeeds.vyMetersPerSecond == 0.0
+ && chassisSpeeds.omegaRadiansPerSecond == 0.0) {
+ for (int i = 0; i < m_numModules; i++) {
+ m_moduleStates[i].speedMetersPerSecond = 0.0;
+ }
+
+ return m_moduleStates;
+ }
+
if (!centerOfRotationMeters.equals(m_prevCoR)) {
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(
@@ -113,7 +131,6 @@
chassisSpeeds.omegaRadiansPerSecond);
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
- SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
for (int i = 0; i < m_numModules; i++) {
double x = moduleStatesMatrix.get(i * 2, 0);
@@ -122,10 +139,10 @@
double speed = Math.hypot(x, y);
Rotation2d angle = new Rotation2d(x, y);
- moduleStates[i] = new SwerveModuleState(speed, angle);
+ m_moduleStates[i] = new SwerveModuleState(speed, angle);
}
- return moduleStates;
+ return m_moduleStates;
}
/**
@@ -171,6 +188,35 @@
}
/**
+ * Performs forward kinematics to return the resulting chassis state from the given module states.
+ * This method is often used for odometry -- determining the robot's position on the field using
+ * data from the real-world speed 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.
+ */
+ public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
+ if (wheelDeltas.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+ var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+ for (int i = 0; i < m_numModules; i++) {
+ var module = wheelDeltas[i];
+ moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
+ moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
+ }
+
+ var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
+ return new Twist2d(
+ chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
+ }
+
+ /**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*
* <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
@@ -192,4 +238,48 @@
}
}
}
+
+ /**
+ * 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.
+ *
+ * <p>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 The current speed of the robot
+ * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
+ * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
+ * can reach while translating
+ * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can
+ * reach while rotating
+ */
+ public static void desaturateWheelSpeeds(
+ SwerveModuleState[] moduleStates,
+ ChassisSpeeds currentChassisSpeed,
+ double attainableMaxModuleSpeedMetersPerSecond,
+ double attainableMaxTranslationalSpeedMetersPerSecond,
+ double attainableMaxRotationalVelocityRadiansPerSecond) {
+ double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+
+ if (attainableMaxTranslationalSpeedMetersPerSecond == 0
+ || attainableMaxRotationalVelocityRadiansPerSecond == 0
+ || realMaxSpeed == 0) {
+ return;
+ }
+ double translationalK =
+ Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
+ / attainableMaxTranslationalSpeedMetersPerSecond;
+ double rotationalK =
+ Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
+ / attainableMaxRotationalVelocityRadiansPerSecond;
+ double k = Math.max(translationalK, rotationalK);
+ double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
+ for (SwerveModuleState moduleState : moduleStates) {
+ moduleState.speedMetersPerSecond *= scale;
+ }
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index 8b4161e..c2e188f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -8,8 +8,6 @@
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.util.WPIUtilJNI;
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
@@ -22,24 +20,38 @@
public class SwerveDriveOdometry {
private final SwerveDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
- private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
+ private final int m_numModules;
+ private SwerveModulePosition[] m_previousModulePositions;
/**
* Constructs a SwerveDriveOdometry object.
*
* @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.
*/
public SwerveDriveOdometry(
- SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPose.getRotation();
+ m_numModules = modulePositions.length;
+
+ m_previousModulePositions = new SwerveModulePosition[m_numModules];
+ for (int index = 0; index < m_numModules; index++) {
+ m_previousModulePositions[index] =
+ new SwerveModulePosition(
+ modulePositions[index].distanceMeters, modulePositions[index].angle);
+ }
+
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
}
@@ -48,9 +60,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.
*/
- public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
- this(kinematics, gyroAngle, new Pose2d());
+ public SwerveDriveOdometry(
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions) {
+ this(kinematics, gyroAngle, modulePositions, new Pose2d());
}
/**
@@ -59,13 +75,28 @@
* <p>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.
+ * <p>Similarly, module positions do not need to be reset in user code.
+ *
* @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.
*/
- public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+ public void resetPosition(
+ Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
+ if (modulePositions.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+
m_poseMeters = pose;
m_previousAngle = pose.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ for (int index = 0; index < m_numModules; index++) {
+ m_previousModulePositions[index] =
+ new SwerveModulePosition(
+ modulePositions[index].distanceMeters, modulePositions[index].angle);
+ }
}
/**
@@ -79,51 +110,43 @@
/**
* 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 currentTimeSeconds The current time in seconds.
- * @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.
- */
- public Pose2d updateWithTime(
- double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
- double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
- m_prevTimeSeconds = currentTimeSeconds;
-
- var angle = gyroAngle.plus(m_gyroOffset);
-
- var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
- var newPose =
- m_poseMeters.exp(
- new Twist2d(
- chassisState.vxMetersPerSecond * period,
- chassisState.vyMetersPerSecond * period,
- angle.minus(m_previousAngle).getRadians()));
-
- m_previousAngle = angle;
- m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
- return m_poseMeters;
- }
-
- /**
- * 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.
+ * @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.
*/
- public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+ public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+ if (modulePositions.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+
+ var moduleDeltas = new SwerveModulePosition[m_numModules];
+ for (int index = 0; index < m_numModules; index++) {
+ var current = modulePositions[index];
+ var previous = m_previousModulePositions[index];
+
+ moduleDeltas[index] =
+ new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
+ previous.distanceMeters = current.distanceMeters;
+ }
+
+ var angle = gyroAngle.plus(m_gyroOffset);
+
+ var twist = m_kinematics.toTwist2d(moduleDeltas);
+ twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+ var newPose = m_poseMeters.exp(twist);
+
+ m_previousAngle = angle;
+ m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+ return m_poseMeters;
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
new file mode 100644
index 0000000..cdd7834
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Objects;
+
+/** Represents the state of one swerve module. */
+public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+ /** Distance measured by the wheel of the module. */
+ public double distanceMeters;
+
+ /** Angle of the module. */
+ public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+ /** Constructs a SwerveModulePosition with zeros for distance and angle. */
+ public SwerveModulePosition() {}
+
+ /**
+ * Constructs a SwerveModulePosition.
+ *
+ * @param distanceMeters The distance measured by the wheel of the module.
+ * @param angle The angle of the module.
+ */
+ public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
+ this.distanceMeters = distanceMeters;
+ this.angle = angle;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof SwerveModulePosition) {
+ return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(distanceMeters);
+ }
+
+ /**
+ * Compares two swerve module positions. One swerve module is "greater" than the other if its
+ * distance is higher than the other.
+ *
+ * @param other The other swerve module.
+ * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+ */
+ @Override
+ public int compareTo(SwerveModulePosition other) {
+ return Double.compare(this.distanceMeters, other.distanceMeters);
+ }
+
+ @Override
+ public String toString() {
+ return String.format(
+ "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
+ }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index 6a9c48c..ec7fd9f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -8,7 +8,6 @@
import java.util.Objects;
/** Represents the state of one swerve module. */
-@SuppressWarnings("MemberName")
public class SwerveModuleState implements Comparable<SwerveModuleState> {
/** Speed of the wheel of the module. */
public double speedMetersPerSecond;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
index 9bbeaf6..6e13039 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
@@ -19,7 +19,6 @@
* @param yInitialControlVector The control vector for the initial point in the y dimension.
* @param yFinalControlVector The control vector for the final point in the y dimension.
*/
- @SuppressWarnings("ParameterName")
public CubicHermiteSpline(
double[] xInitialControlVector,
double[] xFinalControlVector,
@@ -81,25 +80,25 @@
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// 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)]
hermiteBasis =
new SimpleMatrix(
4,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
index 8bad7b1..30b3cae 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
@@ -7,7 +7,6 @@
import edu.wpi.first.math.geometry.Pose2d;
/** Represents a pair of a pose and a curvature. */
-@SuppressWarnings("MemberName")
public class PoseWithCurvature {
// Represents the pose.
public Pose2d poseMeters;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
index 4017044..be90999 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
@@ -19,7 +19,6 @@
* @param yInitialControlVector The control vector for the initial point in the y dimension.
* @param yFinalControlVector The control vector for the final point in the y dimension.
*/
- @SuppressWarnings("ParameterName")
public QuinticHermiteSpline(
double[] xInitialControlVector,
double[] xFinalControlVector,
@@ -80,33 +79,33 @@
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
- // 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.
+ // 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)]
hermiteBasis =
new SimpleMatrix(
6,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
index 5451eea..83b35f3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
@@ -35,7 +35,6 @@
* @param t The point t
* @return The pose and curvature at that point.
*/
- @SuppressWarnings("ParameterName")
public PoseWithCurvature getPoint(double t) {
SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
final var coefficients = getCoefficients();
@@ -46,7 +45,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.
+ // n number of times when n is the derivative we want to take.
SimpleMatrix combined = coefficients.mult(polynomialBases);
// Get x and y
@@ -85,7 +84,6 @@
* <p>Each element in each array represents the value of the derivative at the index. For example,
* the value of x[2] is the second derivative in the x dimension.
*/
- @SuppressWarnings("MemberName")
public static class ControlVector {
public double[] x;
public double[] y;
@@ -96,7 +94,6 @@
* @param x The x dimension of the control vector.
* @param y The y dimension of the control vector.
*/
- @SuppressWarnings("ParameterName")
public ControlVector(double[] x, double[] y) {
this.x = Arrays.copyOf(x, x.length);
this.y = Arrays.copyOf(y, y.length);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
index e5c67f8..651b028 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
@@ -46,7 +46,7 @@
* Returns quintic splines from a set of waypoints.
*
* @param waypoints The waypoints
- * @return List of splines.
+ * @return array of splines.
*/
public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
@@ -78,7 +78,6 @@
* @return A vector of cubic hermite splines that interpolate through the provided waypoints and
* control vectors.
*/
- @SuppressWarnings("LocalVariableName")
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
@@ -202,7 +201,6 @@
* @param controlVectors The control vectors.
* @return A vector of quintic hermite splines that interpolate through the provided waypoints.
*/
- @SuppressWarnings("LocalVariableName")
public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
Spline.ControlVector[] controlVectors) {
QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
@@ -228,7 +226,6 @@
* @param d the vector on the rhs
* @param solutionVector the unknown (solution) vector, modified in-place
*/
- @SuppressWarnings({"ParameterName", "LocalVariableName"})
private static void thomasAlgorithm(
double[] a, double[] b, double[] c, double[] d, double[] solutionVector) {
int N = d.length;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
index 88afc6d..c1a87f2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
@@ -46,7 +46,6 @@
*/
private static final int kMaxIterations = 5000;
- @SuppressWarnings("MemberName")
private static class StackContents {
final double t1;
final double t0;
@@ -57,7 +56,6 @@
}
}
- @SuppressWarnings("serial")
public static class MalformedSplineException extends RuntimeException {
/**
* Create a new exception with the given message.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index ffbd99e..1871803 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -9,7 +9,6 @@
import edu.wpi.first.math.Pair;
import org.ejml.simple.SimpleMatrix;
-@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public final class Discretization {
private Discretization() {
// Utility class
@@ -25,6 +24,7 @@
*/
public static <States extends Num> Matrix<States, States> discretizeA(
Matrix<States, States> contA, double dtSeconds) {
+ // A_d = eᴬᵀ
return contA.times(dtSeconds).exp();
}
@@ -38,24 +38,26 @@
* @param dtSeconds Discretization timestep.
* @return a Pair representing discA and diskB.
*/
- @SuppressWarnings("LocalVariableName")
public static <States extends Num, Inputs extends Num>
Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
- var scaledA = contA.times(dtSeconds);
- var scaledB = contB.times(dtSeconds);
-
int states = contA.getNumRows();
int inputs = contB.getNumCols();
+
+ // M = [A B]
+ // [0 0]
var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
- M.assignBlock(0, 0, scaledA);
- M.assignBlock(0, scaledA.getNumCols(), scaledB);
- var phi = M.exp();
+ M.assignBlock(0, 0, contA);
+ M.assignBlock(0, contA.getNumCols(), contB);
+
+ // ϕ = eᴹᵀ = [A_d B_d]
+ // [ 0 I ]
+ var phi = M.times(dtSeconds).exp();
var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
- var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
-
discA.extractFrom(0, 0, phi);
+
+ var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
discB.extractFrom(0, contB.getNumRows(), phi);
return new Pair<>(discA, discB);
@@ -70,7 +72,6 @@
* @param dtSeconds Discretization timestep.
* @return a pair representing the discrete system matrix and process noise covariance matrix.
*/
- @SuppressWarnings("LocalVariableName")
public static <States extends Num>
Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
@@ -79,18 +80,22 @@
// Make continuous Q symmetric if it isn't already
Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
- // Set up the matrix M = [[-A, Q], [0, A.T]]
+ // M = [−A Q ]
+ // [ 0 Aᵀ]
final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
M.assignBlock(0, 0, contA.times(-1.0));
M.assignBlock(0, states, Q);
M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
M.assignBlock(states, states, contA.transpose());
+ // ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d]
+ // [ 0 A_dᵀ ]
final var phi = M.times(dtSeconds).exp();
- // Phi12 = phi[0:States, States:2*States]
- // Phi22 = phi[States:2*States, States:2*States]
+ // ϕ₁₂ = A_d⁻¹Q_d
final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
+
+ // ϕ₂₂ = A_dᵀ
final Matrix<States, States> phi22 = phi.block(states, states, states, states);
final var discA = phi22.transpose();
@@ -109,9 +114,12 @@
* <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
* we take advantage of the structure of the block matrix of A and Q.
*
- * <p>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>
*
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
@@ -119,10 +127,44 @@
* @param dtSeconds Discretization timestep.
* @return a pair representing the discrete system matrix and process noise covariance matrix.
*/
- @SuppressWarnings("LocalVariableName")
public static <States extends Num>
Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
+ // 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ᵀ² = …
+ // …
+ // ```
+
// Make continuous Q symmetric if it isn't already
Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
@@ -130,17 +172,18 @@
double lastCoeff = dtSeconds;
// Aᵀⁿ
- Matrix<States, States> Atn = contA.transpose();
+ Matrix<States, States> ATn = contA.transpose();
+
Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
- lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
+ lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn));
lastCoeff *= dtSeconds / ((double) i);
phi12 = phi12.plus(lastTerm.times(lastCoeff));
- Atn = Atn.times(contA.transpose());
+ ATn = ATn.times(contA.transpose());
}
var discA = discretizeA(contA, dtSeconds);
@@ -157,11 +200,12 @@
* Note that dt=0.0 divides R by zero.
*
* @param <O> Nat representing the number of outputs.
- * @param R Continuous measurement noise covariance matrix.
+ * @param contR Continuous measurement noise covariance matrix.
* @param dtSeconds Discretization timestep.
* @return Discretized version of the provided continuous measurement noise covariance matrix.
*/
- public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
- return R.div(dtSeconds);
+ public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) {
+ // R_d = 1/T R
+ return contR.div(dtSeconds);
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
index 9e35cfd..d7b6602 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
@@ -8,76 +8,70 @@
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
-@SuppressWarnings("ClassTypeParameterName")
public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
/** Continuous system matrix. */
- @SuppressWarnings("MemberName")
private final Matrix<States, States> m_A;
/** Continuous input matrix. */
- @SuppressWarnings("MemberName")
private final Matrix<States, Inputs> m_B;
/** Output matrix. */
- @SuppressWarnings("MemberName")
private final Matrix<Outputs, States> m_C;
/** Feedthrough matrix. */
- @SuppressWarnings("MemberName")
private final Matrix<Outputs, Inputs> m_D;
/**
* Construct a new LinearSystem from the four system matrices.
*
- * @param a The system matrix A.
- * @param b The input matrix B.
- * @param c The output matrix C.
- * @param d The feedthrough matrix D.
+ * @param A The system matrix A.
+ * @param B The input matrix B.
+ * @param C The output matrix C.
+ * @param D The feedthrough matrix D.
* @throws IllegalArgumentException if any matrix element isn't finite.
*/
- @SuppressWarnings("ParameterName")
public LinearSystem(
- Matrix<States, States> a,
- Matrix<States, Inputs> b,
- Matrix<Outputs, States> c,
- Matrix<Outputs, Inputs> d) {
- for (int row = 0; row < a.getNumRows(); ++row) {
- for (int col = 0; col < a.getNumCols(); ++col) {
- if (!Double.isFinite(a.get(row, col))) {
+ Matrix<States, States> A,
+ Matrix<States, Inputs> B,
+ Matrix<Outputs, States> C,
+ Matrix<Outputs, Inputs> D) {
+ for (int row = 0; row < A.getNumRows(); ++row) {
+ for (int col = 0; col < A.getNumCols(); ++col) {
+ if (!Double.isFinite(A.get(row, col))) {
throw new IllegalArgumentException(
"Elements of A aren't finite. This is usually due to model implementation errors.");
}
}
}
- for (int row = 0; row < b.getNumRows(); ++row) {
- for (int col = 0; col < b.getNumCols(); ++col) {
- if (!Double.isFinite(b.get(row, col))) {
+ for (int row = 0; row < B.getNumRows(); ++row) {
+ for (int col = 0; col < B.getNumCols(); ++col) {
+ if (!Double.isFinite(B.get(row, col))) {
throw new IllegalArgumentException(
"Elements of B aren't finite. This is usually due to model implementation errors.");
}
}
}
- for (int row = 0; row < c.getNumRows(); ++row) {
- for (int col = 0; col < c.getNumCols(); ++col) {
- if (!Double.isFinite(c.get(row, col))) {
+ for (int row = 0; row < C.getNumRows(); ++row) {
+ for (int col = 0; col < C.getNumCols(); ++col) {
+ if (!Double.isFinite(C.get(row, col))) {
throw new IllegalArgumentException(
"Elements of C aren't finite. This is usually due to model implementation errors.");
}
}
}
- for (int row = 0; row < d.getNumRows(); ++row) {
- for (int col = 0; col < d.getNumCols(); ++col) {
- if (!Double.isFinite(d.get(row, col))) {
+ for (int row = 0; row < D.getNumRows(); ++row) {
+ for (int col = 0; col < D.getNumCols(); ++col) {
+ if (!Double.isFinite(D.get(row, col))) {
throw new IllegalArgumentException(
"Elements of D aren't finite. This is usually due to model implementation errors.");
}
}
}
- this.m_A = a;
- this.m_B = b;
- this.m_C = c;
- this.m_D = d;
+ this.m_A = A;
+ this.m_B = B;
+ this.m_C = C;
+ this.m_D = D;
}
/**
@@ -170,7 +164,6 @@
* @param dtSeconds Timestep for model update.
* @return the updated x.
*/
- @SuppressWarnings("ParameterName")
public Matrix<States, N1> calculateX(
Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
@@ -187,7 +180,6 @@
* @param clampedU The control input.
* @return the updated output matrix Y.
*/
- @SuppressWarnings("ParameterName")
public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
return m_C.times(x).plus(m_D.times(clampedU));
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
index dcd4e58..02b1da0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -21,14 +21,13 @@
*
* <p>For everything in this file, "inputs" and "outputs" are defined from the perspective of the
* plant. This means U is an input and Y is an output (because you give the plant U (powers) and it
- * gives you back a Y (sensor values). This is the opposite of what they mean from the perspective
+ * gives you back a Y (sensor values)). This is the opposite of what they mean from the perspective
* of the controller (U is an output because that's what goes to the motors and Y is an input
* because that's what comes back from the sensors).
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
-@SuppressWarnings("ClassTypeParameterName")
public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
@@ -315,7 +314,6 @@
*
* @param y Measurement vector.
*/
- @SuppressWarnings("ParameterName")
public void correct(Matrix<Outputs, N1> y) {
getObserver().correct(getU(), y);
}
@@ -327,7 +325,6 @@
*
* @param dtSeconds Timestep for model update.
*/
- @SuppressWarnings("LocalVariableName")
public void predict(double dtSeconds) {
var u =
clampInput(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
index 274b10a..94be369 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -24,7 +24,7 @@
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x) for dt.
*/
- @SuppressWarnings("ParameterName")
+ @SuppressWarnings("overloads")
public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
final var h = dtSeconds;
final var k1 = f.apply(x);
@@ -44,7 +44,7 @@
* @param dtSeconds The time over which to integrate.
* @return The result of Runge Kutta integration (4th order).
*/
- @SuppressWarnings("ParameterName")
+ @SuppressWarnings("overloads")
public static double rk4(
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
final var h = dtSeconds;
@@ -68,7 +68,7 @@
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+ @SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
@@ -93,7 +93,7 @@
* @param dtSeconds The time over which to integrate.
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+ @SuppressWarnings("overloads")
public static <States extends Num> Matrix<States, N1> rk4(
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var h = dtSeconds;
@@ -107,139 +107,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. By default, the max
- * error is 1e-6.
- *
- * @param <States> A Num representing the states of the system to integrate.
- * @param <Inputs> A Num representing the inputs of the system to integrate.
- * @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 dtSeconds The time over which to integrate.
- * @return the integration of dx/dt = f(x, u) for dt.
- */
- @SuppressWarnings("MethodTypeParameterName")
- public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
- BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
- Matrix<States, N1> x,
- Matrix<Inputs, N1> u,
- double dtSeconds) {
- return rkf45(f, x, u, dtSeconds, 1e-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 <States> A Num representing the states of the system to integrate.
- * @param <Inputs> A Num representing the inputs of the system to integrate.
- * @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 dtSeconds The time over which to integrate.
- * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
- * @return the integration of dx/dt = f(x, u) for dt.
- */
- @SuppressWarnings("MethodTypeParameterName")
- public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
- BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
- Matrix<States, N1> x,
- Matrix<Inputs, N1> u,
- double dtSeconds,
- double maxError) {
- // See
- // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
- // for the Butcher tableau the following arrays came from.
-
- // final double[5][5]
- final double[][] A = {
- {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}
- };
-
- // final double[6]
- final double[] b1 = {
- 16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0
- };
-
- // final double[6]
- final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
-
- Matrix<States, N1> newX;
- double truncationError;
-
- double dtElapsed = 0.0;
- double h = dtSeconds;
-
- // Loop until we've gotten to our desired dt
- while (dtElapsed < dtSeconds) {
- do {
- // Only allow us to advance up to the dt remaining
- h = Math.min(h, dtSeconds - 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
- var k1 = f.apply(x, u);
- var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
- var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
- var k4 =
- f.apply(
- x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
- u);
- var k5 =
- f.apply(
- x.plus(
- k1.times(A[3][0])
- .plus(k2.times(A[3][1]))
- .plus(k3.times(A[3][2]))
- .plus(k4.times(A[3][3]))
- .times(h)),
- u);
- var k6 =
- f.apply(
- x.plus(
- k1.times(A[4][0])
- .plus(k2.times(A[4][1]))
- .plus(k3.times(A[4][2]))
- .plus(k4.times(A[4][3]))
- .plus(k5.times(A[4][4]))
- .times(h)),
- u);
-
- newX =
- x.plus(
- k1.times(b1[0])
- .plus(k2.times(b1[1]))
- .plus(k3.times(b1[2]))
- .plus(k4.times(b1[3]))
- .plus(k5.times(b1[4]))
- .plus(k6.times(b1[5]))
- .times(h));
- truncationError =
- (k1.times(b1[0] - b2[0])
- .plus(k2.times(b1[1] - b2[1]))
- .plus(k3.times(b1[2] - b2[2]))
- .plus(k4.times(b1[3] - b2[3]))
- .plus(k5.times(b1[4] - b2[4]))
- .plus(k6.times(b1[5] - b2[5]))
- .times(h))
- .normF();
-
- h *= 0.9 * Math.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. By default, the max
* error is 1e-6.
*
@@ -251,7 +118,7 @@
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
- @SuppressWarnings("MethodTypeParameterName")
+ @SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
@@ -272,7 +139,7 @@
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
- @SuppressWarnings("MethodTypeParameterName")
+ @SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
index 6c2c896..43eba93 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
@@ -30,7 +30,6 @@
* @param x Vector argument.
* @return The numerical Jacobian with respect to x for f(x, u, ...).
*/
- @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <Rows extends Num, Cols extends Num, States extends Num>
Matrix<Rows, Cols> numericalJacobian(
Nat<Rows> rows,
@@ -44,7 +43,6 @@
var dxMinus = x.copy();
dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
- @SuppressWarnings("LocalVariableName")
var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
@@ -67,7 +65,6 @@
* @param u Input vector.
* @return The numerical Jacobian with respect to x for f(x, u, ...).
*/
- @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
Matrix<Rows, States> numericalJacobianX(
Nat<Rows> rows,
@@ -91,7 +88,6 @@
* @param u Input vector.
* @return the numerical Jacobian with respect to u for f(x, u).
*/
- @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
public static <Rows extends Num, States extends Num, Inputs extends Num>
Matrix<Rows, Inputs> numericalJacobianU(
Nat<Rows> rows,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
index 94c117f..cd431f7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -8,28 +8,13 @@
/** Holds the constants for a DC motor. */
public class DCMotor {
- @SuppressWarnings("MemberName")
public final double nominalVoltageVolts;
-
- @SuppressWarnings("MemberName")
public final double stallTorqueNewtonMeters;
-
- @SuppressWarnings("MemberName")
public final double stallCurrentAmps;
-
- @SuppressWarnings("MemberName")
public final double freeCurrentAmps;
-
- @SuppressWarnings("MemberName")
public final double freeSpeedRadPerSec;
-
- @SuppressWarnings("MemberName")
public final double rOhms;
-
- @SuppressWarnings("MemberName")
public final double KvRadPerSecPerVolt;
-
- @SuppressWarnings("MemberName")
public final double KtNMPerAmp;
/**
@@ -64,7 +49,7 @@
/**
* Estimate the current being drawn by this motor.
*
- * @param speedRadiansPerSec The speed of the rotor.
+ * @param speedRadiansPerSec The speed of the motor.
* @param voltageInputVolts The input voltage.
* @return The estimated current.
*/
@@ -73,6 +58,54 @@
}
/**
+ * Calculate the torque produced by the motor for a given current.
+ *
+ * @param currentAmpere The current drawn by the motor.
+ * @return The torque produced.
+ */
+ public double getTorque(double currentAmpere) {
+ return currentAmpere * KtNMPerAmp;
+ }
+
+ /**
+ * Calculate the voltage provided to the motor at a given torque and angular velocity.
+ *
+ * @param torqueNm The torque produced by the motor.
+ * @param speedRadiansPerSec The speed of the motor.
+ * @return The voltage of the motor.
+ */
+ public double getVoltage(double torqueNm, double speedRadiansPerSec) {
+ return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
+ }
+
+ /**
+ * Calculate the speed of the motor at a given torque and input voltage.
+ *
+ * @param torqueNm The torque produced by the motor.
+ * @param voltageInputVolts The voltage applied to the motor.
+ * @return The speed of the motor.
+ */
+ public double getSpeed(double torqueNm, double voltageInputVolts) {
+ return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+ }
+
+ /**
+ * Returns a copy of this motor with the given gearbox reduction applied.
+ *
+ * @param gearboxReduction The gearbox reduction.
+ * @return A motor with the gearbox reduction applied.
+ */
+ public DCMotor withReduction(double gearboxReduction) {
+ return new DCMotor(
+ nominalVoltageVolts,
+ stallTorqueNewtonMeters * gearboxReduction,
+ stallCurrentAmps,
+ freeCurrentAmps,
+ freeSpeedRadPerSec / gearboxReduction,
+ 1);
+ }
+
+ /**
* Return a gearbox of CIM motors.
*
* @param numMotors Number of motors in the gearbox.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 1a05eb6..332e690 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -20,14 +20,13 @@
* Create a state-space model of an elevator system. The states of the system are [position,
* velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
- * @param motor The motor (or gearbox) attached to the arm.
+ * @param motor The motor (or gearbox) attached to the carriage.
* @param massKg The mass of the elevator carriage, in kilograms.
- * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
+ * @param radiusMeters The radius of the elevator's driving drum, in meters.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or G <= 0.
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> createElevatorSystem(
DCMotor motor, double massKg, double radiusMeters, double G) {
if (massKg <= 0.0) {
@@ -63,15 +62,14 @@
* velocity], inputs are [voltage], and outputs are [angular velocity].
*
* @param motor The motor (or gearbox) attached to the flywheel.
- * @param jKgMetersSquared The moment of inertia J of the flywheel.
+ * @param JKgMetersSquared The moment of inertia J of the flywheel.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
- * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0.
+ * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0.
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
- DCMotor motor, double jKgMetersSquared, double G) {
- if (jKgMetersSquared <= 0.0) {
+ DCMotor motor, double JKgMetersSquared, double G) {
+ if (JKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (G <= 0.0) {
@@ -83,8 +81,8 @@
-G
* G
* motor.KtNMPerAmp
- / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
- VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+ / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
+ VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
Matrix.eye(Nat.N1()),
new Matrix<>(Nat.N1(), Nat.N1()));
}
@@ -94,16 +92,15 @@
* position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
* velocity].
*
- * @param motor The motor (or gearbox) attached to the DC motor.
- * @param jKgMetersSquared The moment of inertia J of the DC motor.
+ * @param motor The motor (or gearbox) attached to system.
+ * @param JKgMetersSquared The moment of inertia J of the DC motor.
* @param G The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
- * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0.
+ * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0.
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N2> createDCMotorSystem(
- DCMotor motor, double jKgMetersSquared, double G) {
- if (jKgMetersSquared <= 0.0) {
+ DCMotor motor, double JKgMetersSquared, double G) {
+ if (JKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (G <= 0.0) {
@@ -119,26 +116,26 @@
-G
* G
* motor.KtNMPerAmp
- / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
- VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+ / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
+ VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
- * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.
+ * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
+ * [left velocity, right velocity]ᵀ.
*
- * @param motor the gearbox representing the motors driving the drivetrain.
- * @param massKg the mass of the robot.
- * @param rMeters the radius of the wheels in meters.
- * @param rbMeters the radius of the base (half the track width) in meters.
- * @param JKgMetersSquared the moment of inertia of the robot.
- * @param G the gearing reduction as output over input.
+ * @param motor The motor (or gearbox) driving the drivetrain.
+ * @param massKg The mass of the robot in kilograms.
+ * @param rMeters The radius of the wheels in meters.
+ * @param rbMeters The radius of the base (half the track width) in meters.
+ * @param JKgMetersSquared The moment of inertia of the robot.
+ * @param G The gearing reduction as output over input.
* @return A LinearSystem representing a differential drivetrain.
* @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or G <= 0.
*/
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
DCMotor motor,
double massKg,
@@ -181,16 +178,15 @@
* angular velocity], inputs are [voltage], and outputs are [angle].
*
* @param motor The motor (or gearbox) attached to the arm.
- * @param jKgSquaredMeters The moment of inertia J of the arm.
+ * @param JKgSquaredMeters The moment of inertia J of the arm.
* @param G The gearing between the motor and arm, in output over input. Most of the time this
* will be greater than 1.
* @return A LinearSystem representing the given characterized constants.
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
- DCMotor motor, double jKgSquaredMeters, double G) {
- if (jKgSquaredMeters <= 0.0) {
- throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero.");
+ DCMotor motor, double JKgSquaredMeters, double G) {
+ if (JKgSquaredMeters <= 0.0) {
+ throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
}
if (G <= 0.0) {
throw new IllegalArgumentException("G must be greater than zero.");
@@ -204,27 +200,30 @@
0,
-Math.pow(G, 2)
* motor.KtNMPerAmp
- / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)),
- VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)),
+ / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
+ VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
}
/**
- * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
- * constants cam be found using SysId. The states of the system are [velocity], inputs are
- * [voltage], and outputs are [velocity].
+ * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
+ * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+ * [velocity], inputs are [voltage], and outputs are [velocity].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.
*
- * @param kV The velocity gain, in volts per (units per second)
- * @param kA The acceleration gain, in volts per (units per second squared)
+ * <p>The parameters provided by the user are from this feedforward model:
+ *
+ * <p>u = K_v v + K_a a
+ *
+ * @param kV The velocity gain, in volts/(unit/sec)
+ * @param kA The acceleration gain, in volts/(unit/sec^2)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV <= 0 or kA <= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
if (kV <= 0.0) {
throw new IllegalArgumentException("Kv must be greater than zero.");
@@ -241,20 +240,23 @@
}
/**
- * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
- * constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs
- * are [voltage], and outputs are [position].
+ * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
+ * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+ * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.
*
- * @param kV The velocity gain, in volts per (units per second)
- * @param kA The acceleration gain, in volts per (units per second squared)
+ * <p>The parameters provided by the user are from this feedforward model:
+ *
+ * <p>u = K_v v + K_a a
+ *
+ * @param kV The velocity gain, in volts/(unit/sec)
+ * @param kA The acceleration gain, in volts/(unit/sec²)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV <= 0 or kA <= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
if (kV <= 0.0) {
throw new IllegalArgumentException("Kv must be greater than zero.");
@@ -271,22 +273,23 @@
}
/**
- * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
- * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
- * volts/(meter/sec^2)) cases. This can be found using SysId. The states of the system are [left
- * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
- * velocity, right velocity]ᵀ.
+ * 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.
*
- * @param kVLinear The linear velocity gain, volts per (meter per second).
- * @param kALinear The linear acceleration gain, volts per (meter per second squared).
- * @param kVAngular The angular velocity gain, volts per (meter per second).
- * @param kAAngular The angular acceleration gain, volts per (meter per second squared).
+ * <p>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 (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).
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or
* kAAngular <= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
if (kVLinear <= 0.0) {
@@ -315,23 +318,25 @@
}
/**
- * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
- * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
- * volts/(radian/sec^2)) cases. This can be found using SysId. The states of the system are [left
- * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
- * velocity, right velocity]ᵀ.
+ * 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.
*
- * @param kVLinear The linear velocity gain, volts per (meter per second).
- * @param kALinear The linear acceleration gain, volts per (meter per second squared).
- * @param kVAngular The angular velocity gain, volts per (radians per second).
- * @param kAAngular The angular acceleration gain, volts per (radians per second squared).
- * @param trackwidth The width of the drivetrain in meters.
+ * <p>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 (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.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* kAAngular <= 0, or trackwidth <= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
- @SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
if (kVLinear <= 0.0) {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
index 2ec4244..f716dc0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
@@ -44,7 +44,6 @@
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
- @SuppressWarnings("ParameterName")
private static double lerp(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * t;
}
@@ -57,7 +56,6 @@
* @param t The fraction for interpolation.
* @return The interpolated pose.
*/
- @SuppressWarnings("ParameterName")
private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
return startValue.plus((endValue.minus(startValue)).times(t));
}
@@ -254,7 +252,6 @@
* Represents a time-parameterized trajectory. The trajectory contains of various States that
* represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
*/
- @SuppressWarnings("MemberName")
public static class State {
// The time elapsed since the beginning of the trajectory.
@JsonProperty("time")
@@ -309,7 +306,6 @@
* @param i The interpolant (fraction).
* @return The interpolated state.
*/
- @SuppressWarnings("ParameterName")
State interpolate(State endValue, double i) {
// Find the new t value.
final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
@@ -332,7 +328,7 @@
final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
// Calculate the change in position.
- // delta_s = v_0 t + 0.5 at^2
+ // delta_s = v_0 t + 0.5at²
final double newS =
(velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
index 0827c15..ce90ce6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
@@ -15,14 +15,13 @@
import edu.wpi.first.math.spline.SplineParameterizer;
import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
import java.util.ArrayList;
-import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.function.BiConsumer;
public final class TrajectoryGenerator {
private static final Trajectory kDoNothingTrajectory =
- new Trajectory(Arrays.asList(new Trajectory.State()));
+ new Trajectory(List.of(new Trajectory.State()));
private static BiConsumer<String, StackTraceElement[]> errorFunc;
/** Private constructor because this is a utility class. */
@@ -193,7 +192,6 @@
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
- @SuppressWarnings("LocalVariableName")
public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
@@ -263,7 +261,6 @@
}
// Work around type erasure signatures
- @SuppressWarnings("serial")
public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
public ControlVectorList(int initialCapacity) {
super(initialCapacity);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
index d7fbf59..cf9b6f8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
@@ -90,7 +90,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.maxVelocityMetersPerSecond =
Math.min(
maxVelocityMetersPerSecond,
@@ -164,7 +164,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.
double newMaxVelocity =
Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
@@ -294,7 +294,6 @@
}
}
- @SuppressWarnings("MemberName")
private static class ConstrainedState {
PoseWithCurvature pose;
double distanceMeters;
@@ -320,7 +319,6 @@
}
}
- @SuppressWarnings("serial")
public static class TrajectoryGenerationException extends RuntimeException {
public TrajectoryGenerationException(String message) {
super(message);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
index 5fb2c34..2cbba90 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
@@ -68,7 +68,7 @@
}
/**
- * 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
* @return The trajectory represented by the file.
@@ -90,7 +90,7 @@
}
/**
- * Deserializes a Trajectory from PathWeaver-style JSON.
+ * Deserializes a Trajectory from JSON exported from PathWeaver.
*
* @param json The string containing the serialized JSON
* @return the trajectory represented by the JSON
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index d41a8eb..3b6559e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -51,10 +51,8 @@
private double m_endDeccel;
public static class Constraints {
- @SuppressWarnings("MemberName")
public final double maxVelocity;
- @SuppressWarnings("MemberName")
public final double maxAcceleration;
/**
@@ -71,10 +69,8 @@
}
public static class State {
- @SuppressWarnings("MemberName")
public double position;
- @SuppressWarnings("MemberName")
public double velocity;
public State() {}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
index 13138e8..7a47fca 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -38,12 +38,12 @@
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
- // 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)
return Math.sqrt(
m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
index 4c7e814..0cc6bc5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -111,7 +111,7 @@
/ 2);
}
- // When turning about a point inside of the wheelbase (i.e. radius less than half
+ // When turning about a point inside the wheelbase (i.e. radius less than half
// the trackwidth), the inner wheel's direction changes, but the magnitude remains
// the same. The formula above changes sign for the inner wheel when this happens.
// We can accurately account for this by simply negating the inner wheel.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
index c3bd226..e0647f2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
@@ -23,7 +23,6 @@
* @param rotation The rotation to apply to all radii around the origin.
* @param constraint The constraint to enforce when the robot is within the region.
*/
- @SuppressWarnings("ParameterName")
public EllipticalRegionConstraint(
Translation2d center,
double xWidth,
@@ -65,11 +64,16 @@
* @return Whether the robot pose is within the constraint region.
*/
public boolean isPoseInRegion(Pose2d robotPose) {
- // 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 Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
+ Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
<= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
index bbf30f7..cc0087c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
@@ -36,7 +36,6 @@
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
/** Represents a minimum and maximum acceleration. */
- @SuppressWarnings("MemberName")
class MinMax {
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
index 017f4d0..ce2b38e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
@@ -91,7 +91,7 @@
* Converts given degrees to rotations.
*
* @param degrees The degrees to convert.
- * @return rotations Converted from radians.
+ * @return rotations Converted from degrees.
*/
public static double degreesToRotations(double degrees) {
return degrees / 360;
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
diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
new file mode 100644
index 0000000..2927847
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class ComputerVisionUtilTest {
+ @Test
+ void testObjectToRobotPose() {
+ var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
+ var cameraToObject =
+ new Transform3d(
+ new Translation3d(1.0, 1.0, 1.0),
+ new Rotation3d(0.0, Units.degreesToRadians(-20.0), Units.degreesToRadians(45.0)));
+ var robotToCamera =
+ new Transform3d(
+ new Translation3d(1.0, 0.0, 2.0),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(25.0)));
+ Pose3d object = robot.plus(robotToCamera).plus(cameraToObject);
+
+ assertEquals(
+ robot, ComputerVisionUtil.objectToRobotPose(object, cameraToObject, robotToCamera));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
index 66daf3d..3050867 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
@@ -10,7 +10,6 @@
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
-@SuppressWarnings({"ParameterName", "LocalVariableName"})
class DrakeTest {
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
for (int i = 0; i < A.numRows(); i++) {
diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index bb116ce..a3de9cc 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -10,7 +10,7 @@
class MathUtilTest {
@Test
- void testApplyDeadband() {
+ void testApplyDeadbandUnityScale() {
// < 0
assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02));
assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02));
@@ -28,6 +28,27 @@
}
@Test
+ void testApplyDeadbandArbitraryScale() {
+ // < 0
+ assertEquals(-2.5, MathUtil.applyDeadband(-2.5, 0.02, 2.5));
+ assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02, 2.5));
+ assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02, 2.5));
+
+ // == 0
+ assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02, 2.5));
+
+ // > 0
+ assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02, 2.5));
+ assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02, 2.5));
+ assertEquals(2.5, MathUtil.applyDeadband(2.5, 0.02, 2.5));
+ }
+
+ @Test
+ void testApplyDeadbandLargeMaxMagnitude() {
+ assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY));
+ }
+
+ @Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.
diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 8cd1e5a..054c29a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -50,7 +50,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testIsStabilizable() {
Matrix<N2, N2> A;
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
@@ -77,7 +76,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testIsDetectable() {
Matrix<N2, N2> A;
Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
new file mode 100644
index 0000000..5488b68
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class VectorTest {
+ @Test
+ void testVectorDot() {
+ var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+ var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+
+ assertEquals(32.0, vec1.dot(vec2));
+
+ var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+ var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+
+ assertEquals(-32.0, vec3.dot(vec4));
+ }
+
+ @Test
+ void testVectorNorm() {
+ assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
+ assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
index f64608e..fcd9de1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
@@ -14,7 +14,6 @@
import org.junit.jupiter.api.Test;
class ControlAffinePlantInversionFeedforwardTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testCalculate() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
@@ -25,7 +24,6 @@
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
}
- @SuppressWarnings("LocalVariableName")
@Test
void testCalculateState() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
@@ -36,7 +34,6 @@
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
}
- @SuppressWarnings("ParameterName")
protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
return Matrix.mat(Nat.N2(), Nat.N2())
.fill(1.000, 0, 0, 1.000)
@@ -44,7 +41,6 @@
.plus(VecBuilder.fill(0, 1).times(u));
}
- @SuppressWarnings("ParameterName")
protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
new file mode 100644
index 0000000..b23ee1e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
@@ -0,0 +1,303 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveAccelerationLimiterTest {
+ @Test
+ void testLowLimits() {
+ final double trackwidth = 0.9;
+ final double dt = 0.005;
+ final double maxA = 2.0;
+ final double maxAlpha = 2.0;
+
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+ var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
+
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+ var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+ // Ensure voltage exceeds acceleration before limiting
+ {
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ assertTrue(Math.abs(a) > maxA);
+ }
+ {
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0)));
+ final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+ assertTrue(Math.abs(alpha) > maxAlpha);
+ }
+
+ // Forward
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(
+ plant
+ .getB()
+ .times(
+ new MatBuilder<>(Nat.N2(), Nat.N1())
+ .fill(voltages.left, voltages.right)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+ assertTrue(Math.abs(a) <= maxA);
+ assertTrue(Math.abs(alpha) <= maxAlpha);
+ }
+
+ // Backward
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(
+ plant
+ .getB()
+ .times(
+ new MatBuilder<>(Nat.N2(), Nat.N1())
+ .fill(voltages.left, voltages.right)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+ assertTrue(Math.abs(a) <= maxA);
+ assertTrue(Math.abs(alpha) <= maxAlpha);
+ }
+
+ // Rotate CCW
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(
+ plant
+ .getB()
+ .times(
+ new MatBuilder<>(Nat.N2(), Nat.N1())
+ .fill(voltages.left, voltages.right)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+ assertTrue(Math.abs(a) <= maxA);
+ assertTrue(Math.abs(alpha) <= maxAlpha);
+ }
+ }
+
+ @Test
+ void testHighLimits() {
+ final double trackwidth = 0.9;
+ final double dt = 0.005;
+
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+ // Limits are so high, they don't get hit, so states of constrained and
+ // unconstrained systems should match
+ var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3);
+
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+ var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+ // Forward
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+ assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+ }
+
+ // Backward
+ x.fill(0.0);
+ xAccelLimiter.fill(0.0);
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+ assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+ }
+
+ // Rotate CCW
+ x.fill(0.0);
+ xAccelLimiter.fill(0.0);
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+ assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+ }
+ }
+
+ @Test
+ void testSeperateMinMaxLowLimits() {
+ final double trackwidth = 0.9;
+ final double dt = 0.005;
+ final double minA = -1.0;
+ final double maxA = 2.0;
+ final double maxAlpha = 2.0;
+
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+ var accelLimiter =
+ new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
+
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+ var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+ // Ensure voltage exceeds acceleration before limiting
+ {
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ System.out.println(a);
+ assertTrue(Math.abs(a) > maxA);
+ assertTrue(Math.abs(a) > -minA);
+ }
+
+ // a should always be within [minA, maxA]
+ // Forward
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(
+ plant
+ .getB()
+ .times(
+ new MatBuilder<>(Nat.N2(), Nat.N1())
+ .fill(voltages.left, voltages.right)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ assertTrue(minA <= a && a <= maxA);
+ }
+
+ // Backward
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ final var voltages =
+ accelLimiter.calculate(
+ xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+ xAccelLimiter =
+ plant.calculateX(
+ xAccelLimiter,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+ dt);
+
+ final var accels =
+ plant
+ .getA()
+ .times(xAccelLimiter)
+ .plus(
+ plant
+ .getB()
+ .times(
+ new MatBuilder<>(Nat.N2(), Nat.N1())
+ .fill(voltages.left, voltages.right)));
+ final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+ assertTrue(minA <= a && a <= maxA);
+ }
+ }
+
+ @Test
+ void testMinAccelGreaterThanMaxAccel() {
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+ assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> new DifferentialDriveAccelerationLimiter(plant, 1, 1, -1, 1e3));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java
new file mode 100644
index 0000000..fdf53ea
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java
@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveFeedforwardTest {
+ private static final double kVLinear = 1.0;
+ private static final double kALinear = 1.0;
+ private static final double kVAngular = 1.0;
+ private static final double kAAngular = 1.0;
+ private static final double trackwidth = 1.0;
+ private static final double dtSeconds = 0.02;
+
+ @Test
+ void testCalculateWithTrackwidth() {
+ DifferentialDriveFeedforward differentialDriveFeedforward =
+ new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+ LinearSystem<N2, N2, N2> plant =
+ LinearSystemId.identifyDrivetrainSystem(
+ kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+ for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+ for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+ for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+ for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+ DifferentialDriveWheelVoltages u =
+ differentialDriveFeedforward.calculate(
+ currentLeftVelocity,
+ nextLeftVelocity,
+ currentRightVelocity,
+ nextRightVelocity,
+ dtSeconds);
+ Matrix<N2, N1> nextX =
+ plant.calculateX(
+ VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+ VecBuilder.fill(u.left, u.right),
+ dtSeconds);
+ assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+ assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+ }
+ }
+ }
+ }
+ }
+
+ @Test
+ void testCalculateWithoutTrackwidth() {
+ DifferentialDriveFeedforward differentialDriveFeedforward =
+ new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+ LinearSystem<N2, N2, N2> plant =
+ LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+ for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+ for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+ for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+ for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+ DifferentialDriveWheelVoltages u =
+ differentialDriveFeedforward.calculate(
+ currentLeftVelocity,
+ nextLeftVelocity,
+ currentRightVelocity,
+ nextRightVelocity,
+ dtSeconds);
+ Matrix<N2, N1> nextX =
+ plant.calculateX(
+ VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+ VecBuilder.fill(u.left, u.right),
+ dtSeconds);
+ assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+ assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
new file mode 100644
index 0000000..674cba3
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class ImplicitModelFollowerTest {
+ @Test
+ void testSameModel() {
+ final double dt = 0.005;
+
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+ var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
+
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+ var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+ // Forward
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertEquals(x.get(0, 0), xImf.get(0, 0));
+ assertEquals(x.get(1, 0), xImf.get(1, 0));
+ }
+
+ // Backward
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertEquals(x.get(0, 0), xImf.get(0, 0));
+ assertEquals(x.get(1, 0), xImf.get(1, 0));
+ }
+
+ // Rotate CCW
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertEquals(x.get(0, 0), xImf.get(0, 0));
+ assertEquals(x.get(1, 0), xImf.get(1, 0));
+ }
+ }
+
+ @Test
+ void testSlowerRefModel() {
+ final double dt = 0.005;
+
+ var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+ // Linear acceleration is slower, but angular acceleration is the same
+ var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
+
+ var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
+
+ var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+ var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+ // Forward
+ var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertTrue(x.get(0, 0) >= xImf.get(0, 0));
+ assertTrue(x.get(1, 0) >= xImf.get(1, 0));
+ }
+
+ // Backward
+ x.fill(0.0);
+ xImf.fill(0.0);
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertTrue(x.get(0, 0) <= xImf.get(0, 0));
+ assertTrue(x.get(1, 0) <= xImf.get(1, 0));
+ }
+
+ // Rotate CCW
+ x.fill(0.0);
+ xImf.fill(0.0);
+ u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+ for (double t = 0.0; t < 3.0; t += dt) {
+ x = plant.calculateX(x, u, dt);
+ xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+ assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5);
+ assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5);
+ }
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
new file mode 100644
index 0000000..efb9c5e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVDifferentialDriveControllerTest {
+ private static final double kTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = Math.toRadians(2);
+
+ /** States of the drivetrain system. */
+ static class State {
+ /// X position in global coordinate frame.
+ public static final int kX = 0;
+
+ /// Y position in global coordinate frame.
+ public static final int kY = 1;
+
+ /// Heading in global coordinate frame.
+ public static final int kHeading = 2;
+
+ /// Left encoder velocity.
+ public static final int kLeftVelocity = 3;
+
+ /// Right encoder velocity.
+ public static final int kRightVelocity = 4;
+ }
+
+ private static final double kLinearV = 3.02; // V/(m/s)
+ private static final double kLinearA = 0.642; // V/(m/s²)
+ private static final double kAngularV = 1.382; // V/(m/s)
+ private static final double kAngularA = 0.08495; // V/(m/s²)
+ private static final LinearSystem<N2, N2, N2> plant =
+ LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
+ private static final double kTrackwidth = 0.9;
+
+ private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+ double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0;
+
+ var xdot = new Matrix<>(Nat.N5(), Nat.N1());
+ xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0)));
+ xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0)));
+ xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth);
+ xdot.assignBlock(
+ 3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u)));
+ return xdot;
+ }
+
+ @Test
+ void testReachesReference() {
+ final double kDt = 0.02;
+
+ final var controller =
+ new LTVDifferentialDriveController(
+ plant,
+ kTrackwidth,
+ VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
+ VecBuilder.fill(12.0, 12.0),
+ kDt);
+ var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+ waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ var x =
+ new MatBuilder<>(Nat.N5(), Nat.N1())
+ .fill(
+ robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
+
+ final var totalTime = trajectory.getTotalTimeSeconds();
+ for (int i = 0; i < (totalTime / kDt); ++i) {
+ var state = trajectory.sample(kDt * i);
+ robotPose =
+ new Pose2d(
+ x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0)));
+ final var output =
+ controller.calculate(
+ robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
+
+ x =
+ NumericalIntegration.rkdp(
+ LTVDifferentialDriveControllerTest::dynamics,
+ x,
+ new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
+ kDt);
+ }
+
+ final var states = trajectory.getStates();
+ final var endPose = states.get(states.size() - 1).poseMeters;
+
+ // Java lambdas require local variables referenced from a lambda expression
+ // must be final or effectively final.
+ final var finalRobotPose = robotPose;
+ assertAll(
+ () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+ () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+ () ->
+ assertEquals(
+ 0.0,
+ MathUtil.angleModulus(
+ endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+ kAngularTolerance));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java
new file mode 100644
index 0000000..463e1ce
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java
@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVUnicycleControllerTest {
+ private static final double kTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = Math.toRadians(2);
+
+ @Test
+ void testReachesReference() {
+ final double kDt = 0.02;
+
+ final var controller =
+ new LTVUnicycleController(
+ VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
+ var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+ waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final var totalTime = trajectory.getTotalTimeSeconds();
+ for (int i = 0; i < (totalTime / kDt); ++i) {
+ var state = trajectory.sample(kDt * i);
+
+ var output = controller.calculate(robotPose, state);
+ robotPose =
+ robotPose.exp(
+ new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
+ }
+
+ final var states = trajectory.getStates();
+ final var endPose = states.get(states.size() - 1).poseMeters;
+
+ // Java lambdas require local variables referenced from a lambda expression
+ // must be final or effectively final.
+ final var finalRobotPose = robotPose;
+ assertAll(
+ () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+ () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+ () ->
+ assertEquals(
+ 0.0,
+ MathUtil.angleModulus(
+ endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+ kAngularTolerance));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
index 98b0e6c..1aba6d7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
@@ -14,7 +14,6 @@
import org.junit.jupiter.api.Test;
class LinearPlantInversionFeedforwardTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testCalculate() {
Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 93b89c8..31213f6 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -17,7 +17,6 @@
class LinearQuadraticRegulatorTest {
@Test
- @SuppressWarnings("LocalVariableName")
void testLQROnElevator() {
var motors = DCMotor.getVex775Pro(2);
@@ -38,7 +37,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testFourMotorElevator() {
var dt = 0.020;
@@ -55,7 +53,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testLQROnArm() {
var motors = DCMotor.getVex775Pro(2);
@@ -89,7 +86,6 @@
* @param Aref Desired state matrix.
* @param dtSeconds Discretization timestep in seconds.
*/
- @SuppressWarnings({"LocalVariableName", "MethodTypeParameterName", "ParameterName"})
<States extends Num, Inputs extends Num> Matrix<Inputs, States> getImplicitModelFollowingK(
Matrix<States, States> A,
Matrix<States, Inputs> B,
@@ -114,7 +110,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testMatrixOverloadsWithSingleIntegrator() {
var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0);
var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
@@ -138,7 +133,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testMatrixOverloadsWithDoubleIntegrator() {
double Kv = 3.02;
double Ka = 0.642;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
index 5fa4939..7b8484d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
@@ -40,7 +40,6 @@
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
- @SuppressWarnings("LocalVariableName")
private static void updateTwoState(
LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
@@ -50,7 +49,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testStateSpaceEnabled() {
m_loop.reset(VecBuilder.fill(0, 0));
Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
@@ -79,7 +77,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testFlywheelEnabled() {
LinearSystem<N1, N1, N1> plant =
LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
index 83fced2..fd4e428 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
@@ -13,7 +13,6 @@
import org.junit.jupiter.api.Test;
class SimpleMotorFeedforwardTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testCalculate() {
double Ks = 0.5;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index 78908e4..c3906d4 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -6,34 +6,37 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
import org.junit.jupiter.api.Test;
class DifferentialDrivePoseEstimatorTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testAccuracy() {
+ var kinematics = new DifferentialDriveKinematics(1);
+
var estimator =
new DifferentialDrivePoseEstimator(
+ kinematics,
new Rotation2d(),
+ 0,
+ 0,
new Pose2d(),
- new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
- new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
- new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
-
- var traj =
+ VecBuilder.fill(0.02, 0.02, 0.01),
+ VecBuilder.fill(0.1, 0.1, 0.1));
+ var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -41,67 +44,165 @@
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(10, 5));
+ new TrajectoryConfig(2, 2));
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ trajectory.getInitialPose(),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 0.25,
+ true);
+ }
+
+ @Test
+ void testBadInitialPose() {
var kinematics = new DifferentialDriveKinematics(1);
- var rand = new Random(4915);
- final double dt = 0.02;
+ var estimator =
+ new DifferentialDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ 0,
+ 0,
+ new Pose2d(),
+ VecBuilder.fill(0.02, 0.02, 0.01),
+ VecBuilder.fill(0.1, 0.1, 0.1));
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(2, 2));
+
+ for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+ for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+ var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+ var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+ var initial_pose =
+ trajectory
+ .getInitialPose()
+ .plus(
+ new Transform2d(
+ new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+ heading_offset));
+
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ initial_pose,
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 0.25,
+ false);
+ }
+ }
+ }
+
+ void testFollowTrajectory(
+ final DifferentialDriveKinematics kinematics,
+ final DifferentialDrivePoseEstimator estimator,
+ final Trajectory trajectory,
+ final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+ final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+ final Pose2d startingPose,
+ final Pose2d endingPose,
+ final double dt,
+ final double visionUpdateRate,
+ final double visionUpdateDelay,
+ final boolean checkError) {
+ double leftDistanceMeters = 0;
+ double rightDistanceMeters = 0;
+
+ estimator.resetPosition(
+ new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose);
+
+ var rand = new Random(3538);
+
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
- double distanceLeft = 0.0;
- double distanceRight = 0.0;
+ final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
- Trajectory.State groundtruthState;
- DifferentialDriveWheelSpeeds input;
- while (t <= traj.getTotalTimeSeconds()) {
- groundtruthState = traj.sample(t);
- input =
- kinematics.toWheelSpeeds(
- new ChassisSpeeds(
- groundtruthState.velocityMetersPerSecond,
- 0.0,
- // ds/dt * dtheta/ds = dtheta/dt
- groundtruthState.velocityMetersPerSecond
- * groundtruthState.curvatureRadPerMeter));
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- var groundPose = groundtruthState.poseMeters;
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()));
- lastVisionUpdateTime = t;
+ // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+ // last vision measurement
+ if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+ Pose2d newVisionPose =
+ visionMeasurementGenerator
+ .apply(groundTruthState)
+ .plus(
+ new Transform2d(
+ new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+ new Rotation2d(rand.nextGaussian() * 0.05)));
+
+ visionUpdateQueue.put(t, newVisionPose);
}
- input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
- input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+ // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+ // since it was measured
+ if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+ var visionEntry = visionUpdateQueue.pollFirstEntry();
+ estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+ }
- distanceLeft += input.leftMetersPerSecond * dt;
- distanceRight += input.rightMetersPerSecond * dt;
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
- var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+ leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
+ rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
+
var xHat =
estimator.updateWithTime(
t,
- groundtruthState.poseMeters.getRotation().plus(rotNoise),
- input,
- distanceLeft,
- distanceRight);
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+ .minus(trajectory.getInitialPose().getRotation()),
+ leftDistanceMeters,
+ rightDistanceMeters);
+
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians());
double error =
- groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -110,7 +211,20 @@
t += dt;
}
- assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.055, "Incorrect max error");
+ assertEquals(
+ endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+ assertEquals(
+ endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+ assertEquals(
+ endingPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ 0.15,
+ "Incorrect Final Theta");
+
+ if (checkError) {
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+ }
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
index 5b07ebc..112ef90 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
@@ -26,7 +26,6 @@
import java.util.List;
import org.junit.jupiter.api.Test;
-@SuppressWarnings("ParameterName")
class ExtendedKalmanFilterTest {
private static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
final var motors = DCMotor.getCIM(2);
@@ -68,7 +67,6 @@
return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
- @SuppressWarnings("LocalVariableName")
@Test
void testInit() {
double dtSeconds = 0.00505;
@@ -99,7 +97,6 @@
});
}
- @SuppressWarnings("LocalVariableName")
@Test
void testConvergence() {
double dtSeconds = 0.00505;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
index 4d709be..848fe93 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
@@ -34,7 +34,6 @@
createElevator();
}
- @SuppressWarnings("LocalVariableName")
private static void createElevator() {
var motors = DCMotor.getVex775Pro(2);
@@ -61,7 +60,6 @@
new Matrix<>(Nat.N3(), Nat.N3())); // D
@Test
- @SuppressWarnings("LocalVariableName")
void testElevatorKalmanFilter() {
var Q = VecBuilder.fill(0.05, 1.0);
var R = VecBuilder.fill(0.0001);
@@ -136,7 +134,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testSwerveKFMovingOverTrajectory() {
var random = new Random();
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index d8756f0..02d2d52 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -9,87 +9,186 @@
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
import org.junit.jupiter.api.Test;
class MecanumDrivePoseEstimatorTest {
@Test
- @SuppressWarnings("LocalVariableName")
- void testAccuracy() {
+ void testAccuracyFacingTrajectory() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
+ var wheelPositions = new MecanumDriveWheelPositions();
+
var estimator =
new MecanumDrivePoseEstimator(
- new Rotation2d(),
- new Pose2d(),
kinematics,
+ new Rotation2d(),
+ wheelPositions,
+ new Pose2d(),
VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.05),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ VecBuilder.fill(0.45, 0.45, 0.1));
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
- new Pose2d(),
- new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
- new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
- new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
- new TrajectoryConfig(0.5, 2));
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(2, 2));
- var rand = new Random(5190);
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ trajectory.getInitialPose(),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 0.25,
+ true);
+ }
- final double dt = 0.02;
+ @Test
+ void testBadInitialPose() {
+ var kinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(1, 1), new Translation2d(1, -1),
+ new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+ var wheelPositions = new MecanumDriveWheelPositions();
+
+ var estimator =
+ new MecanumDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ wheelPositions,
+ new Pose2d(),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.45, 0.45, 0.1));
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(2, 2));
+
+ for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+ for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+ var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+ var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+ var initial_pose =
+ trajectory
+ .getInitialPose()
+ .plus(
+ new Transform2d(
+ new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+ heading_offset));
+
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ initial_pose,
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 0.25,
+ false);
+ }
+ }
+ }
+
+ void testFollowTrajectory(
+ final MecanumDriveKinematics kinematics,
+ final MecanumDrivePoseEstimator estimator,
+ final Trajectory trajectory,
+ final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+ final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+ final Pose2d startingPose,
+ final Pose2d endingPose,
+ final double dt,
+ final double visionUpdateRate,
+ final double visionUpdateDelay,
+ final boolean checkError) {
+ var wheelPositions = new MecanumDriveWheelPositions();
+
+ estimator.resetPosition(new Rotation2d(), wheelPositions, startingPose);
+
+ var rand = new Random(3538);
+
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+
+ final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
+ // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+ // last vision measurement
+ if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+ Pose2d newVisionPose =
+ visionMeasurementGenerator
+ .apply(groundTruthState)
+ .plus(
+ new Transform2d(
+ new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+ new Rotation2d(rand.nextGaussian() * 0.05)));
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
+ visionUpdateQueue.put(t, newVisionPose);
}
- var wheelSpeeds =
- kinematics.toWheelSpeeds(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond,
- 0,
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.curvatureRadPerMeter));
+ // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+ // since it was measured
+ if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+ var visionEntry = visionUpdateQueue.pollFirstEntry();
+ estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+ }
- wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+ wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+ wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+ wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+ wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
var xHat =
estimator.updateWithTime(
@@ -97,8 +196,19 @@
groundTruthState
.poseMeters
.getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
- wheelSpeeds);
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+ .minus(trajectory.getInitialPose().getRotation()),
+ wheelPositions);
+
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -111,7 +221,19 @@
}
assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+ endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+ assertEquals(
+ endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+ assertEquals(
+ endingPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ 0.15,
+ "Incorrect Final Theta");
+
+ if (checkError) {
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+ }
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
index 3c51d6c..2131c35 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -16,7 +16,7 @@
void testZeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
- merweScaledSigmaPoints.sigmaPoints(
+ merweScaledSigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
assertTrue(
@@ -31,8 +31,8 @@
void testNonzeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
- merweScaledSigmaPoints.sigmaPoints(
- VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
+ merweScaledSigmaPoints.squareRootSigmaPoints(
+ VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
assertTrue(
points.isEqual(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index c6126c6..fde2c39 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -9,33 +9,43 @@
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
import org.junit.jupiter.api.Test;
class SwerveDrivePoseEstimatorTest {
@Test
- @SuppressWarnings("LocalVariableName")
- void testAccuracy() {
+ void testAccuracyFacingTrajectory() {
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
+
+ var fl = new SwerveModulePosition();
+ var fr = new SwerveModulePosition();
+ var bl = new SwerveModulePosition();
+ var br = new SwerveModulePosition();
+
var estimator =
new SwerveDrivePoseEstimator(
- new Rotation2d(),
- new Pose2d(),
kinematics,
+ new Rotation2d(),
+ new SwerveModulePosition[] {fl, fr, bl, br},
+ new Pose2d(),
VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.005),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ VecBuilder.fill(0.5, 0.5, 0.5));
var trajectory =
TrajectoryGenerator.generateTrajectory(
@@ -45,49 +55,158 @@
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(0.5, 2));
+ new TrajectoryConfig(2, 2));
- var rand = new Random(4915);
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ trajectory.getInitialPose(),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 0.25,
+ true);
+ }
- final double dt = 0.02;
+ @Test
+ void testBadInitialPose() {
+ var kinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1),
+ new Translation2d(-1, 1));
+
+ var fl = new SwerveModulePosition();
+ var fr = new SwerveModulePosition();
+ var bl = new SwerveModulePosition();
+ var br = new SwerveModulePosition();
+
+ var estimator =
+ new SwerveDrivePoseEstimator(
+ kinematics,
+ new Rotation2d(),
+ new SwerveModulePosition[] {fl, fr, bl, br},
+ new Pose2d(-1, -1, Rotation2d.fromRadians(-1)),
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(2, 2));
+
+ for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+ for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+ var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+ var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+ var initial_pose =
+ trajectory
+ .getInitialPose()
+ .plus(
+ new Transform2d(
+ new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+ heading_offset));
+
+ testFollowTrajectory(
+ kinematics,
+ estimator,
+ trajectory,
+ state ->
+ new ChassisSpeeds(
+ state.velocityMetersPerSecond,
+ 0,
+ state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+ state -> state.poseMeters,
+ initial_pose,
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ 0.02,
+ 0.1,
+ 1.0,
+ false);
+ }
+ }
+ }
+
+ void testFollowTrajectory(
+ final SwerveDriveKinematics kinematics,
+ final SwerveDrivePoseEstimator estimator,
+ final Trajectory trajectory,
+ final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+ final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+ final Pose2d startingPose,
+ final Pose2d endingPose,
+ final double dt,
+ final double visionUpdateRate,
+ final double visionUpdateDelay,
+ final boolean checkError) {
+ SwerveModulePosition[] positions = {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ };
+
+ estimator.resetPosition(new Rotation2d(), positions, startingPose);
+
+ var rand = new Random(3538);
+
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print(
+ "time, est_x, est_y, est_theta, true_x, true_y, true_theta, "
+ + "distance_1, distance_2, distance_3, distance_4, "
+ + "angle_1, angle_2, angle_3, angle_4\n");
+
+ final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
+ // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+ // last vision measurement
+ if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+ Pose2d newVisionPose =
+ visionMeasurementGenerator
+ .apply(groundTruthState)
+ .plus(
+ new Transform2d(
+ new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+ new Rotation2d(rand.nextGaussian() * 0.05)));
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
+ visionUpdateQueue.put(t, newVisionPose);
}
- var moduleStates =
- kinematics.toSwerveModuleStates(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond,
- 0.0,
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.curvatureRadPerMeter));
- for (var moduleState : moduleStates) {
- moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
- moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+ // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+ // since it was measured
+ if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+ var visionEntry = visionUpdateQueue.pollFirstEntry();
+ estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+ }
+
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+ var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
+
+ for (int i = 0; i < moduleStates.length; i++) {
+ positions[i].distanceMeters +=
+ moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt;
+ positions[i].angle =
+ moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
}
var xHat =
@@ -96,8 +215,27 @@
groundTruthState
.poseMeters
.getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
- moduleStates);
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+ .minus(trajectory.getInitialPose().getRotation()),
+ positions);
+
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians(),
+ positions[0].distanceMeters,
+ positions[1].distanceMeters,
+ positions[2].distanceMeters,
+ positions[3].distanceMeters,
+ positions[0].angle.getRadians(),
+ positions[1].angle.getRadians(),
+ positions[2].angle.getRadians(),
+ positions[3].angle.getRadians());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -110,7 +248,19 @@
}
assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+ endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+ assertEquals(
+ endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+ assertEquals(
+ endingPose.getRotation().getRadians(),
+ estimator.getEstimatedPosition().getRotation().getRadians(),
+ 0.15,
+ "Incorrect Final Theta");
+
+ if (checkError) {
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+ }
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index 25def49..a8e4d3b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -16,8 +16,8 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
-import edu.wpi.first.math.numbers.N4;
-import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
@@ -30,92 +30,109 @@
import org.junit.jupiter.api.Test;
class UnscentedKalmanFilterTest {
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
- private static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+ private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
- var gHigh = 7.08;
- var rb = 0.8382 / 2.0;
- var r = 0.0746125;
- var m = 63.503;
- var J = 5.6;
+ // var gLow = 15.32; // Low gear ratio
+ var gHigh = 7.08; // High gear ratio
+ var rb = 0.8382 / 2.0; // Robot radius
+ var r = 0.0746125; // Wheel radius
+ var m = 63.503; // Robot mass
+ var J = 5.6; // Robot moment of inertia
var C1 =
-Math.pow(gHigh, 2)
* motors.KtNMPerAmp
/ (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
+ var k1 = 1.0 / m + Math.pow(rb, 2) / J;
+ var k2 = 1.0 / m - Math.pow(rb, 2) / J;
- var c = x.get(2, 0);
- var s = x.get(3, 0);
- var vl = x.get(4, 0);
- var vr = x.get(5, 0);
-
+ var vl = x.get(3, 0);
+ var vr = x.get(4, 0);
var Vl = u.get(0, 0);
var Vr = u.get(1, 0);
- var k1 = 1.0 / m + rb * rb / J;
- var k2 = 1.0 / m - rb * rb / J;
-
- var xvel = (vl + vr) / 2;
- var w = (vr - vl) / (2.0 * rb);
-
+ var v = 0.5 * (vl + vr);
return VecBuilder.fill(
- xvel * c,
- xvel * s,
- -s * w,
- c * w,
- k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
- k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+ v * Math.cos(x.get(2, 0)),
+ v * Math.sin(x.get(2, 0)),
+ (vr - vl) / (2.0 * rb),
+ k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
+ k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
}
- @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
- private static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
- return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+ return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
- @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
- private static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+ @SuppressWarnings("PMD.UnusedFormalParameter")
+ private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
- @SuppressWarnings("LocalVariableName")
void testInit() {
+ var dtSeconds = 0.005;
assertDoesNotThrow(
() -> {
- UnscentedKalmanFilter<N6, N2, N4> observer =
+ UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
- Nat.N6(),
- Nat.N4(),
+ Nat.N5(),
+ Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
- VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
- VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
- 0.00505);
+ VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+ VecBuilder.fill(0.0001, 0.01, 0.01),
+ AngleStatistics.angleMean(2),
+ AngleStatistics.angleMean(0),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleResidual(0),
+ AngleStatistics.angleAdd(2),
+ dtSeconds);
var u = VecBuilder.fill(12.0, 12.0);
- observer.predict(u, 0.00505);
+ observer.predict(u, dtSeconds);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
+
+ var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+ var R =
+ StateSpaceUtil.makeCovarianceMatrix(
+ Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
+ observer.correct(
+ Nat.N5(),
+ u,
+ globalY,
+ UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+ R,
+ AngleStatistics.angleMean(2),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleAdd(2));
});
}
- @SuppressWarnings("LocalVariableName")
@Test
void testConvergence() {
- double dtSeconds = 0.00505;
+ double dtSeconds = 0.005;
double rbMeters = 0.8382 / 2.0; // Robot radius
- UnscentedKalmanFilter<N6, N2, N4> observer =
+ UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
- Nat.N6(),
- Nat.N4(),
+ Nat.N5(),
+ Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
- VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
- VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+ VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+ VecBuilder.fill(0.0001, 0.5, 0.5),
+ AngleStatistics.angleMean(2),
+ AngleStatistics.angleMean(0),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleResidual(0),
+ AngleStatistics.angleAdd(2),
dtSeconds);
List<Pose2d> waypoints =
@@ -125,56 +142,52 @@
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
- Matrix<N6, N1> nextR;
+ Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
var B =
NumericalJacobian.numericalJacobianU(
- Nat.N6(),
+ Nat.N5(),
Nat.N2(),
UnscentedKalmanFilterTest::getDynamics,
- new Matrix<>(Nat.N6(), Nat.N1()),
- u);
+ new Matrix<>(Nat.N5(), Nat.N1()),
+ new Matrix<>(Nat.N2(), Nat.N1()));
- observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
-
- var ref = trajectory.sample(0.0);
-
- Matrix<N6, N1> r =
+ observer.setXhat(
VecBuilder.fill(
- ref.poseMeters.getTranslation().getX(),
- ref.poseMeters.getTranslation().getY(),
- ref.poseMeters.getRotation().getCos(),
- ref.poseMeters.getRotation().getSin(),
- ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
- ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
- nextR = r.copy();
+ trajectory.getInitialPose().getTranslation().getX(),
+ trajectory.getInitialPose().getTranslation().getY(),
+ trajectory.getInitialPose().getRotation().getRadians(),
+ 0.0,
+ 0.0));
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / dtSeconds); i++) {
- ref = trajectory.sample(dtSeconds * i);
+ var ref = trajectory.sample(dtSeconds * i);
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
- nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
- nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
- nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
- nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
- nextR.set(4, 0, vl);
- nextR.set(5, 0, vr);
+ var nextR =
+ VecBuilder.fill(
+ ref.poseMeters.getTranslation().getX(),
+ ref.poseMeters.getTranslation().getY(),
+ ref.poseMeters.getRotation().getRadians(),
+ vl,
+ vr);
- Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
- var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
+ Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+ var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dtSeconds);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
- r = nextR;
observer.predict(u, dtSeconds);
+
+ r = nextR;
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
}
@@ -183,29 +196,31 @@
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(trueXhat, u);
- var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
+ var R =
+ StateSpaceUtil.makeCovarianceMatrix(
+ Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
- Nat.N6(),
+ Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
R,
- (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
- Matrix::minus,
- Matrix::minus,
- Matrix::plus);
+ AngleStatistics.angleMean(2),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleResidual(2),
+ AngleStatistics.angleAdd(2));
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
- assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
- assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
- assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
- assertEquals(0.0, observer.getXhat(3), 1.0);
- assertEquals(0.0, observer.getXhat(4), 1.0);
+ assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055);
+ assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15);
+ assertEquals(
+ finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005);
+ assertEquals(0.0, observer.getXhat(3), 0.1);
+ assertEquals(0.0, observer.getXhat(4), 0.1);
}
@Test
- @SuppressWarnings({"LocalVariableName", "ParameterName"})
void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
@@ -236,94 +251,22 @@
}
@Test
- void testUnscentedTransform() {
- // From FilterPy
- var ret =
- UnscentedKalmanFilter.unscentedTransform(
- Nat.N4(),
- Nat.N4(),
- Matrix.mat(Nat.N4(), Nat.N9())
- .fill(
- -0.9,
- -0.822540333075852,
- -0.8922540333075852,
- -0.9,
- -0.9,
- -0.9774596669241481,
- -0.9077459666924148,
- -0.9,
- -0.9,
- 1.0,
- 1.0,
- 1.077459666924148,
- 1.0,
- 1.0,
- 1.0,
- 0.9225403330758519,
- 1.0,
- 1.0,
- -0.9,
- -0.9,
- -0.9,
- -0.822540333075852,
- -0.8922540333075852,
- -0.9,
- -0.9,
- -0.9774596669241481,
- -0.9077459666924148,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.077459666924148,
- 1.0,
- 1.0,
- 1.0,
- 0.9225403330758519),
- VecBuilder.fill(
- -132.33333333,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667),
- VecBuilder.fill(
- -129.34333333,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667,
- 16.66666667),
- (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
- Matrix::minus);
+ void testRoundTripP() {
+ var dtSeconds = 0.005;
- assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
+ var observer =
+ new UnscentedKalmanFilter<>(
+ Nat.N2(),
+ Nat.N2(),
+ (x, u) -> x,
+ (x, u) -> x,
+ VecBuilder.fill(0.0, 0.0),
+ VecBuilder.fill(0.0, 0.0),
+ dtSeconds);
- assertTrue(
- Matrix.mat(Nat.N4(), Nat.N4())
- .fill(
- 2.02000002e-01,
- 2.00000500e-02,
- -2.69044710e-29,
- -4.59511477e-29,
- 2.00000500e-02,
- 2.00001000e-01,
- -2.98781068e-29,
- -5.12759588e-29,
- -2.73372625e-29,
- -3.09882635e-29,
- 2.02000002e-01,
- 2.00000500e-02,
- -4.67065917e-29,
- -5.10705197e-29,
- 2.00000500e-02,
- 2.00001000e-01)
- .isEqual(ret.getSecond(), 1E-5));
+ var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
+ observer.setP(P);
+
+ assertTrue(observer.getP().isEqual(P, 1e-9));
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
index f0b39c6..ec43cd5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
@@ -141,7 +141,7 @@
3,
// f(x) = ln(x)
(double x) -> Math.log(x),
- // df/dx = 1 / x
+ // df/dx = 1/x
(double x) -> 1.0 / x,
h,
1.0,
@@ -174,7 +174,7 @@
5,
// f(x) = ln(x)
(double x) -> Math.log(x),
- // d²f/dx² = -1 / x²
+ // d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,
1.0,
@@ -213,7 +213,7 @@
2,
// f(x) = ln(x)
(double x) -> Math.log(x),
- // df/dx = 1 / x
+ // df/dx = 1/x
(double x) -> 1.0 / x,
h,
1.0,
@@ -246,7 +246,7 @@
4,
// f(x) = ln(x)
(double x) -> Math.log(x),
- // d²f/dx² = -1 / x²
+ // d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,
1.0,
@@ -282,7 +282,7 @@
stencil[i] = -(samples - 1) / 2 + i;
}
- var filter = LinearFilter.finiteDifference(derivative, samples, stencil, h);
+ var filter = LinearFilter.finiteDifference(derivative, stencil, h);
for (int i = (int) (min / h); i < (int) (max / h); ++i) {
// Let filter initialize
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
index e7ec644..6e94b3c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
@@ -38,4 +38,13 @@
WPIUtilJNI.setMockTime(1000000L);
assertEquals(limiter.calculate(0.5), 0.5);
}
+
+ @Test
+ void slewRatePositiveNegativeTest() {
+ var limiter = new SlewRateLimiter(1, -0.5, 0);
+ WPIUtilJNI.setMockTime(1000000L);
+ assertEquals(limiter.calculate(2), 1);
+ WPIUtilJNI.setMockTime(2000000L);
+ assertEquals(limiter.calculate(0), 0.5);
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
new file mode 100644
index 0000000..68babdd
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
@@ -0,0 +1,251 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class CoordinateSystemTest {
+ private void checkPose3dConvert(
+ Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
+ // "from" to "to"
+ assertEquals(
+ poseTo.getTranslation(),
+ CoordinateSystem.convert(poseFrom.getTranslation(), coordFrom, coordTo));
+ assertEquals(
+ poseTo.getRotation(), CoordinateSystem.convert(poseFrom.getRotation(), coordFrom, coordTo));
+ assertEquals(poseTo, CoordinateSystem.convert(poseFrom, coordFrom, coordTo));
+
+ // "to" to "from"
+ assertEquals(
+ poseFrom.getTranslation(),
+ CoordinateSystem.convert(poseTo.getTranslation(), coordTo, coordFrom));
+ assertEquals(
+ poseFrom.getRotation(), CoordinateSystem.convert(poseTo.getRotation(), coordTo, coordFrom));
+ assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
+ }
+
+ private void checkTransform3dConvert(
+ Transform3d transformFrom,
+ Transform3d transformTo,
+ CoordinateSystem coordFrom,
+ CoordinateSystem coordTo) {
+ // "from" to "to"
+ assertEquals(
+ transformTo.getTranslation(),
+ CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
+ assertEquals(
+ transformTo.getRotation(),
+ CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
+ assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
+
+ // "to" to "from"
+ assertEquals(
+ transformFrom.getTranslation(),
+ CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
+ assertEquals(
+ transformFrom.getRotation(),
+ CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
+ assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
+ }
+
+ @Test
+ void testPose3dEDNtoNWU() {
+ // No rotation from EDN to NWU
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
+ new Pose3d(
+ 3.0,
+ -1.0,
+ -2.0,
+ new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° roll from EDN to NWU
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+ new Pose3d(
+ 3.0,
+ -1.0,
+ -2.0,
+ new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° pitch from EDN to NWU
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+ new Pose3d(
+ 3.0,
+ -1.0,
+ -2.0,
+ new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° yaw from EDN to NWU
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+ new Pose3d(
+ 3.0,
+ -1.0,
+ -2.0,
+ new Rotation3d(
+ Units.degreesToRadians(-90.0),
+ Units.degreesToRadians(45.0),
+ Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+ }
+
+ @Test
+ void testPose3dEDNtoNED() {
+ // No rotation from EDN to NED
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
+ new Pose3d(
+ 3.0,
+ 1.0,
+ 2.0,
+ new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° roll from EDN to NED
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+ new Pose3d(
+ 3.0,
+ 1.0,
+ 2.0,
+ new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° pitch from EDN to NED
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+ new Pose3d(
+ 3.0,
+ 1.0,
+ 2.0,
+ new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° yaw from EDN to NED
+ checkPose3dConvert(
+ new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+ new Pose3d(
+ 3.0,
+ 1.0,
+ 2.0,
+ new Rotation3d(
+ Units.degreesToRadians(90.0),
+ Units.degreesToRadians(-45.0),
+ Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+ }
+
+ @Test
+ void testTransform3dEDNtoNWU() {
+ // No rotation from EDN to NWU
+ checkTransform3dConvert(
+ new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
+ new Transform3d(
+ new Translation3d(3.0, -1.0, -2.0),
+ new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° roll from EDN to NWU
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+ new Transform3d(
+ new Translation3d(3.0, -1.0, -2.0),
+ new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° pitch from EDN to NWU
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+ new Transform3d(
+ new Translation3d(3.0, -1.0, -2.0),
+ new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+
+ // 45° yaw from EDN to NWU
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+ new Transform3d(
+ new Translation3d(3.0, -1.0, -2.0),
+ new Rotation3d(
+ Units.degreesToRadians(-90.0),
+ Units.degreesToRadians(45.0),
+ Units.degreesToRadians(-90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NWU());
+ }
+
+ @Test
+ void testTransform3dEDNtoNED() {
+ // No rotation from EDN to NED
+ checkTransform3dConvert(
+ new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
+ new Transform3d(
+ new Translation3d(3.0, 1.0, 2.0),
+ new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° roll from EDN to NED
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+ new Transform3d(
+ new Translation3d(3.0, 1.0, 2.0),
+ new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° pitch from EDN to NED
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+ new Transform3d(
+ new Translation3d(3.0, 1.0, 2.0),
+ new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+
+ // 45° yaw from EDN to NED
+ checkTransform3dConvert(
+ new Transform3d(
+ new Translation3d(1.0, 2.0, 3.0),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+ new Transform3d(
+ new Translation3d(3.0, 1.0, 2.0),
+ new Rotation3d(
+ Units.degreesToRadians(90.0),
+ Units.degreesToRadians(-45.0),
+ Units.degreesToRadians(90.0))),
+ CoordinateSystem.EDN(),
+ CoordinateSystem.NED());
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index 5a314b1..780c816 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -21,9 +21,9 @@
var transformed = initial.plus(transformation);
assertAll(
- () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
- () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
- () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon));
+ () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+ () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+ () -> assertEquals(50.0, transformed.getRotation().getDegrees(), kEpsilon));
}
@Test
@@ -34,9 +34,9 @@
var finalRelativeToInitial = last.relativeTo(initial);
assertAll(
- () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
- () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
- () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
+ () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+ () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+ () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
}
@Test
@@ -61,8 +61,8 @@
final var transform = last.minus(initial);
assertAll(
- () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
- () -> assertEquals(transform.getY(), 0.0, kEpsilon),
- () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
+ () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+ () -> assertEquals(0.0, transform.getY(), kEpsilon),
+ () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
new file mode 100644
index 0000000..f13819f
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Pose3dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testTransformByRotations() {
+ var initialPose =
+ new Pose3d(
+ new Translation3d(0.0, 0.0, 0.0),
+ new Rotation3d(
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(0.0)));
+
+ var transform1 =
+ new Transform3d(
+ new Translation3d(0.0, 0.0, 0.0),
+ new Rotation3d(
+ Units.degreesToRadians(90.0),
+ Units.degreesToRadians(45.0),
+ Units.degreesToRadians(0.0)));
+
+ var transform2 =
+ new Transform3d(
+ new Translation3d(0.0, 0.0, 0.0),
+ new Rotation3d(
+ Units.degreesToRadians(-90.0),
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(0.0)));
+
+ var transform3 =
+ new Transform3d(
+ new Translation3d(0.0, 0.0, 0.0),
+ new Rotation3d(
+ Units.degreesToRadians(0.0),
+ Units.degreesToRadians(-45.0),
+ Units.degreesToRadians(0.0)));
+
+ // This sequence of rotations should diverge from the origin and eventually return to it. When
+ // each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
+ // with
+ // the axes of the pose that is being transformed, just like how the translation is done 'from
+ // the
+ // viewpoint' of the pose that is being transformed.
+ var finalPose =
+ initialPose.transformBy(transform1).transformBy(transform2).transformBy(transform3);
+
+ assertAll(
+ () ->
+ assertEquals(
+ finalPose.getRotation().getX(), initialPose.getRotation().getX(), kEpsilon),
+ () ->
+ assertEquals(
+ finalPose.getRotation().getY(), initialPose.getRotation().getY(), kEpsilon),
+ () ->
+ assertEquals(
+ finalPose.getRotation().getZ(), initialPose.getRotation().getZ(), kEpsilon));
+ }
+
+ @Test
+ void testTransformBy() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var initial =
+ new Pose3d(
+ new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var transformation =
+ new Transform3d(
+ new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+ var transformed = initial.plus(transformation);
+
+ assertAll(
+ () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+ () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+ () ->
+ assertEquals(Units.degreesToRadians(50.0), transformed.getRotation().getZ(), kEpsilon));
+ }
+
+ @Test
+ void testRelativeTo() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+ var finalRelativeToInitial = last.relativeTo(initial);
+
+ assertAll(
+ () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+ () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+ () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
+ }
+
+ @Test
+ void testEquality() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+ var two = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+ var two = new Pose3d(0.0, 1.524, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+ assertNotEquals(one, two);
+ }
+
+ @Test
+ void testMinus() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+ final var transform = last.minus(initial);
+
+ assertAll(
+ () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+ () -> assertEquals(0.0, transform.getY(), kEpsilon),
+ () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
+ }
+
+ @Test
+ void testToPose2d() {
+ var pose =
+ new Pose3d(
+ 1.0,
+ 2.0,
+ 3.0,
+ new Rotation3d(
+ Units.degreesToRadians(20.0),
+ Units.degreesToRadians(30.0),
+ Units.degreesToRadians(40.0)));
+ var expected = new Pose2d(1.0, 2.0, new Rotation2d(Units.degreesToRadians(40.0)));
+
+ assertEquals(expected, pose.toPose2d());
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
new file mode 100644
index 0000000..7c7e103
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class QuaternionTest {
+ @Test
+ void testInit() {
+ // Identity
+ var q1 = new Quaternion();
+ assertEquals(1.0, q1.getW());
+ assertEquals(0.0, q1.getX());
+ assertEquals(0.0, q1.getY());
+ assertEquals(0.0, q1.getZ());
+
+ // Normalized
+ var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
+ assertEquals(0.5, q2.getW());
+ assertEquals(0.5, q2.getX());
+ assertEquals(0.5, q2.getY());
+ assertEquals(0.5, q2.getZ());
+
+ // Unnormalized
+ var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
+ assertEquals(0.75, q3.getW());
+ assertEquals(0.3, q3.getX());
+ assertEquals(0.4, q3.getY());
+ assertEquals(0.5, q3.getZ());
+
+ q3 = q3.normalize();
+ double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
+ assertEquals(0.75 / norm, q3.getW());
+ assertEquals(0.3 / norm, q3.getX());
+ assertEquals(0.4 / norm, q3.getY());
+ assertEquals(0.5 / norm, q3.getZ());
+ assertEquals(
+ 1.0,
+ q3.getW() * q3.getW()
+ + q3.getX() * q3.getX()
+ + q3.getY() * q3.getY()
+ + q3.getZ() * q3.getZ());
+ }
+
+ @Test
+ void testTimes() {
+ // 90° CCW rotations around each axis
+ double c = Math.cos(Units.degreesToRadians(90.0) / 2.0);
+ double s = Math.sin(Units.degreesToRadians(90.0) / 2.0);
+ var xRot = new Quaternion(c, s, 0.0, 0.0);
+ var yRot = new Quaternion(c, 0.0, s, 0.0);
+ var zRot = new Quaternion(c, 0.0, 0.0, s);
+
+ // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
+ // produce a 90° CCW Y rotation
+ var expected = yRot;
+ var actual = zRot.times(yRot).times(xRot);
+ assertEquals(expected.getW(), actual.getW(), 1e-9);
+ assertEquals(expected.getX(), actual.getX(), 1e-9);
+ assertEquals(expected.getY(), actual.getY(), 1e-9);
+ assertEquals(expected.getZ(), actual.getZ(), 1e-9);
+
+ // Identity
+ var q =
+ new Quaternion(
+ 0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
+ actual = q.times(q.inverse());
+ assertEquals(1.0, actual.getW());
+ assertEquals(0.0, actual.getX());
+ assertEquals(0.0, actual.getY());
+ assertEquals(0.0, actual.getZ());
+ }
+
+ @Test
+ void testInverse() {
+ var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+ var inv = q.inverse();
+
+ assertEquals(q.getW(), inv.getW());
+ assertEquals(-q.getX(), inv.getX());
+ assertEquals(-q.getY(), inv.getY());
+ assertEquals(-q.getZ(), inv.getZ());
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index cb3f0f3..6702f1d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -15,12 +15,12 @@
@Test
void testRadiansToDegrees() {
- var rot1 = new Rotation2d(Math.PI / 3);
- var rot2 = new Rotation2d(Math.PI / 4);
+ var rot1 = Rotation2d.fromRadians(Math.PI / 3);
+ var rot2 = Rotation2d.fromRadians(Math.PI / 4);
assertAll(
- () -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon),
- () -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon));
+ () -> assertEquals(60.0, rot1.getDegrees(), kEpsilon),
+ () -> assertEquals(45.0, rot2.getDegrees(), kEpsilon));
}
@Test
@@ -29,8 +29,8 @@
var rot2 = Rotation2d.fromDegrees(30.0);
assertAll(
- () -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon),
- () -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon));
+ () -> assertEquals(Math.PI / 4.0, rot1.getRadians(), kEpsilon),
+ () -> assertEquals(Math.PI / 6.0, rot2.getRadians(), kEpsilon));
}
@Test
@@ -39,8 +39,8 @@
var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
- () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
- () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
+ () -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon),
+ () -> assertEquals(90.0, rotated.getDegrees(), kEpsilon));
}
@Test
@@ -48,7 +48,7 @@
var rot = Rotation2d.fromDegrees(90.0);
rot = rot.plus(Rotation2d.fromDegrees(30.0));
- assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+ assertEquals(120.0, rot.getDegrees(), kEpsilon);
}
@Test
@@ -56,7 +56,22 @@
var rot1 = Rotation2d.fromDegrees(70.0);
var rot2 = Rotation2d.fromDegrees(30.0);
- assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon);
+ assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon);
+ }
+
+ @Test
+ void testUnaryMinus() {
+ var rot = Rotation2d.fromDegrees(20.0);
+
+ assertEquals(-20.0, rot.unaryMinus().getDegrees(), kEpsilon);
+ }
+
+ @Test
+ void testMultiply() {
+ var rot = Rotation2d.fromDegrees(10.0);
+
+ assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon);
+ assertEquals(410.0, rot.times(41.0).getDegrees(), kEpsilon);
}
@Test
@@ -65,9 +80,9 @@
var rot2 = Rotation2d.fromDegrees(43.0);
assertEquals(rot1, rot2);
- var rot3 = Rotation2d.fromDegrees(-180.0);
- var rot4 = Rotation2d.fromDegrees(180.0);
- assertEquals(rot3, rot4);
+ rot1 = Rotation2d.fromDegrees(-180.0);
+ rot2 = Rotation2d.fromDegrees(180.0);
+ assertEquals(rot1, rot2);
}
@Test
@@ -76,4 +91,19 @@
var rot2 = Rotation2d.fromDegrees(43.5);
assertNotEquals(rot1, rot2);
}
+
+ @Test
+ void testInterpolate() {
+ // 50 + (70 - 50) * 0.5 = 60
+ var rot1 = Rotation2d.fromDegrees(50);
+ var rot2 = Rotation2d.fromDegrees(70);
+ var interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(60.0, interpolated.getDegrees(), kEpsilon);
+
+ // -160 minus half distance between 170 and -160 (15) = -175
+ rot1 = Rotation2d.fromDegrees(170);
+ rot2 = Rotation2d.fromDegrees(-160);
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(-175.0, interpolated.getDegrees());
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
new file mode 100644
index 0000000..072a2e6
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -0,0 +1,363 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testInitAxisAngleAndRollPitchYaw() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+ final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
+ final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+ assertEquals(rot1, rot2);
+
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+ final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
+ final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+ assertEquals(rot3, rot4);
+
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+ final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
+ final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+ assertEquals(rot5, rot6);
+ }
+
+ @Test
+ void testInitRotationMatrix() {
+ // No rotation
+ final var R1 = Matrix.eye(Nat.N3());
+ final var rot1 = new Rotation3d(R1);
+ assertEquals(new Rotation3d(), rot1);
+
+ // 90 degree CCW rotation around z-axis
+ final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
+ R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0));
+ R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0));
+ R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0));
+ final var rot2 = new Rotation3d(R2);
+ final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0));
+ assertEquals(expected2, rot2);
+
+ // Matrix that isn't orthogonal
+ final var R3 =
+ new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+ assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
+
+ // Matrix that's orthogonal but not special orthogonal
+ final var R4 = Matrix.eye(Nat.N3()).times(2.0);
+ assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4));
+ }
+
+ @Test
+ void testInitTwoVector() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ // 90 degree CW rotation around y-axis
+ final var rot1 = new Rotation3d(xAxis, zAxis);
+ final var expected1 = new Rotation3d(yAxis, -Math.PI / 2.0);
+ assertEquals(expected1, rot1);
+
+ // 45 degree CCW rotation around z-axis
+ final var rot2 = new Rotation3d(xAxis, VecBuilder.fill(1.0, 1.0, 0.0));
+ final var expected2 = new Rotation3d(zAxis, Math.PI / 4.0);
+ assertEquals(expected2, rot2);
+
+ // 0 degree rotation of x-axes
+ final var rot3 = new Rotation3d(xAxis, xAxis);
+ assertEquals(new Rotation3d(), rot3);
+
+ // 0 degree rotation of y-axes
+ final var rot4 = new Rotation3d(yAxis, yAxis);
+ assertEquals(new Rotation3d(), rot4);
+
+ // 0 degree rotation of z-axes
+ final var rot5 = new Rotation3d(zAxis, zAxis);
+ assertEquals(new Rotation3d(), rot5);
+
+ // 180 degree rotation tests. For 180 degree rotations, any quaternion with
+ // an orthogonal rotation axis is acceptable. The rotation axis and initial
+ // vector are orthogonal if their dot product is zero.
+
+ // 180 degree rotation of x-axes
+ final var rot6 = new Rotation3d(xAxis, xAxis.times(-1.0));
+ final var q6 = rot6.getQuaternion();
+ assertEquals(0.0, q6.getW());
+ assertEquals(
+ 0.0,
+ q6.getX() * xAxis.get(0, 0) + q6.getY() * xAxis.get(1, 0) + q6.getZ() * xAxis.get(2, 0));
+
+ // 180 degree rotation of y-axes
+ final var rot7 = new Rotation3d(yAxis, yAxis.times(-1.0));
+ final var q7 = rot7.getQuaternion();
+ assertEquals(0.0, q7.getW());
+ assertEquals(
+ 0.0,
+ q7.getX() * yAxis.get(0, 0) + q7.getY() * yAxis.get(1, 0) + q7.getZ() * yAxis.get(2, 0));
+
+ // 180 degree rotation of z-axes
+ final var rot8 = new Rotation3d(zAxis, zAxis.times(-1.0));
+ final var q8 = rot8.getQuaternion();
+ assertEquals(0.0, q8.getW());
+ assertEquals(
+ 0.0,
+ q8.getX() * zAxis.get(0, 0) + q8.getY() * zAxis.get(1, 0) + q8.getZ() * zAxis.get(2, 0));
+ }
+
+ @Test
+ void testRadiansToDegrees() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(zAxis, Math.PI / 3);
+ assertAll(
+ () -> assertEquals(Units.degreesToRadians(0.0), rot1.getX(), kEpsilon),
+ () -> assertEquals(Units.degreesToRadians(0.0), rot1.getY(), kEpsilon),
+ () -> assertEquals(Units.degreesToRadians(60.0), rot1.getZ(), kEpsilon));
+
+ var rot2 = new Rotation3d(zAxis, Math.PI / 4);
+ assertAll(
+ () -> assertEquals(Units.degreesToRadians(0.0), rot2.getX(), kEpsilon),
+ () -> assertEquals(Units.degreesToRadians(0.0), rot2.getY(), kEpsilon),
+ () -> assertEquals(Units.degreesToRadians(45.0), rot2.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testRadiansAndDegrees() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(45.0));
+ assertAll(
+ () -> assertEquals(0.0, rot1.getX(), kEpsilon),
+ () -> assertEquals(0.0, rot1.getY(), kEpsilon),
+ () -> assertEquals(Math.PI / 4.0, rot1.getZ(), kEpsilon));
+
+ var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+ assertAll(
+ () -> assertEquals(0.0, rot2.getX(), kEpsilon),
+ () -> assertEquals(0.0, rot2.getY(), kEpsilon),
+ () -> assertEquals(Math.PI / 6.0, rot2.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testRotationLoop() {
+ var rot = new Rotation3d();
+
+ rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+ var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
+ assertEquals(expected, rot);
+
+ rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+ expected =
+ new Rotation3d(
+ VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)),
+ Units.degreesToRadians(120.0));
+ assertEquals(expected, rot);
+
+ rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+ expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0);
+ assertEquals(expected, rot);
+
+ rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
+ assertEquals(new Rotation3d(), rot);
+ }
+
+ @Test
+ void testRotateByFromZeroX() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+ final var zero = new Rotation3d();
+ var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+ var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+ assertEquals(expected, rotated);
+ }
+
+ @Test
+ void testRotateByFromZeroY() {
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+ final var zero = new Rotation3d();
+ var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+ var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+ assertEquals(expected, rotated);
+ }
+
+ @Test
+ void testRotateByFromZeroZ() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ final var zero = new Rotation3d();
+ var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+ var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+ assertEquals(expected, rotated);
+ }
+
+ @Test
+ void testRotateByNonZeroX() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+ var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+ rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
+
+ var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
+ assertEquals(expected, rot);
+ }
+
+ @Test
+ void testRotateByNonZeroY() {
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+ var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+ rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
+
+ var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
+ assertEquals(expected, rot);
+ }
+
+ @Test
+ void testRotateByNonZeroZ() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+ rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
+
+ var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
+ assertEquals(expected, rot);
+ }
+
+ @Test
+ void testMinus() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0));
+ var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+
+ assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon);
+ }
+
+ @Test
+ void testEquality() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+ var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+ assertEquals(rot1, rot2);
+
+ rot1 = new Rotation3d(zAxis, Units.degreesToRadians(-180.0));
+ rot2 = new Rotation3d(zAxis, Units.degreesToRadians(180.0));
+ assertEquals(rot1, rot2);
+ }
+
+ @Test
+ void testAxisAngle() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+ assertEquals(xAxis, rot1.getAxis());
+ assertEquals(Math.PI / 2.0, rot1.getAngle(), 1e-9);
+
+ var rot2 = new Rotation3d(yAxis, Units.degreesToRadians(45.0));
+ assertEquals(yAxis, rot2.getAxis());
+ assertEquals(Math.PI / 4.0, rot2.getAngle(), 1e-9);
+
+ var rot3 = new Rotation3d(zAxis, Units.degreesToRadians(60.0));
+ assertEquals(zAxis, rot3.getAxis());
+ assertEquals(Math.PI / 3.0, rot3.getAngle(), 1e-9);
+ }
+
+ @Test
+ void testToRotation2d() {
+ var rotation =
+ new Rotation3d(
+ Units.degreesToRadians(20.0),
+ Units.degreesToRadians(30.0),
+ Units.degreesToRadians(40.0));
+ var expected = new Rotation2d(Units.degreesToRadians(40.0));
+
+ assertEquals(expected, rotation.toRotation2d());
+ }
+
+ @Test
+ void testInequality() {
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+ var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.5));
+ assertNotEquals(rot1, rot2);
+ }
+
+ @Test
+ void testInterpolate() {
+ final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+ final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+ final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ // 50 + (70 - 50) * 0.5 = 60
+ var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(50));
+ var rot2 = new Rotation3d(xAxis, Units.degreesToRadians(70));
+ var interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(60.0), interpolated.getX(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+ // -160 minus half distance between 170 and -160 (15) = -175
+ rot1 = new Rotation3d(xAxis, Units.degreesToRadians(170));
+ rot2 = new Rotation3d(xAxis, Units.degreesToRadians(-160));
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(-175.0), interpolated.getX());
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getZ());
+
+ // 50 + (70 - 50) * 0.5 = 60
+ rot1 = new Rotation3d(yAxis, Units.degreesToRadians(50));
+ rot2 = new Rotation3d(yAxis, Units.degreesToRadians(70));
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+ assertEquals(Units.degreesToRadians(60.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+ // -160 minus half distance between 170 and -160 (165) = 5
+ rot1 = new Rotation3d(yAxis, Units.degreesToRadians(170));
+ rot2 = new Rotation3d(yAxis, Units.degreesToRadians(-160));
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(180.0), interpolated.getX(), kEpsilon);
+ assertEquals(Units.degreesToRadians(-5.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(180.0), interpolated.getZ(), kEpsilon);
+
+ // 50 + (70 - 50) * 0.5 = 60
+ rot1 = new Rotation3d(zAxis, Units.degreesToRadians(50));
+ rot2 = new Rotation3d(zAxis, Units.degreesToRadians(70));
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(60.0), interpolated.getZ(), kEpsilon);
+
+ // -160 minus half distance between 170 and -160 (15) = -175
+ rot1 = new Rotation3d(zAxis, Units.degreesToRadians(170));
+ rot2 = new Rotation3d(zAxis, Units.degreesToRadians(-160));
+ interpolated = rot1.interpolate(rot2, 0.5);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+ assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+ assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
index 7265e25..93d8a0e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
@@ -4,14 +4,11 @@
package edu.wpi.first.math.geometry;
-import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class Transform2dTest {
- private static final double kEpsilon = 1E-9;
-
@Test
void testInverse() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
@@ -20,14 +17,7 @@
var transformed = initial.plus(transform);
var untransformed = transformed.plus(transform.inverse());
- assertAll(
- () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
- () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon),
- () ->
- assertEquals(
- initial.getRotation().getDegrees(),
- untransformed.getRotation().getDegrees(),
- kEpsilon));
+ assertEquals(initial, untransformed);
}
@Test
@@ -39,13 +29,6 @@
var transformedSeparate = initial.plus(transform1).plus(transform2);
var transformedCombined = initial.plus(transform1.plus(transform2));
- assertAll(
- () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon),
- () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon),
- () ->
- assertEquals(
- transformedSeparate.getRotation().getDegrees(),
- transformedCombined.getRotation().getDegrees(),
- kEpsilon));
+ assertEquals(transformedSeparate, transformedCombined);
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
new file mode 100644
index 0000000..51d5f07
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
@@ -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.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Transform3dTest {
+ @Test
+ void testInverse() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var initial =
+ new Pose3d(
+ new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var transform =
+ new Transform3d(
+ new Translation3d(5.0, 4.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+ var transformed = initial.plus(transform);
+ var untransformed = transformed.plus(transform.inverse());
+
+ assertEquals(initial, untransformed);
+ }
+
+ @Test
+ void testComposition() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var initial =
+ new Pose3d(
+ new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var transform1 =
+ new Transform3d(
+ new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+ var transform2 =
+ new Transform3d(
+ new Translation3d(0.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+ var transformedSeparate = initial.plus(transform1).plus(transform2);
+ var transformedCombined = initial.plus(transform1.plus(transform2));
+
+ assertEquals(transformedSeparate, transformedCombined);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index 2d8eeaa..c8f5690 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -21,8 +21,8 @@
var sum = one.plus(two);
assertAll(
- () -> assertEquals(sum.getX(), 3.0, kEpsilon),
- () -> assertEquals(sum.getY(), 8.0, kEpsilon));
+ () -> assertEquals(3.0, sum.getX(), kEpsilon),
+ () -> assertEquals(8.0, sum.getY(), kEpsilon));
}
@Test
@@ -33,8 +33,8 @@
var difference = one.minus(two);
assertAll(
- () -> assertEquals(difference.getX(), -1.0, kEpsilon),
- () -> assertEquals(difference.getY(), -2.0, kEpsilon));
+ () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+ () -> assertEquals(-2.0, difference.getY(), kEpsilon));
}
@Test
@@ -43,8 +43,8 @@
var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
- () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
- () -> assertEquals(rotated.getY(), 3.0, kEpsilon));
+ () -> assertEquals(0.0, rotated.getX(), kEpsilon),
+ () -> assertEquals(3.0, rotated.getY(), kEpsilon));
}
@Test
@@ -53,8 +53,8 @@
var mult = original.times(3);
assertAll(
- () -> assertEquals(mult.getX(), 9.0, kEpsilon),
- () -> assertEquals(mult.getY(), 15.0, kEpsilon));
+ () -> assertEquals(9.0, mult.getX(), kEpsilon),
+ () -> assertEquals(15.0, mult.getY(), kEpsilon));
}
@Test
@@ -63,21 +63,21 @@
var div = original.div(2);
assertAll(
- () -> assertEquals(div.getX(), 1.5, kEpsilon),
- () -> assertEquals(div.getY(), 2.5, kEpsilon));
+ () -> assertEquals(1.5, div.getX(), kEpsilon),
+ () -> assertEquals(2.5, div.getY(), kEpsilon));
}
@Test
void testNorm() {
var one = new Translation2d(3.0, 5.0);
- assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+ assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
}
@Test
void testDistance() {
var one = new Translation2d(1, 1);
var two = new Translation2d(6, 6);
- assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+ assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
}
@Test
@@ -86,8 +86,8 @@
var inverted = original.unaryMinus();
assertAll(
- () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
- () -> assertEquals(inverted.getY(), -7, kEpsilon));
+ () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+ () -> assertEquals(-7.0, inverted.getY(), kEpsilon));
}
@Test
@@ -109,9 +109,9 @@
var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
assertAll(
- () -> assertEquals(one.getX(), 1.0, kEpsilon),
- () -> assertEquals(one.getY(), 1.0, kEpsilon),
- () -> assertEquals(two.getX(), 1.0, kEpsilon),
- () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon));
+ () -> assertEquals(1.0, one.getX(), kEpsilon),
+ () -> assertEquals(1.0, one.getY(), kEpsilon),
+ () -> assertEquals(1.0, two.getX(), kEpsilon),
+ () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
new file mode 100644
index 0000000..762425b
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
@@ -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.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Translation3dTest {
+ private static final double kEpsilon = 1E-9;
+
+ @Test
+ void testSum() {
+ var one = new Translation3d(1.0, 3.0, 5.0);
+ var two = new Translation3d(2.0, 5.0, 8.0);
+
+ var sum = one.plus(two);
+
+ assertAll(
+ () -> assertEquals(3.0, sum.getX(), kEpsilon),
+ () -> assertEquals(8.0, sum.getY(), kEpsilon),
+ () -> assertEquals(13.0, sum.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testDifference() {
+ var one = new Translation3d(1.0, 3.0, 5.0);
+ var two = new Translation3d(2.0, 5.0, 8.0);
+
+ var difference = one.minus(two);
+
+ assertAll(
+ () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+ () -> assertEquals(-2.0, difference.getY(), kEpsilon),
+ () -> assertEquals(-3.0, difference.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testRotateBy() {
+ var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+ var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var translation = new Translation3d(1.0, 2.0, 3.0);
+
+ var rotated1 = translation.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+ assertAll(
+ () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+ () -> assertEquals(-3.0, rotated1.getY(), kEpsilon),
+ () -> assertEquals(2.0, rotated1.getZ(), kEpsilon));
+
+ var rotated2 = translation.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+ assertAll(
+ () -> assertEquals(3.0, rotated2.getX(), kEpsilon),
+ () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+ () -> assertEquals(-1.0, rotated2.getZ(), kEpsilon));
+
+ var rotated3 = translation.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+ assertAll(
+ () -> assertEquals(-2.0, rotated3.getX(), kEpsilon),
+ () -> assertEquals(1.0, rotated3.getY(), kEpsilon),
+ () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testToTranslation2d() {
+ var translation = new Translation3d(1.0, 2.0, 3.0);
+ var expected = new Translation2d(1.0, 2.0);
+
+ assertEquals(expected, translation.toTranslation2d());
+ }
+
+ @Test
+ void testMultiplication() {
+ var original = new Translation3d(3.0, 5.0, 7.0);
+ var mult = original.times(3);
+
+ assertAll(
+ () -> assertEquals(9.0, mult.getX(), kEpsilon),
+ () -> assertEquals(15.0, mult.getY(), kEpsilon),
+ () -> assertEquals(21.0, mult.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testDivision() {
+ var original = new Translation3d(3.0, 5.0, 7.0);
+ var div = original.div(2);
+
+ assertAll(
+ () -> assertEquals(1.5, div.getX(), kEpsilon),
+ () -> assertEquals(2.5, div.getY(), kEpsilon),
+ () -> assertEquals(3.5, div.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testNorm() {
+ var one = new Translation3d(3.0, 5.0, 7.0);
+ assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
+ }
+
+ @Test
+ void testDistance() {
+ var one = new Translation3d(1.0, 1.0, 1.0);
+ var two = new Translation3d(6.0, 6.0, 6.0);
+ assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
+ }
+
+ @Test
+ void testUnaryMinus() {
+ var original = new Translation3d(-4.5, 7.0, 9.0);
+ var inverted = original.unaryMinus();
+
+ assertAll(
+ () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+ () -> assertEquals(-7.0, inverted.getY(), kEpsilon),
+ () -> assertEquals(-9.0, inverted.getZ(), kEpsilon));
+ }
+
+ @Test
+ void testEquality() {
+ var one = new Translation3d(9, 5.5, 3.5);
+ var two = new Translation3d(9, 5.5, 3.5);
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = new Translation3d(9, 5.5, 3.5);
+ var two = new Translation3d(9, 5.7, 3.5);
+ assertNotEquals(one, two);
+ }
+
+ @Test
+ void testPolarConstructor() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var one = new Translation3d(Math.sqrt(2), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+ var two = new Translation3d(2, new Rotation3d(zAxis, Units.degreesToRadians(60.0)));
+ assertAll(
+ () -> assertEquals(1.0, one.getX(), kEpsilon),
+ () -> assertEquals(1.0, one.getY(), kEpsilon),
+ () -> assertEquals(0.0, one.getZ(), kEpsilon),
+ () -> assertEquals(1.0, two.getX(), kEpsilon),
+ () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
+ () -> assertEquals(0.0, two.getZ(), kEpsilon));
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
index 69a4c86..4108a62 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
@@ -4,35 +4,28 @@
package edu.wpi.first.math.geometry;
-import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
class Twist2dTest {
- private static final double kEpsilon = 1E-9;
-
@Test
- void testStraightLineTwist() {
+ void testStraight() {
var straight = new Twist2d(5.0, 0.0, 0.0);
var straightPose = new Pose2d().exp(straight);
- assertAll(
- () -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
- () -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
- () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
+ var expected = new Pose2d(5.0, 0.0, new Rotation2d());
+ assertEquals(expected, straightPose);
}
@Test
- void testQuarterCirleTwist() {
+ void testQuarterCirle() {
var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
var quarterCirclePose = new Pose2d().exp(quarterCircle);
- assertAll(
- () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
- () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
- () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
+ var expected = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+ assertEquals(expected, quarterCirclePose);
}
@Test
@@ -40,10 +33,8 @@
var diagonal = new Twist2d(2.0, 2.0, 0.0);
var diagonalPose = new Pose2d().exp(diagonal);
- assertAll(
- () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
- () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
- () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
+ var expected = new Pose2d(2.0, 2.0, new Rotation2d());
+ assertEquals(expected, diagonalPose);
}
@Test
@@ -67,9 +58,11 @@
final var twist = start.log(end);
- assertAll(
- () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
- () -> assertEquals(twist.dy, 0.0, kEpsilon),
- () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon));
+ var expected = new Twist2d(5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0);
+ assertEquals(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ final var reapplied = start.exp(twist);
+ assertEquals(end, reapplied);
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java
new file mode 100644
index 0000000..bb3bbb7
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java
@@ -0,0 +1,124 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Twist3dTest {
+ @Test
+ void testStraightX() {
+ var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ var straightPose = new Pose3d().exp(straight);
+
+ var expected = new Pose3d(5.0, 0.0, 0.0, new Rotation3d());
+ assertEquals(expected, straightPose);
+ }
+
+ @Test
+ void testStraightY() {
+ var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+ var straightPose = new Pose3d().exp(straight);
+
+ var expected = new Pose3d(0.0, 5.0, 0.0, new Rotation3d());
+ assertEquals(expected, straightPose);
+ }
+
+ @Test
+ void testStraightZ() {
+ var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0);
+ var straightPose = new Pose3d().exp(straight);
+
+ var expected = new Pose3d(0.0, 0.0, 5.0, new Rotation3d());
+ assertEquals(expected, straightPose);
+ }
+
+ @Test
+ void testQuarterCirle() {
+ var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+ var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+ var quarterCirclePose = new Pose3d().exp(quarterCircle);
+
+ var expected = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+ assertEquals(expected, quarterCirclePose);
+ }
+
+ @Test
+ void testDiagonalNoDtheta() {
+ var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0);
+ var diagonalPose = new Pose3d().exp(diagonal);
+
+ var expected = new Pose3d(2.0, 2.0, 0.0, new Rotation3d());
+ assertEquals(expected, diagonalPose);
+ }
+
+ @Test
+ void testEquality() {
+ var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+ var two = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+ assertEquals(one, two);
+ }
+
+ @Test
+ void testInequality() {
+ var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+ var two = new Twist3d(5, 1.2, 0, 0.0, 0.0, 3.0);
+ assertNotEquals(one, two);
+ }
+
+ @Test
+ void testPose3dLogX() {
+ final var start = new Pose3d();
+ final var end =
+ new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+
+ final var twist = start.log(end);
+
+ var expected =
+ new Twist3d(0.0, 5.0 / 2.0 * Math.PI, 0.0, Units.degreesToRadians(90.0), 0.0, 0.0);
+ assertEquals(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ final var reapplied = start.exp(twist);
+ assertEquals(end, reapplied);
+ }
+
+ @Test
+ void testPose3dLogY() {
+ final var start = new Pose3d();
+ final var end =
+ new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+
+ final var twist = start.log(end);
+
+ var expected = new Twist3d(0.0, 0.0, 5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0, 0.0);
+ assertEquals(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ final var reapplied = start.exp(twist);
+ assertEquals(end, reapplied);
+ }
+
+ @Test
+ void testPose3dLogZ() {
+ final var start = new Pose3d();
+ final var end =
+ new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+
+ final var twist = start.log(end);
+
+ var expected = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+ assertEquals(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ final var reapplied = start.exp(twist);
+ assertEquals(end, reapplied);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
index 7b0e334..e65d27f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
@@ -16,15 +16,15 @@
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
buffer.addSample(0, new Rotation2d());
- assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
+ assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
buffer.addSample(1, new Rotation2d(1));
- assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
- assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
+ assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+ assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
buffer.addSample(3, new Rotation2d(2));
- assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
+ assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
buffer.addSample(10.5, new Rotation2d(2));
- assertEquals(new Rotation2d(1), buffer.getSample(0));
+ assertEquals(new Rotation2d(1), buffer.getSample(0).get());
}
@Test
@@ -34,7 +34,7 @@
// We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
- Pose2d sample = buffer.getSample(0.5);
+ Pose2d sample = buffer.getSample(0.5).get();
assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
index f85e8fb..6d15f64 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
@@ -14,11 +14,11 @@
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry =
- new DifferentialDriveOdometry(new Rotation2d());
+ new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
@Test
void testOdometryWithEncoderDistances() {
- m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+ m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, new Pose2d());
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
index b3546f5..18d3ec7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -45,6 +45,17 @@
}
@Test
+ void testStraightLineForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(3.536, twist.dx, 0.1),
+ () -> assertEquals(0, twist.dy, 0.1),
+ () -> assertEquals(0, twist.dtheta, 0.1));
+ }
+
+ @Test
void testStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -68,6 +79,17 @@
}
@Test
+ void testStrafeForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(-2.828427, 2.828427, 2.828427, -2.828427);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(0, twist.dx, 0.1),
+ () -> assertEquals(2.8284, twist.dy, 0.1),
+ () -> assertEquals(0, twist.dtheta, 0.1));
+ }
+
+ @Test
void testRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -91,6 +113,17 @@
}
@Test
+ void testRotationForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(-150.79645, 150.79645, -150.79645, 150.79645);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(0, twist.dx, 0.1),
+ () -> assertEquals(0, twist.dy, 0.1),
+ () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+ }
+
+ @Test
void testMixedTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -114,6 +147,17 @@
}
@Test
+ void testMixedTranslationRotationForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(-17.677670, 20.51, -13.44, 16.26);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(1.413, twist.dx, 0.1),
+ () -> assertEquals(2.122, twist.dy, 0.1),
+ () -> assertEquals(0.707, twist.dtheta, 0.1));
+ }
+
+ @Test
void testOffCenterRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
@@ -137,6 +181,17 @@
}
@Test
+ void testOffCenterRotationForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(0, 16.971, -16.971, 33.941);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(8.48525, twist.dx, 0.1),
+ () -> assertEquals(-8.48525, twist.dy, 0.1),
+ () -> assertEquals(0.707, twist.dtheta, 0.1));
+ }
+
+ @Test
void testOffCenterTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
@@ -160,6 +215,17 @@
}
@Test
+ void testOffCenterRotationTranslationForwardKinematicsKinematicsWithDeltas() {
+ var wheelDeltas = new MecanumDriveWheelPositions(2.12, 21.92, -12.02, 36.06);
+ var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+ assertAll(
+ () -> assertEquals(12.02, twist.dx, 0.1),
+ () -> assertEquals(-7.07, twist.dy, 0.1),
+ () -> assertEquals(0.707, twist.dtheta, 0.1));
+ }
+
+ @Test
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.desaturate(5.5);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
index 3f43109..cf6a439 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -10,6 +10,10 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
import org.junit.jupiter.api.Test;
class MecanumDriveOdometryTest {
@@ -21,15 +25,19 @@
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+ private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions();
+
private final MecanumDriveOdometry m_odometry =
- new MecanumDriveOdometry(m_kinematics, new Rotation2d());
+ new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero);
@Test
void testMultipleConsecutiveUpdates() {
- var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+ var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
- m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
- var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+ m_odometry.resetPosition(new Rotation2d(), wheelPositions, new Pose2d());
+
+ m_odometry.update(new Rotation2d(), wheelPositions);
+ var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
@@ -39,11 +47,12 @@
@Test
void testTwoIterations() {
- // 5 units/sec in the x axis (forward)
- final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+ // 5 units/sec in the x-axis (forward)
+ final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
+ m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
- m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
- var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+ m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+ var pose = m_odometry.update(new Rotation2d(), wheelPositions);
assertAll(
() -> assertEquals(0.3536, pose.getX(), 0.01),
@@ -55,10 +64,11 @@
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
- final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+ final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
+ m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
- m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
- final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+ m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+ final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
assertAll(
() -> assertEquals(8.4855, pose.getX(), 0.01),
@@ -70,14 +80,188 @@
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
- m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
- var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
- m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
- var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+ m_odometry.resetPosition(
+ gyro, new MecanumDriveWheelPositions(), new Pose2d(new Translation2d(), fieldAngle));
+ var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+ m_odometry.update(gyro, new MecanumDriveWheelPositions());
+ var pose = m_odometry.update(gyro, speeds);
assertAll(
() -> assertEquals(3.536, pose.getX(), 0.1),
() -> assertEquals(0.0, pose.getY(), 0.1),
() -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
}
+
+ @Test
+ void testAccuracyFacingTrajectory() {
+ var kinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(1, 1), new Translation2d(1, -1),
+ new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+ var wheelPositions = new MecanumDriveWheelPositions();
+
+ var odometry =
+ new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(),
+ new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+ new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+ new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+ new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+ new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+ new TrajectoryConfig(0.5, 2));
+
+ var rand = new Random(5190);
+
+ final double dt = 0.02;
+ double t = 0.0;
+
+ double maxError = Double.NEGATIVE_INFINITY;
+ double errorSum = 0;
+ double odometryDistanceTravelled = 0;
+ double trajectoryDistanceTravelled = 0;
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
+
+ trajectoryDistanceTravelled +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+ var wheelSpeeds =
+ kinematics.toWheelSpeeds(
+ new ChassisSpeeds(
+ groundTruthState.velocityMetersPerSecond,
+ 0,
+ groundTruthState.velocityMetersPerSecond
+ * groundTruthState.curvatureRadPerMeter));
+
+ wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+ wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+ wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+ wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+ wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+ var lastPose = odometry.getPoseMeters();
+
+ var xHat =
+ odometry.update(
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+ wheelPositions);
+
+ odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+ double error =
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+ assertEquals(
+ 1.0,
+ odometryDistanceTravelled / trajectoryDistanceTravelled,
+ 0.05,
+ "Incorrect distance travelled");
+ }
+
+ @Test
+ void testAccuracyFacingXAxis() {
+ var kinematics =
+ new MecanumDriveKinematics(
+ new Translation2d(1, 1), new Translation2d(1, -1),
+ new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+ var wheelPositions = new MecanumDriveWheelPositions();
+
+ var odometry =
+ new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(),
+ new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+ new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+ new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+ new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+ new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+ new TrajectoryConfig(0.5, 2));
+
+ var rand = new Random(5190);
+
+ final double dt = 0.02;
+ double t = 0.0;
+
+ double maxError = Double.NEGATIVE_INFINITY;
+ double errorSum = 0;
+ double odometryDistanceTravelled = 0;
+ double trajectoryDistanceTravelled = 0;
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
+
+ trajectoryDistanceTravelled +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+ var wheelSpeeds =
+ kinematics.toWheelSpeeds(
+ new ChassisSpeeds(
+ groundTruthState.velocityMetersPerSecond
+ * groundTruthState.poseMeters.getRotation().getCos(),
+ groundTruthState.velocityMetersPerSecond
+ * groundTruthState.poseMeters.getRotation().getSin(),
+ 0));
+
+ wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+ wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+ wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+ wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+ wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+ wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+ var lastPose = odometry.getPoseMeters();
+
+ var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions);
+
+ odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+ double error =
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+ assertEquals(
+ 1.0,
+ odometryDistanceTravelled / trajectoryDistanceTravelled,
+ 0.05,
+ "Incorrect distance travelled");
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index 574e086..43dd02a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -41,16 +41,28 @@
@Test
void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
- SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+ SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(0.0));
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
- () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
- () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+ () -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
+ void testStraightLineForwardKinematicsWithDeltas() {
+ // test forward kinematics going in a straight line
+ SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(0.0));
+ var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+ assertAll(
+ () -> assertEquals(5.0, twist.dx, kEpsilon),
+ () -> assertEquals(0.0, twist.dy, kEpsilon),
+ () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+ }
+
+ @Test
void testStraightStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
@@ -78,13 +90,43 @@
}
@Test
+ void testStraightStrafeForwardKinematicsWithDeltas() {
+ SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(90.0));
+ var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+ assertAll(
+ () -> assertEquals(0.0, twist.dx, kEpsilon),
+ () -> assertEquals(5.0, twist.dy, kEpsilon),
+ () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+ }
+
+ @Test
+ void testConserveWheelAngle() {
+ ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+ m_kinematics.toSwerveModuleStates(speeds);
+ var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
+
+ // Robot is stationary, but module angles are preserved.
+
+ assertAll(
+ () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+ () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+ }
+
+ @Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
/*
- The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
- the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+ The circumference of the wheels about the COR is π * diameter, or 2π * radius
+ the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels
trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
our wheels must trace out 1 rotation (or 106.63 inches) per second.
*/
@@ -116,6 +158,21 @@
}
@Test
+ void testTurnInPlaceForwardKinematicsWithDeltas() {
+ SwerveModulePosition flDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(135));
+ SwerveModulePosition frDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(45));
+ SwerveModulePosition blDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-135));
+ SwerveModulePosition brDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-45));
+
+ var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+ assertAll(
+ () -> assertEquals(0.0, twist.dx, kEpsilon),
+ () -> assertEquals(0.0, twist.dy, kEpsilon),
+ () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+ }
+
+ @Test
void testOffCenterCORRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
@@ -124,7 +181,7 @@
This one is a bit trickier. Because we are rotating about the front-left wheel,
it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
- radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+ radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel
should be pointing straight forward, the back-left wheel should be pointing straight right,
and the back-right wheel should be at a -45 degree angle
*/
@@ -150,12 +207,12 @@
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
/*
- We already know that our omega should be 2pi from the previous test. Next, we need to determine
+ We already know that our omega should be 2π from the previous test. Next, we need to determine
the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
a full revolution about the center of revolution once every second. Therefore, the center of
- mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
- 1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+ mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are
+ 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
*/
assertAll(
@@ -164,6 +221,30 @@
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
+ @Test
+ void testOffCenterCORRotationForwardKinematicsWithDeltas() {
+ SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.fromDegrees(0.0));
+ SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(0.0));
+ SwerveModulePosition blDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(-90));
+ SwerveModulePosition brDelta = new SwerveModulePosition(213.258, Rotation2d.fromDegrees(-45));
+
+ var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+ /*
+ We already know that our omega should be 2π from the previous test. Next, we need to determine
+ the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+ we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+ a full revolution about the center of revolution once every second. Therefore, the center of
+ mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are
+ 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+ */
+
+ assertAll(
+ () -> assertEquals(75.398, twist.dx, 0.1),
+ () -> assertEquals(-75.398, twist.dy, 0.1),
+ () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+ }
+
private void assertModuleState(
SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
assertAll(
@@ -229,6 +310,30 @@
}
@Test
+ void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() {
+ SwerveModulePosition flDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-140.19));
+ SwerveModulePosition frDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-39.81));
+ SwerveModulePosition blDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-109.44));
+ SwerveModulePosition brDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-70.56));
+
+ var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+ /*
+ From equation (13.17), we know that chassis motion is th dot product of the
+ pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+ (0,0) -- we don't want the motion of the center of rotation, we want it of
+ the center of the robot). These above SwerveModuleStates are known to be from
+ a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+ calculated using Numpy's linalg.pinv function.
+ */
+
+ assertAll(
+ () -> assertEquals(0.0, twist.dx, 0.1),
+ () -> assertEquals(-33.0, twist.dy, 0.1),
+ () -> assertEquals(1.5, twist.dtheta, 0.1));
+ }
+
+ @Test
void testDesaturate() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
@@ -246,4 +351,24 @@
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
+
+ @Test
+ void testDesaturateSmooth() {
+ SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
+ SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
+ SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
+ SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
+
+ SwerveModuleState[] arr = {fl, fr, bl, br};
+ SwerveDriveKinematics.desaturateWheelSpeeds(
+ arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
+
+ double factor = 5.5 / 7.0;
+
+ assertAll(
+ () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+ () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
index cb6dfdf..716da03 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
@@ -10,6 +10,10 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
import org.junit.jupiter.api.Test;
class SwerveDriveOdometryTest {
@@ -18,30 +22,34 @@
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
+ private final SwerveModulePosition zero = new SwerveModulePosition();
+
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry =
- new SwerveDriveOdometry(m_kinematics, new Rotation2d());
+ new SwerveDriveOdometry(
+ m_kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
@Test
void testTwoIterations() {
- // 5 units/sec in the x axis (forward)
- final SwerveModuleState[] wheelSpeeds = {
- new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
- new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
- new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
- new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+ // 5 units/sec in the x-axis (forward)
+ final SwerveModulePosition[] wheelDeltas = {
+ new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+ new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+ new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+ new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0))
};
- m_odometry.updateWithTime(
- 0.0,
+ m_odometry.update(
new Rotation2d(),
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState());
- var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition(),
+ new SwerveModulePosition()
+ });
+ var pose = m_odometry.update(new Rotation2d(), wheelDeltas);
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
@@ -57,16 +65,16 @@
// Module 2: speed 18.84955592153876 angle -90.0
// Module 3: speed 42.14888838624436 angle -26.565051177077986
- final SwerveModuleState[] wheelSpeeds = {
- new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
- new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
- new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
- new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+ final SwerveModulePosition[] wheelDeltas = {
+ new SwerveModulePosition(18.85, Rotation2d.fromDegrees(90.0)),
+ new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)),
+ new SwerveModulePosition(18.85, Rotation2d.fromDegrees(-90)),
+ new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565))
};
- final var zero = new SwerveModuleState();
+ final var zero = new SwerveModulePosition();
- m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
- final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+ m_odometry.update(new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+ final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas);
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
@@ -78,15 +86,191 @@
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
- m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
- var state = new SwerveModuleState();
- m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
- state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
- var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+ m_odometry.resetPosition(
+ gyro,
+ new SwerveModulePosition[] {zero, zero, zero, zero},
+ new Pose2d(new Translation2d(), fieldAngle));
+ var delta = new SwerveModulePosition();
+ m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
+ delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
+ var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
}
+
+ @Test
+ void testAccuracyFacingTrajectory() {
+ var kinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1),
+ new Translation2d(-1, 1));
+ var odometry =
+ new SwerveDriveOdometry(
+ kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+
+ SwerveModulePosition fl = new SwerveModulePosition();
+ SwerveModulePosition fr = new SwerveModulePosition();
+ SwerveModulePosition bl = new SwerveModulePosition();
+ SwerveModulePosition br = new SwerveModulePosition();
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(0.5, 2));
+
+ var rand = new Random(4915);
+
+ final double dt = 0.02;
+ double t = 0.0;
+
+ double maxError = Double.NEGATIVE_INFINITY;
+ double errorSum = 0;
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
+
+ var moduleStates =
+ kinematics.toSwerveModuleStates(
+ new ChassisSpeeds(
+ groundTruthState.velocityMetersPerSecond,
+ 0.0,
+ groundTruthState.velocityMetersPerSecond
+ * groundTruthState.curvatureRadPerMeter));
+ for (var moduleState : moduleStates) {
+ moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+ moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+ }
+
+ fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
+ fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
+ bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
+ br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
+
+ fl.angle = moduleStates[0].angle;
+ fr.angle = moduleStates[1].angle;
+ bl.angle = moduleStates[2].angle;
+ br.angle = moduleStates[3].angle;
+
+ var xHat =
+ odometry.update(
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+ new SwerveModulePosition[] {fl, fr, bl, br});
+
+ double error =
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+ assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+ assertEquals(
+ Math.PI / 4,
+ odometry.getPoseMeters().getRotation().getRadians(),
+ 10 * Math.PI / 180,
+ "Incorrect Final Theta");
+
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ }
+
+ @Test
+ void testAccuracyFacingXAxis() {
+ var kinematics =
+ new SwerveDriveKinematics(
+ new Translation2d(1, 1),
+ new Translation2d(1, -1),
+ new Translation2d(-1, -1),
+ new Translation2d(-1, 1));
+ var odometry =
+ new SwerveDriveOdometry(
+ kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+
+ SwerveModulePosition fl = new SwerveModulePosition();
+ SwerveModulePosition fr = new SwerveModulePosition();
+ SwerveModulePosition bl = new SwerveModulePosition();
+ SwerveModulePosition br = new SwerveModulePosition();
+
+ var trajectory =
+ TrajectoryGenerator.generateTrajectory(
+ List.of(
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+ new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+ new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+ new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+ new TrajectoryConfig(0.5, 2));
+
+ var rand = new Random(4915);
+
+ final double dt = 0.02;
+ double t = 0.0;
+
+ double maxError = Double.NEGATIVE_INFINITY;
+ double errorSum = 0;
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
+
+ fl.distanceMeters +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+ fr.distanceMeters +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+ bl.distanceMeters +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+ br.distanceMeters +=
+ groundTruthState.velocityMetersPerSecond * dt
+ + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+ fl.angle = groundTruthState.poseMeters.getRotation();
+ fr.angle = groundTruthState.poseMeters.getRotation();
+ bl.angle = groundTruthState.poseMeters.getRotation();
+ br.angle = groundTruthState.poseMeters.getRotation();
+
+ var xHat =
+ odometry.update(
+ new Rotation2d(rand.nextGaussian() * 0.05),
+ new SwerveModulePosition[] {fl, fr, bl, br});
+
+ double error =
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+ assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+ assertEquals(
+ 0.0,
+ odometry.getPoseMeters().getRotation().getRadians(),
+ 10 * Math.PI / 180,
+ "Incorrect Final Theta");
+
+ assertEquals(
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ }
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
index dd08e45..8e3d02b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
@@ -23,7 +23,6 @@
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
- @SuppressWarnings("ParameterName")
private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
// Start the timer.
// var start = System.nanoTime();
@@ -97,13 +96,11 @@
1E-9));
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSCurve() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
@@ -115,7 +112,6 @@
run(start, waypoints, end);
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testOneInterior() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
@@ -126,7 +122,6 @@
run(start, waypoints, end);
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testWindyPath() {
final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
index 8367070..6435dd5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
@@ -20,7 +20,6 @@
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
- @SuppressWarnings("ParameterName")
private void run(Pose2d a, Pose2d b) {
// Start the timer.
// var start = System.nanoTime();
@@ -68,19 +67,16 @@
1E-9));
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSimpleSCurve() {
run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
}
- @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSquiggly() {
run(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
index 3dff90b..2182674 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -58,9 +58,9 @@
assertEquals(x1Truth, x1Discrete);
}
- // dt
- // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
+ // T
+ // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
@Test
void testDiscretizeSlowModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
@@ -68,6 +68,9 @@
final double dt = 1.0;
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
@@ -87,9 +90,9 @@
+ discQIntegrated);
}
- // dt
- // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
- // 0
+ // T
+ // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
@Test
void testDiscretizeFastModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
@@ -97,6 +100,9 @@
final var dt = 0.005;
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
@@ -130,6 +136,9 @@
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
@@ -173,6 +182,9 @@
assertTrue(esCont.getEigenvalue(i).real >= 0);
}
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
final var discQIntegrated =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N2, N2> x) ->
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index 6671963..72fb5ea 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -31,24 +31,6 @@
}
@Test
- void testExponentialRKF45() {
- Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
-
- var y1 =
- NumericalIntegration.rkf45(
- (x, u) -> {
- var y = new Matrix<>(Nat.N1(), Nat.N1());
- y.set(0, 0, Math.exp(x.get(0, 0)));
- return y;
- },
- y0,
- VecBuilder.fill(0),
- 0.1);
-
- assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
- }
-
- @Test
void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
index 7b5e844..d0e6c12 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
@@ -23,7 +23,6 @@
* @param y The initial value of y.
* @param dtSeconds The time over which to integrate.
*/
- @SuppressWarnings("MethodTypeParameterName")
public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
index e4df93f..3db878e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -15,16 +15,16 @@
class RungeKuttaTimeVaryingTest {
private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
return new MatBuilder<>(Nat.N1(), Nat.N1())
- .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
+ .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
}
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
- // x' = x (2 / (e^t + 1) - 1)
+ // x' = x (2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
- // x(t) = 12 * e^t / ((e^t + 1)^2)
+ // x(t) = 12eᵗ/(eᵗ + 1)²
@Test
void rungeKuttaTimeVaryingTest() {
final var y0 = rungeKuttaTimeVaryingSolution(5.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
index 1805589..fa2314e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
@@ -12,7 +12,6 @@
import org.junit.jupiter.api.Test;
class CentripetalAccelerationConstraintTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testCentripetalAccelerationConstraint() {
double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
index 4c8a8ba..bcc3c16 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -15,7 +15,6 @@
import org.junit.jupiter.api.Test;
class DifferentialDriveKinematicsConstraintTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testDifferentialDriveKinematicsConstraint() {
double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
index 87c1bd9..917edb4 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -20,7 +20,6 @@
import org.junit.jupiter.api.Test;
class DifferentialDriveVoltageConstraintTest {
- @SuppressWarnings("LocalVariableName")
@Test
void testDifferentialDriveVoltageConstraint() {
// Pick an unreasonably large kA to ensure the constraint has to do some work
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
index 97c1858..45ee1d3 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
@@ -50,7 +50,6 @@
}
@Test
- @SuppressWarnings("LocalVariableName")
void testGenerationAndConstraints() {
Trajectory trajectory = getTrajectory(new ArrayList<>());
diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
new file mode 100644
index 0000000..e397af1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
@@ -0,0 +1,18 @@
+// 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 "gtest/gtest.h"
+
+TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
+ frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
+ frc::Transform3d cameraToObject{frc::Translation3d{1_m, 1_m, 1_m},
+ frc::Rotation3d{0_deg, -20_deg, 45_deg}};
+ frc::Transform3d robotToCamera{frc::Translation3d{1_m, 0_m, 2_m},
+ frc::Rotation3d{0_deg, 0_deg, 25_deg}};
+ frc::Pose3d object = robot + robotToCamera + cameraToObject;
+
+ EXPECT_EQ(robot,
+ frc::ObjectToRobotPose(object, cameraToObject, robotToCamera));
+}
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index c1786c3..03be9b3 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -2,34 +2,34 @@
// 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 "Eigen/Core"
#include "Eigen/LU"
+#include "frc/EigenCore.h"
#include "gtest/gtest.h"
TEST(EigenTest, Multiplication) {
- Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
- Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
+ frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
+ frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
- Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
+ frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
- Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
- Eigen::Matrix<double, 3, 4> m4{
+ frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
+ frc::Matrixd<3, 4> m4{
{3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
const auto result2 = m3 * m4;
- Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
- {22.13, 9.82, 13.28, 23.53}};
+ frc::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
+ {22.13, 9.82, 13.28, 23.53}};
EXPECT_TRUE(expectedResult2.isApprox(result2));
}
TEST(EigenTest, Transpose) {
- Eigen::Vector<double, 3> vec{1, 2, 3};
+ frc::Vectord<3> vec{1, 2, 3};
const auto transpose = vec.transpose();
@@ -39,8 +39,7 @@
}
TEST(EigenTest, Inverse) {
- Eigen::Matrix<double, 3, 3> mat{
- {1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
+ frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
const auto inverse = mat.inverse();
const auto identity = Eigen::MatrixXd::Identity(3, 3);
diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
index cd7ef5c..0e2f77c 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
+#include <vector>
+
#include <fmt/format.h>
#include "frc/fmt/Eigen.h"
@@ -10,12 +12,33 @@
#include "units/velocity.h"
TEST(FormatterTest, Eigen) {
- Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
+ Eigen::Matrix<double, 3, 2> A{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
EXPECT_EQ(
- " 1.000000 2.000000\n"
- " 3.000000 4.000000\n"
- " 5.000000 6.000000",
+ " 0.000000 1.000000\n"
+ " 2.000000 3.000000\n"
+ " 4.000000 5.000000",
fmt::format("{}", A));
+
+ Eigen::MatrixXd B{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
+ EXPECT_EQ(
+ " 0.000000 1.000000\n"
+ " 2.000000 3.000000\n"
+ " 4.000000 5.000000",
+ fmt::format("{}", B));
+
+ Eigen::SparseMatrix<double> C{3, 2};
+ std::vector<Eigen::Triplet<double>> triplets;
+ triplets.emplace_back(0, 1, 1.0);
+ triplets.emplace_back(1, 0, 2.0);
+ triplets.emplace_back(1, 1, 3.0);
+ triplets.emplace_back(2, 0, 4.0);
+ triplets.emplace_back(2, 1, 5.0);
+ C.setFromTriplets(triplets.begin(), triplets.end());
+ EXPECT_EQ(
+ " 0.000000 1.000000\n"
+ " 2.000000 3.000000\n"
+ " 4.000000 5.000000",
+ fmt::format("{}", C));
}
TEST(FormatterTest, Units) {
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index 6b5af2b..a836a77 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
+#include <limits>
+
#include "frc/MathUtil.h"
#include "gtest/gtest.h"
#include "units/angle.h"
@@ -10,7 +12,7 @@
#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
-TEST(MathUtilTest, ApplyDeadband) {
+TEST(MathUtilTest, ApplyDeadbandUnityScale) {
// < 0
EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
@@ -29,6 +31,33 @@
EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
}
+TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
+ // < 0
+ EXPECT_DOUBLE_EQ(-2.5, frc::ApplyDeadband(-2.5, 0.02, 2.5));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02, 2.5));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02, 2.5));
+
+ // == 0
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02, 2.5));
+
+ // > 0
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02, 2.5));
+ EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02, 2.5));
+ EXPECT_DOUBLE_EQ(2.5, frc::ApplyDeadband(2.5, 0.02, 2.5));
+}
+
+TEST(MathUtilTest, ApplyDeadbandUnits) {
+ // < 0
+ EXPECT_DOUBLE_EQ(
+ -20, frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad).value());
+}
+
+TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
+ EXPECT_DOUBLE_EQ(
+ 80.0,
+ frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
+}
+
TEST(MathUtilTest, InputModulus) {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.
@@ -71,20 +100,20 @@
TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_NEAR(
- frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
- units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
+ frc::AngleModulus(units::radian_t{-2000 * std::numbers::pi / 180}),
+ units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(
- frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
- units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
- EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
+ frc::AngleModulus(units::radian_t{358 * std::numbers::pi / 180}),
+ units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
+ EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * std::numbers::pi}),
0_rad, 1e-10);
- EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
- units::radian_t(wpi::numbers::pi));
- EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)),
- units::radian_t(wpi::numbers::pi));
- EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)),
- units::radian_t(wpi::numbers::pi / 2));
- EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)),
- units::radian_t(-wpi::numbers::pi / 2));
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{5 * std::numbers::pi}),
+ units::radian_t{std::numbers::pi});
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-5 * std::numbers::pi}),
+ units::radian_t{std::numbers::pi});
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{std::numbers::pi / 2}),
+ units::radian_t{std::numbers::pi / 2});
+ EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
+ units::radian_t{-std::numbers::pi / 2});
}
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index 0dc3978..ee7842c 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -7,7 +7,7 @@
#include <cmath>
#include <random>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -45,8 +45,7 @@
void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
double noise) {
- Eigen::Vector<double, 1> y =
- plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
+ Vectord<1> y = plant.CalculateY(loop.Xhat(), loop.U()) + Vectord<1>{noise};
loop.Correct(y);
loop.Predict(kDt);
}
@@ -55,7 +54,7 @@
std::default_random_engine generator;
std::normal_distribution<double> dist{0.0, kPositionStddev};
- Eigen::Vector<double, 2> references{2.0, 0.0};
+ Vectord<2> references{2.0, 0.0};
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 57b93bb..105a434 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -6,51 +6,12 @@
#include <array>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/NumericalIntegration.h"
-TEST(StateSpaceUtilTest, MakeMatrix) {
- // Column vector
- Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
- EXPECT_NEAR(mat1(0), 1.0, 1e-3);
- EXPECT_NEAR(mat1(1), 2.0, 1e-3);
-
- // Row vector
- Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
- EXPECT_NEAR(mat2(0), 1.0, 1e-3);
- EXPECT_NEAR(mat2(1), 2.0, 1e-3);
-
- // Square matrix
- Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
- EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3);
- EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3);
- EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3);
- EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3);
-
- // Nonsquare matrix with more rows than columns
- Eigen::Matrix<double, 3, 2> mat4 =
- frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
- EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3);
- EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3);
- EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3);
- EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3);
- EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3);
- EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3);
-
- // Nonsquare matrix with more columns than rows
- Eigen::Matrix<double, 2, 3> mat5 =
- frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
- EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3);
- EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3);
- EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3);
- EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3);
- EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3);
- EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3);
-}
-
TEST(StateSpaceUtilTest, CostParameterPack) {
- Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
+ frc::Matrixd<3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -63,7 +24,7 @@
}
TEST(StateSpaceUtilTest, CostArray) {
- Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
+ frc::Matrixd<3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -76,7 +37,7 @@
}
TEST(StateSpaceUtilTest, CovParameterPack) {
- Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
+ frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -89,7 +50,7 @@
}
TEST(StateSpaceUtilTest, CovArray) {
- Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
+ frc::Matrixd<3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -102,59 +63,59 @@
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
- Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+ frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
- Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+ frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
- Eigen::Matrix<double, 2, 1> B{0, 1};
+ frc::Matrixd<2, 1> B{0, 1};
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
- EXPECT_FALSE((frc::IsStabilizable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
+ EXPECT_FALSE(
+ (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
- EXPECT_FALSE((frc::IsStabilizable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
+ EXPECT_FALSE(
+ (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
- EXPECT_TRUE((frc::IsStabilizable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
+ EXPECT_TRUE(
+ (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
- EXPECT_TRUE((frc::IsStabilizable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+ EXPECT_TRUE(
+ (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
}
TEST(StateSpaceUtilTest, IsDetectable) {
- Eigen::Matrix<double, 1, 2> C{0, 1};
+ frc::Matrixd<1, 2> C{0, 1};
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
- EXPECT_FALSE((frc::IsDetectable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+ EXPECT_FALSE(
+ (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
- EXPECT_FALSE((frc::IsDetectable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
+ EXPECT_FALSE(
+ (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
- EXPECT_TRUE((frc::IsDetectable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+ EXPECT_TRUE(
+ (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
- EXPECT_TRUE((frc::IsDetectable<2, 1>(
- Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+ EXPECT_TRUE(
+ (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index d5ba717..1059260 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -1420,6 +1420,7 @@
}
#endif
+#if !defined(UNIT_LIB_DISABLE_FMT)
TEST_F(UnitContainer, fmtlib) {
testing::internal::CaptureStdout();
fmt::print("{}", degree_t(349.87));
@@ -1500,6 +1501,7 @@
EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
#endif
}
+#endif
TEST_F(UnitContainer, to_string) {
foot_t a(3.5);
@@ -3063,14 +3065,17 @@
constexpr meter_t result0(0);
constexpr auto result1 = make_unit<meter_t>(1);
constexpr auto result2 = meter_t(2);
+ constexpr auto result3 = -3_m;
EXPECT_EQ(meter_t(0), result0);
EXPECT_EQ(meter_t(1), result1);
EXPECT_EQ(meter_t(2), result2);
+ EXPECT_EQ(meter_t(-3), result3);
EXPECT_TRUE(noexcept(result0));
EXPECT_TRUE(noexcept(result1));
EXPECT_TRUE(noexcept(result2));
+ EXPECT_TRUE(noexcept(result3));
}
TEST_F(Constexpr, constants) {
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index 354ed18..b2bcee6 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -6,47 +6,46 @@
#include <cmath>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
-Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
- const Eigen::Vector<double, 1>& u) {
- return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
- Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
+Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) {
+ return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
+ Matrixd<2, 1>{0.0, 1.0} * u;
}
-Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
- return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
+Vectord<2> StateDynamics(const Vectord<2>& x) {
+ return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
- std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
- const Eigen::Vector<double, 1>&)>
+ std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, units::second_t{0.02}};
- Eigen::Vector<double, 2> r{2, 2};
- Eigen::Vector<double, 2> nextR{3, 3};
+ Vectord<2> r{2, 2};
+ Vectord<2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
- std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
- modelDynamics = [](auto& x) { return StateDynamics(x); };
+ std::function<Vectord<2>(const Vectord<2>&)> modelDynamics = [](auto& x) {
+ return StateDynamics(x);
+ };
- Eigen::Matrix<double, 2, 1> B{0, 1};
+ Matrixd<2, 1> B{0, 1};
- frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
- modelDynamics, B, units::second_t(0.02)};
+ frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
+ B, 20_ms};
- Eigen::Vector<double, 2> r{2, 2};
- Eigen::Vector<double, 2> nextR{3, 3};
+ Vectord<2> r{2, 2};
+ Vectord<2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
new file mode 100644
index 0000000..ab8359c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
@@ -0,0 +1,257 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/controller/DifferentialDriveAccelerationLimiter.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/math.h"
+
+namespace frc {
+
+TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
+ constexpr auto trackwidth = 0.9_m;
+ constexpr auto dt = 5_ms;
+ constexpr auto maxA = 2_mps_sq;
+ constexpr auto maxAlpha = 2_rad_per_s_sq;
+
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+
+ DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
+ maxAlpha};
+
+ Vectord<2> x{0.0, 0.0};
+ Vectord<2> xAccelLimiter{0.0, 0.0};
+
+ // Ensure voltage exceeds acceleration before limiting
+ {
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ EXPECT_GT(units::math::abs(a), maxA);
+ }
+ {
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
+ units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+ trackwidth.value()};
+ EXPECT_GT(units::math::abs(alpha), maxAlpha);
+ }
+
+ // Forward
+ Vectord<2> u{12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+ trackwidth.value()};
+ EXPECT_LE(units::math::abs(a), maxA);
+ EXPECT_LE(units::math::abs(alpha), maxAlpha);
+ }
+
+ // Backward
+ u = Vectord<2>{-12.0, -12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+ trackwidth.value()};
+ EXPECT_LE(units::math::abs(a), maxA);
+ EXPECT_LE(units::math::abs(alpha), maxAlpha);
+ }
+
+ // Rotate CCW
+ u = Vectord<2>{-12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+ trackwidth.value()};
+ EXPECT_LE(units::math::abs(a), maxA);
+ EXPECT_LE(units::math::abs(alpha), maxAlpha);
+ }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
+ constexpr auto trackwidth = 0.9_m;
+ constexpr auto dt = 5_ms;
+
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+
+ // Limits are so high, they don't get hit, so states of constrained and
+ // unconstrained systems should match
+ DifferentialDriveAccelerationLimiter accelLimiter{
+ plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq};
+
+ Vectord<2> x{0.0, 0.0};
+ Vectord<2> xAccelLimiter{0.0, 0.0};
+
+ // Forward
+ Vectord<2> u{12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+ EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+ }
+
+ // Backward
+ x.setZero();
+ xAccelLimiter.setZero();
+ u = Vectord<2>{-12.0, -12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+ EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+ }
+
+ // Rotate CCW
+ x.setZero();
+ xAccelLimiter.setZero();
+ u = Vectord<2>{-12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+ EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+ }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, SeperateMinMaxLowLimits) {
+ constexpr auto trackwidth = 0.9_m;
+ constexpr auto dt = 5_ms;
+ constexpr auto minA = -1_mps_sq;
+ constexpr auto maxA = 2_mps_sq;
+ constexpr auto maxAlpha = 2_rad_per_s_sq;
+
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+
+ DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA,
+ maxA, maxAlpha};
+
+ Vectord<2> x{0.0, 0.0};
+ Vectord<2> xAccelLimiter{0.0, 0.0};
+
+ // Ensure voltage exceeds acceleration before limiting
+ {
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ EXPECT_GT(units::math::abs(a), maxA);
+ EXPECT_GT(units::math::abs(a), -minA);
+ }
+
+ // a should always be within [minA, maxA]
+ // Forward
+ Vectord<2> u{12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ EXPECT_GE(a, minA);
+ EXPECT_LE(a, maxA);
+ }
+
+ // Backward
+ u = Vectord<2>{-12.0, -12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ auto [left, right] =
+ accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+ units::meters_per_second_t{xAccelLimiter(1)},
+ units::volt_t{u(0)}, units::volt_t{u(1)});
+ xAccelLimiter =
+ plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+ Vectord<2> accels =
+ plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+ units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+ EXPECT_GE(a, minA);
+ EXPECT_LE(a, maxA);
+ }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+ EXPECT_NO_THROW({
+ DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq,
+ 1_rad_per_s_sq);
+ });
+
+ EXPECT_THROW(
+ {
+ DifferentialDriveAccelerationLimiter accelLimiter(
+ plant, 1_m, 1_mps_sq, -1_mps_sq, 1_rad_per_s_sq);
+ },
+ std::invalid_argument);
+}
+
+} // namespace frc
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
new file mode 100644
index 0000000..74d945c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
@@ -0,0 +1,84 @@
+// 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 <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/DifferentialDriveFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
+ constexpr auto kVLinear = 1_V / 1_mps;
+ constexpr auto kALinear = 1_V / 1_mps_sq;
+ constexpr auto kVAngular = 1_V / 1_rad_per_s;
+ constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
+ constexpr auto trackwidth = 1_m;
+ constexpr auto dt = 20_ms;
+
+ frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+ kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
+ frc::LinearSystem<2, 2, 2> plant =
+ frc::LinearSystemId::IdentifyDrivetrainSystem(
+ kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+ for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+ currentLeftVelocity += 2_mps) {
+ for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+ currentRightVelocity += 2_mps) {
+ for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+ nextLeftVelocity += 2_mps) {
+ for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+ nextRightVelocity += 2_mps) {
+ auto [left, right] = differentialDriveFeedforward.Calculate(
+ currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
+ nextRightVelocity, dt);
+ frc::Matrixd<2, 1> nextX = plant.CalculateX(
+ frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
+ frc::Vectord<2>{left, right}, dt);
+ EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
+ EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
+ }
+ }
+ }
+ }
+}
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
+ constexpr auto kVLinear = 1_V / 1_mps;
+ constexpr auto kALinear = 1_V / 1_mps_sq;
+ constexpr auto kVAngular = 1_V / 1_mps;
+ constexpr auto kAAngular = 1_V / 1_mps_sq;
+ constexpr auto dt = 20_ms;
+
+ frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+ kVLinear, kALinear, kVAngular, kAAngular};
+ frc::LinearSystem<2, 2, 2> plant =
+ frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
+ kVAngular, kAAngular);
+ for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+ currentLeftVelocity += 2_mps) {
+ for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+ currentRightVelocity += 2_mps) {
+ for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+ nextLeftVelocity += 2_mps) {
+ for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+ nextRightVelocity += 2_mps) {
+ auto [left, right] = differentialDriveFeedforward.Calculate(
+ currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
+ nextRightVelocity, dt);
+ frc::Matrixd<2, 1> nextX = plant.CalculateX(
+ frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
+ frc::Vectord<2>{left, right}, dt);
+ EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
+ EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
+ }
+ }
+ }
+ }
+}
diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index c6b669c..134a4e0 100644
--- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/MathUtil.h"
#include "frc/controller/HolonomicDriveController.h"
@@ -16,7 +16,7 @@
EXPECT_LE(units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
-static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(HolonomicDriveControllerTest, ReachesReference) {
@@ -25,10 +25,10 @@
frc::ProfiledPIDController<units::radian>{
1.0, 0.0, 0.0,
frc::TrapezoidProfile<units::radian>::Constraints{
- units::radians_per_second_t{2.0 * wpi::numbers::pi},
- units::radians_per_second_squared_t{wpi::numbers::pi}}}};
+ units::radians_per_second_t{2.0 * std::numbers::pi},
+ units::radians_per_second_squared_t{std::numbers::pi}}}};
- frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+ frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
@@ -60,7 +60,7 @@
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
- frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+ frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.value());
}
diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
new file mode 100644
index 0000000..afa491a
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
@@ -0,0 +1,109 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/controller/ImplicitModelFollower.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+namespace frc {
+
+TEST(ImplicitModelFollowerTest, SameModel) {
+ constexpr auto dt = 5_ms;
+
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+
+ ImplicitModelFollower<2, 2> imf{plant, plant};
+
+ Vectord<2> x{0.0, 0.0};
+ Vectord<2> xImf{0.0, 0.0};
+
+ // Forward
+ Vectord<2> u{12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xImf(0));
+ EXPECT_DOUBLE_EQ(x(1), xImf(1));
+ }
+
+ // Backward
+ u = Vectord<2>{-12.0, -12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xImf(0));
+ EXPECT_DOUBLE_EQ(x(1), xImf(1));
+ }
+
+ // Rotate CCW
+ u = Vectord<2>{-12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_DOUBLE_EQ(x(0), xImf(0));
+ EXPECT_DOUBLE_EQ(x(1), xImf(1));
+ }
+}
+
+TEST(ImplicitModelFollowerTest, SlowerRefModel) {
+ constexpr auto dt = 5_ms;
+
+ using Kv_t = decltype(1_V / 1_mps);
+ using Ka_t = decltype(1_V / 1_mps_sq);
+
+ auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+ Kv_t{1.0}, Ka_t{1.0});
+
+ // Linear acceleration is slower, but angular acceleration is the same
+ auto plantRef = LinearSystemId::IdentifyDrivetrainSystem(
+ Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0});
+
+ ImplicitModelFollower<2, 2> imf{plant, plantRef};
+
+ Vectord<2> x{0.0, 0.0};
+ Vectord<2> xImf{0.0, 0.0};
+
+ // Forward
+ Vectord<2> u{12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_GE(x(0), xImf(0));
+ EXPECT_GE(x(1), xImf(1));
+ }
+
+ // Backward
+ x.setZero();
+ xImf.setZero();
+ u = Vectord<2>{-12.0, -12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_LE(x(0), xImf(0));
+ EXPECT_LE(x(1), xImf(1));
+ }
+
+ // Rotate CCW
+ x.setZero();
+ xImf.setZero();
+ u = Vectord<2>{-12.0, 12.0};
+ for (auto t = 0_s; t < 3_s; t += dt) {
+ x = plant.CalculateX(x, u, dt);
+ xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+ EXPECT_NEAR(x(0), xImf(0), 1e-5);
+ EXPECT_NEAR(x(1), xImf(1), 1e-5);
+ }
+}
+
+} // namespace frc
diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
new file mode 100644
index 0000000..753ea34
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
@@ -0,0 +1,101 @@
+// 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 <cmath>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/LTVDifferentialDriveController.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
+ 180.0};
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+ /// X position in global coordinate frame.
+ 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.
+ static constexpr int kLeftVelocity = 3;
+
+ /// Right encoder velocity.
+ static constexpr int kRightVelocity = 4;
+};
+
+static constexpr auto kLinearV = 3.02_V / 1_mps;
+static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
+static constexpr auto kAngularV = 1.382_V / 1_mps;
+static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
+static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
+ kLinearV, kLinearA, kAngularV, kAngularA);
+static constexpr auto kTrackwidth = 0.9_m;
+
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
+ double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+ frc::Vectord<5> xdot;
+ xdot(0) = v * std::cos(x(State::kHeading));
+ xdot(1) = v * std::sin(x(State::kHeading));
+ xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth)
+ .value();
+ xdot.block<2, 1>(3, 0) = plant.A() * x.block<2, 1>(3, 0) + plant.B() * u;
+ return xdot;
+}
+
+TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
+ constexpr auto kDt = 0.02_s;
+
+ frc::LTVDifferentialDriveController controller{
+ plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
+ frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ frc::Vectord<5> x = frc::Vectord<5>::Zero();
+ x(State::kX) = robotPose.X().value();
+ x(State::kY) = robotPose.Y().value();
+ x(State::kHeading) = robotPose.Rotation().Radians().value();
+
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ robotPose =
+ frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
+ units::radian_t{x(State::kHeading)}};
+ auto [leftVoltage, rightVoltage] = controller.Calculate(
+ robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
+ units::meters_per_second_t{x(State::kRightVelocity)}, state);
+
+ x = frc::RKDP(&Dynamics, x,
+ frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
+ kDt);
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+ EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+ robotPose.Rotation().Radians()),
+ 0_rad, kAngularTolerance);
+}
diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
new file mode 100644
index 0000000..56faf1d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
@@ -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.
+
+#include "frc/MathUtil.h"
+#include "frc/controller/LTVUnicycleController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
+ 180.0};
+
+TEST(LTVUnicycleControllerTest, ReachesReference) {
+ constexpr auto kDt = 0.02_s;
+
+ frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
+ frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+ static_cast<void>(vy);
+
+ robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+ EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+ robotPose.Rotation().Radians()),
+ 0_rad, kAngularTolerance);
+}
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index 6e61706..e7107d0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -6,21 +6,20 @@
#include <cmath>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
- Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
- Eigen::Matrix<double, 2, 1> B{0, 1};
+ Matrixd<2, 2> A{{1, 0}, {0, 1}};
+ Matrixd<2, 1> B{0, 1};
- frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
- units::second_t(0.02)};
+ frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
- Eigen::Vector<double, 2> r{2, 2};
- Eigen::Vector<double, 2> nextR{3, 3};
+ Vectord<2> r{2, 2};
+ Vectord<2> nextR{3, 3};
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 7055530..1127fc2 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -6,7 +6,7 @@
#include <cmath>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
@@ -30,7 +30,7 @@
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
- Eigen::Matrix<double, 1, 2> K =
+ Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
@@ -54,7 +54,7 @@
motors, 1.0 / 3.0 * m * r * r, G);
}();
- Eigen::Matrix<double, 1, 2> K =
+ Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
.K();
@@ -77,7 +77,7 @@
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
- Eigen::Matrix<double, 1, 2> K =
+ Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
EXPECT_NEAR(10.38, K(0, 0), 1e-1);
@@ -99,50 +99,44 @@
* @param dt Discretization timestep.
*/
template <int States, int Inputs>
-Eigen::Matrix<double, Inputs, States> GetImplicitModelFollowingK(
- 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, States>& Aref, units::second_t dt) {
+Matrixd<Inputs, States> GetImplicitModelFollowingK(
+ const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+ const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+ const Matrixd<States, States>& Aref, units::second_t dt) {
// Discretize real dynamics
- Eigen::Matrix<double, States, States> discA;
- Eigen::Matrix<double, States, Inputs> discB;
+ Matrixd<States, States> discA;
+ Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
// Discretize desired dynamics
- Eigen::Matrix<double, States, States> discAref;
+ Matrixd<States, States> discAref;
DiscretizeA<States>(Aref, dt, &discAref);
- Eigen::Matrix<double, States, States> Qimf =
+ Matrixd<States, States> Qimf =
(discA - discAref).transpose() * Q * (discA - discAref);
- Eigen::Matrix<double, Inputs, Inputs> Rimf =
- discB.transpose() * Q * discB + R;
- Eigen::Matrix<double, States, Inputs> Nimf =
- (discA - discAref).transpose() * Q * discB;
+ Matrixd<Inputs, Inputs> Rimf = discB.transpose() * Q * discB + R;
+ Matrixd<States, Inputs> Nimf = (discA - discAref).transpose() * Q * discB;
return LinearQuadraticRegulator<States, Inputs>{A, B, Qimf, Rimf, Nimf, dt}
.K();
}
TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) {
- Eigen::Matrix<double, 2, 2> A{Eigen::Matrix<double, 2, 2>::Zero()};
- Eigen::Matrix<double, 2, 2> B{Eigen::Matrix<double, 2, 2>::Identity()};
- Eigen::Matrix<double, 2, 2> Q{Eigen::Matrix<double, 2, 2>::Identity()};
- Eigen::Matrix<double, 2, 2> R{Eigen::Matrix<double, 2, 2>::Identity()};
+ Matrixd<2, 2> A{Matrixd<2, 2>::Zero()};
+ Matrixd<2, 2> B{Matrixd<2, 2>::Identity()};
+ Matrixd<2, 2> Q{Matrixd<2, 2>::Identity()};
+ Matrixd<2, 2> R{Matrixd<2, 2>::Identity()};
// QR overload
- Eigen::Matrix<double, 2, 2> K =
- LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
+ Matrixd<2, 2> K = LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10);
EXPECT_NEAR(0.0, K(0, 1), 1e-10);
EXPECT_NEAR(0.0, K(1, 0), 1e-10);
EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10);
// QRN overload
- Eigen::Matrix<double, 2, 2> N{Eigen::Matrix<double, 2, 2>::Identity()};
- Eigen::Matrix<double, 2, 2> Kimf =
- LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
+ Matrixd<2, 2> N{Matrixd<2, 2>::Identity()};
+ Matrixd<2, 2> Kimf = LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10);
EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10);
@@ -153,21 +147,19 @@
double Kv = 3.02;
double Ka = 0.642;
- Eigen::Matrix<double, 2, 2> A{{0, 1}, {0, -Kv / Ka}};
- Eigen::Matrix<double, 2, 1> B{{0}, {1.0 / Ka}};
- Eigen::Matrix<double, 2, 2> Q{{1, 0}, {0, 0.2}};
- Eigen::Matrix<double, 1, 1> R{0.25};
+ Matrixd<2, 2> A{{0, 1}, {0, -Kv / Ka}};
+ Matrixd<2, 1> B{{0}, {1.0 / Ka}};
+ Matrixd<2, 2> Q{{1, 0}, {0, 0.2}};
+ Matrixd<1, 1> R{0.25};
// QR overload
- Eigen::Matrix<double, 1, 2> K =
- LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
+ Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10);
EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
// QRN overload
- Eigen::Matrix<double, 2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
- Eigen::Matrix<double, 1, 2> Kimf =
- GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
+ Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+ Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
}
diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index 379db9e..f1034f5 100644
--- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -9,7 +9,7 @@
protected:
frc2::PIDController* controller;
- void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+ void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
void TearDown() override { delete controller; }
};
diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
index da402ae..44d1f41 100644
--- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/controller/ProfiledPIDController.h"
#include "gtest/gtest.h"
@@ -40,8 +40,8 @@
TEST_F(ProfiledPIDInputOutputTest, ContinuousInput2) {
controller->SetP(1);
- controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
- units::radian_t{wpi::numbers::pi});
+ controller->EnableContinuousInput(-units::radian_t{std::numbers::pi},
+ units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.4826633343199735};
static constexpr units::radian_t kMeasurement{-3.1352207333939606};
@@ -52,13 +52,13 @@
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
- units::radian_t{wpi::numbers::pi});
+ units::radian_t{std::numbers::pi});
}
TEST_F(ProfiledPIDInputOutputTest, ContinuousInput3) {
controller->SetP(1);
- controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
- units::radian_t{wpi::numbers::pi});
+ controller->EnableContinuousInput(-units::radian_t{std::numbers::pi},
+ units::radian_t{std::numbers::pi});
static constexpr units::radian_t kSetpoint{-3.5176604690006377};
static constexpr units::radian_t kMeasurement{3.1191729343822456};
@@ -69,13 +69,13 @@
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
- units::radian_t{wpi::numbers::pi});
+ units::radian_t{std::numbers::pi});
}
TEST_F(ProfiledPIDInputOutputTest, ContinuousInput4) {
controller->SetP(1);
controller->EnableContinuousInput(0_rad,
- units::radian_t{2.0 * wpi::numbers::pi});
+ units::radian_t{2.0 * std::numbers::pi});
static constexpr units::radian_t kSetpoint{2.78};
static constexpr units::radian_t kMeasurement{3.12};
@@ -86,7 +86,7 @@
// Error must be less than half the input range at all times
EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
- units::radian_t{wpi::numbers::pi});
+ units::radian_t{std::numbers::pi});
}
TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 8829deb..2fd26bb 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -12,13 +12,13 @@
EXPECT_LE(units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
-static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(RamseteControllerTest, ReachesReference) {
frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m),
0.7 / 1_rad};
- frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+ frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index 3cf944e..6d10e05 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -6,7 +6,7 @@
#include <cmath>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/acceleration.h"
@@ -21,16 +21,16 @@
double Ka = 0.6;
auto dt = 0.02_s;
- Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
- Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
+ Matrixd<1, 1> A{-Kv / Ka};
+ Matrixd<1, 1> B{1.0 / Ka};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
- Eigen::Vector<double, 1> r{2.0};
- Eigen::Vector<double, 1> nextR{3.0};
+ Vectord<1> r{2.0};
+ Vectord<1> nextR{3.0};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index ee1da7f..b2ee87d 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -4,15 +4,15 @@
#include <gtest/gtest.h>
-#include <wpi/numbers>
+#include <numbers>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "frc/estimator/AngleStatistics.h"
TEST(AngleStatisticsTest, Mean) {
- Eigen::Matrix<double, 3, 3> sigmas{
+ frc::Matrixd<3, 3> sigmas{
{1, 1.2, 0},
- {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
+ {359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
{1, 2, 0}};
// Weights need to produce the mean of the sigmas
Eigen::Vector3d weights;
@@ -23,16 +23,16 @@
}
TEST(AngleStatisticsTest, Residual) {
- Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
- Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+ Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
+ Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
- Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
+ Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
}
TEST(AngleStatisticsTest, Add) {
- Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
- Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+ Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
+ Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 4a854fd..1d623ca 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -4,81 +4,103 @@
#include <limits>
#include <random>
+#include <tuple>
+#include <utility>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
-TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
- frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
- frc::Pose2d(),
- {0.02, 0.02, 0.01, 0.02, 0.02},
- {0.01, 0.01, 0.001},
- {0.1, 0.1, 0.01}};
+void testFollowTrajectory(
+ const frc::DifferentialDriveKinematics& kinematics,
+ frc::DifferentialDrivePoseEstimator& estimator,
+ const frc::Trajectory& trajectory,
+ std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+ chassisSpeedsGenerator,
+ std::function<frc::Pose2d(frc::Trajectory::State&)>
+ visionMeasurementGenerator,
+ const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+ const units::second_t dt, const units::second_t kVisionUpdateRate,
+ const units::second_t kVisionUpdateDelay, const bool checkError,
+ const bool debug) {
+ units::meter_t leftDistance = 0_m;
+ units::meter_t rightDistance = 0_m;
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
- frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
- frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
- frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
-
- frc::DifferentialDriveKinematics kinematics{1.0_m};
- frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
+ estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
+ startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
- units::second_t dt = 0.02_s;
- units::second_t t = 0.0_s;
+ units::second_t t = 0_s;
- units::meter_t leftDistance = 0_m;
- units::meter_t rightDistance = 0_m;
-
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+ std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+ std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+ visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
- while (t <= trajectory.TotalTime()) {
- auto groundTruthState = trajectory.Sample(t);
- auto input = kinematics.ToWheelSpeeds(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
+ if (debug) {
+ fmt::print(
+ "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
+ "right\n");
+ }
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d()) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d(
- frc::Translation2d(distribution(generator) * 0.1 * 1_m,
- distribution(generator) * 0.1 * 1_m),
- frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
- lastVisionUpdateTime = t;
+ // We are due for a new vision measurement if it's been `visionUpdateRate`
+ // seconds since the last vision measurement
+ if (visionPoses.empty() ||
+ visionPoses.back().first + kVisionUpdateRate < t) {
+ auto visionPose =
+ visionMeasurementGenerator(groundTruthState) +
+ frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+ distribution(generator) * 0.1_m},
+ frc::Rotation2d{distribution(generator) * 0.05_rad}};
+ visionPoses.push_back({t, visionPose});
}
- leftDistance += input.left * distribution(generator) * 0.01 * dt;
- rightDistance += input.right * distribution(generator) * 0.01 * dt;
+ // We should apply the oldest vision measurement if it has been
+ // `visionUpdateDelay` seconds since it was measured
+ if (!visionPoses.empty() &&
+ visionPoses.front().first + kVisionUpdateDelay < t) {
+ auto visionEntry = visionPoses.front();
+ estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+ visionPoses.erase(visionPoses.begin());
+ visionLog.push_back({t, visionEntry.first, visionEntry.second});
+ }
+
+ auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+ auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ leftDistance += wheelSpeeds.left * dt;
+ rightDistance += wheelSpeeds.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
- frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
- input, leftDistance, rightDistance);
+ frc::Rotation2d{distribution(generator) * 0.05_rad} -
+ trajectory.InitialPose().Rotation(),
+ leftDistance, rightDistance);
+
+ if (debug) {
+ fmt::print(
+ "{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+ xhat.Y().value(), xhat.Rotation().Radians().value(),
+ groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
+ groundTruthState.pose.Rotation().Radians().value(),
+ leftDistance.value(), rightDistance.value());
+ }
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -92,7 +114,96 @@
t += dt;
}
- EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
- 0.2);
- EXPECT_NEAR(0.0, maxError, 0.4);
+ if (debug) {
+ fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+ units::second_t apply_time;
+ units::second_t measure_time;
+ frc::Pose2d vision_pose;
+ for (auto record : visionLog) {
+ std::tie(apply_time, measure_time, vision_pose) = record;
+ fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+ measure_time.value(), vision_pose.X().value(),
+ vision_pose.Y().value(),
+ vision_pose.Rotation().Radians().value());
+ }
+ }
+
+ EXPECT_NEAR(endingPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 0.08);
+ EXPECT_NEAR(endingPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 0.08);
+ EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 0.15);
+
+ if (checkError) {
+ EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
+ EXPECT_LT(maxError, 0.2);
+ }
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
+ frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+ frc::DifferentialDrivePoseEstimator estimator{
+ kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+ {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2_mps, 2_mps_sq));
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
+ 0.1_s, 0.25_s, true, false);
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
+ frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+ frc::DifferentialDrivePoseEstimator estimator{
+ kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+ {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2_mps, 2_mps_sq));
+
+ for (units::degree_t offset_direction_degs = 0_deg;
+ offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+ for (units::degree_t offset_heading_degs = 0_deg;
+ offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+ auto pose_offset = frc::Rotation2d{offset_direction_degs};
+ auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+ auto initial_pose =
+ trajectory.InitialPose() +
+ frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+ pose_offset.Sin() * 1_m},
+ heading_offset};
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+ 0.25_s, false, false);
+ }
+ }
}
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 6d51185..1924b73 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/NumericalJacobian.h"
@@ -18,8 +18,7 @@
namespace {
-Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
- const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -41,7 +40,7 @@
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
- return Eigen::Vector<double, 5>{
+ return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -50,16 +49,16 @@
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
-Eigen::Vector<double, 3> LocalMeasurementModel(
- const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
+ const frc::Vectord<2>& u) {
static_cast<void>(u);
- return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
+ return frc::Vectord<3>{x(2), x(3), x(4)};
}
-Eigen::Vector<double, 5> GlobalMeasurementModel(
- const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
+ const frc::Vectord<2>& u) {
static_cast<void>(u);
- return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
+ return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -71,7 +70,7 @@
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
- Eigen::Vector<double, 2> u{12.0, 12.0};
+ frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,14 +97,13 @@
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
- Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
- Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
+ frc::Vectord<5> r = frc::Vectord<5>::Zero();
+ frc::Vectord<2> u = frc::Vectord<2>::Zero();
- auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
- Eigen::Vector<double, 5>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
+ frc::Vectord<2>::Zero());
- observer.SetXhat(Eigen::Vector<double, 5>{
+ observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -118,17 +116,15 @@
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
- Eigen::Vector<double, 5> nextR{
+ frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
- auto localY =
- LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
+ auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
- Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
- u = B.householderQr().solve(rdot -
- Dynamics(r, Eigen::Vector<double, 2>::Zero()));
+ frc::Vectord<5> rdot = (nextR - r) / dt.value();
+ u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 881d4e8..13dc5aa 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -4,74 +4,95 @@
#include <limits>
#include <random>
+#include <tuple>
#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
-TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
- frc::MecanumDriveKinematics kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+void testFollowTrajectory(
+ const frc::MecanumDriveKinematics& kinematics,
+ frc::MecanumDrivePoseEstimator& estimator,
+ const frc::Trajectory& trajectory,
+ std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+ chassisSpeedsGenerator,
+ std::function<frc::Pose2d(frc::Trajectory::State&)>
+ visionMeasurementGenerator,
+ const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+ const units::second_t dt, const units::second_t kVisionUpdateRate,
+ const units::second_t kVisionUpdateDelay, const bool checkError,
+ const bool debug) {
+ frc::MecanumDriveWheelPositions wheelPositions{};
- frc::MecanumDrivePoseEstimator estimator{
- frc::Rotation2d(), frc::Pose2d(), kinematics,
- {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
-
- frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
- frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
- frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
- frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+ estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
- units::second_t dt = 0.02_s;
units::second_t t = 0_s;
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
-
- std::vector<frc::Pose2d> visionPoses;
+ std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+ std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+ visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
+ if (debug) {
+ fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+ }
+
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d()) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d(
- frc::Translation2d(distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m),
- frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = t;
+ // We are due for a new vision measurement if it's been `visionUpdateRate`
+ // seconds since the last vision measurement
+ if (visionPoses.empty() ||
+ visionPoses.back().first + kVisionUpdateRate < t) {
+ auto visionPose =
+ visionMeasurementGenerator(groundTruthState) +
+ frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+ distribution(generator) * 0.1_m},
+ frc::Rotation2d{distribution(generator) * 0.05_rad}};
+ visionPoses.push_back({t, visionPose});
}
- auto wheelSpeeds = kinematics.ToWheelSpeeds(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
+ // We should apply the oldest vision measurement if it has been
+ // `visionUpdateDelay` seconds since it was measured
+ if (!visionPoses.empty() &&
+ visionPoses.front().first + kVisionUpdateDelay < t) {
+ auto visionEntry = visionPoses.front();
+ estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+ visionPoses.erase(visionPoses.begin());
+ visionLog.push_back({t, visionEntry.first, visionEntry.second});
+ }
+
+ auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+ auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+ wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+ wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+ wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
- frc::Rotation2d(distribution(generator) * 0.05_rad),
- wheelSpeeds);
+ frc::Rotation2d{distribution(generator) * 0.05_rad} -
+ trajectory.InitialPose().Rotation(),
+ wheelPositions);
+
+ if (debug) {
+ fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+ xhat.Y().value(), xhat.Rotation().Radians().value(),
+ groundTruthState.pose.X().value(),
+ groundTruthState.pose.Y().value(),
+ groundTruthState.pose.Rotation().Radians().value());
+ }
+
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -84,6 +105,104 @@
t += dt;
}
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
- EXPECT_LT(maxError, 0.4);
+ if (debug) {
+ fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+ units::second_t apply_time;
+ units::second_t measure_time;
+ frc::Pose2d vision_pose;
+ for (auto record : visionLog) {
+ std::tie(apply_time, measure_time, vision_pose) = record;
+ fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+ measure_time.value(), vision_pose.X().value(),
+ vision_pose.Y().value(),
+ vision_pose.Rotation().Radians().value());
+ }
+ }
+
+ EXPECT_NEAR(endingPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 0.08);
+ EXPECT_NEAR(endingPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 0.08);
+ EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 0.15);
+
+ if (checkError) {
+ EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
+ EXPECT_LT(maxError, 0.2);
+ }
+}
+
+TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDriveWheelPositions wheelPositions;
+
+ frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
+ wheelPositions, frc::Pose2d{},
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
+ 0.1_s, 0.25_s, true, false);
+}
+
+TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDriveWheelPositions wheelPositions;
+
+ frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
+ wheelPositions, frc::Pose2d{},
+ {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+
+ for (units::degree_t offset_direction_degs = 0_deg;
+ offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+ for (units::degree_t offset_heading_degs = 0_deg;
+ offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+ auto pose_offset = frc::Rotation2d{offset_direction_degs};
+ auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+ auto initial_pose =
+ trajectory.InitialPose() +
+ frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+ pose_offset.Sin() * 1_m},
+ heading_offset};
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+ 0.25_s, false, false);
+ }
+ }
}
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index c012435..19a734b 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -10,26 +10,23 @@
namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
- auto points = sigmaPoints.SigmaPoints(
- Eigen::Vector<double, 2>{0.0, 0.0},
- Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
+ auto points = sigmaPoints.SquareRootSigmaPoints(
+ frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
- (points -
- Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
+ (points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
- auto points = sigmaPoints.SigmaPoints(
- Eigen::Vector<double, 2>{1.0, 2.0},
- Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
+ auto points = sigmaPoints.SquareRootSigmaPoints(
+ frc::Vectord<2>{1.0, 2.0},
+ frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
- (points -
- Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
+ (points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index ee01f6f..554ca59 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -3,75 +3,99 @@
// the WPILib BSD license file in the root directory of this project.
#include <limits>
+#include <numbers>
#include <random>
+#include <tuple>
+
+#include <fmt/format.h>
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveOdometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
-TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
- frc::SwerveDriveKinematics<4> kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+void testFollowTrajectory(
+ const frc::SwerveDriveKinematics<4>& kinematics,
+ frc::SwerveDrivePoseEstimator<4>& estimator,
+ const frc::Trajectory& trajectory,
+ std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+ chassisSpeedsGenerator,
+ std::function<frc::Pose2d(frc::Trajectory::State&)>
+ visionMeasurementGenerator,
+ const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+ const units::second_t dt, const units::second_t kVisionUpdateRate,
+ const units::second_t kVisionUpdateDelay, const bool checkError,
+ const bool debug) {
+ wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
- frc::SwerveDrivePoseEstimator<4> estimator{
- frc::Rotation2d(), frc::Pose2d(), kinematics,
- {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
-
- frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
- frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
- frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
- frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
- frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+ estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose);
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
- units::second_t dt = 0.02_s;
units::second_t t = 0_s;
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
-
- std::vector<frc::Pose2d> visionPoses;
+ std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+ std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+ visionLog;
double maxError = -std::numeric_limits<double>::max();
double errorSum = 0;
+ if (debug) {
+ fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+ }
+
while (t < trajectory.TotalTime()) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d()) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d(
- frc::Translation2d(distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m),
- frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = t;
+ // We are due for a new vision measurement if it's been `visionUpdateRate`
+ // seconds since the last vision measurement
+ if (visionPoses.empty() ||
+ visionPoses.back().first + kVisionUpdateRate < t) {
+ auto visionPose =
+ visionMeasurementGenerator(groundTruthState) +
+ frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+ distribution(generator) * 0.1_m},
+ frc::Rotation2d{distribution(generator) * 0.05_rad}};
+ visionPoses.push_back({t, visionPose});
}
- auto moduleStates = kinematics.ToSwerveModuleStates(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
+ // We should apply the oldest vision measurement if it has been
+ // `visionUpdateDelay` seconds since it was measured
+ if (!visionPoses.empty() &&
+ visionPoses.front().first + kVisionUpdateDelay < t) {
+ auto visionEntry = visionPoses.front();
+ estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+ visionPoses.erase(visionPoses.begin());
+ visionLog.push_back({t, visionEntry.first, visionEntry.second});
+ }
+
+ auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+ auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
+
+ for (size_t i = 0; i < 4; i++) {
+ positions[i].distance += moduleStates[i].speed * dt;
+ positions[i].angle = moduleStates[i].angle;
+ }
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
- frc::Rotation2d(distribution(generator) * 0.05_rad),
- moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+ frc::Rotation2d{distribution(generator) * 0.05_rad} -
+ trajectory.InitialPose().Rotation(),
+ positions);
+
+ if (debug) {
+ fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+ xhat.Y().value(), xhat.Rotation().Radians().value(),
+ groundTruthState.pose.X().value(),
+ groundTruthState.pose.Y().value(),
+ groundTruthState.pose.Rotation().Radians().value());
+ }
+
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -84,6 +108,110 @@
t += dt;
}
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
- EXPECT_LT(maxError, 0.4);
+ if (debug) {
+ fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+ units::second_t apply_time;
+ units::second_t measure_time;
+ frc::Pose2d vision_pose;
+ for (auto record : visionLog) {
+ std::tie(apply_time, measure_time, vision_pose) = record;
+ fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+ measure_time.value(), vision_pose.X().value(),
+ vision_pose.Y().value(),
+ vision_pose.Rotation().Radians().value());
+ }
+ }
+
+ EXPECT_NEAR(endingPose.X().value(),
+ estimator.GetEstimatedPosition().X().value(), 0.08);
+ EXPECT_NEAR(endingPose.Y().value(),
+ estimator.GetEstimatedPosition().Y().value(), 0.08);
+ EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+ estimator.GetEstimatedPosition().Rotation().Radians().value(),
+ 0.15);
+
+ if (checkError) {
+ EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
+ EXPECT_LT(maxError, 0.2);
+ }
+}
+
+TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
+ frc::SwerveDriveKinematics<4> kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::SwerveModulePosition fl;
+ frc::SwerveModulePosition fr;
+ frc::SwerveModulePosition bl;
+ frc::SwerveModulePosition br;
+
+ frc::SwerveDrivePoseEstimator<4> estimator{
+ kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
+ frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
+ 0.02_s, 0.1_s, 0.25_s, true, false);
+}
+
+TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
+ frc::SwerveDriveKinematics<4> kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::SwerveModulePosition fl;
+ frc::SwerveModulePosition fr;
+ frc::SwerveModulePosition bl;
+ frc::SwerveModulePosition br;
+
+ frc::SwerveDrivePoseEstimator<4> estimator{
+ kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
+ frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
+
+ for (units::degree_t offset_direction_degs = 0_deg;
+ offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+ for (units::degree_t offset_heading_degs = 0_deg;
+ offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+ auto pose_offset = frc::Rotation2d{offset_direction_degs};
+ auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+ auto initial_pose =
+ trajectory.InitialPose() +
+ frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+ pose_offset.Sin() * 1_m},
+ heading_offset};
+
+ testFollowTrajectory(
+ kinematics, estimator, trajectory,
+ [&](frc::Trajectory::State& state) {
+ return frc::ChassisSpeeds{state.velocity, 0_mps,
+ state.velocity * state.curvature};
+ },
+ [&](frc::Trajectory::State& state) { return state.pose; },
+ initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+ 0.25_s, false, false);
+ }
+ }
}
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 9665442..68f9c40 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
-#include "Eigen/Core"
#include "Eigen/QR"
+#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
@@ -20,11 +20,10 @@
namespace {
-Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
- const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
- // constexpr double Glow = 15.32; // Low gear ratio
+ // constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
@@ -43,7 +42,7 @@
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
- return Eigen::Vector<double, 5>{
+ return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -52,16 +51,16 @@
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
-Eigen::Vector<double, 3> LocalMeasurementModel(
- const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
+ const frc::Vectord<2>& u) {
static_cast<void>(u);
- return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
+ return frc::Vectord<3>{x(2), x(3), x(4)};
}
-Eigen::Vector<double, 5> GlobalMeasurementModel(
- const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
+ const frc::Vectord<2>& u) {
static_cast<void>(u);
- return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
+ return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -72,8 +71,13 @@
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
+ frc::AngleMean<5, 5>(2),
+ frc::AngleMean<3, 5>(0),
+ frc::AngleResidual<5>(2),
+ frc::AngleResidual<3>(0),
+ frc::AngleAdd<5>(2),
dt};
- Eigen::Vector<double, 2> u{12.0, 12.0};
+ frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -94,6 +98,11 @@
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
+ frc::AngleMean<5, 5>(2),
+ frc::AngleMean<3, 5>(0),
+ frc::AngleResidual<5>(2),
+ frc::AngleResidual<3>(0),
+ frc::AngleAdd<5>(2),
dt};
auto waypoints =
@@ -102,14 +111,13 @@
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
- Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
- Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
+ frc::Vectord<5> r = frc::Vectord<5>::Zero();
+ frc::Vectord<2> u = frc::Vectord<2>::Zero();
- auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
- Eigen::Vector<double, 5>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
+ frc::Vectord<2>::Zero());
- observer.SetXhat(Eigen::Vector<double, 5>{
+ observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -124,17 +132,15 @@
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
- Eigen::Vector<double, 5> nextR{
+ frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
- auto localY =
- LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
+ auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
- Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
- u = B.householderQr().solve(rdot -
- Dynamics(r, Eigen::Vector<double, 2>::Zero()));
+ frc::Vectord<5> rdot = (nextR - r) / dt.value();
+ u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -154,12 +160,28 @@
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
- ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
- 1.0);
- ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
- 1.0);
- ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
- 1.0);
- ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
- ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
+ EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+ 0.055);
+ EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+ 0.15);
+ EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+ 0.000005);
+ EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
+ EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
+}
+
+TEST(UnscentedKalmanFilterTest, RoundTripP) {
+ constexpr auto dt = 5_ms;
+
+ frc::UnscentedKalmanFilter<2, 2, 2> observer{
+ [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+ [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+ {0.0, 0.0},
+ {0.0, 0.0},
+ dt};
+
+ frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
+ observer.SetP(P);
+
+ ASSERT_TRUE(observer.P().isApprox(P));
}
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
index 5ccd829..8299a71 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -5,10 +5,9 @@
#include "frc/filter/LinearFilter.h" // NOLINT(build/include_order)
#include <cmath>
+#include <numbers>
#include <random>
-#include <wpi/numbers>
-
#include "gtest/gtest.h"
#include "units/time.h"
@@ -21,7 +20,7 @@
enum LinearFilterNoiseTestType { kTestSinglePoleIIR, kTestMovAvg };
static double GetData(double t) {
- return 100.0 * std::sin(2.0 * wpi::numbers::pi * t);
+ return 100.0 * std::sin(2.0 * std::numbers::pi * t);
}
class LinearFilterNoiseTest
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index 56121fa..152737d 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -7,10 +7,10 @@
#include <cmath>
#include <functional>
#include <memory>
+#include <numbers>
#include <random>
#include <wpi/array.h>
-#include <wpi/numbers>
#include "gtest/gtest.h"
#include "units/time.h"
@@ -33,8 +33,8 @@
};
static double GetData(double t) {
- return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) +
- 20.0 * std::cos(50.0 * wpi::numbers::pi * t);
+ return 100.0 * std::sin(2.0 * std::numbers::pi * t) +
+ 20.0 * std::cos(50.0 * std::numbers::pi * t);
}
static double GetPulseData(double t) {
@@ -207,14 +207,14 @@
return std::log(x);
},
[](double x) {
- // df/dx = 1 / x
+ // df/dx = 1/x
return 1.0 / x;
},
h, 1.0, 20.0);
AssertCentralResults<2, 5>(
[](double x) {
- // f(x) = x^2
+ // f(x) = x²
return x * x;
},
[](double x) {
@@ -240,7 +240,7 @@
return std::log(x);
},
[](double x) {
- // d²f/dx² = -1 / x²
+ // d²f/dx² = -1/x²
return -1.0 / (x * x);
},
h, 1.0, 20.0);
@@ -280,14 +280,14 @@
return std::log(x);
},
[](double x) {
- // df/dx = 1 / x
+ // df/dx = 1/x
return 1.0 / x;
},
h, 1.0, 20.0);
AssertBackwardResults<2, 4>(
[](double x) {
- // f(x) = x^2
+ // f(x) = x²
return x * x;
},
[](double x) {
@@ -313,7 +313,7 @@
return std::log(x);
},
[](double x) {
- // d²f/dx² = -1 / x²
+ // d²f/dx² = -1/x²
return -1.0 / (x * x);
},
h, 1.0, 20.0);
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index 4896073..5dbe8c8 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -38,3 +38,15 @@
EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
}
+
+TEST_F(SlewRateLimiterTest, SlewRatePositveNegativeLimit) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps, -0.5_mps);
+
+ now += 1_s;
+
+ EXPECT_EQ(limiter.Calculate(2_m), 1_m);
+
+ now += 1_s;
+
+ EXPECT_EQ(limiter.Calculate(0_m), 0.5_m);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
new file mode 100644
index 0000000..fc44fa5
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
@@ -0,0 +1,163 @@
+// 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 "frc/geometry/Pose3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+void CheckPose3dConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
+ const CoordinateSystem& coordFrom,
+ const CoordinateSystem& coordTo) {
+ // "from" to "to"
+ EXPECT_EQ(
+ poseTo.Translation(),
+ CoordinateSystem::Convert(poseFrom.Translation(), coordFrom, coordTo));
+ EXPECT_EQ(poseTo.Rotation(),
+ CoordinateSystem::Convert(poseFrom.Rotation(), coordFrom, coordTo));
+ EXPECT_EQ(poseTo, CoordinateSystem::Convert(poseFrom, coordFrom, coordTo));
+
+ // "to" to "from"
+ EXPECT_EQ(
+ poseFrom.Translation(),
+ CoordinateSystem::Convert(poseTo.Translation(), coordTo, coordFrom));
+ EXPECT_EQ(poseFrom.Rotation(),
+ CoordinateSystem::Convert(poseTo.Rotation(), coordTo, coordFrom));
+ EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
+}
+
+void CheckTransform3dConvert(const Transform3d& transformFrom,
+ const Transform3d& transformTo,
+ const CoordinateSystem& coordFrom,
+ const CoordinateSystem& coordTo) {
+ // "from" to "to"
+ EXPECT_EQ(transformTo.Translation(),
+ CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
+ coordTo));
+ EXPECT_EQ(
+ transformTo.Rotation(),
+ CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
+ EXPECT_EQ(transformTo,
+ CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
+
+ // "to" to "from"
+ EXPECT_EQ(
+ transformFrom.Translation(),
+ CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
+ EXPECT_EQ(
+ transformFrom.Rotation(),
+ CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
+ EXPECT_EQ(transformFrom,
+ CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
+}
+
+TEST(CoordinateSystemTest, Pose3dEDNtoNWU) {
+ // No rotation from EDN to NWU
+ CheckPose3dConvert(
+ Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+ Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° roll from EDN to NWU
+ CheckPose3dConvert(
+ Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+ Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° pitch from EDN to NWU
+ CheckPose3dConvert(
+ Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+ Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° yaw from EDN to NWU
+ CheckPose3dConvert(
+ Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+ Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+}
+
+TEST(CoordinateSystemTest, Pose3dEDNtoNED) {
+ // No rotation from EDN to NED
+ CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+ Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° roll from EDN to NED
+ CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+ Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° pitch from EDN to NED
+ CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+ Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° yaw from EDN to NED
+ CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+ Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNWU) {
+ // No rotation from EDN to NWU
+ CheckTransform3dConvert(
+ Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
+ Transform3d{Translation3d{3_m, -1_m, -2_m},
+ Rotation3d{-90_deg, 0_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° roll from EDN to NWU
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{45_deg, 0_deg, 0_deg}},
+ Transform3d{Translation3d{3_m, -1_m, -2_m},
+ Rotation3d{-45_deg, 0_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° pitch from EDN to NWU
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{0_deg, 45_deg, 0_deg}},
+ Transform3d{Translation3d{3_m, -1_m, -2_m},
+ Rotation3d{-90_deg, 0_deg, -135_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+ // 45° yaw from EDN to NWU
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{0_deg, 0_deg, 45_deg}},
+ Transform3d{Translation3d{3_m, -1_m, -2_m},
+ Rotation3d{-90_deg, 45_deg, -90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NWU());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNED) {
+ // No rotation from EDN to NED
+ CheckTransform3dConvert(
+ Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
+ Transform3d{Translation3d{3_m, 1_m, 2_m},
+ Rotation3d{90_deg, 0_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° roll from EDN to NED
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{45_deg, 0_deg, 0_deg}},
+ Transform3d{Translation3d{3_m, 1_m, 2_m},
+ Rotation3d{135_deg, 0_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° pitch from EDN to NED
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{0_deg, 45_deg, 0_deg}},
+ Transform3d{Translation3d{3_m, 1_m, 2_m},
+ Rotation3d{90_deg, 0_deg, 135_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+ // 45° yaw from EDN to NED
+ CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+ Rotation3d{0_deg, 0_deg, 45_deg}},
+ Transform3d{Translation3d{3_m, 1_m, 2_m},
+ Rotation3d{90_deg, -45_deg, 90_deg}},
+ CoordinateSystem::EDN(), CoordinateSystem::NED());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index cd5b127..5ce6819 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -9,51 +9,66 @@
using namespace frc;
-static constexpr double kEpsilon = 1E-9;
-
TEST(Pose2dTest, TransformBy) {
- const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
- const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+ const Pose2d initial{1_m, 2_m, 45_deg};
+ const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
const auto transformed = initial + transform;
- EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
+ EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
+ EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value());
}
TEST(Pose2dTest, RelativeTo) {
- const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
- const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+ const Pose2d initial{0_m, 0_m, 45_deg};
+ const Pose2d final{5_m, 5_m, 45.0_deg};
const auto finalRelativeToInitial = final.RelativeTo(initial);
- EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
- kEpsilon);
- EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
- EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
- kEpsilon);
+ EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+ EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
+ EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
}
TEST(Pose2dTest, Equality) {
- const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
- const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+ const Pose2d a{0_m, 5_m, 43_deg};
+ const Pose2d b{0_m, 5_m, 43_deg};
EXPECT_TRUE(a == b);
}
TEST(Pose2dTest, Inequality) {
- const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
- const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+ const Pose2d a{0_m, 5_m, 43_deg};
+ const Pose2d b{0_m, 5_ft, 43_deg};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
- const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
- const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+ const Pose2d initial{0_m, 0_m, 45_deg};
+ const Pose2d final{5_m, 5_m, 45_deg};
const auto transform = final - initial;
- EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
- EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
- EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+ EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
+ EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
+}
+
+TEST(Pose2dTest, Constexpr) {
+ constexpr Pose2d defaultConstructed;
+ constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},
+ Rotation2d{0_deg}};
+ constexpr Pose2d coordRotation{0_m, 0_m, Rotation2d{45_deg}};
+
+ constexpr auto added =
+ translationRotation + Transform2d{Translation2d{}, Rotation2d{45_deg}};
+ constexpr auto multiplied = coordRotation * 2;
+
+ static_assert(defaultConstructed.X() == 0_m);
+ static_assert(translationRotation.Y() == 1_m);
+ static_assert(coordRotation.Rotation().Degrees() == 45_deg);
+ static_assert(added.X() == 0_m);
+ static_assert(added.Y() == 1_m);
+ static_assert(added.Rotation().Degrees() == 45_deg);
+ static_assert(multiplied.Rotation().Degrees() == 90_deg);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
new file mode 100644
index 0000000..8c4452c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
@@ -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.
+
+#include <cmath>
+
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Pose3dTest, TestTransformByRotations) {
+ const double kEpsilon = 1E-9;
+
+ const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}};
+ const Transform3d transform1{Translation3d{0_m, 0_m, 0_m},
+ Rotation3d{90_deg, 45_deg, 0_deg}};
+ const Transform3d transform2{Translation3d{0_m, 0_m, 0_m},
+ Rotation3d{-90_deg, 0_deg, 0_deg}};
+ const Transform3d transform3{Translation3d{0_m, 0_m, 0_m},
+ Rotation3d{0_deg, -45_deg, 0_deg}};
+
+ Pose3d finalPose = initialPose.TransformBy(transform1)
+ .TransformBy(transform2)
+ .TransformBy(transform3);
+
+ EXPECT_NEAR(finalPose.Rotation().X().value(),
+ initialPose.Rotation().X().value(), kEpsilon);
+ EXPECT_NEAR(finalPose.Rotation().Y().value(),
+ initialPose.Rotation().Y().value(), kEpsilon);
+ EXPECT_NEAR(finalPose.Rotation().Z().value(),
+ initialPose.Rotation().Z().value(), kEpsilon);
+}
+
+TEST(Pose3dTest, TransformBy) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45.0_deg}};
+ const Transform3d transform{Translation3d{5_m, 0_m, 0_m},
+ Rotation3d{zAxis, 5_deg}};
+
+ const auto transformed = initial + transform;
+
+ EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
+ EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
+ EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(),
+ units::radian_t{50_deg}.value());
+}
+
+TEST(Pose3dTest, RelativeTo) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
+ const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
+
+ const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+ EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+ EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value());
+ EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value());
+}
+
+TEST(Pose3dTest, Equality) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+ const Pose3d b{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+ EXPECT_TRUE(a == b);
+}
+
+TEST(Pose3dTest, Inequality) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+ const Pose3d b{0_m, 5_ft, 0_m, Rotation3d{zAxis, 43_deg}};
+ EXPECT_TRUE(a != b);
+}
+
+TEST(Pose3dTest, Minus) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
+ const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
+
+ const auto transform = final - initial;
+
+ EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+ EXPECT_DOUBLE_EQ(0.0, transform.Y().value());
+ EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value());
+}
+
+TEST(Pose3dTest, ToPose2d) {
+ Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
+ Pose2d expected{1_m, 2_m, 40_deg};
+
+ EXPECT_EQ(expected, pose.ToPose2d());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
new file mode 100644
index 0000000..5b95abb
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
@@ -0,0 +1,82 @@
+// 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 <numbers>
+
+#include "frc/geometry/Quaternion.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/math.h"
+
+using namespace frc;
+
+TEST(QuaternionTest, Init) {
+ // Identity
+ Quaternion q1;
+ EXPECT_DOUBLE_EQ(1.0, q1.W());
+ EXPECT_DOUBLE_EQ(0.0, q1.X());
+ EXPECT_DOUBLE_EQ(0.0, q1.Y());
+ EXPECT_DOUBLE_EQ(0.0, q1.Z());
+
+ // Normalized
+ Quaternion q2{0.5, 0.5, 0.5, 0.5};
+ EXPECT_DOUBLE_EQ(0.5, q2.W());
+ EXPECT_DOUBLE_EQ(0.5, q2.X());
+ EXPECT_DOUBLE_EQ(0.5, q2.Y());
+ EXPECT_DOUBLE_EQ(0.5, q2.Z());
+
+ // Unnormalized
+ Quaternion q3{0.75, 0.3, 0.4, 0.5};
+ EXPECT_DOUBLE_EQ(0.75, q3.W());
+ EXPECT_DOUBLE_EQ(0.3, q3.X());
+ EXPECT_DOUBLE_EQ(0.4, q3.Y());
+ EXPECT_DOUBLE_EQ(0.5, q3.Z());
+
+ q3 = q3.Normalize();
+ double norm = std::sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
+ EXPECT_DOUBLE_EQ(0.75 / norm, q3.W());
+ EXPECT_DOUBLE_EQ(0.3 / norm, q3.X());
+ EXPECT_DOUBLE_EQ(0.4 / norm, q3.Y());
+ EXPECT_DOUBLE_EQ(0.5 / norm, q3.Z());
+ EXPECT_DOUBLE_EQ(1.0, q3.W() * q3.W() + q3.X() * q3.X() + q3.Y() * q3.Y() +
+ q3.Z() * q3.Z());
+}
+
+TEST(QuaternionTest, Multiply) {
+ // 90° CCW rotations around each axis
+ double c = units::math::cos(90_deg / 2.0);
+ double s = units::math::sin(90_deg / 2.0);
+ Quaternion xRot{c, s, 0.0, 0.0};
+ Quaternion yRot{c, 0.0, s, 0.0};
+ Quaternion zRot{c, 0.0, 0.0, s};
+
+ // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
+ // produce a 90° CCW Y rotation
+ auto expected = yRot;
+ auto actual = zRot * yRot * xRot;
+ EXPECT_NEAR(expected.W(), actual.W(), 1e-9);
+ EXPECT_NEAR(expected.X(), actual.X(), 1e-9);
+ EXPECT_NEAR(expected.Y(), actual.Y(), 1e-9);
+ EXPECT_NEAR(expected.Z(), actual.Z(), 1e-9);
+
+ // Identity
+ Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+ 0.48507125007266594};
+ actual = q * q.Inverse();
+ EXPECT_DOUBLE_EQ(1.0, actual.W());
+ EXPECT_DOUBLE_EQ(0.0, actual.X());
+ EXPECT_DOUBLE_EQ(0.0, actual.Y());
+ EXPECT_DOUBLE_EQ(0.0, actual.Z());
+}
+
+TEST(QuaternionTest, Inverse) {
+ Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+ 0.48507125007266594};
+ auto inv = q.Inverse();
+
+ EXPECT_DOUBLE_EQ(q.W(), inv.W());
+ EXPECT_DOUBLE_EQ(-q.X(), inv.X());
+ EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
+ EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index ed3b6b5..4e2e683 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -3,66 +3,82 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
-
-#include <wpi/numbers>
+#include <numbers>
#include "frc/geometry/Rotation2d.h"
#include "gtest/gtest.h"
using namespace frc;
-static constexpr double kEpsilon = 1E-9;
-
TEST(Rotation2dTest, RadiansToDegrees) {
- const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
- const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
+ const Rotation2d rot1{units::radian_t{std::numbers::pi / 3.0}};
+ const Rotation2d rot2{units::radian_t{std::numbers::pi / 4.0}};
- EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
- EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value());
+ EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value());
}
TEST(Rotation2dTest, DegreesToRadians) {
- const auto rot1 = Rotation2d(45.0_deg);
- const auto rot2 = Rotation2d(30.0_deg);
+ const auto rot1 = Rotation2d{45_deg};
+ const auto rot2 = Rotation2d{30_deg};
- EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
- EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot1.Radians().value());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 6.0, rot2.Radians().value());
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
- auto sum = zero + Rotation2d(90.0_deg);
+ auto rotated = zero + Rotation2d{90_deg};
- EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
- EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2.0, rotated.Radians().value());
+ EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value());
}
TEST(Rotation2dTest, RotateByNonZero) {
- auto rot = Rotation2d(90.0_deg);
- rot = rot + Rotation2d(30.0_deg);
+ auto rot = Rotation2d{90_deg};
+ rot = rot + Rotation2d{30_deg};
- EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
}
TEST(Rotation2dTest, Minus) {
- const auto rot1 = Rotation2d(70.0_deg);
- const auto rot2 = Rotation2d(30.0_deg);
+ const auto rot1 = Rotation2d{70_deg};
+ const auto rot2 = Rotation2d{30_deg};
- EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(40.0, (rot1 - rot2).Degrees().value());
}
TEST(Rotation2dTest, Equality) {
- const auto rot1 = Rotation2d(43_deg);
- const auto rot2 = Rotation2d(43_deg);
+ auto rot1 = Rotation2d{43_deg};
+ auto rot2 = Rotation2d{43_deg};
EXPECT_EQ(rot1, rot2);
- const auto rot3 = Rotation2d(-180_deg);
- const auto rot4 = Rotation2d(180_deg);
- EXPECT_EQ(rot3, rot4);
+ rot1 = Rotation2d{-180_deg};
+ rot2 = Rotation2d{180_deg};
+ EXPECT_EQ(rot1, rot2);
}
TEST(Rotation2dTest, Inequality) {
- const auto rot1 = Rotation2d(43_deg);
- const auto rot2 = Rotation2d(43.5_deg);
+ const auto rot1 = Rotation2d{43_deg};
+ const auto rot2 = Rotation2d{43.5_deg};
EXPECT_NE(rot1, rot2);
}
+
+TEST(Rotation2dTest, Constexpr) {
+ constexpr Rotation2d defaultCtor;
+ constexpr Rotation2d radianCtor{5_rad};
+ constexpr Rotation2d degreeCtor{270_deg};
+ constexpr Rotation2d rotation45{45_deg};
+ constexpr Rotation2d cartesianCtor{3.5, -3.5};
+
+ constexpr auto negated = -radianCtor;
+ constexpr auto multiplied = radianCtor * 2;
+ constexpr auto subtracted = cartesianCtor - degreeCtor;
+
+ static_assert(defaultCtor.Radians() == 0_rad);
+ static_assert(degreeCtor.Degrees() == 270_deg);
+ static_assert(negated.Radians() == -5_rad);
+ static_assert(multiplied.Radians() == 10_rad);
+ static_assert(subtracted == rotation45);
+ static_assert(radianCtor != degreeCtor);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
new file mode 100644
index 0000000..4709ed0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
@@ -0,0 +1,321 @@
+// 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 <cmath>
+#include <numbers>
+
+#include <wpi/MathExtras.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Rotation3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+ const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
+ const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+ EXPECT_EQ(rot1, rot2);
+
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+ const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
+ const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+ EXPECT_EQ(rot3, rot4);
+
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+ const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
+ const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
+ EXPECT_EQ(rot5, rot6);
+}
+
+TEST(Rotation3dTest, InitRotationMatrix) {
+ // No rotation
+ const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
+ const Rotation3d rot1{R1};
+ EXPECT_EQ(Rotation3d{}, rot1);
+
+ // 90 degree CCW rotation around z-axis
+ Matrixd<3, 3> R2;
+ R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
+ R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
+ R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
+ const Rotation3d rot2{R2};
+ const Rotation3d expected2{0_deg, 0_deg, 90_deg};
+ EXPECT_EQ(expected2, rot2);
+
+ // Matrix that isn't orthogonal
+ const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
+ EXPECT_THROW(Rotation3d{R3}, std::domain_error);
+
+ // Matrix that's orthogonal but not special orthogonal
+ const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
+ EXPECT_THROW(Rotation3d{R4}, std::domain_error);
+}
+
+TEST(Rotation3dTest, InitTwoVector) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ // 90 degree CW rotation around y-axis
+ const Rotation3d rot1{xAxis, zAxis};
+ const Rotation3d expected1{yAxis, units::radian_t{-std::numbers::pi / 2.0}};
+ EXPECT_EQ(expected1, rot1);
+
+ // 45 degree CCW rotation around z-axis
+ const Rotation3d rot2{xAxis, Eigen::Vector3d{1.0, 1.0, 0.0}};
+ const Rotation3d expected2{zAxis, units::radian_t{std::numbers::pi / 4.0}};
+ EXPECT_EQ(expected2, rot2);
+
+ // 0 degree rotation of x-axes
+ const Rotation3d rot3{xAxis, xAxis};
+ EXPECT_EQ(Rotation3d{}, rot3);
+
+ // 0 degree rotation of y-axes
+ const Rotation3d rot4{yAxis, yAxis};
+ EXPECT_EQ(Rotation3d{}, rot4);
+
+ // 0 degree rotation of z-axes
+ const Rotation3d rot5{zAxis, zAxis};
+ EXPECT_EQ(Rotation3d{}, rot5);
+
+ // 180 degree rotation tests. For 180 degree rotations, any quaternion with an
+ // orthogonal rotation axis is acceptable. The rotation axis and initial
+ // vector are orthogonal if their dot product is zero.
+
+ // 180 degree rotation of x-axes
+ const Rotation3d rot6{xAxis, -xAxis};
+ const auto q6 = rot6.GetQuaternion();
+ EXPECT_EQ(0.0, q6.W());
+ EXPECT_EQ(0.0, q6.X() * xAxis(0) + q6.Y() * xAxis(1) + q6.Z() * xAxis(2));
+
+ // 180 degree rotation of y-axes
+ const Rotation3d rot7{yAxis, -yAxis};
+ const auto q7 = rot7.GetQuaternion();
+ EXPECT_EQ(0.0, q7.W());
+ EXPECT_EQ(0.0, q7.X() * yAxis(0) + q7.Y() * yAxis(1) + q7.Z() * yAxis(2));
+
+ // 180 degree rotation of z-axes
+ const Rotation3d rot8{zAxis, -zAxis};
+ const auto q8 = rot8.GetQuaternion();
+ EXPECT_EQ(0.0, q8.W());
+ EXPECT_EQ(0.0, q8.X() * zAxis(0) + q8.Y() * zAxis(1) + q8.Z() * zAxis(2));
+}
+
+TEST(Rotation3dTest, RadiansToDegrees) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Rotation3d rot1{zAxis, units::radian_t{std::numbers::pi / 3}};
+ EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
+ EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
+ EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value());
+
+ const Rotation3d rot2{zAxis, units::radian_t{std::numbers::pi / 4}};
+ EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
+ EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
+ EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value());
+}
+
+TEST(Rotation3dTest, DegreesToRadians) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const auto rot1 = Rotation3d{zAxis, 45_deg};
+ EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
+ EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot1.Z().value());
+
+ const auto rot2 = Rotation3d{zAxis, 30_deg};
+ EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
+ EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 6.0, rot2.Z().value());
+}
+
+TEST(Rotation3dTest, RotationLoop) {
+ Rotation3d rot;
+
+ rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
+ Rotation3d expected{90_deg, 0_deg, 0_deg};
+ EXPECT_EQ(expected, rot);
+
+ rot = rot + Rotation3d{0_deg, 90_deg, 0_deg};
+ expected = Rotation3d{
+ {1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg};
+ EXPECT_EQ(expected, rot);
+
+ rot = rot + Rotation3d{0_deg, 0_deg, 90_deg};
+ expected = Rotation3d{0_deg, 90_deg, 0_deg};
+ EXPECT_EQ(expected, rot);
+
+ rot = rot + Rotation3d{0_deg, -90_deg, 0_deg};
+ EXPECT_EQ(Rotation3d{}, rot);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroX) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+
+ const Rotation3d zero;
+ auto rotated = zero + Rotation3d{xAxis, 90_deg};
+
+ Rotation3d expected{xAxis, 90_deg};
+ EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroY) {
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+
+ const Rotation3d zero;
+ auto rotated = zero + Rotation3d{yAxis, 90_deg};
+
+ Rotation3d expected{yAxis, 90_deg};
+ EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroZ) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Rotation3d zero;
+ auto rotated = zero + Rotation3d{zAxis, 90_deg};
+
+ Rotation3d expected{zAxis, 90_deg};
+ EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroX) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+
+ auto rot = Rotation3d{xAxis, 90_deg};
+ rot = rot + Rotation3d{xAxis, 30_deg};
+
+ Rotation3d expected{xAxis, 120_deg};
+ EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroY) {
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+
+ auto rot = Rotation3d{yAxis, 90_deg};
+ rot = rot + Rotation3d{yAxis, 30_deg};
+
+ Rotation3d expected{yAxis, 120_deg};
+ EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroZ) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ auto rot = Rotation3d{zAxis, 90_deg};
+ rot = rot + Rotation3d{zAxis, 30_deg};
+
+ Rotation3d expected{zAxis, 120_deg};
+ EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, Minus) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const auto rot1 = Rotation3d{zAxis, 70_deg};
+ const auto rot2 = Rotation3d{zAxis, 30_deg};
+
+ EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value());
+}
+
+TEST(Rotation3dTest, AxisAngle) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ Rotation3d rot1{xAxis, 90_deg};
+ EXPECT_EQ(xAxis, rot1.Axis());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 2.0, rot1.Angle().value());
+
+ Rotation3d rot2{yAxis, 45_deg};
+ EXPECT_EQ(yAxis, rot2.Axis());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot2.Angle().value());
+
+ Rotation3d rot3{zAxis, 60_deg};
+ EXPECT_EQ(zAxis, rot3.Axis());
+ EXPECT_DOUBLE_EQ(std::numbers::pi / 3.0, rot3.Angle().value());
+}
+
+TEST(Rotation3dTest, ToRotation2d) {
+ Rotation3d rotation{20_deg, 30_deg, 40_deg};
+ Rotation2d expected{40_deg};
+
+ EXPECT_EQ(expected, rotation.ToRotation2d());
+}
+
+TEST(Rotation3dTest, Equality) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const auto rot1 = Rotation3d{zAxis, 43_deg};
+ const auto rot2 = Rotation3d{zAxis, 43_deg};
+ EXPECT_EQ(rot1, rot2);
+
+ const auto rot3 = Rotation3d{zAxis, -180_deg};
+ const auto rot4 = Rotation3d{zAxis, 180_deg};
+ EXPECT_EQ(rot3, rot4);
+}
+
+TEST(Rotation3dTest, Inequality) {
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const auto rot1 = Rotation3d{zAxis, 43_deg};
+ const auto rot2 = Rotation3d{zAxis, 43.5_deg};
+ EXPECT_NE(rot1, rot2);
+}
+
+TEST(Rotation3dTest, Interpolate) {
+ const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+ const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+ const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ // 50 + (70 - 50) * 0.5 = 60
+ auto rot1 = Rotation3d{xAxis, 50_deg};
+ auto rot2 = Rotation3d{xAxis, 70_deg};
+ auto interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+ // -160 minus half distance between 170 and -160 (15) = -175
+ rot1 = Rotation3d{xAxis, 170_deg};
+ rot2 = Rotation3d{xAxis, -160_deg};
+ interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+ // 50 + (70 - 50) * 0.5 = 60
+ rot1 = Rotation3d{yAxis, 50_deg};
+ rot2 = Rotation3d{yAxis, 70_deg};
+ interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+ // -160 plus half distance between 170 and -160 (165) = 5
+ rot1 = Rotation3d{yAxis, 170_deg};
+ rot2 = Rotation3d{yAxis, -160_deg};
+ interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value());
+
+ // 50 + (70 - 50) * 0.5 = 60
+ rot1 = Rotation3d{zAxis, 50_deg};
+ rot2 = Rotation3d{zAxis, 70_deg};
+ interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value());
+
+ // -160 minus half distance between 170 and -160 (15) = -175
+ rot1 = Rotation3d{zAxis, 170_deg};
+ rot2 = Rotation3d{zAxis, -160_deg};
+ interpolated = wpi::Lerp(rot1, rot2, 0.5);
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+ EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+ EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 968ab29..1b0934d 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -12,8 +12,6 @@
using namespace frc;
-static constexpr double kEpsilon = 1E-9;
-
TEST(Transform2dTest, Inverse) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{{5_m, 0_m}, 5_deg};
@@ -21,10 +19,7 @@
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
- EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
- EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
- EXPECT_NEAR(initial.Rotation().Degrees().value(),
- untransformed.Rotation().Degrees().value(), kEpsilon);
+ EXPECT_EQ(initial, untransformed);
}
TEST(Transform2dTest, Composition) {
@@ -35,10 +30,23 @@
auto transformedSeparate = initial + transform1 + transform2;
auto transformedCombined = initial + (transform1 + transform2);
- EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
- kEpsilon);
- EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
- kEpsilon);
- EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
- transformedCombined.Rotation().Degrees().value(), kEpsilon);
+ EXPECT_EQ(transformedSeparate, transformedCombined);
+}
+
+TEST(Transform2dTest, Constexpr) {
+ constexpr Transform2d defaultCtor;
+ constexpr Transform2d translationRotationCtor{Translation2d{},
+ Rotation2d{10_deg}};
+ constexpr auto multiplied = translationRotationCtor * 5;
+ constexpr auto divided = translationRotationCtor / 2;
+
+ static_assert(defaultCtor.Translation().X() == 0_m);
+ static_assert(translationRotationCtor.X() == 0_m);
+ static_assert(translationRotationCtor.Y() == 0_m);
+ static_assert(multiplied.Rotation().Degrees() == 50_deg);
+ static_assert(translationRotationCtor.Inverse().Rotation().Degrees() ==
+ (-10_deg));
+ static_assert(translationRotationCtor.Inverse().X() == 0_m);
+ static_assert(translationRotationCtor.Inverse().Y() == 0_m);
+ static_assert(divided.Rotation().Degrees() == 5_deg);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
new file mode 100644
index 0000000..904ec9c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "frc/geometry/Translation3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Transform3dTest, Inverse) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}};
+ const Transform3d transform{{5_m, 4_m, 3_m}, Rotation3d{zAxis, 5_deg}};
+
+ auto transformed = initial + transform;
+ auto untransformed = transformed + transform.Inverse();
+
+ EXPECT_EQ(initial, untransformed);
+}
+
+TEST(Transform3dTest, Composition) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}};
+ const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
+ const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}};
+
+ auto transformedSeparate = initial + transform1 + transform2;
+ auto transformedCombined = initial + (transform1 + transform2);
+
+ EXPECT_EQ(transformedSeparate, transformedCombined);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index efdcace..5493c43 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -9,69 +9,67 @@
using namespace frc;
-static constexpr double kEpsilon = 1E-9;
-
TEST(Translation2dTest, Sum) {
- const Translation2d one{1.0_m, 3.0_m};
- const Translation2d two{2.0_m, 5.0_m};
+ const Translation2d one{1_m, 3_m};
+ const Translation2d two{2_m, 5_m};
const auto sum = one + two;
- EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
- EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(3.0, sum.X().value());
+ EXPECT_DOUBLE_EQ(8.0, sum.Y().value());
}
TEST(Translation2dTest, Difference) {
- const Translation2d one{1.0_m, 3.0_m};
- const Translation2d two{2.0_m, 5.0_m};
+ const Translation2d one{1_m, 3_m};
+ const Translation2d two{2_m, 5_m};
const auto difference = one - two;
- EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
- EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(-1.0, difference.X().value());
+ EXPECT_DOUBLE_EQ(-2.0, difference.Y().value());
}
TEST(Translation2dTest, RotateBy) {
- const Translation2d another{3.0_m, 0.0_m};
- const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+ const Translation2d another{3_m, 0_m};
+ const auto rotated = another.RotateBy(90_deg);
- EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
- EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
+ EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
+ EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
}
TEST(Translation2dTest, Multiplication) {
- const Translation2d original{3.0_m, 5.0_m};
+ const Translation2d original{3_m, 5_m};
const auto mult = original * 3;
- EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
- EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(9.0, mult.X().value());
+ EXPECT_DOUBLE_EQ(15.0, mult.Y().value());
}
TEST(Translation2dTest, Division) {
- const Translation2d original{3.0_m, 5.0_m};
+ const Translation2d original{3_m, 5_m};
const auto div = original / 2;
- EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
- EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
+ EXPECT_DOUBLE_EQ(1.5, div.X().value());
+ EXPECT_DOUBLE_EQ(2.5, div.Y().value());
}
TEST(Translation2dTest, Norm) {
- const Translation2d one{3.0_m, 5.0_m};
- EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
+ const Translation2d one{3_m, 5_m};
+ EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
- EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
+ EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
- EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
- EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
+ EXPECT_DOUBLE_EQ(4.5, inverted.X().value());
+ EXPECT_DOUBLE_EQ(-7.0, inverted.Y().value());
}
TEST(Translation2dTest, Equality) {
@@ -87,11 +85,29 @@
}
TEST(Translation2dTest, PolarConstructor) {
- Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
- EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
- EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
+ Translation2d one{std::sqrt(2) * 1_m, Rotation2d{45_deg}};
+ EXPECT_DOUBLE_EQ(1.0, one.X().value());
+ EXPECT_DOUBLE_EQ(1.0, one.Y().value());
- Translation2d two{2_m, Rotation2d(60_deg)};
- EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
- EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
+ Translation2d two{2_m, Rotation2d{60_deg}};
+ EXPECT_DOUBLE_EQ(1.0, two.X().value());
+ EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
+}
+
+TEST(Translation2dTest, Constexpr) {
+ constexpr Translation2d defaultCtor;
+ constexpr Translation2d componentCtor{1_m, 2_m};
+ constexpr auto added = defaultCtor + componentCtor;
+ constexpr auto subtracted = defaultCtor - componentCtor;
+ constexpr auto negated = -componentCtor;
+ constexpr auto multiplied = componentCtor * 2;
+ constexpr auto divided = componentCtor / 2;
+
+ static_assert(defaultCtor.X() == 0_m);
+ static_assert(componentCtor.Y() == 2_m);
+ static_assert(added.X() == 1_m);
+ static_assert(subtracted.Y() == (-2_m));
+ static_assert(negated.X() == (-1_m));
+ static_assert(multiplied.X() == 2_m);
+ static_assert(divided.Y() == 1_m);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
new file mode 100644
index 0000000..58c611e
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
@@ -0,0 +1,149 @@
+// 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 <cmath>
+
+#include "frc/geometry/Translation3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation3dTest, Sum) {
+ const Translation3d one{1_m, 3_m, 5_m};
+ const Translation3d two{2_m, 5_m, 8_m};
+
+ const auto sum = one + two;
+
+ EXPECT_NEAR(3.0, sum.X().value(), kEpsilon);
+ EXPECT_NEAR(8.0, sum.Y().value(), kEpsilon);
+ EXPECT_NEAR(13.0, sum.Z().value(), kEpsilon);
+}
+
+TEST(Translation3dTest, Difference) {
+ const Translation3d one{1_m, 3_m, 5_m};
+ const Translation3d two{2_m, 5_m, 8_m};
+
+ const auto difference = one - two;
+
+ EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
+ EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
+ EXPECT_NEAR(difference.Z().value(), -3.0, kEpsilon);
+}
+
+TEST(Translation3dTest, RotateBy) {
+ Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+ Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Translation3d translation{1_m, 2_m, 3_m};
+
+ const auto rotated1 = translation.RotateBy(Rotation3d{xAxis, 90_deg});
+ EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(rotated1.Y().value(), -3.0, kEpsilon);
+ EXPECT_NEAR(rotated1.Z().value(), 2.0, kEpsilon);
+
+ const auto rotated2 = translation.RotateBy(Rotation3d{yAxis, 90_deg});
+ EXPECT_NEAR(rotated2.X().value(), 3.0, kEpsilon);
+ EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
+ EXPECT_NEAR(rotated2.Z().value(), -1.0, kEpsilon);
+
+ const auto rotated3 = translation.RotateBy(Rotation3d{zAxis, 90_deg});
+ EXPECT_NEAR(rotated3.X().value(), -2.0, kEpsilon);
+ EXPECT_NEAR(rotated3.Y().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
+}
+
+TEST(Translation3dTest, ToTranslation2d) {
+ Translation3d translation{1_m, 2_m, 3_m};
+ Translation2d expected{1_m, 2_m};
+
+ EXPECT_EQ(expected, translation.ToTranslation2d());
+}
+
+TEST(Translation3dTest, Multiplication) {
+ const Translation3d original{3_m, 5_m, 7_m};
+ const auto mult = original * 3;
+
+ EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
+ EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
+ EXPECT_NEAR(mult.Z().value(), 21.0, kEpsilon);
+}
+
+TEST(Translation3dTest, Division) {
+ const Translation3d original{3_m, 5_m, 7_m};
+ const auto div = original / 2;
+
+ EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
+ EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
+ EXPECT_NEAR(div.Z().value(), 3.5, kEpsilon);
+}
+
+TEST(Translation3dTest, Norm) {
+ const Translation3d one{3_m, 5_m, 7_m};
+ EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
+}
+
+TEST(Translation3dTest, Distance) {
+ const Translation3d one{1_m, 1_m, 1_m};
+ const Translation3d two{6_m, 6_m, 6_m};
+ EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
+}
+
+TEST(Translation3dTest, UnaryMinus) {
+ const Translation3d original{-4.5_m, 7_m, 9_m};
+ const auto inverted = -original;
+
+ EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
+ EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
+ EXPECT_NEAR(inverted.Z().value(), -9, kEpsilon);
+}
+
+TEST(Translation3dTest, Equality) {
+ const Translation3d one{9_m, 5.5_m, 3.5_m};
+ const Translation3d two{9_m, 5.5_m, 3.5_m};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Translation3dTest, Inequality) {
+ const Translation3d one{9_m, 5.5_m, 3.5_m};
+ const Translation3d two{9_m, 5.7_m, 3.5_m};
+ EXPECT_TRUE(one != two);
+}
+
+TEST(Translation3dTest, PolarConstructor) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ Translation3d one{std::sqrt(2) * 1_m, Rotation3d{zAxis, 45_deg}};
+ EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon);
+
+ Translation3d two{2_m, Rotation3d{zAxis, 60_deg}};
+ EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
+ EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
+ EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
+}
+
+TEST(Translation3dTest, Constexpr) {
+ constexpr Translation3d defaultCtor;
+ constexpr Translation3d componentCtor{1_m, 2_m, 3_m};
+ constexpr auto added = defaultCtor + componentCtor;
+ constexpr auto subtracted = defaultCtor - componentCtor;
+ constexpr auto negated = -componentCtor;
+ constexpr auto multiplied = componentCtor * 2;
+ constexpr auto divided = componentCtor / 2;
+ constexpr Translation2d projected = componentCtor.ToTranslation2d();
+
+ static_assert(defaultCtor.X() == 0_m);
+ static_assert(componentCtor.Y() == 2_m);
+ static_assert(added.Z() == 3_m);
+ static_assert(subtracted.X() == (-1_m));
+ static_assert(negated.Y() == (-2_m));
+ static_assert(multiplied.Z() == 6_m);
+ static_assert(divided.Y() == 1_m);
+ static_assert(projected.X() == 1_m);
+ static_assert(projected.Y() == 2_m);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index fa9eecc..33970cd 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -3,63 +3,74 @@
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
-
-#include <wpi/numbers>
+#include <numbers>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
using namespace frc;
-static constexpr double kEpsilon = 1E-9;
-
TEST(Twist2dTest, Straight) {
- const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
- const auto straightPose = Pose2d().Exp(straight);
+ const Twist2d straight{5_m, 0_m, 0_rad};
+ const auto straightPose = Pose2d{}.Exp(straight);
- EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
- EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
- EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
+ EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
+ EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value());
}
TEST(Twist2dTest, QuarterCircle) {
- const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
- units::radian_t(wpi::numbers::pi / 2.0)};
- const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+ const Twist2d quarterCircle{5_m / 2.0 * std::numbers::pi, 0_m,
+ units::radian_t{std::numbers::pi / 2.0}};
+ const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle);
- EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
- EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
- EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
+ EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value());
+ EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value());
}
TEST(Twist2dTest, DiagonalNoDtheta) {
- const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
- const auto diagonalPose = Pose2d().Exp(diagonal);
+ const Twist2d diagonal{2_m, 2_m, 0_deg};
+ const auto diagonalPose = Pose2d{}.Exp(diagonal);
- EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
- EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
- EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
+ EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value());
+ EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value());
+ EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value());
}
TEST(Twist2dTest, Equality) {
- const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
- const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+ const Twist2d one{5_m, 1_m, 3_rad};
+ const Twist2d two{5_m, 1_m, 3_rad};
EXPECT_TRUE(one == two);
}
TEST(Twist2dTest, Inequality) {
- const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
- const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+ const Twist2d one{5_m, 1_m, 3_rad};
+ const Twist2d two{5_m, 1.2_m, 3_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist2dTest, Pose2dLog) {
- const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
- const Pose2d start{};
+ const Pose2d end{5_m, 5_m, 90_deg};
+ const Pose2d start;
const auto twist = start.Log(end);
- EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
- EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
- EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
+ Twist2d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi}, 0_m,
+ units::radian_t{std::numbers::pi / 2.0}};
+ EXPECT_EQ(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ const auto reapplied = start.Exp(twist);
+ EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist2dTest, Constexpr) {
+ constexpr Twist2d defaultCtor;
+ constexpr Twist2d componentCtor{1_m, 2_m, 3_rad};
+ constexpr auto multiplied = componentCtor * 2;
+
+ static_assert(defaultCtor.dx == 0_m);
+ static_assert(componentCtor.dy == 2_m);
+ static_assert(multiplied.dtheta == 6_rad);
}
diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
new file mode 100644
index 0000000..0d8a8f4
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
@@ -0,0 +1,131 @@
+// 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 <cmath>
+#include <numbers>
+
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Twist3dTest, StraightX) {
+ const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
+ const auto straightPose = Pose3d{}.Exp(straight);
+
+ Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
+ EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, StraightY) {
+ const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
+ const auto straightPose = Pose3d{}.Exp(straight);
+
+ Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
+ EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, StraightZ) {
+ const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
+ const auto straightPose = Pose3d{}.Exp(straight);
+
+ Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
+ EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, QuarterCircle) {
+ Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+ const Twist3d quarterCircle{
+ 5_m / 2.0 * std::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
+ units::radian_t{std::numbers::pi / 2.0}};
+ const auto quarterCirclePose = Pose3d{}.Exp(quarterCircle);
+
+ Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
+ EXPECT_EQ(expected, quarterCirclePose);
+}
+
+TEST(Twist3dTest, DiagonalNoDtheta) {
+ const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
+ const auto diagonalPose = Pose3d{}.Exp(diagonal);
+
+ Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
+ EXPECT_EQ(expected, diagonalPose);
+}
+
+TEST(Twist3dTest, Equality) {
+ const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+ const Twist3d two{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Twist3dTest, Inequality) {
+ const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+ const Twist3d two{5_m, 1.2_m, 0_m, 0_rad, 0_rad, 3_rad};
+ EXPECT_TRUE(one != two);
+}
+
+TEST(Twist3dTest, Pose3dLogX) {
+ const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}};
+ const Pose3d start;
+
+ const auto twist = start.Log(end);
+
+ Twist3d expected{0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
+ 0_m, 90_deg,
+ 0_deg, 0_deg};
+ EXPECT_EQ(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ const auto reapplied = start.Exp(twist);
+ EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Pose3dLogY) {
+ const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}};
+ const Pose3d start;
+
+ const auto twist = start.Log(end);
+
+ Twist3d expected{0_m, 0_m, units::meter_t{5.0 / 2.0 * std::numbers::pi},
+ 0_deg, 90_deg, 0_deg};
+ EXPECT_EQ(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ const auto reapplied = start.Exp(twist);
+ EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Pose3dLogZ) {
+ const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}};
+ const Pose3d start;
+
+ const auto twist = start.Log(end);
+
+ Twist3d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi},
+ 0_m,
+ 0_m,
+ 0_deg,
+ 0_deg,
+ 90_deg};
+ EXPECT_EQ(expected, twist);
+
+ // Make sure computed twist gives back original end pose
+ const auto reapplied = start.Exp(twist);
+ EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Constexpr) {
+ constexpr Twist3d defaultCtor;
+ constexpr Twist3d componentCtor{1_m, 2_m, 3_m, 4_rad, 5_rad, 6_rad};
+ constexpr auto multiplied = componentCtor * 2;
+
+ static_assert(defaultCtor.dx == 0_m);
+ static_assert(componentCtor.dy == 2_m);
+ static_assert(componentCtor.dz == 3_m);
+ static_assert(multiplied.dx == 2_m);
+ static_assert(multiplied.rx == 8_rad);
+ static_assert(multiplied.ry == 10_rad);
+ static_assert(multiplied.rz == 12_rad);
+}
diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index ee2ec9b..8a920f6 100644
--- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -13,16 +13,16 @@
TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
- buffer.AddSample(0_s, frc::Rotation2d(0_rad));
- EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
- buffer.AddSample(1_s, frc::Rotation2d(1_rad));
- EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
- EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
- buffer.AddSample(3_s, frc::Rotation2d(2_rad));
- EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
+ buffer.AddSample(0_s, 0_rad);
+ EXPECT_TRUE(buffer.Sample(0_s).value() == 0_rad);
+ buffer.AddSample(1_s, 1_rad);
+ EXPECT_TRUE(buffer.Sample(0.5_s).value() == 0.5_rad);
+ EXPECT_TRUE(buffer.Sample(1_s).value() == 1_rad);
+ buffer.AddSample(3_s, 2_rad);
+ EXPECT_TRUE(buffer.Sample(2_s).value() == 1.5_rad);
- buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
- EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
+ buffer.AddSample(10.5_s, 2_rad);
+ EXPECT_TRUE(buffer.Sample(0_s) == 1_rad);
}
TEST(TimeInterpolatableBufferTest, Pose2d) {
@@ -30,9 +30,8 @@
// We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
- frc::Pose2d sample = buffer.Sample(0.5_s);
- EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
- 0.01);
- EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
- EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
+ frc::Pose2d sample = buffer.Sample(0.5_s).value();
+ EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
+ EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
+ EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 7665a97..4e0b3e5 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -9,7 +9,7 @@
TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
- 1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
+ 1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 224e231..4af5ac8 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
@@ -56,21 +56,21 @@
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{
- 0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
+ 0.0_mps, 0.0_mps, units::radians_per_second_t{std::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
- EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
- EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * std::numbers::pi, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
- units::meters_per_second_t(+0.381 * wpi::numbers::pi),
- units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
+ units::meters_per_second_t{+0.381 * std::numbers::pi},
+ units::meters_per_second_t{-0.381 * std::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon);
}
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index da16b28..e480941 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
@@ -13,10 +13,10 @@
using namespace frc;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
- DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
+ DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
- const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
- units::meter_t(5 * wpi::numbers::pi));
+ const auto& pose =
+ odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 8cc454e..364163e 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
@@ -40,6 +40,15 @@
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{5_m, 5_m, 5_m, 5_m};
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(5.0, twist.dx.value(), 0.1);
+ EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+ EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -59,9 +68,18 @@
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
}
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{-5_m, 5_m, 5_m, -5_m};
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+ EXPECT_NEAR(5.0, twist.dy.value(), 0.1);
+ EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::numbers::pi)};
+ units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
@@ -77,7 +95,17 @@
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
- EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1);
+ EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{-150.79644737_m, 150.79644737_m,
+ -150.79644737_m, 150.79644737_m};
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+ EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+ EXPECT_NEAR(2 * std::numbers::pi, twist.dtheta.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
@@ -101,6 +129,18 @@
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
+TEST_F(MecanumDriveKinematicsTest,
+ MixedRotationTranslationForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{-17.677670_m, 20.506097_m, -13.435_m,
+ 16.26_m};
+
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(1.41335, twist.dx.value(), 0.1);
+ EXPECT_NEAR(2.1221, twist.dy.value(), 0.1);
+ EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -122,6 +162,16 @@
}
TEST_F(MecanumDriveKinematicsTest,
+ OffCenterRotationForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{0_m, 16.971_m, -16.971_m, 33.941_m};
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(8.48525, twist.dx.value(), 0.1);
+ EXPECT_NEAR(-8.48525, twist.dy.value(), 0.1);
+ EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationInverseKinematics) {
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -143,6 +193,16 @@
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
}
+TEST_F(MecanumDriveKinematicsTest,
+ OffCenterTranslationRotationForwardKinematicsWithDeltas) {
+ MecanumDriveWheelPositions wheelDeltas{2.12_m, 21.92_m, -12.02_m, 36.06_m};
+ auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+ EXPECT_NEAR(12.02, twist.dx.value(), 0.1);
+ EXPECT_NEAR(-7.07, twist.dy.value(), 0.1);
+ EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Desaturate(5.5_mps);
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index 152506d..bfaf91b 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -2,7 +2,11 @@
// 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 <limits>
+#include <random>
+
#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
using namespace frc;
@@ -14,17 +18,19 @@
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
+ MecanumDriveWheelPositions zero;
+
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
- MecanumDriveOdometry odometry{kinematics, 0_rad};
+ MecanumDriveOdometry odometry{kinematics, 0_rad, zero};
};
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
- odometry.ResetPosition(Pose2d(), 0_rad);
- MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
- 3.536_mps};
+ MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
- odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
- auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
+ odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{});
+
+ odometry.Update(0_deg, wheelDeltas);
+ auto secondPose = odometry.Update(0_deg, wheelDeltas);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -32,11 +38,12 @@
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
- odometry.ResetPosition(Pose2d(), 0_rad);
- MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+ odometry.ResetPosition(0_rad, zero, Pose2d{});
+ MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+ 0.3536_m};
- odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
- auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
+ odometry.Update(0_deg, MecanumDriveWheelPositions{});
+ auto pose = odometry.Update(0_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -44,11 +51,11 @@
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
- odometry.ResetPosition(Pose2d(), 0_rad);
- MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
- 39.986_mps};
- odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
- auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
+ odometry.ResetPosition(0_rad, zero, Pose2d{});
+ MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
+ 39.986_m};
+ odometry.Update(0_deg, MecanumDriveWheelPositions{});
+ auto pose = odometry.Update(90_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -56,14 +63,140 @@
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
- odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+ odometry.ResetPosition(90_deg, zero, Pose2d{});
- MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+ MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+ 0.3536_m};
- odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
- auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+ odometry.Update(90_deg, MecanumDriveWheelPositions{});
+ auto pose = odometry.Update(90_deg, wheelDeltas);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
}
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDriveWheelPositions wheelPositions;
+
+ frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+ wheelPositions, frc::Pose2d{}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ auto wheelSpeeds = kinematics.ToWheelSpeeds(
+ {groundTruthState.velocity, 0_mps,
+ groundTruthState.velocity * groundTruthState.curvature});
+
+ wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+ wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+ wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+ wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+ wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+ wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+ wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+ wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+ auto xhat =
+ odometry.Update(groundTruthState.pose.Rotation() +
+ frc::Rotation2d{distribution(generator) * 0.05_rad},
+ wheelPositions);
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+ EXPECT_LT(maxError, 0.125);
+}
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
+ frc::MecanumDriveKinematics kinematics{
+ frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+ frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+ frc::MecanumDriveWheelPositions wheelPositions;
+
+ frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+ wheelPositions, frc::Pose2d{}};
+
+ frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 135_deg},
+ frc::Pose2d{-3_m, 0_m, -90_deg},
+ frc::Pose2d{0_m, 0_m, 45_deg}},
+ frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ auto wheelSpeeds = kinematics.ToWheelSpeeds(
+ {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
+ groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
+ 0_rad_per_s});
+
+ wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+ wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+ wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+ wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+ wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+ wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+ wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+ wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+ auto xhat = odometry.Update(
+ frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+ EXPECT_LT(maxError, 0.125);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 55ebdcf..8e0dc8f 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -2,7 +2,7 @@
// 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 <wpi/numbers>
+#include <numbers>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
@@ -40,7 +40,7 @@
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
- SwerveModuleState state{5.0_mps, Rotation2d()};
+ SwerveModuleState state{5.0_mps, 0_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
@@ -49,6 +49,16 @@
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+ SwerveModulePosition delta{5.0_m, 0_deg};
+
+ auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+ EXPECT_NEAR(twist.dx.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
@@ -65,7 +75,7 @@
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
- SwerveModuleState state{5_mps, Rotation2d(90_deg)};
+ SwerveModuleState state{5_mps, 90_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
@@ -73,9 +83,19 @@
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
}
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
+ SwerveModulePosition delta{5_m, 90_deg};
+
+ auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+ EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), 5.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::numbers::pi)};
+ units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
@@ -89,22 +109,52 @@
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
+TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t{2 * std::numbers::pi}};
+ m_kinematics.ToSwerveModuleStates(speeds);
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+ EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
+}
+
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
- SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
- SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
- SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
- SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
+ SwerveModuleState fl{106.629_mps, 135_deg};
+ SwerveModuleState fr{106.629_mps, 45_deg};
+ SwerveModuleState bl{106.629_mps, -135_deg};
+ SwerveModuleState br{106.629_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
+ SwerveModulePosition fl{106.629_m, 135_deg};
+ SwerveModulePosition fr{106.629_m, 45_deg};
+ SwerveModulePosition bl{106.629_m, -135_deg};
+ SwerveModulePosition br{106.629_m, -45_deg};
+
+ auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+ EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
- units::radians_per_second_t(2 * wpi::numbers::pi)};
+ units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
@@ -119,23 +169,37 @@
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
- SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
- SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
- SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
- SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
+ SwerveModuleState fl{0.0_mps, 0_deg};
+ SwerveModuleState fr{150.796_mps, 0_deg};
+ SwerveModuleState bl{150.796_mps, -90_deg};
+ SwerveModuleState br{213.258_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
- EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationForwardKinematicsWithDeltas) {
+ SwerveModulePosition fl{0.0_m, 0_deg};
+ SwerveModulePosition fr{150.796_m, 0_deg};
+ SwerveModulePosition bl{150.796_m, -90_deg};
+ SwerveModulePosition br{213.258_m, -45_deg};
+
+ auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+ EXPECT_NEAR(twist.dx.value(), 75.398, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), -75.398, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] =
- m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
+ m_kinematics.ToSwerveModuleStates(speeds, Translation2d{24_m, 0_m});
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
@@ -150,10 +214,10 @@
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationForwardKinematics) {
- SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
- SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
- SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
- SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
+ SwerveModuleState fl{23.43_mps, -140.19_deg};
+ SwerveModuleState fr{23.43_mps, -39.81_deg};
+ SwerveModuleState bl{54.08_mps, -109.44_deg};
+ SwerveModuleState br{54.08_mps, -70.56_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
@@ -162,11 +226,25 @@
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
}
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationAndTranslationForwardKinematicsWithDeltas) {
+ SwerveModulePosition fl{23.43_m, -140.19_deg};
+ SwerveModulePosition fr{23.43_m, -39.81_deg};
+ SwerveModulePosition bl{54.08_m, -109.44_deg};
+ SwerveModulePosition br{54.08_m, -70.56_deg};
+
+ auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+ EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dy.value(), -33.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.value(), 1.5, kEpsilon);
+}
+
TEST_F(SwerveDriveKinematicsTest, Desaturate) {
- SwerveModuleState state1{5.0_mps, Rotation2d()};
- SwerveModuleState state2{6.0_mps, Rotation2d()};
- SwerveModuleState state3{4.0_mps, Rotation2d()};
- SwerveModuleState state4{7.0_mps, Rotation2d()};
+ SwerveModuleState state1{5.0_mps, 0_deg};
+ SwerveModuleState state2{6.0_mps, 0_deg};
+ SwerveModuleState state3{4.0_mps, 0_deg};
+ SwerveModuleState state4{7.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
@@ -178,3 +256,21 @@
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}
+
+TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
+ SwerveModuleState state1{5.0_mps, 0_deg};
+ SwerveModuleState state2{6.0_mps, 0_deg};
+ SwerveModuleState state3{4.0_mps, 0_deg};
+ SwerveModuleState state4{7.0_mps, 0_deg};
+
+ wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+ SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+ &arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
+
+ double kFactor = 5.5 / 7.0;
+
+ EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 27d2a6e..8c67ab5 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -2,8 +2,14 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
+#include <limits>
+#include <random>
+
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
using namespace frc;
@@ -18,18 +24,20 @@
Translation2d m_br{-12_m, -12_m};
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
- SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
+ SwerveModulePosition zero;
+ SwerveDriveOdometry<4> m_odometry{
+ m_kinematics, 0_rad, {zero, zero, zero, zero}};
};
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
- SwerveModuleState state{5_mps, Rotation2d()};
+ SwerveModulePosition position{0.5_m, 0_deg};
- m_odometry.ResetPosition(Pose2d(), 0_rad);
- m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
- SwerveModuleState(), SwerveModuleState(),
- SwerveModuleState());
- auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
- state, state);
+ m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
+
+ m_odometry.Update(0_deg, {zero, zero, zero, zero});
+
+ auto pose =
+ m_odometry.Update(0_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -37,17 +45,13 @@
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
- SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
- SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
- SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
- SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
+ SwerveModulePosition fl{18.85_m, 90_deg};
+ SwerveModulePosition fr{42.15_m, 26.565_deg};
+ SwerveModulePosition bl{18.85_m, -90_deg};
+ SwerveModulePosition br{42.15_m, -26.565_deg};
- SwerveModuleState zero{0_mps, Rotation2d()};
-
- m_odometry.ResetPosition(Pose2d(), 0_rad);
- m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
- auto pose =
- m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
+ m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
+ auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -55,17 +59,140 @@
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
- m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+ m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{});
- SwerveModuleState state{5_mps, Rotation2d()};
+ SwerveModulePosition position{0.5_m, 0_deg};
- m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
- SwerveModuleState(), SwerveModuleState(),
- SwerveModuleState());
- auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
- state, state);
+ auto pose =
+ m_odometry.Update(90_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
+
+TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
+ SwerveDriveKinematics<4> kinematics{
+ Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+ Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+ SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+ SwerveModulePosition fl;
+ SwerveModulePosition fr;
+ SwerveModulePosition bl;
+ SwerveModulePosition br;
+
+ Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+ std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+ Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+ Pose2d{0_m, 0_m, 45_deg}},
+ TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ auto moduleStates = kinematics.ToSwerveModuleStates(
+ {groundTruthState.velocity, 0_mps,
+ groundTruthState.velocity * groundTruthState.curvature});
+
+ fl.distance += moduleStates[0].speed * dt;
+ fr.distance += moduleStates[1].speed * dt;
+ bl.distance += moduleStates[2].speed * dt;
+ br.distance += moduleStates[3].speed * dt;
+
+ fl.angle = moduleStates[0].angle;
+ fr.angle = moduleStates[1].angle;
+ bl.angle = moduleStates[2].angle;
+ br.angle = moduleStates[3].angle;
+
+ auto xhat =
+ odometry.Update(groundTruthState.pose.Rotation() +
+ frc::Rotation2d{distribution(generator) * 0.05_rad},
+ {fl, fr, bl, br});
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
+ EXPECT_LT(maxError, 0.125);
+}
+
+TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
+ SwerveDriveKinematics<4> kinematics{
+ Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+ Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+ SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+ SwerveModulePosition fl;
+ SwerveModulePosition fr;
+ SwerveModulePosition bl;
+ SwerveModulePosition br;
+
+ Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+ std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+ Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+ Pose2d{0_m, 0_m, 45_deg}},
+ TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+ std::default_random_engine generator;
+ std::normal_distribution<double> distribution(0.0, 1.0);
+
+ units::second_t dt = 0.02_s;
+ units::second_t t = 0_s;
+
+ double maxError = -std::numeric_limits<double>::max();
+ double errorSum = 0;
+
+ while (t < trajectory.TotalTime()) {
+ Trajectory::State groundTruthState = trajectory.Sample(t);
+
+ fl.distance += groundTruthState.velocity * dt +
+ 0.5 * groundTruthState.acceleration * dt * dt;
+ fr.distance += groundTruthState.velocity * dt +
+ 0.5 * groundTruthState.acceleration * dt * dt;
+ bl.distance += groundTruthState.velocity * dt +
+ 0.5 * groundTruthState.acceleration * dt * dt;
+ br.distance += groundTruthState.velocity * dt +
+ 0.5 * groundTruthState.acceleration * dt * dt;
+
+ fl.angle = groundTruthState.pose.Rotation();
+ fr.angle = groundTruthState.pose.Rotation();
+ bl.angle = groundTruthState.pose.Rotation();
+ br.angle = groundTruthState.pose.Rotation();
+
+ auto xhat = odometry.Update(
+ frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
+ double error = groundTruthState.pose.Translation()
+ .Distance(xhat.Translation())
+ .value();
+
+ if (error > maxError) {
+ maxError = error;
+ }
+ errorSum += error;
+
+ t += dt;
+ }
+
+ EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+ EXPECT_LT(maxError, 0.125);
+}
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 41c34f9..7422a7f 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -93,31 +93,29 @@
} // namespace frc
TEST_F(CubicHermiteSplineTest, StraightLine) {
- Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+ Run(Pose2d{}, std::vector<Translation2d>(), Pose2d{3_m, 0_m, 0_deg});
}
TEST_F(CubicHermiteSplineTest, SCurve) {
- Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
- std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
- Translation2d(2_m, -1_m)};
- Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+ Pose2d start{0_m, 0_m, 90_deg};
+ std::vector<Translation2d> waypoints{Translation2d{1_m, 1_m},
+ Translation2d{2_m, -1_m}};
+ Pose2d end{3_m, 0_m, 90_deg};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, OneInterior) {
Pose2d start{0_m, 0_m, 0_rad};
- std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+ std::vector<Translation2d> waypoints{Translation2d{2_m, 0_m}};
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
- EXPECT_THROW(
- Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
- Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
- SplineParameterizer::MalformedSplineException);
- EXPECT_THROW(
- Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
- Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
- SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, std::vector<Translation2d>{},
+ Pose2d{1_m, 0_m, 180_deg}),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, std::vector<Translation2d>{},
+ Pose2d{10_m, 11_m, -90_deg}),
+ SplineParameterizer::MalformedSplineException);
}
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index e045622..e45df7b 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -65,23 +65,20 @@
} // namespace frc
TEST_F(QuinticHermiteSplineTest, StraightLine) {
- Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+ Run(Pose2d{}, Pose2d{3_m, 0_m, 0_deg});
}
TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
- Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+ Run(Pose2d{}, Pose2d{1_m, 1_m, 0_deg});
}
TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
- Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
- Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+ Run(Pose2d{0_m, 0_m, 90_deg}, Pose2d{-1_m, 0_m, 90_deg});
}
TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
- EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
- Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+ EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}),
SplineParameterizer::MalformedSplineException);
- EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
- Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+ EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, Pose2d{10_m, 11_m, -90_deg}),
SplineParameterizer::MalformedSplineException);
}
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index b5a0fdc..d735338 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -6,8 +6,8 @@
#include <functional>
-#include "Eigen/Core"
#include "Eigen/Eigenvalues"
+#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
@@ -15,17 +15,16 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
- Eigen::Vector<double, 2> x0{1, 1};
- Eigen::Matrix<double, 2, 2> discA;
+ frc::Vectord<2> x0{1, 1};
+ frc::Matrixd<2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
- Eigen::Vector<double, 2> x1Discrete = discA * x0;
+ frc::Vectord<2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
- Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
- 0.0 * x0(0) + 1.0 * x0(1)};
+ frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -33,46 +32,48 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeAB) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
- Eigen::Matrix<double, 2, 1> contB{0, 1};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 1> contB{0, 1};
- Eigen::Vector<double, 2> x0{1, 1};
- Eigen::Vector<double, 1> u{1};
- Eigen::Matrix<double, 2, 2> discA;
- Eigen::Matrix<double, 2, 1> discB;
+ frc::Vectord<2> x0{1, 1};
+ frc::Vectord<1> u{1};
+ frc::Matrixd<2, 2> discA;
+ frc::Matrixd<2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
- Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
+ frc::Vectord<2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
- Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
- 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
+ frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
+ 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
EXPECT_EQ(x1Truth, x1Discrete);
}
-// dt
-// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-// 0
+// T
+// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
- Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
- Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<Eigen::Matrix<double, 2, 2>(
- units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
- Eigen::Matrix<double, 2, 2>>(
- [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
- return Eigen::Matrix<double, 2, 2>(
- (contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
+ // T
+ // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
+ frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+ std::function<frc::Matrixd<2, 2>(units::second_t,
+ const frc::Matrixd<2, 2>&)>,
+ frc::Matrixd<2, 2>>(
+ [&](units::second_t t, const frc::Matrixd<2, 2>&) {
+ return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+ 0_s, frc::Matrixd<2, 2>::Zero(), dt);
- Eigen::Matrix<double, 2, 2> discA;
- Eigen::Matrix<double, 2, 2> discQ;
+ frc::Matrixd<2, 2> discA;
+ frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
@@ -81,28 +82,30 @@
<< discQIntegrated;
}
-// dt
-// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-// 0
+// T
+// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
- Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
+ frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
- Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<Eigen::Matrix<double, 2, 2>(
- units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
- Eigen::Matrix<double, 2, 2>>(
- [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
- return Eigen::Matrix<double, 2, 2>(
- (contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
+ frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+ std::function<frc::Matrixd<2, 2>(units::second_t,
+ const frc::Matrixd<2, 2>&)>,
+ frc::Matrixd<2, 2>>(
+ [&](units::second_t t, const frc::Matrixd<2, 2>&) {
+ return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+ 0_s, frc::Matrixd<2, 2>::Zero(), dt);
- Eigen::Matrix<double, 2, 2> discA;
- Eigen::Matrix<double, 2, 2> discQ;
+ frc::Matrixd<2, 2> discA;
+ frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
@@ -113,14 +116,14 @@
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
- Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+ frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
- Eigen::Matrix<double, 2, 2> discQTaylor;
- Eigen::Matrix<double, 2, 2> discA;
- Eigen::Matrix<double, 2, 2> discATaylor;
+ frc::Matrixd<2, 2> discQTaylor;
+ frc::Matrixd<2, 2> discA;
+ frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
@@ -128,16 +131,18 @@
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
- Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<Eigen::Matrix<double, 2, 2>(
- units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
- Eigen::Matrix<double, 2, 2>>(
- [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
- return Eigen::Matrix<double, 2, 2>(
- (contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
+ frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+ std::function<frc::Matrixd<2, 2>(units::second_t,
+ const frc::Matrixd<2, 2>&)>,
+ frc::Matrixd<2, 2>>(
+ [&](units::second_t t, const frc::Matrixd<2, 2>&) {
+ return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+ 0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -157,14 +162,14 @@
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
- Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
- Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
+ frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
+ frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
- Eigen::Matrix<double, 2, 2> discQTaylor;
- Eigen::Matrix<double, 2, 2> discA;
- Eigen::Matrix<double, 2, 2> discATaylor;
+ frc::Matrixd<2, 2> discQTaylor;
+ frc::Matrixd<2, 2> discA;
+ frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
@@ -172,16 +177,18 @@
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
- Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
- std::function<Eigen::Matrix<double, 2, 2>(
- units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
- Eigen::Matrix<double, 2, 2>>(
- [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
- return Eigen::Matrix<double, 2, 2>(
- (contA * t.value()).exp() * contQ *
- (contA.transpose() * t.value()).exp());
+ // T
+ // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+ // 0
+ frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+ std::function<frc::Matrixd<2, 2>(units::second_t,
+ const frc::Matrixd<2, 2>&)>,
+ frc::Matrixd<2, 2>>(
+ [&](units::second_t t, const frc::Matrixd<2, 2>&) {
+ return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
+ (contA.transpose() * t.value()).exp());
},
- 0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+ 0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -201,10 +208,10 @@
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
- Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
- Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
+ frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
+ frc::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
- Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
+ frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
EXPECT_LT((discRTruth - discR).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscR:\n"
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index fbf86f1..dbc4284 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -16,49 +16,43 @@
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
ASSERT_TRUE(model.A().isApprox(
- Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
- 0.001));
+ frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
ASSERT_TRUE(model.B().isApprox(
- Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
- 0.001));
- ASSERT_TRUE(model.C().isApprox(
- Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
- ASSERT_TRUE(model.D().isApprox(
- Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
+ frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
+ ASSERT_TRUE(
+ model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+ ASSERT_TRUE(
+ model.D().isApprox(frc::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
0.05_m, 12);
ASSERT_TRUE(model.A().isApprox(
- Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
- ASSERT_TRUE(
- model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
- ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
- ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
+ frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));
+ ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
- ASSERT_TRUE(
- model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
- ASSERT_TRUE(
- model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
- ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
- ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
+ ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
+ ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, DCMotorSystem) {
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
- ASSERT_TRUE(model.A().isApprox(
- Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(
- model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 0.001));
- ASSERT_TRUE(model.C().isApprox(
- Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
- ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 0.0}, 0.001));
+ model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
+ ASSERT_TRUE(
+ model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+ ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -70,9 +64,8 @@
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
ASSERT_TRUE(model.A().isApprox(
- Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
- ASSERT_TRUE(
- model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
+ frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -84,6 +77,6 @@
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
- ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
- ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
+ ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
+ ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));
}
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index fd9c039..b1793ad 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -10,48 +10,34 @@
// Tests that integrating dx/dt = e^x works.
TEST(NumericalIntegrationTest, Exponential) {
- Eigen::Vector<double, 1> y0{0.0};
+ frc::Vectord<1> y0{0.0};
- Eigen::Vector<double, 1> y1 = frc::RK4(
- [](const Eigen::Vector<double, 1>& x) {
- return Eigen::Vector<double, 1>{std::exp(x(0))};
- },
+ frc::Vectord<1> y1 = frc::RK4(
+ [](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; },
y0, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(NumericalIntegrationTest, ExponentialWithU) {
- Eigen::Vector<double, 1> y0{0.0};
+ frc::Vectord<1> y0{0.0};
- Eigen::Vector<double, 1> y1 = frc::RK4(
- [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
- return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
+ frc::Vectord<1> y1 = frc::RK4(
+ [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+ return frc::Vectord<1>{std::exp(u(0) * x(0))};
},
- y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
- EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-// Tests that integrating dx/dt = e^x works with RKF45.
-TEST(NumericalIntegrationTest, ExponentialRKF45) {
- Eigen::Vector<double, 1> y0{0.0};
-
- Eigen::Vector<double, 1> y1 = frc::RKF45(
- [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
- return Eigen::Vector<double, 1>{std::exp(x(0))};
- },
- y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+ y0, frc::Vectord<1>{1.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
- Eigen::Vector<double, 1> y0{0.0};
+ frc::Vectord<1> y0{0.0};
- Eigen::Vector<double, 1> y1 = frc::RKDP(
- [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
- return Eigen::Vector<double, 1>{std::exp(x(0))};
+ frc::Vectord<1> y1 = frc::RKDP(
+ [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+ return frc::Vectord<1>{std::exp(x(0))};
},
- y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+ y0, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
index 4e64825..513684b 100644
--- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -6,53 +6,46 @@
#include "frc/system/NumericalJacobian.h"
-Eigen::Matrix<double, 4, 4> A{
- {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
-Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
+frc::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+frc::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
// Function from which to recover A and B
-Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
- const Eigen::Vector<double, 2>& u) {
+frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax) {
- Eigen::Matrix<double, 4, 4> newA =
- frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
+ AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu) {
- Eigen::Matrix<double, 4, 2> newB =
- frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
+ AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
-Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
-Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
+frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
+frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
// Function from which to recover C and D
-Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
- const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
return C * x + D * u;
}
// Test that we can recover C from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Cx) {
- Eigen::Matrix<double, 3, 4> newC =
- frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
+ CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newC.isApprox(C));
}
// Test that we can recover D from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Du) {
- Eigen::Matrix<double, 3, 2> newD =
- frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
- Eigen::Vector<double, 2>::Zero());
+ frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
+ CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index f1be861..70f1938 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -9,26 +9,25 @@
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
-Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
- return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
- (std::pow(std::exp(t) + 1.0, 2.0))};
+frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) {
+ return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)};
}
} // namespace
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-// x' = x (2 / (e^t + 1) - 1)
+// x' = x (2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
-// x(t) = 12 * e^t / ((e^t + 1)^2)
+// x(t) = 12eᵗ/((eᵗ + 1)²)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
- Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+ frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);
- Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
- [](units::second_t t, const Eigen::Vector<double, 1>& x) {
- return Eigen::Vector<double, 1>{
- x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+ frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying(
+ [](units::second_t t, const frc::Vectord<1>& x) {
+ return frc::Vectord<1>{x(0) *
+ (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index b5f6406..e3d6b7f 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -72,12 +72,12 @@
DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
- Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
- Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
+ Pose2d{1_m, 0_m, 90_deg}, std::vector<Translation2d>{},
+ Pose2d{0_m, 1_m, 180_deg}, config));
config.SetReversed(true);
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
- Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
- Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
+ Pose2d{0_m, 1_m, 180_deg}, std::vector<Translation2d>{},
+ Pose2d{1_m, 0_m, 90_deg}, config));
}
diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 88dc2b8..8d9e221 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -21,9 +21,8 @@
auto config = TrajectoryConfig(13_fps, 13_fps_sq);
MaxVelocityConstraint maxVelConstraint(maxVelocity);
- EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
- 10_ft, 5_ft, Rotation2d(180_deg),
- maxVelConstraint);
+ EllipticalRegionConstraint regionConstraint(
+ frc::Translation2d{5_ft, 2.5_ft}, 10_ft, 5_ft, 180_deg, maxVelConstraint);
config.AddConstraint(regionConstraint);
auto trajectory = TestTrajectory::GetTrajectory(config);
@@ -45,14 +44,12 @@
constexpr auto maxVelocity = 2_fps;
MaxVelocityConstraint maxVelConstraint(maxVelocity);
EllipticalRegionConstraint regionConstraintNoRotation(
- frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
- maxVelConstraint);
+ frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 0_deg, maxVelConstraint);
EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
- frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+ frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
EllipticalRegionConstraint regionConstraintWithRotation(
- frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
- maxVelConstraint);
+ frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 90_deg, maxVelConstraint);
EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
- frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+ frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
}
diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 77310ae..0bf6b1a 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -47,7 +47,6 @@
frc::Translation2d{5_ft, 27_ft},
maxVelConstraint);
- EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
- EXPECT_TRUE(
- regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
+ EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d{}));
+ EXPECT_TRUE(regionConstraint.IsPoseInRegion(Pose2d{3_ft, 14.5_ft, 0_deg}));
}
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 175becf..5892461 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -34,8 +34,7 @@
TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
const auto t = TrajectoryGenerator::GenerateTrajectory(
- std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
- Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+ std::vector<Pose2d>{Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}},
TrajectoryConfig(12_fps, 12_fps_sq));
ASSERT_EQ(t.States().size(), 1u);
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 0c6e07a..5c77a5b 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -31,11 +31,9 @@
TEST(TrajectoryTransformsTest, TransformBy) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
- config);
+ frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, 90_deg}, config);
- auto transformedTrajectory =
- trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+ auto transformedTrajectory = trajectory.TransformBy({{1_m, 2_m}, 30_deg});
auto firstPose = transformedTrajectory.Sample(0_s).pose;
@@ -49,11 +47,9 @@
TEST(TrajectoryTransformsTest, RelativeTo) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
- frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+ frc::Pose2d{1_m, 2_m, 30_deg}, {}, frc::Pose2d{5_m, 7_m, 90_deg}, config);
- auto transformedTrajectory =
- trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+ auto transformedTrajectory = trajectory.RelativeTo({1_m, 2_m, 30_deg});
auto firstPose = transformedTrajectory.Sample(0_s).pose;
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
index 23a20e8..5e7b165 100644
--- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -6,7 +6,7 @@
#include <array>
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
diff --git a/wpimath/src/test/native/include/trajectory/TestTrajectory.h b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
index de7b8b8..89e735c 100644
--- a/wpimath/src/test/native/include/trajectory/TestTrajectory.h
+++ b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
@@ -17,16 +17,15 @@
public:
static Trajectory GetTrajectory(TrajectoryConfig& config) {
// 2018 cross scale auto waypoints
- const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
- const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
+ const Pose2d sideStart{1.54_ft, 23.23_ft, 180_deg};
+ const Pose2d crossScale{23.7_ft, 6.8_ft, -160_deg};
config.SetReversed(true);
auto vector = std::vector<Translation2d>{
- (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
+ (sideStart + Transform2d{Translation2d{-13_ft, 0_ft}, 0_deg})
.Translation(),
- (sideStart +
- Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
+ (sideStart + Transform2d{Translation2d{-19.5_ft, 5.0_ft}, -90_deg})
.Translation()};
return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,