464 lines
18 KiB
C#
464 lines
18 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using FarseerPhysics.Collision;
|
||
using FarseerPhysics.Collision.Shapes;
|
||
using FarseerPhysics.Dynamics;
|
||
using Microsoft.Xna.Framework;
|
||
|
||
namespace FarseerPhysics.Common.PhysicsLogic
|
||
{
|
||
internal struct ShapeData
|
||
{
|
||
public Body Body;
|
||
public float Max;
|
||
public float Min; // absolute angles
|
||
}
|
||
|
||
/// <summary>
|
||
/// This is a comprarer used for
|
||
/// detecting angle difference between rays
|
||
/// </summary>
|
||
internal class RayDataComparer : IComparer<float>
|
||
{
|
||
#region IComparer<float> Members
|
||
|
||
int IComparer<float>.Compare(float a, float b)
|
||
{
|
||
float diff = (a - b);
|
||
if (diff > 0)
|
||
return 1;
|
||
if (diff < 0)
|
||
return -1;
|
||
return 0;
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
|
||
/* Methodology:
|
||
* Force applied at a ray is inversely proportional to the square of distance from source
|
||
* AABB is used to query for shapes that may be affected
|
||
* For each RIGID BODY (not shape -- this is an optimization) that is matched, loop through its vertices to determine
|
||
* the extreme points -- if there is structure that contains outlining polygon, use that as an additional optimization
|
||
* Evenly cast a number of rays against the shape - number roughly proportional to the arc coverage
|
||
* -Something like every 3 degrees should do the trick although this can be altered depending on the distance (if really close don't need such a high density of rays)
|
||
* -There should be a minimum number of rays (3-5?) applied to each body so that small bodies far away are still accurately modeled
|
||
* -Be sure to have the forces of each ray be proportional to the average arc length covered by each.
|
||
* For each ray that actually intersects with the shape (non intersections indicate something blocking the path of explosion):
|
||
* > apply the appropriate force dotted with the negative of the collision normal at the collision point
|
||
* > optionally apply linear interpolation between aforementioned Normal force and the original explosion force in the direction of ray to simulate "surface friction" of sorts
|
||
*/
|
||
|
||
/// <summary>
|
||
/// This is an explosive... it explodes.
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// Original Code by Steven Lu - see http://www.box2d.org/forum/viewtopic.php?f=3&t=1688
|
||
/// Ported to Farseer 3.0 by Nicol<6F>s Hormaz<61>bal
|
||
/// </remarks>
|
||
public sealed class Explosion : PhysicsLogic
|
||
{
|
||
/// <summary>
|
||
/// Two degrees: maximum angle from edges to first ray tested
|
||
/// </summary>
|
||
private const float MaxEdgeOffset = MathHelper.Pi / 90;
|
||
|
||
/// <summary>
|
||
/// Ratio of arc length to angle from edges to first ray tested.
|
||
/// Defaults to 1/40.
|
||
/// </summary>
|
||
public float EdgeRatio = 1.0f / 40.0f;
|
||
|
||
/// <summary>
|
||
/// Ignore Explosion if it happens inside a shape.
|
||
/// Default value is false.
|
||
/// </summary>
|
||
public bool IgnoreWhenInsideShape = false;
|
||
|
||
/// <summary>
|
||
/// Max angle between rays (used when segment is large).
|
||
/// Defaults to 15 degrees
|
||
/// </summary>
|
||
public float MaxAngle = MathHelper.Pi / 15;
|
||
|
||
/// <summary>
|
||
/// Maximum number of shapes involved in the explosion.
|
||
/// Defaults to 100
|
||
/// </summary>
|
||
public int MaxShapes = 100;
|
||
|
||
/// <summary>
|
||
/// How many rays per shape/body/segment.
|
||
/// Defaults to 5
|
||
/// </summary>
|
||
public int MinRays = 5;
|
||
|
||
private List<ShapeData> _data = new List<ShapeData>();
|
||
private Dictionary<Fixture, List<Vector2>> _exploded;
|
||
private RayDataComparer _rdc;
|
||
|
||
public Explosion(World world)
|
||
: base(world, PhysicsLogicType.Explosion)
|
||
{
|
||
_exploded = new Dictionary<Fixture, List<Vector2>>();
|
||
_rdc = new RayDataComparer();
|
||
_data = new List<ShapeData>();
|
||
}
|
||
|
||
/// <summary>
|
||
/// This makes the explosive explode
|
||
/// </summary>
|
||
/// <param name="pos">
|
||
/// The position where the explosion happens
|
||
/// </param>
|
||
/// <param name="radius">
|
||
/// The explosion radius
|
||
/// </param>
|
||
/// <param name="maxForce">
|
||
/// The explosion force at the explosion point
|
||
/// (then is inversely proportional to the square of the distance)
|
||
/// </param>
|
||
/// <returns>
|
||
/// A dictionnary containing all the "exploded" fixtures
|
||
/// with a list of the applied impulses
|
||
/// </returns>
|
||
public Dictionary<Fixture, List<Vector2>> Activate(Vector2 pos, float radius, float maxForce)
|
||
{
|
||
_exploded.Clear();
|
||
|
||
AABB aabb;
|
||
aabb.LowerBound = pos + new Vector2(-radius, -radius);
|
||
aabb.UpperBound = pos + new Vector2(radius, radius);
|
||
Fixture[] shapes = new Fixture[MaxShapes];
|
||
|
||
// More than 5 shapes in an explosion could be possible, but still strange.
|
||
Fixture[] containedShapes = new Fixture[5];
|
||
bool exit = false;
|
||
|
||
int shapeCount = 0;
|
||
int containedShapeCount = 0;
|
||
|
||
// Query the world for overlapping shapes.
|
||
World.QueryAABB(
|
||
fixture =>
|
||
{
|
||
if (fixture.TestPoint(ref pos))
|
||
{
|
||
if (IgnoreWhenInsideShape)
|
||
exit = true;
|
||
else
|
||
containedShapes[containedShapeCount++] = fixture;
|
||
}
|
||
else
|
||
{
|
||
shapes[shapeCount++] = fixture;
|
||
}
|
||
|
||
// Continue the query.
|
||
return true;
|
||
}, ref aabb);
|
||
|
||
if (exit)
|
||
{
|
||
return _exploded;
|
||
}
|
||
|
||
// Per shape max/min angles for now.
|
||
float[] vals = new float[shapeCount * 2];
|
||
int valIndex = 0;
|
||
for (int i = 0; i < shapeCount; ++i)
|
||
{
|
||
PolygonShape ps;
|
||
CircleShape cs = shapes[i].Shape as CircleShape;
|
||
if (cs != null)
|
||
{
|
||
// We create a "diamond" approximation of the circle
|
||
Vertices v = new Vertices();
|
||
Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0);
|
||
v.Add(vec);
|
||
vec = Vector2.Zero + new Vector2(0, cs.Radius);
|
||
v.Add(vec);
|
||
vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius);
|
||
v.Add(vec);
|
||
vec = Vector2.Zero + new Vector2(0, -cs.Radius);
|
||
v.Add(vec);
|
||
ps = new PolygonShape(v, 0);
|
||
}
|
||
else
|
||
ps = shapes[i].Shape as PolygonShape;
|
||
|
||
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
|
||
{
|
||
Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
|
||
float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X);
|
||
float min = float.MaxValue;
|
||
float max = float.MinValue;
|
||
float minAbsolute = 0.0f;
|
||
float maxAbsolute = 0.0f;
|
||
|
||
for (int j = 0; j < (ps.Vertices.Count()); ++j)
|
||
{
|
||
Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
|
||
float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X);
|
||
float diff = (newAngle - angleToCentroid);
|
||
|
||
diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
|
||
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
|
||
|
||
if (diff < 0.0f)
|
||
diff += 2 * MathHelper.Pi; // correction for not handling negs
|
||
|
||
diff -= MathHelper.Pi;
|
||
|
||
if (Math.Abs(diff) > MathHelper.Pi)
|
||
throw new ArgumentException("OMG!");
|
||
// Something's wrong, point not in shape but exists angle diff > 180
|
||
|
||
if (diff > max)
|
||
{
|
||
max = diff;
|
||
maxAbsolute = newAngle;
|
||
}
|
||
if (diff < min)
|
||
{
|
||
min = diff;
|
||
minAbsolute = newAngle;
|
||
}
|
||
}
|
||
|
||
vals[valIndex] = minAbsolute;
|
||
++valIndex;
|
||
vals[valIndex] = maxAbsolute;
|
||
++valIndex;
|
||
}
|
||
}
|
||
|
||
Array.Sort(vals, 0, valIndex, _rdc);
|
||
_data.Clear();
|
||
bool rayMissed = true;
|
||
|
||
for (int i = 0; i < valIndex; ++i)
|
||
{
|
||
Fixture shape = null;
|
||
float midpt;
|
||
|
||
int iplus = (i == valIndex - 1 ? 0 : i + 1);
|
||
if (vals[i] == vals[iplus])
|
||
continue;
|
||
|
||
if (i == valIndex - 1)
|
||
{
|
||
// the single edgecase
|
||
midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
|
||
}
|
||
else
|
||
{
|
||
midpt = (vals[i + 1] + vals[i]);
|
||
}
|
||
|
||
midpt = midpt / 2;
|
||
|
||
Vector2 p1 = pos;
|
||
Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt),
|
||
(float)Math.Sin(midpt)) + pos;
|
||
|
||
// RaycastOne
|
||
bool hitClosest = false;
|
||
World.RayCast((f, p, n, fr) =>
|
||
{
|
||
Body body = f.Body;
|
||
|
||
if (!IsActiveOn(body))
|
||
return 0;
|
||
|
||
if (body.UserData != null)
|
||
{
|
||
int index = (int)body.UserData;
|
||
if (index == 0)
|
||
{
|
||
// filter
|
||
return -1.0f;
|
||
}
|
||
}
|
||
|
||
hitClosest = true;
|
||
shape = f;
|
||
return fr;
|
||
}, p1, p2);
|
||
|
||
//draws radius points
|
||
if ((hitClosest) && (shape.Body.BodyType == BodyType.Dynamic))
|
||
{
|
||
if ((_data.Count() > 0) && (_data.Last().Body == shape.Body) && (!rayMissed))
|
||
{
|
||
int laPos = _data.Count - 1;
|
||
ShapeData la = _data[laPos];
|
||
la.Max = vals[iplus];
|
||
_data[laPos] = la;
|
||
}
|
||
else
|
||
{
|
||
// make new
|
||
ShapeData d;
|
||
d.Body = shape.Body;
|
||
d.Min = vals[i];
|
||
d.Max = vals[iplus];
|
||
_data.Add(d);
|
||
}
|
||
|
||
if ((_data.Count() > 1)
|
||
&& (i == valIndex - 1)
|
||
&& (_data.Last().Body == _data.First().Body)
|
||
&& (_data.Last().Max == _data.First().Min))
|
||
{
|
||
ShapeData fi = _data[0];
|
||
fi.Min = _data.Last().Min;
|
||
_data.RemoveAt(_data.Count() - 1);
|
||
_data[0] = fi;
|
||
while (_data.First().Min >= _data.First().Max)
|
||
{
|
||
fi.Min -= MathHelper.Pi * 2;
|
||
_data[0] = fi;
|
||
}
|
||
}
|
||
|
||
int lastPos = _data.Count - 1;
|
||
ShapeData last = _data[lastPos];
|
||
while ((_data.Count() > 0)
|
||
&& (_data.Last().Min >= _data.Last().Max)) // just making sure min<max
|
||
{
|
||
last.Min = _data.Last().Min - 2 * MathHelper.Pi;
|
||
_data[lastPos] = last;
|
||
}
|
||
rayMissed = false;
|
||
}
|
||
else
|
||
{
|
||
rayMissed = true; // raycast did not find a shape
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < _data.Count(); ++i)
|
||
{
|
||
if (!IsActiveOn(_data[i].Body))
|
||
continue;
|
||
|
||
float arclen = _data[i].Max - _data[i].Min;
|
||
|
||
float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
|
||
int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);
|
||
|
||
if (insertedRays < 0)
|
||
insertedRays = 0;
|
||
|
||
float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);
|
||
|
||
//Note: This loop can go into infinite as it operates on floats.
|
||
//Added FloatEquals with a large epsilon.
|
||
for (float j = _data[i].Min + first;
|
||
j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
|
||
j += offset)
|
||
{
|
||
Vector2 p1 = pos;
|
||
Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
|
||
Vector2 hitpoint = Vector2.Zero;
|
||
float minlambda = float.MaxValue;
|
||
|
||
List<Fixture> fl = _data[i].Body.FixtureList;
|
||
for (int x = 0; x < fl.Count; x++)
|
||
{
|
||
Fixture f = fl[x];
|
||
RayCastInput ri;
|
||
ri.Point1 = p1;
|
||
ri.Point2 = p2;
|
||
ri.MaxFraction = 50f;
|
||
|
||
RayCastOutput ro;
|
||
if (f.RayCast(out ro, ref ri, 0))
|
||
{
|
||
if (minlambda > ro.Fraction)
|
||
{
|
||
minlambda = ro.Fraction;
|
||
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
|
||
}
|
||
}
|
||
|
||
// the force that is to be applied for this particular ray.
|
||
// offset is angular coverage. lambda*length of segment is distance.
|
||
float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi *
|
||
(1.0f - Math.Min(1.0f, minlambda));
|
||
|
||
// We Apply the impulse!!!
|
||
Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j),
|
||
(float)Math.Sin(j)), -ro.Normal) *
|
||
new Vector2((float)Math.Cos(j),
|
||
(float)Math.Sin(j));
|
||
|
||
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
|
||
|
||
// We gather the fixtures for returning them
|
||
Vector2 val = Vector2.Zero;
|
||
List<Vector2> vectorList;
|
||
if (_exploded.TryGetValue(f, out vectorList))
|
||
{
|
||
val.X += Math.Abs(vectImp.X);
|
||
val.Y += Math.Abs(vectImp.Y);
|
||
|
||
vectorList.Add(val);
|
||
}
|
||
else
|
||
{
|
||
vectorList = new List<Vector2>();
|
||
val.X = Math.Abs(vectImp.X);
|
||
val.Y = Math.Abs(vectImp.Y);
|
||
|
||
vectorList.Add(val);
|
||
_exploded.Add(f, vectorList);
|
||
}
|
||
|
||
if (minlambda > 1.0f)
|
||
{
|
||
hitpoint = p2;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// We check contained shapes
|
||
for (int i = 0; i < containedShapeCount; ++i)
|
||
{
|
||
Fixture fix = containedShapes[i];
|
||
|
||
if (!IsActiveOn(fix.Body))
|
||
continue;
|
||
|
||
float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi;
|
||
Vector2 hitPoint;
|
||
|
||
CircleShape circShape = fix.Shape as CircleShape;
|
||
if (circShape != null)
|
||
{
|
||
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
|
||
}
|
||
else
|
||
{
|
||
PolygonShape shape = fix.Shape as PolygonShape;
|
||
hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
|
||
}
|
||
|
||
Vector2 vectImp = impulse * (hitPoint - pos);
|
||
|
||
List<Vector2> vectorList = new List<Vector2>();
|
||
vectorList.Add(vectImp);
|
||
|
||
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
|
||
|
||
if (!_exploded.ContainsKey(fix))
|
||
_exploded.Add(fix, vectorList);
|
||
}
|
||
|
||
return _exploded;
|
||
}
|
||
}
|
||
} |