public static b2Vec2 operator *(float a, b2Vec2 v1)
{
b2Vec2 v = new b2Vec2();
v.Set(v1.x * a, v1.y * a);
return(v);
}
public void updateAutoAnchor()
{
if (autoConfigureAnchor)
{
Vector3 myPos = GetComponent <Transform>().position;
Transform connectedT = null;
Vector3 connectedPos = myPos;
if (connectedB2BodyObject != null)
{
connectedT = connectedB2BodyObject.GetComponent <Transform>();
connectedPos = connectedT.position;
}
float localAnchor1X = 0, localAnchor1Y = 0;
float localAnchor2X = myPos.x - connectedPos.x, localAnchor2Y = myPos.y - connectedPos.y;
if (connectedT != null)
{
Quaternion q = Quaternion.Euler(0, 0, -connectedT.rotation.eulerAngles.z);
Vector2 p = new Vector2(localAnchor2X, localAnchor2Y);
p = q * p;
localAnchor2X = p.x;
localAnchor2Y = p.y;
}
localAnchor1.Set(localAnchor1X, localAnchor1Y);
localAnchor2.Set(localAnchor2X, localAnchor2Y);
}
}
public static b2Vec2 operator -(b2Vec2 v1, b2Vec2 v2)
{
b2Vec2 v = new b2Vec2();
v.Set(v1.x - v2.x, v1.y - v2.y);
return(v);
}
public Test()
{
m_destructionListener = new DestructionListener();
m_debugDraw = new CCBox2dDraw("fonts/arial-12");
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);
}
/// <summary>
/// Negate this vector.
/// </summary>
public static b2Vec2 operator -(b2Vec2 v1)
{
b2Vec2 v = new b2Vec2();
v.Set(-v1.x, -v1.y);
return(v);
}
/// <summary>
/// Perform the cross product on a scalar and a vector.
/// In 2D this produces a vector.
/// </summary>
public static b2Vec2 Cross(float s, b2Vec2 a)
{
b2Vec2 v = new b2Vec2();
v.Set(-s * a.y, s * a.x);
return(v);
}
/// <summary>
/// Perform the cross product on a vector and a scalar.
/// In 2D this produces a vector.
/// </summary>
public static b2Vec2 Cross(b2Vec2 a, float s)
{
b2Vec2 v = new b2Vec2();
v.Set(s * a.y, -s * a.x);
return(v);
}
public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
if (m_frequencyHz > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
m_impulse.z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
b2Vec2 impulse1 = -b2Math.b2Mul22(m_mass, Cdot1);
m_impulse.x += impulse1.x;
m_impulse.y += impulse1.y;
b2Vec2 P = impulse1;
vA -= mA * P;
wA -= iA * b2Math.b2Cross(ref m_rA, ref P);
vB += mB * P;
wB += iB * b2Math.b2Cross(ref m_rB, ref P);
}
else
{
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
float Cdot2 = wB - wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -b2Math.b2Mul(m_mass, Cdot);
m_impulse += impulse;
b2Vec2 P = b2Vec2.Zero;
P.Set(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(ref m_rA, ref P) + impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(ref m_rB, ref P) + impulse.z);
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public b2DistanceJointDef()
{
JointType = b2JointType.e_distanceJoint;
localAnchorA.Set(0.0f, 0.0f);
localAnchorB.Set(0.0f, 0.0f);
length = 1.0f;
frequencyHz = 0.0f;
dampingRatio = 0.0f;
}
/**
* Ray cast against this segment with another segment
* @param xf the shape world transform.
* @param lambda returns the hit fraction. You can use this to compute the contact point
* p = (1 - lambda) * segment.p1 + lambda * segment.p2.
* @param normal returns the normal at the contact point. If there is no intersection, the normal
* is not set.
* @param segment defines the begin and end point of the ray cast.
* @param maxLambda a number typically in the range [0,1].
* @return true if there was an intersection.
* @see Box2D.Collision.Shapes.b2Shape#TestSegment
*/
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.4.1
// x = mu1 * p1 + mu2 * p2
// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
// mu1 = 1 - mu2;
// x = (1 - mu2) * p1 + mu2 * p2
// = p1 + mu2 * (p2 - p1)
// x = s + a * r (s := start, r := end - start)
// s + a * r = p1 + mu2 * d (d := p2 - p1)
// -a * r + mu2 * d = b (b := s - p1)
// [-r d] * [a; mu2] = b
// Cramer's rule:
// denom = det[-r d]
// a = det[b d] / denom
// mu2 = det[-r b] / denom
public bool TestSegment(List <float> lambda, // float pointer
b2Vec2 normal, // pointer
b2Segment segment,
float maxLambda)
{
//b2Vec2 s = segment.p1;
b2Vec2 s = segment.p1;
//b2Vec2 r = segment.p2 - s;
float rX = segment.p2.x - s.x;
float rY = segment.p2.y - s.y;
//b2Vec2 d = p2 - p1;
float dX = p2.x - p1.x;
float dY = p2.y - p1.y;
//b2Vec2 n = b2Cross(d, 1.0f);
float nX = dY;
float nY = -dX;
float k_slop = 100.0f * float.MinValue;
//var denom:Number = -b2Dot(r, n);
float denom = -(rX * nX + rY * nY);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
//b2Vec2 b = s - p1;
float bX = s.x - p1.x;
float bY = s.y - p1.y;
//var a:Number = b2Dot(b, n);
float a = (bX * nX + bY * nY);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -rX * bY + rY * bX;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
//n.Normalize();
float nLen = Mathf.Sqrt(nX * nX + nY * nY);
nX /= nLen;
nY /= nLen;
//*lambda = a;
lambda[0] = a;
//*normal = n;
normal.Set(nX, nY);
return(true);
}
}
}
return(false);
}
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 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(ref e1, ref 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);
}
/// This constructor sets the body definition default values.
public b2BodyDef()
{
userData = null;
position = new b2Vec2();
position.Set(0.0f, 0.0f);
angle = 0.0f;
linearVelocity = new b2Vec2();
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 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);
}
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;
}
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 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;
b2Vec2 diff;
// Barycentric coordinates
diff = B - Q;
float u = b2Math.b2Dot(ref e, ref diff); // B - Q);
diff = Q - A;
float v = b2Math.b2Dot(ref e, ref diff); // 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 = d.LengthSquared; // 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;
diff = B1 - Q;
float u1 = b2Math.b2Dot(ref e1, ref diff);
// 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 = d.LengthSquared; // 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;
diff = Q - A2;
float v2 = b2Math.b2Dot(ref e2, ref diff);
// 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 = e.Length; // 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 = xd.LengthSquared; // b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x);
n.m_x = -e.y;
n.m_y = e.x;
diff = Q - A;
if (b2Math.b2Dot(ref n, ref diff) < 0.0f)
{
// n.Set(-n.x, -n.y);
n.Set(-n.m_x, -n.m_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 override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
// Handle singularity.
float length = m_u.Length;
if (length > b2Settings.b2_linearSlop)
{
m_u *= 1.0f / length;
}
else
{
m_u.Set(0.0f, 0.0f);
}
float crAu = b2Math.b2Cross(m_rA, m_u);
float crBu = b2Math.b2Cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
// Compute the effective mass matrix.
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (m_frequencyHz > 0.0f)
{
float C = length - m_length;
// Frequency
float omega = 2.0f * (float)Math.PI * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
float k = m_mass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invMass += m_gamma;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = m_impulse * m_u;
vA -= m_invMassA * P;
wA -= m_invIA * b2Math.b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * b2Math.b2Cross(m_rB, P);
}
else
{
m_impulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
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);
}
}
}
// The normal points from 1 to 2
static public void CollidePolygons(b2Manifold manifold,
b2PolygonShape polyA, b2Transform xfA,
b2PolygonShape polyB, b2Transform xfB)
{
ClipVertex cv;
manifold.m_pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
int edgeA = 0;
s_edgeAO[0] = edgeA;
float separationA = FindMaxSeparation(s_edgeAO, polyA, xfA, polyB, xfB);
edgeA = s_edgeAO[0];
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
s_edgeBO[0] = edgeB;
float separationB = FindMaxSeparation(s_edgeBO, polyB, xfB, polyA, xfA);
edgeB = s_edgeBO[0];
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference poly
b2PolygonShape poly2; // incident poly
b2Transform xf1;
b2Transform xf2;
int edge1; // reference edge
uint flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2Mat22 tMat;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.m_type = b2Manifold.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.m_type = b2Manifold.e_faceA;
flip = 0;
}
ClipVertex[] incidentEdge = s_incidentEdge;
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
List <b2Vec2> vertices1 = poly1.m_vertices;
b2Vec2 local_v11 = vertices1[edge1];
b2Vec2 local_v12;
if (edge1 + 1 < count1)
{
local_v12 = vertices1[(int)(edge1 + 1)];
}
else
{
local_v12 = vertices1[0];
}
b2Vec2 localTangent = s_localTangent;
localTangent.Set(local_v12.x - local_v11.x, local_v12.y - local_v11.y);
localTangent.Normalize();
b2Vec2 localNormal = s_localNormal;
localNormal.x = localTangent.y;
localNormal.y = -localTangent.x;
b2Vec2 planePoint = s_planePoint;
planePoint.Set(0.5f * (local_v11.x + local_v12.x), 0.5f * (local_v11.y + local_v12.y));
b2Vec2 tangent = s_tangent;
//tangent = b2Math.b2MulMV(xf1.R, localTangent);
tMat = xf1.R;
tangent.x = (tMat.col1.x * localTangent.x + tMat.col2.x * localTangent.y);
tangent.y = (tMat.col1.y * localTangent.x + tMat.col2.y * localTangent.y);
b2Vec2 tangent2 = s_tangent2;
tangent2.x = -tangent.x;
tangent2.y = -tangent.y;
b2Vec2 normal = s_normal;
normal.x = tangent.y;
normal.y = -tangent.x;
//v11 = b2Math.MulX(xf1, local_v11);
//v12 = b2Math.MulX(xf1, local_v12);
b2Vec2 v11 = s_v11;
b2Vec2 v12 = s_v12;
v11.x = xf1.position.x + (tMat.col1.x * local_v11.x + tMat.col2.x * local_v11.y);
v11.y = xf1.position.y + (tMat.col1.y * local_v11.x + tMat.col2.y * local_v11.y);
v12.x = xf1.position.x + (tMat.col1.x * local_v12.x + tMat.col2.x * local_v12.y);
v12.y = xf1.position.y + (tMat.col1.y * local_v12.x + tMat.col2.y * local_v12.y);
// Face offset
float frontOffset = normal.x * v11.x + normal.y * v11.y;
// Side offsets, extended by polytope skin thickness
float sideOffset1 = -tangent.x * v11.x - tangent.y * v11.y + totalRadius;
float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = s_clipPoints1;
ClipVertex[] clipPoints2 = s_clipPoints2;
int np;
// Clip to box side 1
//np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
np = ClipSegmentToLine(clipPoints1, incidentEdge, tangent2, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.m_localPlaneNormal.SetV(localNormal);
manifold.m_localPoint.SetV(planePoint);
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
cv = clipPoints2[i];
float separation = normal.x * cv.v.x + normal.y * cv.v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.m_points[pointCount];
//cp.m_localPoint = b2Math.b2MulXT(xf2, cv.v);
tMat = xf2.R;
float tX = cv.v.x - xf2.position.x;
float tY = cv.v.y - xf2.position.y;
cp.m_localPoint.x = (tX * tMat.col1.x + tY * tMat.col1.y);
cp.m_localPoint.y = (tX * tMat.col2.x + tY * tMat.col2.y);
cp.m_id.Set(cv.id);
cp.m_id.features.flip = (int)flip;
++pointCount;
}
}
manifold.m_pointCount = pointCount;
}