public static b2Vec2 b2Cross(float ax, float ay, float s)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(s * ay, -s * ax);
return(b);
}
public static b2Vec2 b2MulT(ref b2Mat22 A, ref b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(b2Dot(v, A.ex), b2Dot(v, A.ey));
return(b);
}
public static b2Vec2 b2Mul22(b2Mat33 A, b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
return(b);
}
public static b2Vec2 b2Cross(ref b2Vec2 a, float s)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(s * a.m_y, -s * a.m_x);
return(b);
}
public static b2Vec2 b2Mul(b2Transform T, b2Vec2 v)
{
float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
b2Vec2 b = b2Vec2.Zero;
b.Set(x, y);
return b;
}
public static b2Vec2 b2MulT(b2Rot q, b2Vec2 v)
{
float x = q.c * v.x + q.s * v.y;
float y = -q.s * v.x + q.c * v.y;
b2Vec2 b = b2Vec2.Zero;
b.Set(x, y);
return b;
}
public static b2Mat22 b2MulT(b2Mat22 A, b2Mat22 B)
{
b2Vec2 c1 = b2Vec2.Zero;
b2Vec2 c2 = b2Vec2.Zero;
c1.Set(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
c2.Set(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
return new b2Mat22(c1, c2);
}
public static b2Vec2 b2MulT(b2Transform T, b2Vec2 v)
{
float px = v.x - T.p.x;
float py = v.y - T.p.y;
float x = (T.q.c * px + T.q.s * py);
float y = (-T.q.s * px + T.q.c * py);
b2Vec2 b = b2Vec2.Zero;
b.Set(x, y);
return b;
}
public virtual bool MouseDown(b2Vec2 p)
{
m_mouseWorld = p;
if (m_mouseJoint != null)
{
return false;
}
// Make a small box.
b2AABB aabb = new b2AABB();
b2Vec2 d = new b2Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
QueryCallback callback = new QueryCallback(p);
m_world.QueryAABB(callback, aabb);
if (callback.m_fixture != null)
{
b2Body body = callback.m_fixture.Body;
b2MouseJointDef md = new b2MouseJointDef();
md.BodyA = m_groundBody;
md.BodyB = body;
md.target = p;
md.maxForce = 1000.0f * body.Mass;
m_mouseJoint = (b2MouseJoint)m_world.CreateJoint(md);
body.SetAwake(true);
return true;
}
else
{
//Como no ha encontrado un objeto empezamos el arrastre
IsArrastring = true;
originPosition = new CCPoint(p.x, p.y);
return true;
}
}
public void Defaults()
{
userData = null;
position = b2Vec2.Zero;
position.Set(0.0f, 0.0f);
angle = 0.0f;
linearVelocity = b2Vec2.Zero;
linearVelocity.Set(0.0f, 0.0f);
angularVelocity = 0.0f;
linearDamping = 0.0f;
angularDamping = 0.0f;
allowSleep = true;
awake = true;
fixedRotation = false;
bullet = false;
type = b2BodyType.b2_staticBody;
active = true;
gravityScale = 1.0f;
}
public static b2Vec2 ComputeCentroid(b2Vec2[] vs, int count)
{
b2Vec2 c = new b2Vec2();
c.Set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef = new b2Vec2(0.0f, 0.0f);
float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i)
{
// Triangle vertices.
b2Vec2 p1 = pRef;
b2Vec2 p2 = vs[i];
b2Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float D = b2Math.b2Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if (area <= b2Settings.b2_epsilon)
{
#if NETFX_CORE
throw (new OverflowException("Centroid is not defined, area is zero."));
#else
throw (new NotFiniteNumberException("Centroid is not defined, area is zero."));
#endif
}
c *= 1.0f / area;
return c;
}
public Mouse()
{
//m_destructionListener = new DestructionListener();
m_debugDraw = new CCBox2dDraw("fonts/arial-16");
b2Vec2 gravity = new b2Vec2();
gravity.Set(500, 500);
m_world = new b2World(gravity);
m_world.SetAllowSleeping(false);
m_world.SetContinuousPhysics(true);
m_world.SetDebugDraw(m_debugDraw);
m_debugDraw.AppendFlags(b2DrawFlags.e_shapeBit | b2DrawFlags.e_aabbBit | b2DrawFlags.e_centerOfMassBit | b2DrawFlags.e_jointBit | b2DrawFlags.e_pairBit);
m_world.SetContinuousPhysics(true);
m_world.SetWarmStarting(true);
}
public Dominos()
{
b2Body b1;
{
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
b2BodyDef bd = new b2BodyDef();
b1 = m_world.CreateBody(bd);
b1.CreateFixture(shape, 0.0f);
}
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(6.0f, 0.25f);
b2BodyDef bd = new b2BodyDef();
bd.position.Set(-1.5f, 10.0f);
b2Body ground = m_world.CreateBody(bd);
ground.CreateFixture(shape, 0.0f);
}
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
b2FixtureDef fd = new b2FixtureDef();
fd.shape = shape;
fd.density = 20.0f;
fd.friction = 0.1f;
for (int i = 0; i < 10; ++i)
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(-6.0f + 1.0f * i, 11.25f);
b2Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
}
}
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(7.0f, 0.25f, b2Vec2.Zero, 0.3f);
b2BodyDef bd = new b2BodyDef();
bd.position.Set(1.0f, 6.0f);
b2Body ground = m_world.CreateBody(bd);
ground.CreateFixture(shape, 0.0f);
}
b2Body b2;
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(0.25f, 1.5f);
b2BodyDef bd = new b2BodyDef();
bd.position.Set(-7.0f, 4.0f);
b2 = m_world.CreateBody(bd);
b2.CreateFixture(shape, 0.0f);
}
b2Body b3;
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(6.0f, 0.125f);
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(-0.9f, 1.0f);
bd.angle = -0.15f;
b3 = m_world.CreateBody(bd);
b3.CreateFixture(shape, 10.0f);
}
b2RevoluteJointDef jd = new b2RevoluteJointDef();
b2Vec2 anchor = new b2Vec2();
anchor.Set(-2.0f, 1.0f);
jd.Initialize(b1, b3, anchor);
jd.CollideConnected = true;
m_world.CreateJoint(jd);
b2Body b4;
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(0.25f, 0.25f);
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(-10.0f, 15.0f);
b4 = m_world.CreateBody(bd);
b4.CreateFixture(shape, 10.0f);
}
anchor.Set(-7.0f, 15.0f);
jd.Initialize(b2, b4, anchor);
m_world.CreateJoint(jd);
b2Body b5;
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(6.5f, 3.0f);
b5 = m_world.CreateBody(bd);
b2PolygonShape shape = new b2PolygonShape();
b2FixtureDef fd = new b2FixtureDef();
fd.shape = shape;
fd.density = 10.0f;
fd.friction = 0.1f;
shape.SetAsBox(1.0f, 0.1f, new b2Vec2(0.0f, -0.9f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f, new b2Vec2(-0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f, new b2Vec2(0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
}
anchor.Set(6.0f, 2.0f);
jd.Initialize(b1, b5, anchor);
m_world.CreateJoint(jd);
b2Body b6;
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(1.0f, 0.1f);
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(6.5f, 4.1f);
b6 = m_world.CreateBody(bd);
b6.CreateFixture(shape, 30.0f);
}
anchor.Set(7.5f, 4.0f);
jd.Initialize(b5, b6, anchor);
m_world.CreateJoint(jd);
b2Body b7;
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(7.4f, 1.0f);
b7 = m_world.CreateBody(bd);
b7.CreateFixture(shape, 10.0f);
}
b2DistanceJointDef djd = new b2DistanceJointDef();
djd.BodyA = b3;
djd.BodyB = b7;
djd.localAnchorA.Set(6.0f, 0.0f);
djd.localAnchorB.Set(0.0f, -1.0f);
b2Vec2 d = djd.BodyB.GetWorldPoint(djd.localAnchorB) - djd.BodyA.GetWorldPoint(djd.localAnchorA);
djd.length = d.Length;
m_world.CreateJoint(djd);
{
float radius = 0.2f;
b2CircleShape shape = new b2CircleShape();
shape.Radius = radius;
for (int i = 0; i < 4; ++i)
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(5.9f + 2.0f * radius * i, 2.4f);
b2Body body = m_world.CreateBody(bd);
body.CreateFixture(shape, 10.0f);
}
}
}
public virtual bool MouseDown(b2Vec2 p)
{
m_mouseWorld = p;
if (m_mouseJoint != null)
{
return false;
}
// Make a small box.
b2AABB aabb = new b2AABB();
b2Vec2 d = new b2Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
QueryCallback callback = new QueryCallback(p);
m_world.QueryAABB(callback, aabb);
if (callback.m_fixture != null)
{
b2Body body = callback.m_fixture.Body;
b2MouseJointDef md = new b2MouseJointDef();
md.BodyA = m_groundBody;
md.BodyB = body;
md.target = p;
md.maxForce = 1000.0f * body.Mass;
m_mouseJoint = (b2MouseJoint) m_world.CreateJoint(md);
body.SetAwake(true);
return true;
}
return false;
}
public Test()
{
m_destructionListener = new DestructionListener();
m_debugDraw = new CCBox2dDraw("fonts/arial-12", 1);
b2Vec2 gravity = new b2Vec2();
gravity.Set(0.0f, -10.0f);
m_world = new b2World(gravity);
m_bomb = null;
m_textLine = 30;
m_mouseJoint = null;
m_pointCount = 0;
m_destructionListener.test = this;
m_world.SetDestructionListener(m_destructionListener);
m_world.SetContactListener(this);
m_world.SetDebugDraw(m_debugDraw);
m_world.SetContinuousPhysics(true);
m_world.SetWarmStarting(true);
m_bombSpawning = false;
m_stepCount = 0;
b2BodyDef bodyDef = new b2BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
}
public override b2MassData ComputeMass(float density)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
b2Vec2 center = new b2Vec2();
center.Set(0.0f, 0.0f);
float area = 0.0f;
float I = 0.0f;
// s is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 s = new b2Vec2(0.0f, 0.0f);
// This code would put the reference point inside the polygon.
for (int i = 0; i < m_vertexCount; ++i)
{
s += Vertices[i];
}
s *= 1.0f / m_vertexCount;
float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < m_vertexCount; ++i)
{
// Triangle vertices.
b2Vec2 e1 = Vertices[i] - s;
b2Vec2 e2 = i + 1 < m_vertexCount ? Vertices[i + 1] - s : Vertices[0] - s;
float D = b2Math.b2Cross(ref e1, ref e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (e1 + e2);
float ex1 = e1.x, ey1 = e1.y;
float ex2 = e2.x, ey2 = e2.y;
float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
b2MassData massData = new b2MassData();
massData.mass = density * area;
// Center of mass
if (area <= b2Settings.b2_epsilon)
{
#if NETFX_CORE
throw (new OverflowException("Area is zero in mass calculation."));
#else
throw (new NotFiniteNumberException("Area is zero in mass calculation."));
#endif
}
center *= 1.0f / area;
massData.center = center + s;
// Inertia tensor relative to the local origin (point s).
massData.I = density * I;
// Shift to center of mass then to original body origin.
massData.I += massData.mass * (b2Math.b2Dot(ref massData.center, ref massData.center) - b2Math.b2Dot(ref center, ref center));
return (massData);
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));
b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float u = b2Math.b2Dot(e, B - Q);
float v = b2Math.b2Dot(e, Q - A);
float radius = edgeA.Radius + circleB.Radius;
b2ContactFeature cf = b2ContactFeature.Zero;
cf.indexB = 0;
cf.typeB = b2ContactFeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float dd = b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
b2Vec2 A1 = edgeA.Vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float u1 = b2Math.b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float dd = b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
b2Vec2 B2 = edgeA.Vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float v2 = b2Math.b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region AB
float den = b2Math.b2Dot(e, e);
System.Diagnostics.Debug.Assert(den > 0.0f);
b2Vec2 xP = (1.0f / den) * (u * A + v * B);
b2Vec2 xd = Q - xP;
float xdd = b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = new b2Vec2(-e.y, e.x);
if (b2Math.b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_face;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
}
public b2Vec2[] points; //< world contact point (point of intersection)
#endregion Fields
#region Methods
// Evaluate the manifold with supplied transforms. This assumes
// modest motion from the original state. This does not change the
// point count, impulses, etc. The radii must come from the shapes
// that generated the manifold.
public void Initialize(ref b2Manifold manifold,
b2Transform xfA, float radiusA,
b2Transform xfB, float radiusB)
{
points = new b2Vec2[b2Settings.b2_maxManifoldPoints];
for (int p = 0; p < b2Settings.b2_maxManifoldPoints; p++)
points[p] = b2Vec2.Zero;
normal = b2Vec2.Zero;
if (manifold.pointCount == 0)
{
return;
}
switch (manifold.type)
{
case b2ManifoldType.e_circles:
{
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Math.b2Mul(xfA, manifold.localPoint);
b2Vec2 pointB = b2Math.b2Mul(xfB, manifold.points[0].localPoint);
if (b2Math.b2DistanceSquared(pointA, pointB) > b2Settings.b2_epsilonSqrd)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case b2ManifoldType.e_faceA:
{
normal = b2Math.b2Mul(xfA.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfA, manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(xfB, manifold.points[i].localPoint);
b2Vec2 cA = clipPoint + (radiusA - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case b2ManifoldType.e_faceB:
{
normal = b2Math.b2Mul(xfB.q, manifold.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfB, manifold.localPoint);
for (int i = 0; i < manifold.pointCount; ++i)
{
b2Vec2 clipPoint = b2Math.b2Mul(xfA, manifold.points[i].localPoint);
b2Vec2 cB = clipPoint + (radiusB - b2Math.b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
private void MoveAABB(b2AABB aabb)
{
b2Vec2 d = new b2Vec2();
d.x = Rand.RandomFloat(-0.5f, 0.5f);
d.y = Rand.RandomFloat(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb.LowerBound += d;
aabb.UpperBound += d;
b2Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
b2Vec2 min = new b2Vec2();
min.Set(-m_worldExtent, 0.0f);
b2Vec2 max = new b2Vec2();
max.Set(m_worldExtent, 2.0f * m_worldExtent);
b2Vec2 c = b2Math.b2Clamp(c0, min, max);
aabb.LowerBound += c - c0;
aabb.UpperBound += c - c0;
}
private void GetRandomAABB(ref b2AABB aabb)
{
b2Vec2 w = new b2Vec2();
w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent);
//aabb->lowerBound.x = -m_proxyExtent;
//aabb->lowerBound.y = -m_proxyExtent + m_worldExtent;
aabb.LowerBoundX = Rand.RandomFloat(-m_worldExtent, m_worldExtent);
aabb.LowerBoundY = Rand.RandomFloat(0.0f, 2.0f * m_worldExtent);
aabb.UpperBound = aabb.LowerBound + w;
}
public static b2Vec2 b2Mul(b2Mat22 A, b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
return b;
}
