public static CollisionData CircleCircle(Circle a, Circle b)
{
Vector2 delta = b.Entity.Position - a.Entity.Position;
float length = delta.Length();
float distance = a.Radius + b.Radius;
if (length < distance)
{
CollisionData result = new CollisionData(distance-length);
result.Normal = delta.Normalized();
result.Position = a.Entity.Position;
result.Position += (-a.Radius + result.Interpenetration * .5f) * result.Normal;
return result;
}
return CollisionData.Empty;
}
public virtual void BounceEnergy(CollisionData collision, Entity other, float factor)
{
Vector2 normal = collision.Normal * factor;
float dot = Vector2.Dot(normal, this.Velocity);
if (dot < 0)
return;
this.Velocity -= 2 * dot * normal;
float friction = this.Friction;
float bounce = this.Bounce;
if (other.Mover != null && other.Mass < Entity.Mass * 100)
{
friction = (this.Friction + other.Mover.Friction) * .5f;
bounce = (this.Bounce + other.Mover.Bounce) * .5f;
}
this.ApplyFrictionBounce(normal, friction, bounce);
}
public static CollisionData CirclePolygon(Circle a, Polygon b)
{
Matrix rotation = Matrix.CreateRotationZ(b.Entity.Orientation);
Matrix inverseRotation = Matrix.CreateTranslation(new Vector3(-b.Entity.Position, 0)) * Matrix.CreateRotationZ(-b.Entity.Orientation) * Matrix.CreateTranslation(new Vector3(b.Entity.Position, 0));
Vector2 circlePosition = Vector2.Transform(a.Entity.Position, inverseRotation);
Vector2 delta = b.Entity.Position - circlePosition;
Polygon.Projection proj;
CollisionData result = new CollisionData(float.MaxValue);
float inter1 = 0, inter2 = 0;
for (int i = 0; i < b.normals.Length; i++)
{
proj = a.Project(b.normals[i], -delta);
inter1 = b.projections[i].Max - proj.Min;
inter2 = -(b.projections[i].Min - proj.Max);
#if DEBUG && SATDEBUG
Vector2 p = b.Entity.Position;
Integrator.line(p + Vector2.TransformNormal(b.normals[i], rotation) * proj.Min, p + Vector2.TransformNormal(b.normals[i], rotation) * proj.Max, Color.LimeGreen);
#endif
if (inter1 < 0 || inter2 < 0)
return CollisionData.Empty;
if (inter1 < inter2 && inter1 < result.Interpenetration)
{
result.Interpenetration = inter1;
result.Normal = -Vector2.TransformNormal(b.normals[i], rotation);
}
else if (inter2 < result.Interpenetration)
{
result.Interpenetration = inter2;
result.Normal = Vector2.TransformNormal(b.normals[i], rotation);
}
}
Vector2 closest = Vector2.UnitX;
float maxDist = float.MaxValue;
for (int i = 0; i < b.Vertices.Length; i++)
{
Vector2 v = b.Vertices[i] + b.Entity.Position;
Vector2 d = v - circlePosition;
if (d.LengthSquared() < maxDist)
{
closest = d;
maxDist = d.LengthSquared();
}
}
Vector2 normal = closest.Normalized();
proj = b.Project(normal, delta);
Polygon.Projection self = a.Project(normal, Vector2.Zero);
inter1 = self.Max - proj.Min;
inter2 = -(self.Min - proj.Max);
#if DEBUG && SATDEBUG
Vector2 p2 = a.Entity.Position;
Integrator.line(p2 + Vector2.TransformNormal(normal, rotation) * proj.Min, p2 + Vector2.TransformNormal(normal, rotation) * proj.Max, Color.LimeGreen);
#endif
if (inter1 < 0 || inter2 < 0)
return CollisionData.Empty;
if (inter1 < inter2 && inter1 < result.Interpenetration)
{
result.Interpenetration = inter1;
result.Normal = Vector2.TransformNormal(normal, rotation);
}
else if (inter2 < result.Interpenetration)
{
result.Interpenetration = inter2;
result.Normal = -Vector2.TransformNormal(normal, rotation);
}
#if DEBUG && SATDEBUG
Vector2 pos = a.Entity.Position;
Integrator.line(pos, pos + result.Normal * result.Interpenetration, Color.White);
#endif
return result;
}
public static CollisionData PolygonPolygon(Polygon a, Polygon b)
{
Vector2 delta = b.Entity.Position - a.Entity.Position;
float inter1 = 0, inter2 = 0;
CollisionData result = new CollisionData(float.MaxValue);
Polygon lookingAt = a;
delta = Vector2.Transform(delta, Matrix.CreateRotationZ(-b.Entity.Orientation));
Matrix rot = Matrix.CreateRotationZ(a.Entity.Orientation - b.Entity.Orientation);
for (int i = 0; i < a.normals.Length; i++)
{
Polygon.Projection proj = b.Project(Vector2.TransformNormal(a.normals[i], rot), delta);
inter1 = a.projections[i].Max - proj.Min;
inter2 = -(a.projections[i].Min - proj.Max);
#if DEBUG && SATDEBUG
Vector2 p = a.Entity.Position;
Matrix r = Matrix.CreateRotationZ(a.Entity.Orientation);
Integrator.line(p + Vector2.TransformNormal(a.normals[i], r) * proj.Min, p + Vector2.TransformNormal(a.normals[i], r) * proj.Max, Color.HotPink);
#endif
if (inter1 < 0 || inter2 < 0)
return CollisionData.Empty;
if (inter1 < inter2 && inter1 < result.Interpenetration)
{
result.Interpenetration = inter1;
result.Normal = Vector2.TransformNormal(a.normals[i], Matrix.CreateRotationZ(a.Entity.Orientation));
lookingAt = b;
}
else if( inter2 < result.Interpenetration)
{
result.Interpenetration = inter2;
result.Normal = -Vector2.TransformNormal(a.normals[i], Matrix.CreateRotationZ(a.Entity.Orientation));
lookingAt = b;
}
}
delta = b.Entity.Position - a.Entity.Position;
delta = Vector2.Transform(delta, Matrix.CreateRotationZ(-a.Entity.Orientation));
rot = Matrix.CreateRotationZ(b.Entity.Orientation - a.Entity.Orientation);
for (int i = 0; i < b.normals.Length; i++)
{
Polygon.Projection proj = a.Project(Vector2.TransformNormal(b.normals[i], rot), -delta);
inter1 = b.projections[i].Max - proj.Min;
inter2 = -(b.projections[i].Min - proj.Max);
#if DEBUG && SATDEBUG
Vector2 p = b.Entity.Position;
Matrix r = Matrix.CreateRotationZ(b.Entity.Orientation);
Integrator.line(p + Vector2.TransformNormal(b.normals[i], r) * proj.Min, p + Vector2.TransformNormal(b.normals[i], r) * proj.Max, Color.LimeGreen);
#endif
if (inter1 < 0 || inter2 < 0)
return CollisionData.Empty;
if (inter1 < inter2 && inter1 < result.Interpenetration)
{
result.Interpenetration = inter1;
result.Normal = -Vector2.TransformNormal(b.normals[i], Matrix.CreateRotationZ(b.Entity.Orientation));
lookingAt = a;
}
else if (inter2 < result.Interpenetration)
{
result.Interpenetration = inter2;
result.Normal = Vector2.TransformNormal(b.normals[i], Matrix.CreateRotationZ(b.Entity.Orientation));
lookingAt = a;
}
}
int dir = lookingAt == a ? -1 : 1;
float bestDot = 0;
Vector2 guess = Vector2.Zero;
Vector2[] verts = lookingAt.RotatedVertices(lookingAt.Entity.Orientation);
for (int i = 0; i < verts.Length; i++)
{
Vector2 v = verts[i];
float d = Vector2.Dot(v, result.Normal) * dir;
if (d < bestDot - SATPositioningAngle)
{
bestDot = d;
guess = v;
}
else if (d < bestDot + SATPositioningAngle)
{
bestDot = d;
guess += v;
guess *= .5f;
}
}
result.Position = lookingAt.Entity.Position + guess;
#if DEBUG && SATDEBUG
Vector2 pos = result.Position;
Integrator.line(pos, pos + result.Normal * result.Interpenetration * 10, Color.White);
#endif
return result;
}
public virtual void RespondToCollision(CollisionData collision, Entity other, float factor)
{
this.Entity.Position -= factor * collision.Normal * collision.Interpenetration;
}
public virtual void TransferEnergy(CollisionData collision, Entity other)
{
if (other.Mover == null)
return;
Vector2 n = this.Entity.Position - other.Position;
if( n.LengthSquared() > 0 )
n.Normalize();
float a1 = Vector2.Dot(this.Velocity, n);
float a2 = Vector2.Dot(other.Mover.Velocity, n);
float optimusP = (2f * (a1 - a2)) / (this.Entity.Mass + other.Mass);
this.TransferImpuls(-n * optimusP * other.Mass);
other.Mover.TransferImpuls(n * optimusP * this.Entity.Mass);
}
