public static bool TryCollision(Particle p, Line line)
{
Vector L = p.Position - line.Point1;
Vector u = line.Point2 - line.Point1; u.Normalize();
double d = Math.Abs(L.OrthogonalDistance(u));
if (d > p.Radius)
{
return(false);
}
Vector v = p.Velocity;
Vector x = (v * u) * u;
Vector n = u.Orthogonal(); n.Normalize();
if (Vector.Direction(line.Point1, line.Point2, p.Position) < 0)
{
n = -n;
}
Vector y = (v * n) * n;
double e = Math.Min(p.Restitution, line.Restitution);
p.Velocity -= (2 * y) * e;
Vector G = p.Mass * new Vector(0, -World.Gravity);
//p.Force += G;
//Vector F = (G * u) * u;
Vector N = (G * n) * n;
//p.Force += N;
//? czy jest ok ?
if ((n ^ p.Velocity) > 0)
{
u = -u;
}
Vector T = 10000.9 * (N * u) * u;
//p.Force += T;
p.Position += (p.Radius - d) * -n;
return(true);
}
public override void Render(Graphics g)
{
if (RenderOn)
{
g.DrawEllipse(Pen, (float)(Position.X - Radius), (float)(Position.Y - Radius), (float)(2 * Radius), (float)(2 * Radius));
Vector n = new Vector(Velocity.X, Velocity.Y); n.Normalize(); n *= Radius;
//g.DrawLine(Pen, (int)Position.X, (int)Position.Y, (int)n.X + (int)Position.X, (int)n.Y + (int)Position.Y);
}
if (RenderStatOn)
{
string text = string.Format("M={0:F1} V={1:F1} E={2:F1}", Mass, Velocity.Length, Restitution);
g.DrawString(text, Font, Brushes.Crimson, (float)Position.X, (float)Position.Y);
}
}
public override void Render(Graphics g)
{
if (RenderOn)
{
Vector u = Point1 - Point2;
u.Normalize();
Vector p1 = Point1 - 1000 * u;
Vector p2 = Point2 + 1000 * u;
g.DrawLine(Pen, (int)p1.X, (int)p1.Y, (int)p2.X, (int)p2.Y);
}
if (RenderStatOn)
{
Vector mid = Middle();
string text = string.Format("E={0:F1}", Restitution);
g.DrawString(text, Font, Brushes.Crimson, (float)mid.X, (float)mid.Y);
}
}
public static bool TryCollision(Particle p1, Particle p2)
{
if ((p1.Position - p2.Position).Length > (p1.Radius + p2.Radius))
{
return(false);
}
Vector n = (p1.Position - p2.Position);
double d = (p1.Radius + p2.Radius) - n.Length;
n.Normalize();
Vector vr = p1.Velocity - p2.Velocity;
//double m1 = p1.Mass, m2 = p2.Mass;
//if (!p1.SimulateOn) p1.Mass = double.PositiveInfinity;
//if (!p2.SimulateOn) p2.Mass = double.PositiveInfinity;
double e = Math.Min(p1.Restitution, p2.Restitution);
double j = (-vr * n) * (e + 1) / (1 / p1.Mass + 1 / p2.Mass);
p1.Velocity += (j * n) / p1.Mass;
p2.Velocity -= (j * n) / p2.Mass;
if (d > 0)
{
if (p1.SimulateOn)
{
p1.Position += (d / 2) * n;
}
if (p2.SimulateOn)
{
p2.Position -= (d / 2) * n;
}
}
//if (!p1.SimulateOn) p1.Mass = m1;
//if (!p2.SimulateOn) p2.Mass = m2;
//Console.Beep(200, 30);
return(true);
}
public virtual void Apply()
{
double ks = SpringConstant; double kt = DampingConstant;
Vector Lv = Particle1.Position - Particle2.Position;
double L = Lv.Length; double r = RestLength;
Vector v1 = Particle1.Velocity; Vector v2 = Particle2.Velocity;
Vector F1 = -(ks * (L - r) + kt * ((v1 - v2) * Lv) / L) * Lv / L;
Particle1.Force += F1;
Particle2.Force -= F1;
if (Segment != null)
{
Vector u = Particle1.Position - Particle2.Position;
u.Normalize();
Segment.Point1 = Particle1.Position - u * (Particle1.Radius + D);
Segment.Point2 = Particle2.Position + u * (Particle2.Radius + D);
Segment.Particle1.Position = Particle1.Position - u * (Particle1.Radius + D);
Segment.Particle2.Position = Particle2.Position + u * (Particle2.Radius + D);
}
}
public static void Update(Particle[] particles)
{
int particlesCount = particles.Length;
for (int i = 0; i < particlesCount - 1; i++)
{
for (int j = i + 1; j < particlesCount; j++)
{
Vector p1 = particles[i].position;
Vector p2 = particles[j].position;
Vector penetrationDirection = (p2 - p1);
float penetrationSquareLength = penetrationDirection.SquareLength();
if (penetrationSquareLength < Math.Sqrt(particles[i].radius + particles[j].radius))
{
penetrationDirection.Normalize();
float penetrationLength = (float)Math.Sqrt(penetrationSquareLength);
float penetrationDepth = 0.5f * (particles[i].radius + particles[j].radius - penetrationLength);
particles[i].position -= penetrationDirection * penetrationDepth;
particles[j].position += penetrationDirection * penetrationDepth;
}
}
}
}
public static bool TryCollision(Particle p, Segment s)
{
Vector L = p.Position - s.Point1;
Vector u = s.Point2 - s.Point1; u.Normalize();
double d = Math.Abs(L.OrthogonalDistance(u));
if (d > p.Radius)
{
return(false);
}
Vector v = p.Velocity;
Vector x = (v * u) * u;
Vector n = u.Orthogonal(); n.Normalize();
if (Vector.Direction(s.Point1, s.Point2, p.Position) < 0)
{
n = -n;
}
Vector s1 = s.Point1 + n;
Vector s2 = s.Point2 + n;
Vector p1 = p.Position - s.Point1;
Vector p2 = p.Position - s.Point2;
Vector mid = s.Middle();
if ((mid - p.Position).Length > s.Length / 2) //nie ma zderzenia z odcinkiem
{
return(false);
}
//if ((s.Point1 - p.Position).Length > p.Radius && (s.Point2 - p.Position).Length > p.Radius)
// return false;
Vector y = (v * n) * n;
double e = Math.Min(p.Restitution, s.Restitution);
p.Velocity -= (2 * y) * e;
Vector G = p.Mass * new Vector(0, -World.Gravity);
//p.Force += G;
//Vector F = (G * u) * u;
Vector N = (G * n) * n;
//p.Force += N;
if ((n ^ p.Velocity) > 0)
{
u = -u;
}
Vector T = 10000.9 * (N * u) * u;
//p.Force += T;
p.Position += (p.Radius - d) * -n;
return(true);
}
