internal override void UpdateSweepRadius(Vec2 center)
{
// Update the sweep radius (maximum radius) as measured from
// a local center point.
Vec2 d = _localPosition - center;
_sweepRadius = d.Length() + _radius - Settings.ToiSlop;
}
public override void DrawSolidCircle(Vec2 center, float radius, Vec2 axis, Color color)
{
float k_segments = 16.0f;
float k_increment = 2.0f * Box2DX.Common.Settings.Pi / k_segments;
float theta = 0.0f;
Gl.glEnable(Gl.GL_BLEND);
Gl.glBlendFunc(Gl.GL_SRC_ALPHA, Gl.GL_ONE_MINUS_SRC_ALPHA);
Gl.glColor4f(0.5f * color.R, 0.5f * color.G, 0.5f * color.B, 0.5f);
Gl.glBegin(Gl.GL_TRIANGLE_FAN);
for (int i = 0; i < k_segments; ++i)
{
Vec2 v = center + radius * new Vec2((float)SysMath.Cos(theta), (float)SysMath.Sin(theta));
Gl.glVertex2f(v.X, v.Y);
theta += k_increment;
}
Gl.glEnd();
Gl.glDisable(Gl.GL_BLEND);
theta = 0.0f;
Gl.glColor4f(color.R, color.G, color.B, 1.0f);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (int i = 0; i < k_segments; ++i)
{
Vec2 v = center + radius * new Vec2((float)SysMath.Cos(theta), (float)SysMath.Sin(theta));
Gl.glVertex2f(v.X, v.Y);
theta += k_increment;
}
Gl.glEnd();
Vec2 p = center + radius * axis;
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex2f(center.X, center.Y);
Gl.glVertex2f(p.X, p.Y);
Gl.glEnd();
}
public World(AABB worldAABB, Vec2 gravity, bool doSleep)
{
this._destructionListener = null;
this._boundaryListener = null;
this._contactFilter = WorldCallback.DefaultFilter;
this._contactListener = null;
this._debugDraw = null;
this._bodyList = null;
this._contactList = null;
this._jointList = null;
this._bodyCount = 0;
this._contactCount = 0;
this._jointCount = 0;
this._warmStarting = true;
this._continuousPhysics = true;
this._allowSleep = doSleep;
this._gravity = gravity;
this._lock = false;
this._inv_dt0 = 0f;
this._contactManager = new ContactManager();
this._contactManager._world = this;
this._broadPhase = new BroadPhase(worldAABB, this._contactManager);
BodyDef def = new BodyDef();
this._groundBody = this.CreateBody(def);
}
public override void DrawSolidCircle(Vec2 center, float radius, Box2DX.Common.Vec2 axis, Color color)
{
float k_segments = 16.0f;
float k_increment = 2.0f * (float)System.Math.PI / k_segments;
float theta = 0.0f;
GL.Color3(0.5f * color.R, 0.5f * color.G, 0.5f * color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.TriangleFan);
for (int i = 0; i < k_segments; ++i) {
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex3(v.X, v.Y, ZLayer);
theta += k_increment;
}
GL.End();
theta = 0.0f;
GL.Color4(color.R, color.G, color.B, 1.0f);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < k_segments; ++i) {
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex3(v.X, v.Y, ZLayer);
theta += k_increment;
}
GL.End();
Vec2 p = center + radius * axis;
GL.Begin(BeginMode.Lines);
GL.Vertex3(center.X, center.Y, ZLayer);
GL.Vertex3(p.X, p.Y, 0);
GL.End();
GL.Enable(EnableCap.Texture2D);
}
public void Set(Vec2 x1, float a1, Vec2 x2, float a2)
{
this.Linear1 = x1;
this.Angular1 = a1;
this.Linear2 = x2;
this.Angular2 = a2;
}
public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = default(Vec2);
Vec2 p = segment.P1;
Vec2 a = segment.P2 - p;
Vec2 a2 = this.P2 - this.P1;
Vec2 vec = Vec2.Cross(a2, 1f);
float num = 100f * Settings.FLT_EPSILON;
float num2 = -Vec2.Dot(a, vec);
bool result;
if (num2 > num)
{
Vec2 a3 = p - this.P1;
float num3 = Vec2.Dot(a3, vec);
if (0f <= num3 && num3 <= maxLambda * num2)
{
float num4 = -a.X * a3.Y + a.Y * a3.X;
if (-num * num2 <= num4 && num4 <= num2 * (1f + num))
{
num3 /= num2;
vec.Normalize();
lambda = num3;
normal = vec;
result = true;
return result;
}
}
}
result = false;
return result;
}
public override void Step(TimeStep step)
{
if (_bodyList == null) return;
if (useWorldGravity)
{
gravity = _world.Gravity;
}
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
Vec2 areac = new Vec2(0, 0);
Vec2 massc = new Vec2(0, 0);
float area = 0;
float mass = 0;
for (Shape shape = body.GetShapeList(); shape != null; shape = shape.GetNext())
{
Vec2 sc;
float sarea = shape.ComputeSubmergedArea(normal, offset, body.GetXForm(), out sc);
area += sarea;
areac.X += sarea * sc.X;
areac.Y += sarea * sc.Y;
float shapeDensity = 0;
if (useDensity)
{
//TODO: Expose density publicly
shapeDensity = shape.Density;
}
else
{
shapeDensity = 1;
}
mass += sarea * shapeDensity;
massc.X += sarea * sc.X * shapeDensity;
massc.Y += sarea * sc.Y * shapeDensity;
}
areac.X /= area;
areac.Y /= area;
massc.X /= mass;
massc.Y /= mass;
if (area < Box2DX.Common.Settings.FLT_EPSILON)
continue;
//Buoyancy
Vec2 buoyancyForce = -density * area * gravity;
body.ApplyForce(buoyancyForce, massc);
//Linear drag
Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - velocity;
dragForce *= -linearDrag * area;
body.ApplyForce(dragForce, areac);
//Angular drag
//TODO: Something that makes more physical sense?
body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * angularDrag);
}
}
public static void DrawSegment(Vec2 p1, Vec2 p2, Color color, params object[] p)
{
Gl.Color3(color.R, color.G, color.B);
Gl.Begin(BeginMode.Lines);
Gl.Vertex2(p1.X, p1.Y);
Gl.Vertex2(p2.X, p2.Y);
Gl.End();
}
public void Initialize(Body body1, Body body2, Vec2 anchor)
{
this.Body1 = body1;
this.Body2 = body2;
this.LocalAnchor1 = body1.GetLocalPoint(anchor);
this.LocalAnchor2 = body2.GetLocalPoint(anchor);
this.ReferenceAngle = body2.GetAngle() - body1.GetAngle();
}
public override float ReportFixture(Fixture fixture, Vec2 point, Vec2 normal, float fraction)
{
_fixture = fixture;
_point = point;
_normal = normal;
return fraction;
}
public void SetTarget(Vec2 target)
{
if (this._body2.IsSleeping())
{
this._body2.WakeUp();
}
this._target = target;
}
public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
{
this.Body1 = body1;
this.Body2 = body2;
this.LocalAnchor1 = body1.GetLocalPoint(anchor1);
this.LocalAnchor2 = body2.GetLocalPoint(anchor2);
this.Length = (anchor2 - anchor1).Length();
}
public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
{
this.Body1 = body1;
this.Body2 = body2;
this.localAnchor1 = body1.GetLocalPoint(anchor);
this.localAnchor2 = body2.GetLocalPoint(anchor);
this.localAxis1 = body1.GetLocalVector(axis);
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="anchor"></param>
/// <param name="axis"></param>
public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
{
Body1 = body1;
Body2 = body2;
LocalAnchor1 = body1.GetLocalPoint(anchor);
LocalAnchor2 = body2.GetLocalPoint(anchor);
LocalAxis1 = body1.GetLocalVector(axis);
ReferenceAngle = body2.GetAngle() - body1.GetAngle();
}
public static void DrawPoint(Vec2 p, float size, Color color)
{
Gl.glPointSize(size);
Gl.glBegin(Gl.GL_POINTS);
Gl.glColor3f(color.R, color.G, color.B);
Gl.glVertex2f(p.X, p.Y);
Gl.glEnd();
Gl.glPointSize(1.0f);
}
/// <summary>
/// Initialize the bodies, anchors, and length using the world anchors.
/// </summary>
public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
{
Body1 = body1;
Body2 = body2;
LocalAnchor1 = body1.GetLocalPoint(anchor1);
LocalAnchor2 = body2.GetLocalPoint(anchor2);
Vec2 d = anchor2 - anchor1;
Length = d.Length();
}
public static void DrawPoint(Vec2 p, float size, Color color)
{
Gl.PointSize(size);
Gl.Begin(BeginMode.Points);
Gl.Color3(color.R, color.G, color.B);
Gl.Vertex2(p.X, p.Y);
Gl.End();
Gl.PointSize(1.0f);
}
internal CircleShape(ShapeDef def)
: base(def)
{
Box2DXDebug.Assert(def.Type == ShapeType.CircleShape);
CircleDef circleDef = (CircleDef)def;
this._type = ShapeType.CircleShape;
this._localPosition = circleDef.LocalPosition;
this._radius = circleDef.Radius;
}
public void Advance(float t)
{
if (this.T0 < t && 1f - this.T0 > Math.FLOAT32_EPSILON)
{
float num = (t - this.T0) / (1f - this.T0);
this.C0 = (1f - num) * this.C0 + num * this.C;
this.A0 = (1f - num) * this.A0 + num * this.A;
this.T0 = t;
}
}
public float T0; //time interval = [T0,1], where T0 is in [0,1]
#endregion Fields
#region Methods
/// <summary>
/// Advance the sweep forward, yielding a new initial state.
/// </summary>
/// <param name="t">The new initial time.</param>
public void Advance(float t)
{
if (T0 < t && 1.0f - T0 > Math.FLOAT32_EPSILON)
{
float alpha = (t - T0) / (1.0f - T0);
C0 = (1.0f - alpha) * C0 + alpha * C;
A0 = (1.0f - alpha) * A0 + alpha * A;
T0 = t;
}
}
public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color)
{
Gl.glColor3f(color.R, color.G, color.B);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (int i = 0; i < vertexCount; ++i)
{
Gl.glVertex2f(vertices[i].X, vertices[i].Y);
}
Gl.glEnd();
}
/// <summary>
/// Advance the sweep forward, yielding a new initial state.
/// </summary>
/// <param name="t">The new initial time.</param>
public void Advance(float t)
{
if (T0 < t && 1.0f - T0 > Settings.FLT_EPSILON)
{
float alpha = (t - T0) / (1.0f - T0);
C0 = (1.0f - alpha) * C0 + alpha * C;
A0 = (1.0f - alpha) * A0 + alpha * A;
T0 = t;
}
}
public void SetAsBox(float hx, float hy, Vec2 center, float angle)
{
this.SetAsBox(hx, hy);
XForm t = default(XForm);
t.Position = center;
t.R.Set(angle);
for (int i = 0; i < this.VertexCount; i++)
{
this.Vertices[i] = Box2DX.Common.Math.Mul(t, this.Vertices[i]);
}
}
public override void DrawPolygon(Vec2[] vertices, int vertexCount, Color color)
{
GL.Color3(color.R, color.G, color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < vertexCount; ++i) {
GL.Vertex3(vertices[i].X, vertices[i].Y, ZLayer);
}
GL.End();
GL.Enable(EnableCap.Texture2D);
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases. Solve only the upper
/// 2-by-2 matrix equation.
/// </summary>
public Vec2 Solve22(Vec2 b)
{
float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y;
float det = a11 * a22 - a12 * a21;
Box2DXDebug.Assert(det != 0.0f);
det = 1.0f / det;
Vec2 x = new Vec2();
x.X = det * (a22 * b.X - a12 * b.Y);
x.Y = det * (a11 * b.Y - a21 * b.X);
return x;
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">The half-width</param>
/// <param name="hy">The half-height.</param>
/// <param name="center">The center of the box in local coordinates.</param>
/// <param name="angle">The rotation of the box in local coordinates.</param>
public void SetAsBox(float hx, float hy, Vec2 center, float angle)
{
SetAsBox(hx, hy);
XForm xf = new XForm();
xf.Position = center;
xf.R.Set(angle);
for (int i = 0; i < VertexCount; ++i) {
Vertices[i] = Common.Math.Mul(xf, Vertices[i]);
}
}
public BuoyancyController()
{
normal = new Vec2(0,1);
offset = 0;
density =0;
velocity = new Vec2(0,0);
linearDrag=0;
angularDrag=0;
useDensity = false;
useWorldGravity = true;
gravity = new Vec2(0,0);
}
public DistanceJoint(DistanceJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_length = def.Length;
_frequencyHz = def.FrequencyHz;
_dampingRatio = def.DampingRatio;
_impulse = 0.0f;
_gamma = 0.0f;
_bias = 0.0f;
}
public void Set(Vec2 v1, Vec2 v2)
{
_v1 = v1;
_v2 = v2;
_direction = _v2 - _v1;
_length = _direction.Normalize();
_normal = Vec2.Cross(_direction, 1.0f);
_cornerDir1 = _normal;
_cornerDir2 = -1.0f * _normal;
}
public override void DrawSegment(Vec2 p1, Box2DX.Common.Vec2 p2, Color color)
{
GL.Color3(color.R, color.G, color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.Lines);
{
GL.Vertex3(p1.X, p1.Y, ZLayer);
GL.Vertex3(p2.X, p2.Y, ZLayer);
}
GL.End();
GL.Enable(EnableCap.Texture2D);
}
/// <summary>
/// Copy vertices. This assumes the vertices define a convex polygon.
/// It is assumed that the exterior is the the right of each edge.
/// </summary>
public void Set(Vec2[] vertices, int count)
{
Box2DXDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices);
_vertexCount = count;
int i;
// Copy vertices.
for (i = 0; i < _vertexCount; ++i)
{
_vertices[i] = vertices[i];
}
// Compute normals. Ensure the edges have non-zero length.
for (i = 0; i < _vertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < count ? i + 1 : 0;
Vec2 edge = _vertices[i2] - _vertices[i1];
Box2DXDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED);
_normals[i] = Vec2.Cross(edge, 1.0f);
_normals[i].Normalize();
}
#if DEBUG
// Ensure the polygon is convex and the interior
// is to the left of each edge.
for (i = 0; i < _vertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < count ? i + 1 : 0;
Vec2 edge = _vertices[i2] - _vertices[i1];
for (int j = 0; j < _vertexCount; ++j)
{
// Don't check vertices on the current edge.
if (j == i1 || j == i2)
{
continue;
}
Vec2 r = _vertices[j] - _vertices[i1];
// Your polygon is non-convex (it has an indentation) or
// has colinear edges.
float s = Vec2.Cross(edge, r);
Box2DXDebug.Assert(s > 0.0f);
}
}
#endif
// Compute the polygon centroid.
_centroid = ComputeCentroid(_vertices, _vertexCount);
}
public override void DrawSolidCircle(Vec2 center, float radius, Box2DX.Common.Vec2 axis, Color color)
{
float k_segments = 16.0f;
float k_increment = 2.0f * (float)System.Math.PI / k_segments;
float theta = 0.0f;
GL.Color3(0.5f * color.R, 0.5f * color.G, 0.5f * color.B);
GL.Disable(EnableCap.Texture2D);
GL.Begin(BeginMode.TriangleFan);
for (int i = 0; i < k_segments; ++i)
{
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex3(v.X, v.Y, ZLayer);
theta += k_increment;
}
GL.End();
theta = 0.0f;
GL.Color4(color.R, color.G, color.B, 1.0f);
GL.Begin(BeginMode.LineLoop);
for (int i = 0; i < k_segments; ++i)
{
Vec2 v = center + radius * new Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
GL.Vertex3(v.X, v.Y, ZLayer);
theta += k_increment;
}
GL.End();
Vec2 p = center + radius * axis;
GL.Begin(BeginMode.Lines);
GL.Vertex3(center.X, center.Y, ZLayer);
GL.Vertex3(p.X, p.Y, 0);
GL.End();
GL.Enable(EnableCap.Texture2D);
}
