public void TestInequalityOperator()
{
var one = new Rotation2d(Angle.FromDegrees(42));
var two = new Rotation2d(Angle.FromDegrees(42.5));
Assert.False(one == two);
}
public void TestMinus()
{
var one = new Rotation2d(Angle.FromDegrees(70));
var two = new Rotation2d(Angle.FromDegrees(30));
AssertEqualAngles(Angle.FromDegrees(40), (one - two).Angle);
}
public void TestInequality()
{
var one = new Rotation2d(Angle.FromDegrees(42));
var two = new Rotation2d(Angle.FromDegrees(42.5));
Assert.False(one.Equals(two));
}
public void TestRotateByFromZero()
{
var zero = new Rotation2d(Angle.FromDegrees(0));
var sum = zero + new Rotation2d(Angle.FromDegrees(90));
AssertEqualAngles(Angle.FromDegrees(90), sum.Angle);
}
public void TestRotateByNonZero()
{
var rot = new Rotation2d(Angle.FromDegrees(90));
rot += new Rotation2d(Angle.FromDegrees(30));
AssertEqualAngles(Angle.FromDegrees(120), rot.Angle);
}
public void TestRadiansToDegrees()
{
var one = new Rotation2d(Angle.FromRadians(Math.PI) / 3);
var two = new Rotation2d(Angle.FromRadians(Math.PI) / 4);
AssertEqualAngles(Angle.FromDegrees(60), one.Angle);
AssertEqualAngles(Angle.FromDegrees(45), two.Angle);
}
public void TestDegreesToRadians()
{
var one = new Rotation2d(Angle.FromDegrees(45));
var two = new Rotation2d(Angle.FromDegrees(30));
AssertEqualAngles(Angle.FromDegrees(45), one.Angle);
AssertEqualAngles(Angle.FromDegrees(30), two.Angle);
}
public void CanTurnByDegrees(float r1, float degrees, float r2)
{
var rot = Rotation2d.FromDegrees(r1);
rot.TurnDegrees(degrees);
DolphAssert.EqualF(r2, rot.Degrees);
}
public void CanTurnByRadians(float r1, float radians, float r2)
{
var rot = new Rotation2d(r1);
rot.Turn(radians);
DolphAssert.EqualF(r2, rot.Radians);
}
public void CanTurnByRotation()
{
var rot1 = Rotation2d.FromDegrees(45);
var rot2 = Rotation2d.FromDegrees(90);
rot1.Turn(rot2);
DolphAssert.EqualF(135, rot1.Degrees);
}
public override void Draw(Entity entity)
{
var sprite = entity.GetComponent <SpriteComponent>();
if (!TryGetAnimatedSprite(sprite, out Rect2d src) && !TryGetStaticSprite(sprite, out src))
{
// Can't determine a valid sprite to draw
return;
}
Rect2d dest = entity.Space;
// Get the origin before any transformations are applied
Vector2d origin = (dest.GetOriginPosition() - dest.TopLeft).ToVector();
Rotation2d rotation = sprite.Rotation;
if (sprite.RotationAnimation != null)
{
rotation.Turn(sprite.RotationAnimation.GetFrame(this.Timer.Total));
}
dest.Shift(sprite.Offset);
if (sprite.OffsetAnimation != null)
{
dest.Shift(sprite.OffsetAnimation.GetFrame(this.Timer.Total));
}
dest.Scale(sprite.Scale);
if (sprite.ScaleAnimation != null)
{
var animScale = sprite.ScaleAnimation.GetFrame(this.Timer.Total);
dest.Scale(animScale);
}
entity.SetDirective <SpriteDirective>("simple-sprite", sd =>
{
sd.Asset = sprite.SpriteSheet.Name;
sd.Source = src;
sd.Destination = dest.GetOriginPosition();
sd.Size = dest.GetSize();
sd.Rotation = rotation.Radians;
sd.Origin = origin;
});
if (sprite.EnableBoxOutline)
{
entity.SetDirective <PolygonDirective>("simple-sprite-box-outline", pd =>
{
pd.Color = new ColorRGBA(255, 255, 0);
pd.Points = dest.ToPolygon().Points;
});
}
}
public void TestHashCode()
{
var rot1 = new Rotation2d(5);
var rot2 = new Rotation2d(5);
Assert.Equal(rot1.GetHashCode(), rot2.GetHashCode());
rot2.Turn(0.1f);
Assert.NotEqual(rot1.GetHashCode(), rot2.GetHashCode());
}
public void TestEquality(float radians)
{
var rot1 = new Rotation2d(radians);
var rot2 = new Rotation2d(radians);
Assert.True(rot1 == rot2);
Assert.True(rot1.Equals(rot2));
rot1.Radians += Constants.FloatTolerance / 10;
Assert.True(rot1 == rot2);
Assert.True(rot1.Equals(rot2));
rot1.Radians += Constants.FloatTolerance * 10;
Assert.False(rot1 == rot2);
Assert.False(rot1.Equals(rot2));
}
private static void getSegmentArc(Spline s, ref List <Pose2dWithCurvature> rv, double t0, double t1, double maxDx,
double maxDy, double maxDTheta)
{
Translation2d p0 = s.getPoint(t0);
Translation2d p1 = s.getPoint(t1);
Rotation2d r0 = s.getHeading(t0);
Rotation2d r1 = s.getHeading(t1);
Pose2d transformation = new Pose2d(new Translation2d(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse()));
Twist2d twist = Pose2d.log(transformation);
if (twist.dy > maxDy || twist.dx > maxDx || twist.dtheta > maxDTheta)
{
getSegmentArc(s, ref rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta);
getSegmentArc(s, ref rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta);
}
else
{
rv.Add(s.getPose2dWithCurvature(t1));
}
}
public Form1()
{
InitializeComponent();
//Create a list of waypoints
List <Pose2d> waypoints = new List <Pose2d>(3);
waypoints.Add(new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)));
waypoints.Add(new Pose2d(new Translation2d(100, 20), Rotation2d.fromDegrees(0)));
waypoints.Add(new Pose2d(new Translation2d(254.8, 50.87), Rotation2d.fromDegrees(45)));
// For a reversed trajectory
List <Pose2d> waypoints_maybe_flipped = waypoints;
Pose2d flip = Pose2d.fromRotation(new Rotation2d(-1, 0, false));
if (false)
{
waypoints_maybe_flipped = new List <Pose2d>(waypoints.Count);
for (int i = 0; i < waypoints.Count; ++i)
{
waypoints_maybe_flipped.Add(waypoints[i].transformBy(flip));
}
}
//Create a list of splines between each pair of waypoints
List <QuinticSpline> splines = new List <QuinticSpline>(waypoints.Count - 1);
for (int i = 1; i < waypoints.Count; ++i)
{
splines.Add(new QuinticSpline(waypoints[i - 1], waypoints[i]));
}
//Doesnt do much for simple curves
//Optimize spline based on curvature
QuinticSpline.optimizeSpline(ref splines);
Console.WriteLine("Spline Unsegmented");
foreach (QuinticSpline S in splines)
{
for (double t = 0; t <= 1; t += 1 / 100.0)
{
Console.WriteLine(S.getPoint(t).x() + " , " + S.getPoint(t).y() + " , " + S.getHeading(t).getDegrees());
}
}
Console.WriteLine("Spline Combined and Segmented");
//Create the untimed trajectory (Splines into segment generator)
UntimedTrajectory trajectory = new UntimedTrajectory(SegmentedSplineGenerator.parameterizeSplines(splines));
for (int i = 0; i < trajectory.length(); i++)
{
double x = trajectory.getState(i).getTranslation().x();
double y = trajectory.getState(i).getTranslation().y();
double theta = trajectory.getState(i).getRotation().getDegrees();
double curvature = trajectory.getState(i).getCurvature();
double dCurvatureDs = trajectory.getState(i).getDCurvatureDs();
Console.WriteLine(i + " , " + x + " , " + y + " , " + theta + " , " + curvature + " , " + dCurvatureDs);
}
//For a reversed trajectory
if (false)
{
List <Pose2dWithCurvature> flipped = new List <Pose2dWithCurvature>(trajectory.length());
for (int i = 0; i < trajectory.length(); ++i)
{
flipped.Add(new Pose2dWithCurvature(trajectory.getState(i).getPose().transformBy(flip),
-trajectory.getState(i).getCurvature(), trajectory.getState(i).getDCurvatureDs()));
}
trajectory = new UntimedTrajectory(flipped);
}
Console.WriteLine("Trajectory");
TrajectoryContainer final_trajectory = TrajectoryGenerator.parameterizeTrajectory(false, trajectory, 2.0, 0.0, 0.0, 120.0, 120.0, 24.0, 20);
final_trajectory.setDefaultVelocity(72.0 / 150.0);
for (int i = 0; i < final_trajectory.length(); i++)
{
double x = final_trajectory.getState(i).get_state().getTranslation().x();
double y = final_trajectory.getState(i).get_state().getTranslation().y();
double position = Math.Sqrt(x * x + y * y);
double theta = final_trajectory.getState(i).get_state().getRotation().getDegrees();
double time = final_trajectory.getState(i).get_t();
double velocity = final_trajectory.getState(i).get_velocity();
double accel = final_trajectory.getState(i).get_acceleration();
Console.WriteLine(time + " , " + position + " , " + velocity + " , " + accel);
}
}
public void CanGetFromDegrees(float degrees)
{
var rot = Rotation2d.FromDegrees(degrees);
DolphAssert.EqualF(degrees, rot.Degrees);
}
