From 7918b65f5c694a861f10372ce9386a6e56346baa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 19 Jun 2024 00:08:07 -0700 Subject: [PATCH] Move set function definitions into headers and fix constraint dllexports (#183) --- .../trajopt/constraint/LinePointConstraint.h | 3 +- .../trajopt/constraint/PointAtConstraint.h | 4 +- .../trajopt/constraint/PointLineConstraint.h | 3 +- .../trajopt/constraint/PointPointConstraint.h | 3 +- include/trajopt/path/SwervePathBuilder.h | 2 - include/trajopt/set/ConeSet2d.h | 6 ++- include/trajopt/set/EllipticalSet2d.h | 15 ++++-- include/trajopt/set/IntervalSet1d.h | 36 +++++++++---- include/trajopt/set/LinearSet2d.h | 28 ++++++++++- include/trajopt/set/RectangularSet2d.h | 12 +++-- include/trajopt/set/Set2d.h | 1 - {src => include/trajopt}/set/setnotation.txt | 0 include/trajopt/solution/Solution.h | 1 + .../differential/DifferentialConstraint.cpp | 1 - ...fferentialTangentialVelocityConstraint.cpp | 1 - src/path/SwervePathBuilder.cpp | 10 +--- src/set/ConeSet2d.cpp | 13 ----- src/set/EllipticalSet2d.cpp | 27 ---------- src/set/IntervalSet1d.cpp | 50 ------------------- src/set/LinearSet2d.cpp | 34 ------------- src/set/RectangularSet2d.cpp | 23 --------- 21 files changed, 87 insertions(+), 186 deletions(-) rename {src => include/trajopt}/set/setnotation.txt (100%) delete mode 100644 src/constraint/differential/DifferentialConstraint.cpp delete mode 100644 src/constraint/differential/DifferentialTangentialVelocityConstraint.cpp delete mode 100644 src/set/ConeSet2d.cpp delete mode 100644 src/set/EllipticalSet2d.cpp delete mode 100644 src/set/IntervalSet1d.cpp delete mode 100644 src/set/LinearSet2d.cpp delete mode 100644 src/set/RectangularSet2d.cpp diff --git a/include/trajopt/constraint/LinePointConstraint.h b/include/trajopt/constraint/LinePointConstraint.h index 8dd96550..16463fb9 100644 --- a/include/trajopt/constraint/LinePointConstraint.h +++ b/include/trajopt/constraint/LinePointConstraint.h @@ -2,6 +2,7 @@ #pragma once +#include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" namespace trajopt { @@ -10,7 +11,7 @@ namespace trajopt { * Specifies the required minimum distance between a line segment on the * robot's frame and a point on the field. */ -struct LinePointConstraint { +struct TRAJOPT_DLLEXPORT LinePointConstraint { /// robot line start x (meters) double robotLineStartX; /// robot line start y (meters) diff --git a/include/trajopt/constraint/PointAtConstraint.h b/include/trajopt/constraint/PointAtConstraint.h index b49fb32f..ec79edc9 100644 --- a/include/trajopt/constraint/PointAtConstraint.h +++ b/include/trajopt/constraint/PointAtConstraint.h @@ -2,14 +2,14 @@ #pragma once -#include "trajopt/set/IntervalSet1d.h" +#include "trajopt/SymbolExports.h" namespace trajopt { /** * Specifies a point on the field at which the robot should point. */ -struct PointAtConstraint { +struct TRAJOPT_DLLEXPORT PointAtConstraint { /// field point x (meters) double fieldPointX; /// field point y (meters) diff --git a/include/trajopt/constraint/PointLineConstraint.h b/include/trajopt/constraint/PointLineConstraint.h index 8dfaed23..937b793a 100644 --- a/include/trajopt/constraint/PointLineConstraint.h +++ b/include/trajopt/constraint/PointLineConstraint.h @@ -2,6 +2,7 @@ #pragma once +#include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" namespace trajopt { @@ -10,7 +11,7 @@ namespace trajopt { * Specifies the required minimum distance between a point on the robot's * frame and a line segment on the field. */ -struct PointLineConstraint { +struct TRAJOPT_DLLEXPORT PointLineConstraint { /// robot point x (meters) double robotPointX; /// robot point y (meters) diff --git a/include/trajopt/constraint/PointPointConstraint.h b/include/trajopt/constraint/PointPointConstraint.h index 711caf40..2db37a81 100644 --- a/include/trajopt/constraint/PointPointConstraint.h +++ b/include/trajopt/constraint/PointPointConstraint.h @@ -2,6 +2,7 @@ #pragma once +#include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" namespace trajopt { @@ -10,7 +11,7 @@ namespace trajopt { * Specifies the required distance between a point on the robot's frame * and a point on the field. */ -struct PointPointConstraint { +struct TRAJOPT_DLLEXPORT PointPointConstraint { /// robot point x (meters) double robotPointX; /// robot point y (meters) diff --git a/include/trajopt/path/SwervePathBuilder.h b/include/trajopt/path/SwervePathBuilder.h index 421d80ae..69c4b075 100644 --- a/include/trajopt/path/SwervePathBuilder.h +++ b/include/trajopt/path/SwervePathBuilder.h @@ -13,8 +13,6 @@ #include "trajopt/obstacle/Obstacle.h" #include "trajopt/path/InitialGuessPoint.h" #include "trajopt/path/Path.h" -#include "trajopt/set/IntervalSet1d.h" -#include "trajopt/set/Set2d.h" #include "trajopt/solution/Solution.h" #include "trajopt/solution/SwerveSolution.h" diff --git a/include/trajopt/set/ConeSet2d.h b/include/trajopt/set/ConeSet2d.h index c4848954..456decbe 100644 --- a/include/trajopt/set/ConeSet2d.h +++ b/include/trajopt/set/ConeSet2d.h @@ -2,6 +2,8 @@ #pragma once +#include + #include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" @@ -17,7 +19,9 @@ struct TRAJOPT_DLLEXPORT ConeSet2d { /** * Returns true if the set is valid. */ - bool IsValid() const noexcept; + bool IsValid() const noexcept { + return thetaBound.Range() > 0.0 && thetaBound.Range() <= std::numbers::pi; + } }; } // namespace trajopt diff --git a/include/trajopt/set/EllipticalSet2d.h b/include/trajopt/set/EllipticalSet2d.h index 4eeadf0d..8e30d01c 100644 --- a/include/trajopt/set/EllipticalSet2d.h +++ b/include/trajopt/set/EllipticalSet2d.h @@ -2,6 +2,8 @@ #pragma once +#include + #include "trajopt/SymbolExports.h" namespace trajopt { @@ -38,22 +40,27 @@ struct TRAJOPT_DLLEXPORT EllipticalSet2d { * @param direction The direction. */ static EllipticalSet2d CircularSet2d( - double radius, Direction direction = Direction::kInside); + double radius, Direction direction = Direction::kInside) { + return EllipticalSet2d{radius, radius, direction}; + } /** * Returns true if the ellipse is a circle. */ - bool IsCircular() const noexcept; + bool IsCircular() const noexcept { return xRadius == yRadius; } /** * Returns true if the set spans R². */ - bool IsR2() const noexcept; + bool IsR2() const noexcept { + return xRadius >= std::numeric_limits::infinity() && + yRadius >= std::numeric_limits::infinity(); + } /** * Returns true if the set is valid. */ - bool IsValid() const noexcept; + bool IsValid() const noexcept { return xRadius > 0.0 && yRadius > 0.0; } }; } // namespace trajopt diff --git a/include/trajopt/set/IntervalSet1d.h b/include/trajopt/set/IntervalSet1d.h index fffc23f2..69d9303d 100644 --- a/include/trajopt/set/IntervalSet1d.h +++ b/include/trajopt/set/IntervalSet1d.h @@ -2,6 +2,8 @@ #pragma once +#include + #include "trajopt/SymbolExports.h" namespace trajopt { @@ -23,21 +25,24 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * @param lower The lower bound. * @param upper The upper bound. */ - IntervalSet1d(double lower, double upper); + IntervalSet1d(double lower, double upper) : lower(lower), upper(upper) {} /** * Construct a Scalar Bound that represents the interval [value, value]. * * @param value the value to bound the number between. */ - IntervalSet1d(double value); // NOLINT + IntervalSet1d(double value) : lower(value), upper(value) {} // NOLINT IntervalSet1d() = default; /** * Returns an IntervalSet1d spanning R¹. */ - static IntervalSet1d R1(); + static IntervalSet1d R1() { + return IntervalSet1d(-std::numeric_limits::infinity(), + +std::numeric_limits::infinity()); + } /** * Returns an IntervalSet1d that contains all the real numbers less than or @@ -46,7 +51,10 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * @param max the maximum value * @return [-∞, max] */ - static IntervalSet1d LessThan(double max); + static IntervalSet1d LessThan(double max) { + return IntervalSet1d(-std::numeric_limits::infinity(), max); + } + /** * Returns an IntervalSet1d that contains all the real numbers greater than or * equal to a minimum value @@ -54,7 +62,9 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * @param min the minimum value * @return [min, ∞] */ - static IntervalSet1d GreaterThan(double min); + static IntervalSet1d GreaterThan(double min) { + return IntervalSet1d(min, +std::numeric_limits::infinity()); + } /** * Check if this scalar bound is equivalent to another scalar bound. @@ -74,7 +84,7 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * * @return upper - lower */ - double Range() const noexcept; + double Range() const noexcept { return upper - lower; } /** * Check if this scalar bound only contains one point. This only @@ -82,7 +92,7 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * * @return lower == upper */ - bool IsExact() const noexcept; + bool IsExact() const noexcept { return lower == upper; } /** * Check if this scalar bound only contains 0. This occurs when @@ -90,17 +100,21 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * * @return lower == 0.0 && upper == 0.0 */ - bool IsZero() const noexcept; + bool IsZero() const noexcept { return lower == 0.0 && upper == 0.0; } /** * Returns true if this IntervalSet1d has a lower bound. */ - bool IsLowerBounded() const noexcept; + bool IsLowerBounded() const noexcept { + return lower > -std::numeric_limits::infinity(); + } /** * Returns true if this IntervalSet1d has an upper bound. */ - bool IsUpperBounded() const noexcept; + bool IsUpperBounded() const noexcept { + return upper < +std::numeric_limits::infinity(); + } /** * Check if this scalar bound is valid. A scalar bound is valid @@ -109,7 +123,7 @@ struct TRAJOPT_DLLEXPORT IntervalSet1d { * * @return lower <= upper */ - bool IsValid() const noexcept; + bool IsValid() const noexcept { return lower <= upper; } }; } // namespace trajopt diff --git a/include/trajopt/set/LinearSet2d.h b/include/trajopt/set/LinearSet2d.h index 6ffcc73d..eb30f8ff 100644 --- a/include/trajopt/set/LinearSet2d.h +++ b/include/trajopt/set/LinearSet2d.h @@ -2,6 +2,8 @@ #pragma once +#include + #include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" #include "trajopt/set/RectangularSet2d.h" @@ -22,7 +24,31 @@ struct TRAJOPT_DLLEXPORT LinearSet2d { * @param rBound FIXME What does this do? */ static RectangularSet2d RBoundToRectangular(double theta, - const IntervalSet1d& rBound); + const IntervalSet1d& rBound) { + double sinTheta = std::sin(theta); + double cosTheta = std::cos(theta); + if (sinTheta > std::abs(cosTheta)) { // y > |x|, up cone + double lowerVectorY = rBound.lower * sinTheta; + double upperVectorY = rBound.upper * sinTheta; + return RectangularSet2d{IntervalSet1d::R1(), + {lowerVectorY, upperVectorY}}; + } else if (sinTheta < -std::abs(cosTheta)) { // y < -|x|, down cone + double lowerVectorY = rBound.upper * sinTheta; + double upperVectorY = rBound.lower * sinTheta; + return RectangularSet2d{IntervalSet1d::R1(), + {lowerVectorY, upperVectorY}}; + } else if (cosTheta >= std::abs(sinTheta)) { // x ≥ |y|, right cone + double lowerVectorX = rBound.lower * cosTheta; + double upperVectorX = rBound.upper * cosTheta; + return RectangularSet2d{{lowerVectorX, upperVectorX}, + IntervalSet1d::R1()}; + } else /*if (cosTheta <= -std::abs(sinTheta))*/ { // x ≤ -|y|, left cone + double lowerVectorX = rBound.upper * cosTheta; + double upperVectorX = rBound.lower * cosTheta; + return RectangularSet2d{{lowerVectorX, upperVectorX}, + IntervalSet1d::R1()}; + } + } }; } // namespace trajopt diff --git a/include/trajopt/set/RectangularSet2d.h b/include/trajopt/set/RectangularSet2d.h index 9e359b0c..68fd9874 100644 --- a/include/trajopt/set/RectangularSet2d.h +++ b/include/trajopt/set/RectangularSet2d.h @@ -2,6 +2,8 @@ #pragma once +#include + #include "trajopt/SymbolExports.h" #include "trajopt/set/IntervalSet1d.h" @@ -23,12 +25,16 @@ struct TRAJOPT_DLLEXPORT RectangularSet2d { * @param r The distance. * @param theta The heading. */ - static RectangularSet2d PolarExactSet2d(double r, double theta); + static RectangularSet2d PolarExactSet2d(double r, double theta) { + return RectangularSet2d{r * std::cos(theta), r * std::sin(theta)}; + } /** * Construct a RectangularSet2d spanning R². */ - static RectangularSet2d R2(); + static RectangularSet2d R2() { + return RectangularSet2d{IntervalSet1d::R1(), IntervalSet1d::R1()}; + } /** * @brief Check if this planar bound is valid. A planar bound is valid when @@ -38,7 +44,7 @@ struct TRAJOPT_DLLEXPORT RectangularSet2d { * * @return true if and only if this planar bound is valid */ - bool IsValid() const noexcept; + bool IsValid() const noexcept { return xBound.IsValid() && yBound.IsValid(); } }; } // namespace trajopt diff --git a/include/trajopt/set/Set2d.h b/include/trajopt/set/Set2d.h index cbafddad..d3781b5d 100644 --- a/include/trajopt/set/Set2d.h +++ b/include/trajopt/set/Set2d.h @@ -4,7 +4,6 @@ #include -#include "trajopt/SymbolExports.h" #include "trajopt/set/ConeSet2d.h" #include "trajopt/set/EllipticalSet2d.h" #include "trajopt/set/LinearSet2d.h" diff --git a/src/set/setnotation.txt b/include/trajopt/set/setnotation.txt similarity index 100% rename from src/set/setnotation.txt rename to include/trajopt/set/setnotation.txt diff --git a/include/trajopt/solution/Solution.h b/include/trajopt/solution/Solution.h index 08c2923d..47c104bf 100644 --- a/include/trajopt/solution/Solution.h +++ b/include/trajopt/solution/Solution.h @@ -24,4 +24,5 @@ struct TRAJOPT_DLLEXPORT Solution { /// Headings. std::vector theta; }; + } // namespace trajopt diff --git a/src/constraint/differential/DifferentialConstraint.cpp b/src/constraint/differential/DifferentialConstraint.cpp deleted file mode 100644 index 60ddbc6b..00000000 --- a/src/constraint/differential/DifferentialConstraint.cpp +++ /dev/null @@ -1 +0,0 @@ -// Copyright (c) TrajoptLib contributors diff --git a/src/constraint/differential/DifferentialTangentialVelocityConstraint.cpp b/src/constraint/differential/DifferentialTangentialVelocityConstraint.cpp deleted file mode 100644 index 60ddbc6b..00000000 --- a/src/constraint/differential/DifferentialTangentialVelocityConstraint.cpp +++ /dev/null @@ -1 +0,0 @@ -// Copyright (c) TrajoptLib contributors diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index 6b066f2a..b460a7e9 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -2,13 +2,9 @@ #include "trajopt/path/SwervePathBuilder.h" -#include - #include #include -#include -#include -#include +#include #include "optimization/Cancellation.h" #include "optimization/TrajoptUtil.h" @@ -19,15 +15,11 @@ #include "trajopt/constraint/PointLineConstraint.h" #include "trajopt/constraint/TranslationConstraint.h" #include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.h" -#include "trajopt/drivetrain/SwerveDrivetrain.h" #include "trajopt/obstacle/Obstacle.h" -#include "trajopt/path/InitialGuessPoint.h" -#include "trajopt/path/Path.h" #include "trajopt/set/EllipticalSet2d.h" #include "trajopt/set/IntervalSet1d.h" #include "trajopt/set/LinearSet2d.h" #include "trajopt/set/RectangularSet2d.h" -#include "trajopt/solution/Solution.h" namespace trajopt { diff --git a/src/set/ConeSet2d.cpp b/src/set/ConeSet2d.cpp deleted file mode 100644 index 594e291f..00000000 --- a/src/set/ConeSet2d.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/set/ConeSet2d.h" - -#include - -namespace trajopt { - -bool ConeSet2d::IsValid() const noexcept { - return thetaBound.Range() > 0.0 && thetaBound.Range() <= std::numbers::pi; -} - -} // namespace trajopt diff --git a/src/set/EllipticalSet2d.cpp b/src/set/EllipticalSet2d.cpp deleted file mode 100644 index 4bd77f8d..00000000 --- a/src/set/EllipticalSet2d.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/set/EllipticalSet2d.h" - -#include - -namespace trajopt { - -EllipticalSet2d EllipticalSet2d::CircularSet2d(double radius, - Direction direction) { - return EllipticalSet2d{radius, radius, direction}; -} - -bool EllipticalSet2d::IsCircular() const noexcept { - return xRadius == yRadius; -} - -bool EllipticalSet2d::IsR2() const noexcept { - return xRadius >= std::numeric_limits::infinity() && - yRadius >= std::numeric_limits::infinity(); -} - -bool EllipticalSet2d::IsValid() const noexcept { - return xRadius > 0.0 && yRadius > 0.0; -} - -} // namespace trajopt diff --git a/src/set/IntervalSet1d.cpp b/src/set/IntervalSet1d.cpp deleted file mode 100644 index a9b75929..00000000 --- a/src/set/IntervalSet1d.cpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/set/IntervalSet1d.h" - -#include - -namespace trajopt { - -IntervalSet1d::IntervalSet1d(double lower, double upper) - : lower(lower), upper(upper) {} - -IntervalSet1d::IntervalSet1d(double value) : lower(value), upper(value) {} - -IntervalSet1d IntervalSet1d::R1() { - return IntervalSet1d(-std::numeric_limits::infinity(), - +std::numeric_limits::infinity()); -} - -IntervalSet1d IntervalSet1d::LessThan(double max) { - return IntervalSet1d(-std::numeric_limits::infinity(), max); -} - -IntervalSet1d IntervalSet1d::GreaterThan(double min) { - return IntervalSet1d(min, +std::numeric_limits::infinity()); -} - -double IntervalSet1d::Range() const noexcept { - return upper - lower; -} - -bool IntervalSet1d::IsExact() const noexcept { - return lower == upper; -} - -bool IntervalSet1d::IsZero() const noexcept { - return lower == 0.0 && upper == 0.0; -} - -bool IntervalSet1d::IsLowerBounded() const noexcept { - return lower > -std::numeric_limits::infinity(); -} -bool IntervalSet1d::IsUpperBounded() const noexcept { - return upper < +std::numeric_limits::infinity(); -} - -bool IntervalSet1d::IsValid() const noexcept { - return lower <= upper; -} - -} // namespace trajopt diff --git a/src/set/LinearSet2d.cpp b/src/set/LinearSet2d.cpp deleted file mode 100644 index 811170d9..00000000 --- a/src/set/LinearSet2d.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/set/LinearSet2d.h" - -#include - -#include "trajopt/set/IntervalSet1d.h" -#include "trajopt/set/RectangularSet2d.h" - -namespace trajopt { - -RectangularSet2d LinearSet2d::RBoundToRectangular(double theta, - const IntervalSet1d& rBound) { - double sinTheta = std::sin(theta); - double cosTheta = std::cos(theta); - if (sinTheta > std::abs(cosTheta)) { // y > |x|, up cone - double lowerVectorY = rBound.lower * sinTheta; - double upperVectorY = rBound.upper * sinTheta; - return RectangularSet2d{IntervalSet1d::R1(), {lowerVectorY, upperVectorY}}; - } else if (sinTheta < -std::abs(cosTheta)) { // y < -|x|, down cone - double lowerVectorY = rBound.upper * sinTheta; - double upperVectorY = rBound.lower * sinTheta; - return RectangularSet2d{IntervalSet1d::R1(), {lowerVectorY, upperVectorY}}; - } else if (cosTheta >= std::abs(sinTheta)) { // x ≥ |y|, right cone - double lowerVectorX = rBound.lower * cosTheta; - double upperVectorX = rBound.upper * cosTheta; - return RectangularSet2d{{lowerVectorX, upperVectorX}, IntervalSet1d::R1()}; - } else /*if (cosTheta <= -std::abs(sinTheta))*/ { // x ≤ -|y|, left cone - double lowerVectorX = rBound.upper * cosTheta; - double upperVectorX = rBound.lower * cosTheta; - return RectangularSet2d{{lowerVectorX, upperVectorX}, IntervalSet1d::R1()}; - } -} -} // namespace trajopt diff --git a/src/set/RectangularSet2d.cpp b/src/set/RectangularSet2d.cpp deleted file mode 100644 index 2c8cbc40..00000000 --- a/src/set/RectangularSet2d.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/set/RectangularSet2d.h" - -#include - -#include "trajopt/set/IntervalSet1d.h" - -namespace trajopt { - -RectangularSet2d RectangularSet2d::PolarExactSet2d(double r, double theta) { - return RectangularSet2d{r * std::cos(theta), r * std::sin(theta)}; -} - -RectangularSet2d RectangularSet2d::R2() { - return RectangularSet2d{IntervalSet1d::R1(), IntervalSet1d::R1()}; -} - -bool RectangularSet2d::IsValid() const noexcept { - return xBound.IsValid() && yBound.IsValid(); -} - -} // namespace trajopt