Move set function definitions into headers and fix constraint dllexpo…

…rts (#183)

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)

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)

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)

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)

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"
 

include/trajopt/set/ConeSet2d.h

@@ -2,6 +2,8 @@
 
 #pragma once
 
+#include <numbers>
+
 #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

include/trajopt/set/EllipticalSet2d.h

@@ -2,6 +2,8 @@
 
 #pragma once
 
+#include <limits>
+
 #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<double>::infinity() &&
+           yRadius >= std::numeric_limits<double>::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

include/trajopt/set/IntervalSet1d.h

@@ -2,6 +2,8 @@
 
 #pragma once
 
+#include <limits>
+
 #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<double>::infinity(),
+                         +std::numeric_limits<double>::infinity());
+  }
 
   /**
    * Returns an IntervalSet1d that contains all the real numbers less than or
@@ -46,15 +51,20 @@ 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<double>::infinity(), max);
+  }
+
   /**
    * Returns an IntervalSet1d that contains all the real numbers greater than or
    * equal to a minimum value
    *
    * @param min the minimum value
    * @return [min, ∞]
    */
-  static IntervalSet1d GreaterThan(double min);
+  static IntervalSet1d GreaterThan(double min) {
+    return IntervalSet1d(min, +std::numeric_limits<double>::infinity());
+  }
 
   /**
    * Check if this scalar bound is equivalent to another scalar bound.
@@ -74,33 +84,37 @@ 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
    * occurs when lower == upper.
    *
    * @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
    * this scalar bound equals 0.0.
    *
    * @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<double>::infinity();
+  }
 
   /**
    * Returns true if this IntervalSet1d has an upper bound.
    */
-  bool IsUpperBounded() const noexcept;
+  bool IsUpperBounded() const noexcept {
+    return upper < +std::numeric_limits<double>::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

include/trajopt/set/LinearSet2d.h

@@ -2,6 +2,8 @@
 
 #pragma once
 
+#include <cmath>
+
 #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

include/trajopt/set/RectangularSet2d.h

@@ -2,6 +2,8 @@
 
 #pragma once
 
+#include <cmath>
+
 #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

include/trajopt/set/Set2d.h

@@ -4,7 +4,6 @@
 
 #include <variant>
 
-#include "trajopt/SymbolExports.h"
 #include "trajopt/set/ConeSet2d.h"
 #include "trajopt/set/EllipticalSet2d.h"
 #include "trajopt/set/LinearSet2d.h"

include/trajopt/solution/Solution.h

@@ -24,4 +24,5 @@ struct TRAJOPT_DLLEXPORT Solution {
   /// Headings.
   std::vector<double> theta;
 };
+
 }  // namespace trajopt

src/path/SwervePathBuilder.cpp

@@ -2,13 +2,9 @@
 
 #include "trajopt/path/SwervePathBuilder.h"
 
-#include <stdint.h>
-
 #include <cassert>
 #include <cmath>
-#include <memory>
-#include <stdexcept>
-#include <vector>
+#include <utility>
 
 #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 {