public static Line ProjectionLine(Puck p, Line reference)
{
PointD ij = reference.IntersectionWith(reference.Perpendicular(p.Location));
double dist = p.Location.DistanceTo(ij);
double scale = p.Radius / dist;
PointD projection_line_point = ij.Offset((p.Location.X - ij.X) * scale, (p.Location.Y - ij.Y) * scale);
return(reference.Parallel(projection_line_point));
}
public static InformationPacket Update(Puck p, Puck c, Table table, double delta_time)
{
double offset = p.Velocity.Speed * delta_time;
double x = offset * Math.Cos(p.Velocity.Direction);
double y = offset * Math.Sin(p.Velocity.Direction);
PointD ghost_puck_location = p.Location.Offset(x, y);
//Collect debugging information as we loop through the collision algorithm
InformationPacket info = new InformationPacket();
info.Points.Add(ghost_puck_location);
//Collision correction
CollisionData data;
while ((data = GetClosestCollisionData(p, offset, ghost_puck_location, c, table)) != null)
{
p.Location = data.collision_point;
p.Velocity.Direction = p.Velocity.Direction.Reflect(data.projection_line.Perpendicular(data.collision_point).Angle) + Angle._180;
PointD translation_reflection_point = data.projection_line.IntersectionWith(data.projection_line.Perpendicular(ghost_puck_location));
ghost_puck_location = translation_reflection_point.Offset(translation_reflection_point.X - ghost_puck_location.X, translation_reflection_point.Y - ghost_puck_location.Y);
offset = ghost_puck_location.DistanceTo(p.Location);
if (energy_loss_enabled)
{
p.Velocity.Speed *= .8;
}
info.Points.Add(ghost_puck_location);
}
//Shazam (physics calculations are complete)
p.Location = ghost_puck_location;
if (energy_loss_enabled)
{
p.Velocity.Speed -= delta_time * table.Friction;
if (p.Velocity.Speed < 0)
{
p.Velocity.Speed = 0;
}
}
return(info);
}
public static CollisionData GetClosestCollisionData(Puck p, double offset, PointD ghost_puck_location, Puck c, Table table)
{
List<Line> collision_lines = new List<Line>();
collision_lines.AddRange(table.Walls);
for (int j = 0; j < table.Arcs.Count; ++j)
{
PointD[] center_point_collisions = p.Path.IntersectionWith(table.Arcs[j]);
if (center_point_collisions != null)
{
for (int i = 0; i < center_point_collisions.Length; ++i)
{
if (center_point_collisions[i] != null)
{
Line line_thru_center = new Line(table.Arcs[j].Point, (double?)((center_point_collisions[i].Y - table.Arcs[j].Point.Y) / (center_point_collisions[i].X - table.Arcs[j].Point.X)));
Line projection_line = line_thru_center.Perpendicular(center_point_collisions[i]);
collision_lines.Add(projection_line);
}
}
}
}
List<CollisionData> potential_collision_points = new List<CollisionData>();
for (int j = 0; j < collision_lines.Count; ++j) {
PointD center_point_collision = p.Path.IntersectionWith(collision_lines[j]);
if (center_point_collision != null) {
double sc = p.Location.DistanceTo(center_point_collision);
double fc = ghost_puck_location.DistanceTo(center_point_collision);
if (!(fc > sc && fc > offset)) {
Line projection_line = ProjectionLine(p, collision_lines[j]);
PointD projection_collision = p.Path.IntersectionWith(projection_line);
if (offset > projection_collision.DistanceTo(p.Location)) {
potential_collision_points.Add(new CollisionData(projection_collision, projection_line));
}
}
}
}
//TODO: check for collision with computer puck, add result to potential collision points right here
//for (int j = 0; j < table.Arcs.Count; ++j)
//{
// Arc projection_arc = p.Location.DistanceTo(table.Arcs[j].Point) < table.Arcs[j].Radius
// ? new Arc(table.Arcs[j].Point, table.Arcs[j].Radius - p.Radius)
// : new Arc(table.Arcs[j].Point, table.Arcs[j].Radius + p.Radius);
//
// PointD[] center_point_collisions = p.Path.IntersectionWith(table.Arcs[j]);
// if (center_point_collisions != null)
// {
// for (int i = 0; i < center_point_collisions.Length; ++i)
// {
// if (center_point_collisions[i] != null)
// {
// double sc = p.Location.DistanceTo(center_point_collisions[i]);
// double fc = ghost_puck_location.DistanceTo(center_point_collisions[i]);
// if (!(fc > sc && fc > offset))
// {
// PointD[] projection_collisions = p.Path.IntersectionWith(projection_arc);
// foreach (var projection_collision in projection_collisions)
// {
// Line line_thru_center = new Line(table.Arcs[j].Point, (double?)((projection_collision.Y - projection_arc.Point.Y) / (projection_collision.X - projection_arc.Point.X)));
// Line projection_line = line_thru_center.Perpendicular(projection_collision);
// if (offset > projection_collision.DistanceTo(p.Location)) {
// potential_collision_points.Add(new CollisionData(projection_collision, projection_line));
// }
// }
// }
// }
// }
// }
//}
potential_collision_points = potential_collision_points.OrderBy(z => z.collision_point.DistanceTo(p.Location)).ToList();
return potential_collision_points.Count > 0 ? potential_collision_points[0] : null;
}
public static InformationPacket Update(Puck p, Puck c, Table table, double delta_time)
{
double offset = p.Velocity.Speed * delta_time;
double x = offset * Math.Cos(p.Velocity.Direction);
double y = offset * Math.Sin(p.Velocity.Direction);
PointD ghost_puck_location = p.Location.Offset(x, y);
//Collect debugging information as we loop through the collision algorithm
InformationPacket info = new InformationPacket();
info.Points.Add(ghost_puck_location);
//Collision correction
CollisionData data;
while ((data = GetClosestCollisionData(p, offset, ghost_puck_location, c, table)) != null) {
p.Location = data.collision_point;
p.Velocity.Direction = p.Velocity.Direction.Reflect(data.projection_line.Perpendicular(data.collision_point).Angle) + Angle._180;
PointD translation_reflection_point = data.projection_line.IntersectionWith(data.projection_line.Perpendicular(ghost_puck_location));
ghost_puck_location = translation_reflection_point.Offset(translation_reflection_point.X - ghost_puck_location.X, translation_reflection_point.Y - ghost_puck_location.Y);
offset = ghost_puck_location.DistanceTo(p.Location);
if (energy_loss_enabled)
p.Velocity.Speed *= .8;
info.Points.Add(ghost_puck_location);
}
//Shazam (physics calculations are complete)
p.Location = ghost_puck_location;
if (energy_loss_enabled) {
p.Velocity.Speed -= delta_time * table.Friction;
if (p.Velocity.Speed < 0) { p.Velocity.Speed = 0; }
}
return info;
}
public static Line ProjectionLine(Puck p, Line reference)
{
PointD ij = reference.IntersectionWith(reference.Perpendicular(p.Location));
double dist = p.Location.DistanceTo(ij);
double scale = p.Radius / dist;
PointD projection_line_point = ij.Offset((p.Location.X - ij.X) * scale, (p.Location.Y - ij.Y) * scale);
return reference.Parallel(projection_line_point);
}
public static CollisionData GetClosestCollisionData(Puck p, double offset, PointD ghost_puck_location, Puck c, Table table)
{
List <Line> collision_lines = new List <Line>();
collision_lines.AddRange(table.Walls);
for (int j = 0; j < table.Arcs.Count; ++j)
{
PointD[] center_point_collisions = p.Path.IntersectionWith(table.Arcs[j]);
if (center_point_collisions != null)
{
for (int i = 0; i < center_point_collisions.Length; ++i)
{
if (center_point_collisions[i] != null)
{
Line line_thru_center = new Line(table.Arcs[j].Point, (double?)((center_point_collisions[i].Y - table.Arcs[j].Point.Y) / (center_point_collisions[i].X - table.Arcs[j].Point.X)));
Line projection_line = line_thru_center.Perpendicular(center_point_collisions[i]);
collision_lines.Add(projection_line);
}
}
}
}
List <CollisionData> potential_collision_points = new List <CollisionData>();
for (int j = 0; j < collision_lines.Count; ++j)
{
PointD center_point_collision = p.Path.IntersectionWith(collision_lines[j]);
if (center_point_collision != null)
{
double sc = p.Location.DistanceTo(center_point_collision);
double fc = ghost_puck_location.DistanceTo(center_point_collision);
if (!(fc > sc && fc > offset))
{
Line projection_line = ProjectionLine(p, collision_lines[j]);
PointD projection_collision = p.Path.IntersectionWith(projection_line);
if (offset > projection_collision.DistanceTo(p.Location))
{
potential_collision_points.Add(new CollisionData(projection_collision, projection_line));
}
}
}
}
//TODO: check for collision with computer puck, add result to potential collision points right here
//for (int j = 0; j < table.Arcs.Count; ++j)
//{
// Arc projection_arc = p.Location.DistanceTo(table.Arcs[j].Point) < table.Arcs[j].Radius
// ? new Arc(table.Arcs[j].Point, table.Arcs[j].Radius - p.Radius)
// : new Arc(table.Arcs[j].Point, table.Arcs[j].Radius + p.Radius);
//
// PointD[] center_point_collisions = p.Path.IntersectionWith(table.Arcs[j]);
// if (center_point_collisions != null)
// {
// for (int i = 0; i < center_point_collisions.Length; ++i)
// {
// if (center_point_collisions[i] != null)
// {
// double sc = p.Location.DistanceTo(center_point_collisions[i]);
// double fc = ghost_puck_location.DistanceTo(center_point_collisions[i]);
// if (!(fc > sc && fc > offset))
// {
// PointD[] projection_collisions = p.Path.IntersectionWith(projection_arc);
// foreach (var projection_collision in projection_collisions)
// {
// Line line_thru_center = new Line(table.Arcs[j].Point, (double?)((projection_collision.Y - projection_arc.Point.Y) / (projection_collision.X - projection_arc.Point.X)));
// Line projection_line = line_thru_center.Perpendicular(projection_collision);
// if (offset > projection_collision.DistanceTo(p.Location)) {
// potential_collision_points.Add(new CollisionData(projection_collision, projection_line));
// }
// }
// }
// }
// }
// }
//}
potential_collision_points = potential_collision_points.OrderBy(z => z.collision_point.DistanceTo(p.Location)).ToList();
return(potential_collision_points.Count > 0 ? potential_collision_points[0] : null);
}
private RectangleF getPuckRectangle(Puck p)
{
return new RectangleF(s(p.Location.X - p.Radius), s(p.Location.Y - p.Radius), s(p.Radius * 2), s(p.Radius * 2));
}
public void Init()
{
Table = new Table();
Robot = new Arm();
Puck = new Puck();
#region Physical Defaults
Table.Height = 1.987; //198.7 cm, or 8 feet
Table.Width = 0.965;
Line Left_Side = new Line(new PointD(0, 0), new Angle(90, AngleType.Degrees));
Line Right_Side = new Line(new PointD(Table.Width, 0), new Angle(90, AngleType.Degrees));
Line Light_Side = new Line(new PointD(0, Table.Height), new Angle(0.001, AngleType.Degrees));
Line Dark_Side = new Line(new PointD(0, 0), new Angle(0.001, AngleType.Degrees));
Table.Walls.Add(Left_Side); Table.Walls.Add(Right_Side); Table.Walls.Add(Light_Side); Table.Walls.Add(Dark_Side);
Table.Friction = 0.000250 / 1000;
Table.Goal_Width = .26; //26 cm
Arc arc_test = new Arc(new PointD(Table.Width, Table.Height / 2), 0.4175);
Table.Arcs.Add(arc_test);
Arc arc_test2 = new Arc(new PointD(0, Table.Height / 2), Table.Width);
Table.Arcs.Add(arc_test2);
Robot.Arm_Length = .3; //30 cm
Robot.Arm_Angle = new Angle(90, AngleType.Degrees);
Robot.Forearm_Length = .3;
Robot.Forearm_Angle = new Angle(90, AngleType.Degrees);
Puck.Weight = 18; //18 grams
Puck.Radius = 0.03175; //The diameter of a standard hockey puck is 6.35 cm
Puck.Location = new PointD(Table.Width / 2, Table.Height / 2);
Puck.Velocity.Direction = new Angle(((double)(new Random()).Next(0, 36000) / 100.0), AngleType.Degrees);
//Puck.Velocity.Speed = 10.30 / 1000;
#endregion
//Timer
Updated = OnUpdated;
tm.Interval = (1000 * 1000) / 60;
tm.MicroTimerElapsed += Tm_MicroTimerElapsed;
tm.Start();
}
