Restructure
This commit is contained in:
parent
65640473f1
commit
5b291916d7
4
.gitignore
vendored
Normal file
4
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
packages/
|
||||
Debug/
|
||||
*/Debug/
|
||||
.vs/
|
||||
41
FalconLibraryCPP.sln
Normal file
41
FalconLibraryCPP.sln
Normal file
|
|
@ -0,0 +1,41 @@
|
|||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.29009.5
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "FalconLibraryCPP", "FalconLibraryCPP\FalconLibraryCPP.vcxproj", "{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}"
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Tests", "Tests\Tests.vcxproj", "{52260548-392E-4370-8760-D749F5BEBEE6}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
Debug|x86 = Debug|x86
|
||||
Release|x64 = Release|x64
|
||||
Release|x86 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x64.Build.0 = Debug|x64
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x86.Build.0 = Debug|Win32
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x64.ActiveCfg = Release|x64
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x64.Build.0 = Release|x64
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x86.ActiveCfg = Release|Win32
|
||||
{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x86.Build.0 = Release|Win32
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x64.Build.0 = Debug|x64
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x86.Build.0 = Debug|Win32
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Release|x64.ActiveCfg = Release|x64
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Release|x64.Build.0 = Release|x64
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Release|x86.ActiveCfg = Release|Win32
|
||||
{52260548-392E-4370-8760-D749F5BEBEE6}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
EndGlobalSection
|
||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||
SolutionGuid = {E5EA9E50-DE77-48F2-AF5D-555E9A23AA53}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
139
FalconLibraryCPP/FalconLibraryCPP.vcxproj
Normal file
139
FalconLibraryCPP/FalconLibraryCPP.vcxproj
Normal file
|
|
@ -0,0 +1,139 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<ProjectGuid>{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}</ProjectGuid>
|
||||
<RootNamespace>FalconLibraryCPP</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup />
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="src\FalconLibrary.h" />
|
||||
<ClInclude Include="src\Interpolatable.h" />
|
||||
<ClInclude Include="src\Pose2d.h" />
|
||||
<ClInclude Include="src\Pose2dWithCurvature.h" />
|
||||
<ClInclude Include="src\Rotation2d.h" />
|
||||
<ClInclude Include="src\Translation2d.h" />
|
||||
<ClInclude Include="src\Twist2d.h" />
|
||||
<ClInclude Include="src\Utilities.h" />
|
||||
<ClInclude Include="src\VaryInterpolatable.h" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
46
FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
Normal file
46
FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup>
|
||||
<Filter Include="Source Files">
|
||||
<UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
|
||||
<Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
|
||||
</Filter>
|
||||
<Filter Include="Header Files">
|
||||
<UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
|
||||
<Extensions>h;hh;hpp;hxx;hm;inl;inc;ipp;xsd</Extensions>
|
||||
</Filter>
|
||||
<Filter Include="Resource Files">
|
||||
<UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
|
||||
<Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
|
||||
</Filter>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="src\VaryInterpolatable.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Interpolatable.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Pose2d.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Pose2dWithCurvature.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Rotation2d.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Translation2d.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Twist2d.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\Utilities.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="src\FalconLibrary.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
6
FalconLibraryCPP/FalconLibraryCPP.vcxproj.user
Normal file
6
FalconLibraryCPP/FalconLibraryCPP.vcxproj.user
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<PropertyGroup>
|
||||
<ShowAllFiles>true</ShowAllFiles>
|
||||
</PropertyGroup>
|
||||
</Project>
|
||||
12
FalconLibraryCPP/src/FalconLibrary.h
Normal file
12
FalconLibraryCPP/src/FalconLibrary.h
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
#pragma once
|
||||
|
||||
#include "Interpolatable.h"
|
||||
#include "Pose2d.h"
|
||||
#include "Pose2dWithCurvature.h"
|
||||
#include "Rotation2d.h"
|
||||
#include "Translation2d.h"
|
||||
#include "Twist2d.h"
|
||||
#include "Utilities.h"
|
||||
#include "VaryInterpolatable.h"
|
||||
|
||||
namespace frc5190 {}
|
||||
17
FalconLibraryCPP/src/Interpolatable.h
Normal file
17
FalconLibraryCPP/src/Interpolatable.h
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
#pragma once
|
||||
|
||||
#include "Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
template <typename T>
|
||||
class Interpolatable {
|
||||
public:
|
||||
virtual ~Interpolatable() = default;
|
||||
virtual T Interpolate(const T& end_value, double t) = 0;
|
||||
|
||||
static constexpr double Lerp(const double start_value, const double end_value,
|
||||
const double t) {
|
||||
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
||||
}
|
||||
};
|
||||
} // namespace frc5190
|
||||
92
FalconLibraryCPP/src/Pose2d.h
Normal file
92
FalconLibraryCPP/src/Pose2d.h
Normal file
|
|
@ -0,0 +1,92 @@
|
|||
#pragma once
|
||||
|
||||
#include "VaryInterpolatable.h"
|
||||
#include "Translation2d.h"
|
||||
#include "Rotation2d.h"
|
||||
#include "Twist2d.h"
|
||||
#include "Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
class Pose2d final : public VaryInterpolatable<Pose2d> {
|
||||
public:
|
||||
// Constructors
|
||||
Pose2d() : translation_(Translation2d()), rotation_(Rotation2d()) {}
|
||||
|
||||
Pose2d(Translation2d translation, const Rotation2d rotation)
|
||||
: translation_(std::move(translation)), rotation_(rotation) {}
|
||||
|
||||
Pose2d(const double x, const double y, const Rotation2d rotation)
|
||||
: translation_(Translation2d(x, y)), rotation_(rotation) {}
|
||||
|
||||
// Overriden Methods
|
||||
double Distance(const Pose2d& other) override {
|
||||
return (-*this + other).AsTwist().Norm();
|
||||
}
|
||||
Pose2d Interpolate(const Pose2d& end_value, const double t) override {
|
||||
if (t <= 0) {
|
||||
return *this;
|
||||
}
|
||||
if (t >= 1) {
|
||||
return end_value;
|
||||
}
|
||||
const auto twist = (-*this + end_value).AsTwist();
|
||||
return *this + (twist * t).AsPose();
|
||||
}
|
||||
|
||||
// Operator Overloads
|
||||
Pose2d operator+(const Pose2d& other) const { return TransformBy(other); }
|
||||
Pose2d operator-(const Pose2d& other) const { return TransformBy(-other); }
|
||||
Pose2d operator-() const {
|
||||
const auto inverted_rotation = -rotation_;
|
||||
return Pose2d{(-translation_) * inverted_rotation, inverted_rotation};
|
||||
}
|
||||
|
||||
// Accessors
|
||||
const Translation2d& Translation() const { return translation_; }
|
||||
const Rotation2d& Rotation() const { return rotation_; }
|
||||
|
||||
Twist2d AsTwist() const {
|
||||
const auto dtheta = rotation_.Radians();
|
||||
const auto half_dtheta = dtheta / 2.0;
|
||||
const auto cos_minus_one = rotation_.Cos() - 1.0;
|
||||
|
||||
double half_theta_by_tan_of_half_dtheta;
|
||||
|
||||
if (std::abs(cos_minus_one) < kEpsilon) {
|
||||
half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
|
||||
} else {
|
||||
half_theta_by_tan_of_half_dtheta =
|
||||
-(half_dtheta * rotation_.Sin()) / cos_minus_one;
|
||||
}
|
||||
|
||||
const auto translation_part =
|
||||
translation_ *
|
||||
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
|
||||
|
||||
return Twist2d{translation_part.X(), translation_part.Y(),
|
||||
rotation_.Radians()};
|
||||
}
|
||||
|
||||
Pose2d Mirror() const {
|
||||
return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()},
|
||||
-rotation_};
|
||||
}
|
||||
|
||||
Pose2d TransformBy(const Pose2d& other) const {
|
||||
return Pose2d{translation_ + (other.translation_ * rotation_),
|
||||
rotation_ + other.rotation_};
|
||||
}
|
||||
|
||||
bool IsCollinear(const Pose2d& other) const {
|
||||
if (!rotation_.IsParallel(other.rotation_)) {
|
||||
return false;
|
||||
}
|
||||
const auto twist = (-(*this) + other).AsTwist();
|
||||
return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
|
||||
}
|
||||
|
||||
private:
|
||||
Translation2d translation_;
|
||||
Rotation2d rotation_;
|
||||
};
|
||||
} // namespace frc5190
|
||||
46
FalconLibraryCPP/src/Pose2dWithCurvature.h
Normal file
46
FalconLibraryCPP/src/Pose2dWithCurvature.h
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include "VaryInterpolatable.h"
|
||||
#include "Pose2d.h"
|
||||
#include "Interpolatable.h"
|
||||
|
||||
namespace frc5190 {
|
||||
class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
|
||||
public:
|
||||
// Constructors
|
||||
Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
|
||||
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
|
||||
|
||||
// Overriden Methods
|
||||
double Distance(const Pose2dWithCurvature& other) override {
|
||||
return pose_.Distance(other.pose_);
|
||||
}
|
||||
|
||||
Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value,
|
||||
double t) override {
|
||||
return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t),
|
||||
Lerp(curvature_, end_value.curvature_, t),
|
||||
Lerp(dkds_, end_value.dkds_, t)};
|
||||
}
|
||||
|
||||
// Operator Overloads
|
||||
Pose2dWithCurvature operator+(const Pose2d& other) const {
|
||||
return Pose2dWithCurvature{pose_ + other, curvature_, dkds_};
|
||||
}
|
||||
|
||||
// Accessors
|
||||
const Pose2d& Pose() const { return pose_; }
|
||||
double Curvature() const { return curvature_; }
|
||||
double Dkds() const { return dkds_; }
|
||||
|
||||
Pose2dWithCurvature Mirror() const {
|
||||
return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_};
|
||||
}
|
||||
|
||||
private:
|
||||
Pose2d pose_;
|
||||
double curvature_;
|
||||
double dkds_;
|
||||
};
|
||||
} // namespace frc5190
|
||||
61
FalconLibraryCPP/src/Rotation2d.h
Normal file
61
FalconLibraryCPP/src/Rotation2d.h
Normal file
|
|
@ -0,0 +1,61 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
class Rotation2d final {
|
||||
public:
|
||||
// Constructors
|
||||
Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {}
|
||||
explicit Rotation2d(const double value)
|
||||
: value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
|
||||
|
||||
Rotation2d(const double x, const double y, const bool normalize) {
|
||||
if (normalize) {
|
||||
const auto magnitude = std::hypot(x, y);
|
||||
if (magnitude > kEpsilon) {
|
||||
sin_ = y / magnitude;
|
||||
cos_ = x / magnitude;
|
||||
} else {
|
||||
sin_ = 0.0;
|
||||
cos_ = 1.0;
|
||||
}
|
||||
} else {
|
||||
cos_ = x;
|
||||
sin_ = y;
|
||||
}
|
||||
value_ = std::atan2(sin_, cos_);
|
||||
}
|
||||
|
||||
static Rotation2d FromDegrees(const double degrees) {
|
||||
return Rotation2d(Deg2Rad(degrees));
|
||||
}
|
||||
|
||||
// Operator Overloads
|
||||
Rotation2d operator-(const Rotation2d& other) const { return *this + -other; }
|
||||
Rotation2d operator-() const { return Rotation2d(-value_); }
|
||||
|
||||
Rotation2d operator+(const Rotation2d& other) const {
|
||||
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
|
||||
Cos() * other.Sin() + Sin() * other.Cos(), true};
|
||||
}
|
||||
|
||||
// Accessors
|
||||
double Radians() const { return value_; }
|
||||
double Degrees() const { return Rad2Deg(value_); }
|
||||
double Cos() const { return cos_; }
|
||||
double Sin() const { return sin_; }
|
||||
double Tan() const { return sin_ / cos_; }
|
||||
|
||||
bool IsParallel(const Rotation2d& other) const {
|
||||
return EpsilonEquals((*this - other).Radians(), 0.0);
|
||||
}
|
||||
|
||||
private:
|
||||
double value_;
|
||||
double cos_;
|
||||
double sin_;
|
||||
};
|
||||
} // namespace frc5190
|
||||
69
FalconLibraryCPP/src/Translation2d.h
Normal file
69
FalconLibraryCPP/src/Translation2d.h
Normal file
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Interpolatable.h"
|
||||
#include "VaryInterpolatable.h"
|
||||
#include "Rotation2d.h"
|
||||
|
||||
namespace frc5190 {
|
||||
|
||||
class Translation2d final : public VaryInterpolatable<Translation2d> {
|
||||
public:
|
||||
// Constructors
|
||||
Translation2d() : x_(0.0), y_(0.0) {}
|
||||
Translation2d(const double x, const double y) : x_(x), y_(y) {}
|
||||
Translation2d(const double distance, const Rotation2d& rotation)
|
||||
: x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {}
|
||||
|
||||
// Overriden Methods
|
||||
double Distance(const Translation2d& other) override {
|
||||
return std::hypot(other.X() - X(), other.Y() - Y());
|
||||
}
|
||||
|
||||
Translation2d Interpolate(const Translation2d& end_value,
|
||||
const double t) override {
|
||||
if (t <= 0) {
|
||||
return *this;
|
||||
}
|
||||
if (t >= 1) {
|
||||
return end_value;
|
||||
}
|
||||
return Translation2d{Lerp(X(), end_value.X(), t),
|
||||
Lerp(Y(), end_value.Y(), t)};
|
||||
}
|
||||
|
||||
// Operator Overloads
|
||||
Translation2d operator+(const Translation2d& other) const {
|
||||
return Translation2d{X() + other.X(), Y() + other.Y()};
|
||||
}
|
||||
|
||||
Translation2d operator-(const Translation2d& other) const {
|
||||
return Translation2d{X() - other.X(), Y() - other.Y()};
|
||||
}
|
||||
|
||||
Translation2d operator*(const double scalar) const {
|
||||
return Translation2d{X() * scalar, Y() * scalar};
|
||||
}
|
||||
|
||||
Translation2d operator*(const Rotation2d& rotation) const {
|
||||
return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
|
||||
x_ * rotation.Sin() + y_ * rotation.Cos()};
|
||||
}
|
||||
|
||||
Translation2d
|
||||
operator/(const double scalar) const {
|
||||
return Translation2d{X() / scalar, Y() / scalar};
|
||||
}
|
||||
|
||||
Translation2d operator-() const { return Translation2d{-X(), -Y()}; }
|
||||
|
||||
// Accessors
|
||||
double X() const { return x_; }
|
||||
double Y() const { return y_; }
|
||||
|
||||
private:
|
||||
double x_;
|
||||
double y_;
|
||||
};
|
||||
} // namespace frc5190
|
||||
53
FalconLibraryCPP/src/Twist2d.h
Normal file
53
FalconLibraryCPP/src/Twist2d.h
Normal file
|
|
@ -0,0 +1,53 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Pose2d.h"
|
||||
#include "Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
class Twist2d {
|
||||
public:
|
||||
// Constructors
|
||||
Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
|
||||
Twist2d(const double dx, const double dy, const double dtheta)
|
||||
: dx_(dx), dy_(dy), dtheta_(dtheta) {}
|
||||
|
||||
// Operator Overloads
|
||||
Twist2d operator*(const double scalar) const {
|
||||
return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
|
||||
}
|
||||
|
||||
// Accessors
|
||||
double Dx() const { return dx_; }
|
||||
double Dy() const { return dy_; }
|
||||
double Dtheta() const { return dtheta_; }
|
||||
|
||||
double Norm() const {
|
||||
if (dy_ == 0.0) return std::abs(dx_);
|
||||
return std::hypot(dx_, dy_);
|
||||
}
|
||||
|
||||
Pose2d AsPose() const {
|
||||
const auto sin_theta = std::sin(dtheta_);
|
||||
const auto cos_theta = std::cos(dtheta_);
|
||||
|
||||
double s, c;
|
||||
if (std::abs(dtheta_) < kEpsilon) {
|
||||
s = 1.0 - 1.0 / 6.0 * dtheta_ * dtheta_;
|
||||
c = 0.5 * dtheta_;
|
||||
} else {
|
||||
s = sin_theta / dtheta_;
|
||||
c = (1 - cos_theta) / dtheta_;
|
||||
}
|
||||
|
||||
return Pose2d{Translation2d{dx_ * s - dy_ * c, dx_ * c + dy_ * s},
|
||||
Rotation2d{cos_theta, sin_theta, false}};
|
||||
}
|
||||
|
||||
private:
|
||||
double dx_;
|
||||
double dy_;
|
||||
double dtheta_;
|
||||
};
|
||||
} // namespace frc5190
|
||||
30
FalconLibraryCPP/src/Utilities.h
Normal file
30
FalconLibraryCPP/src/Utilities.h
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace frc5190 {
|
||||
|
||||
constexpr double kEpsilon = 1E-9;
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
template <typename T>
|
||||
T Clamp(const T& n, const T& lower, const T& upper) {
|
||||
return std::max(lower, std::min(n, upper));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr T Rad2Deg(const T& rad) {
|
||||
return rad * 180.0 / kPi;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr T Deg2Rad(const T& deg) {
|
||||
return deg * kPi / 180.0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool EpsilonEquals(const T& a, const T& b) {
|
||||
return std::abs(a - b) < kEpsilon;
|
||||
}
|
||||
|
||||
} // namespace frc5190
|
||||
11
FalconLibraryCPP/src/VaryInterpolatable.h
Normal file
11
FalconLibraryCPP/src/VaryInterpolatable.h
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "Interpolatable.h"
|
||||
|
||||
namespace frc5190 {
|
||||
template <typename T>
|
||||
class VaryInterpolatable : public Interpolatable<T> {
|
||||
public:
|
||||
virtual double Distance(const T& other) = 0;
|
||||
};
|
||||
} // namespace frc5190
|
||||
128
Tests/Tests.vcxproj
Normal file
128
Tests/Tests.vcxproj
Normal file
|
|
@ -0,0 +1,128 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<ProjectGuid>{52260548-392e-4370-8760-d749f5bebee6}</ProjectGuid>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<WindowsTargetPlatformVersion>10.0.17763.0</WindowsTargetPlatformVersion>
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings" />
|
||||
<ImportGroup Label="Shared" />
|
||||
<ImportGroup Label="PropertySheets" />
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<ItemGroup>
|
||||
<ClInclude Include="pch.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="geometry-tests.cpp" />
|
||||
<ClCompile Include="pch.cpp">
|
||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">Create</PrecompiledHeader>
|
||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">Create</PrecompiledHeader>
|
||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
|
||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\FalconLibraryCPP\FalconLibraryCPP.vcxproj">
|
||||
<Project>{9f48b50f-67e0-4f56-99b6-586ea5fffeba}</Project>
|
||||
</ProjectReference>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<None Include="packages.config" />
|
||||
</ItemGroup>
|
||||
<ItemDefinitionGroup />
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
<Import Project="..\packages\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.1.8.1\build\native\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.targets" Condition="Exists('..\packages\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.1.8.1\build\native\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.targets')" />
|
||||
</ImportGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
|
||||
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>X64;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
|
||||
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<PreprocessorDefinitions>X64;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Target Name="EnsureNuGetPackageBuildImports" BeforeTargets="PrepareForBuild">
|
||||
<PropertyGroup>
|
||||
<ErrorText>This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}.</ErrorText>
|
||||
</PropertyGroup>
|
||||
<Error Condition="!Exists('..\packages\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.1.8.1\build\native\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.targets')" Text="$([System.String]::Format('$(ErrorText)', '..\packages\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.1.8.1\build\native\Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn.targets'))" />
|
||||
</Target>
|
||||
</Project>
|
||||
4
Tests/Tests.vcxproj.user
Normal file
4
Tests/Tests.vcxproj.user
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<PropertyGroup />
|
||||
</Project>
|
||||
22
Tests/geometry-tests.cpp
Normal file
22
Tests/geometry-tests.cpp
Normal file
|
|
@ -0,0 +1,22 @@
|
|||
#include "pch.h"
|
||||
#include "../FalconLibraryCPP/src/Translation2d.h"
|
||||
#include "../FalconLibraryCPP/src/Rotation2d.h"
|
||||
|
||||
constexpr double kTestEpsilon = 1E-9;
|
||||
|
||||
TEST(TestRotation2d, TestRotation2d) {
|
||||
auto rot = frc5190::Rotation2d();
|
||||
EXPECT_EQ(1.0, rot.Cos());
|
||||
EXPECT_EQ(0.0, rot.Sin());
|
||||
EXPECT_EQ(0.0, rot.Tan());
|
||||
EXPECT_EQ(0.0, rot.Radians());
|
||||
EXPECT_EQ(0.0, rot.Degrees());
|
||||
|
||||
rot = frc5190::Rotation2d(1, 1, true);
|
||||
EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon);
|
||||
EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon);
|
||||
EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon);
|
||||
EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
|
||||
EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon);
|
||||
|
||||
}
|
||||
4
Tests/packages.config
Normal file
4
Tests/packages.config
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<packages>
|
||||
<package id="Microsoft.googletest.v140.windesktop.msvcstl.static.rt-dyn" version="1.8.1" targetFramework="native" />
|
||||
</packages>
|
||||
6
Tests/pch.cpp
Normal file
6
Tests/pch.cpp
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
//
|
||||
// pch.cpp
|
||||
// Include the standard header and generate the precompiled header.
|
||||
//
|
||||
|
||||
#include "pch.h"
|
||||
8
Tests/pch.h
Normal file
8
Tests/pch.h
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
//
|
||||
// pch.h
|
||||
// Header for standard system include files.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
21
Tests/test.cpp
Normal file
21
Tests/test.cpp
Normal file
|
|
@ -0,0 +1,21 @@
|
|||
#include "pch.h"
|
||||
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||||
|
||||
constexpr double kTestEpsilon = 1E-9;
|
||||
|
||||
TEST(TestRotation2d, TestRotation2d) {
|
||||
auto rot = frc5190::Rotation2d();
|
||||
EXPECT_EQ(1.0, rot.Cos());
|
||||
EXPECT_EQ(0.0, rot.Sin());
|
||||
EXPECT_EQ(0.0, rot.Tan());
|
||||
EXPECT_EQ(0.0, rot.Radians());
|
||||
EXPECT_EQ(0.0, rot.Degrees());
|
||||
|
||||
rot = frc5190::Rotation2d(1, 1, true);
|
||||
EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon);
|
||||
EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon);
|
||||
EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon);
|
||||
EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
|
||||
EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon);
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user